Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
autopilot_guided.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2016 The Paparazzi Team
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
30 #include "autopilot.h"
32 #include "state.h"
33 #include "pprzlink/dl_protocol.h"
34 
35 
36 bool autopilot_guided_goto_ned(float x, float y, float z, float heading)
37 {
42  return true;
43  }
44  return false;
45 }
46 
47 bool autopilot_guided_goto_ned_relative(float dx, float dy, float dz, float dyaw)
48 {
50  float x = stateGetPositionNed_f()->x + dx;
51  float y = stateGetPositionNed_f()->y + dy;
52  float z = stateGetPositionNed_f()->z + dz;
53  float heading = stateGetNedToBodyEulers_f()->psi + dyaw;
54  return autopilot_guided_goto_ned(x, y, z, heading);
55  }
56  return false;
57 }
58 
59 bool autopilot_guided_goto_body_relative(float dx, float dy, float dz, float dyaw)
60 {
63  float x = stateGetPositionNed_f()->x + cosf(-psi) * dx + sinf(-psi) * dy;
64  float y = stateGetPositionNed_f()->y - sinf(-psi) * dx + cosf(-psi) * dy;
65  float z = stateGetPositionNed_f()->z + dz;
66  float heading = psi + dyaw;
67  return autopilot_guided_goto_ned(x, y, z, heading);
68  }
69  return false;
70 }
71 
72 bool autopilot_guided_move_ned(float vx, float vy, float vz, float heading)
73 {
78  return true;
79  }
80  return false;
81 }
82 
83 /* Set guided mode setpoint
84  * Note: Offset position command in NED frame or body frame will only be implemented if
85  * local reference frame has been initialised.
86  * Flag definition:
87  bit 0: x,y as offset coordinates
88  bit 1: x,y in body coordinates
89  bit 2: z as offset coordinates
90  bit 3: yaw as offset coordinates
91  bit 4: free
92  bit 5: x,y as vel
93  bit 6: z as vel
94  bit 7: yaw as rate
95  */
96 void autopilot_guided_update(uint8_t flags, float x, float y, float z, float yaw)
97 {
98  /* only update setpoints when in guided mode */
100  return;
101  }
102 
103  // handle x,y
104  struct FloatVect2 setpoint = {.x = x, .y = y};
105  if (bit_is_set(flags, 5)) { // velocity setpoint
106  if (bit_is_set(flags, 1)) { // set velocity in body frame
107  guidance_h_set_guided_body_vel(setpoint.x, setpoint.y);
108  } else {
109  guidance_h_set_guided_vel(setpoint.x, setpoint.y);
110  }
111  } else { // position setpoint
112  if (!bit_is_set(flags, 0) && !bit_is_set(flags, 1)) { // set absolute position setpoint
113  guidance_h_set_guided_pos(setpoint.x, setpoint.y);
114  } else {
116  if (bit_is_set(flags, 1)) { // set position as offset in body frame
117  float psi = stateGetNedToBodyEulers_f()->psi;
118 
119  setpoint.x = stateGetPositionNed_f()->x + cosf(-psi) * x + sinf(-psi) * y;
120  setpoint.y = stateGetPositionNed_f()->y - sinf(-psi) * x + cosf(-psi) * y;
121  } else { // set position as offset in NED frame
122  setpoint.x += stateGetPositionNed_f()->x;
123  setpoint.y += stateGetPositionNed_f()->y;
124  }
125  guidance_h_set_guided_pos(setpoint.x, setpoint.y);
126  }
127  }
128  }
129 
130  //handle z
131  if (bit_is_set(flags, 6)) { // speed set-point
133  } else { // position set-point
134  if (bit_is_set(flags, 2)) { // set position as offset in NED frame
136  z += stateGetPositionNed_f()->z;
138  }
139  } else {
141  }
142  }
143 
144  //handle yaw
145  if (bit_is_set(flags, 7)) { // speed set-point
147  } else { // position set-point
148  if (bit_is_set(flags, 3)) { // set yaw as offset
149  yaw += stateGetNedToBodyEulers_f()->psi; // will be wrapped to [-pi,pi] later
150  }
152  }
153 }
154 
158  if (DL_GUIDED_SETPOINT_NED_ac_id(buf) != AC_ID) { return; }
159 
161  DL_GUIDED_SETPOINT_NED_flags(buf),
162  DL_GUIDED_SETPOINT_NED_x(buf),
163  DL_GUIDED_SETPOINT_NED_y(buf),
164  DL_GUIDED_SETPOINT_NED_z(buf),
165  DL_GUIDED_SETPOINT_NED_yaw(buf));
166 }
167 
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
guidance_v_set_guided_vz
bool guidance_v_set_guided_vz(float vz)
Set z velocity setpoint in GUIDED mode.
Definition: guidance_v.c:568
stateGetPositionNed_f
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
Definition: state.h:710
stateIsLocalCoordinateValid
static bool stateIsLocalCoordinateValid(void)
Test if local coordinates are valid.
Definition: state.h:508
NedCoor_f::z
float z
in meters
Definition: pprz_geodetic_float.h:66
stateGetNedToBodyEulers_f
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
autopilot_guided_goto_ned_relative
bool autopilot_guided_goto_ned_relative(float dx, float dy, float dz, float dyaw)
Set position and heading setpoints wrt.
Definition: autopilot_guided.c:47
guidance_h_set_guided_pos
bool guidance_h_set_guided_pos(float x, float y)
Set horizontal position setpoint in GUIDED mode.
Definition: guidance_h.c:720
autopilot_guided_goto_ned
bool autopilot_guided_goto_ned(float x, float y, float z, float heading)
Set position and heading setpoints in GUIDED mode.
Definition: autopilot_guided.c:36
AP_MODE_GUIDED
#define AP_MODE_GUIDED
Definition: autopilot_static.h:54
FloatVect2
Definition: pprz_algebra_float.h:49
autopilot_guided.h
guidance_h_set_guided_body_vel
bool guidance_h_set_guided_body_vel(float vx, float vy)
Set body relative horizontal velocity setpoint in GUIDED mode.
Definition: guidance_h.c:742
autopilot_guided_move_ned
bool autopilot_guided_move_ned(float vx, float vy, float vz, float heading)
Set velocity and heading setpoints in GUIDED mode.
Definition: autopilot_guided.c:72
guidance.h
autopilot.h
guidance_h_set_guided_heading
bool guidance_h_set_guided_heading(float heading)
Set heading setpoint in GUIDED mode.
Definition: guidance_h.c:731
FloatVect2::y
float y
Definition: pprz_algebra_float.h:51
NedCoor_f::y
float y
in meters
Definition: pprz_geodetic_float.h:65
guidance_v_set_guided_z
bool guidance_v_set_guided_z(float z)
Set z setpoint in GUIDED mode.
Definition: guidance_v.c:551
autopilot_guided_goto_body_relative
bool autopilot_guided_goto_body_relative(float dx, float dy, float dz, float dyaw)
Set position and heading setpoints wrt.
Definition: autopilot_guided.c:59
autopilot_guided_parse_GUIDED
void autopilot_guided_parse_GUIDED(uint8_t *buf)
Parse GUIDED_SETPOINT_NED messages from datalink.
Definition: autopilot_guided.c:157
NedCoor_f::x
float x
in meters
Definition: pprz_geodetic_float.h:64
FloatVect2::x
float x
Definition: pprz_algebra_float.h:50
autopilot_get_mode
uint8_t autopilot_get_mode(void)
get autopilot mode
Definition: autopilot.c:216
state.h
autopilot_guided_update
void autopilot_guided_update(uint8_t flags, float x, float y, float z, float yaw)
Set guided setpoints using flag mask in GUIDED mode.
Definition: autopilot_guided.c:96
FloatEulers::psi
float psi
in radians
Definition: pprz_algebra_float.h:87
guidance_h_set_guided_vel
bool guidance_h_set_guided_vel(float vx, float vy)
Set horizontal velocity setpoint in GUIDED mode.
Definition: guidance_h.c:750
guidance_h_set_guided_heading_rate
bool guidance_h_set_guided_heading_rate(float rate)
Set heading rate setpoint in GUIDED mode.
Definition: guidance_h.c:761
heading
float heading
Definition: wedgebug.c:258