Paparazzi UAS  v7.0_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 #ifdef AP_MODE_GUIDED
36 
37 bool autopilot_guided_goto_ned(float x, float y, float z, float heading)
38 {
40  guidance_h_set_pos(x, y);
43  return true;
44  }
45  return false;
46 }
47 
48 bool autopilot_guided_goto_ned_relative(float dx, float dy, float dz, float dyaw)
49 {
51  float x = stateGetPositionNed_f()->x + dx;
52  float y = stateGetPositionNed_f()->y + dy;
53  float z = stateGetPositionNed_f()->z + dz;
54  float heading = stateGetNedToBodyEulers_f()->psi + dyaw;
55  return autopilot_guided_goto_ned(x, y, z, heading);
56  }
57  return false;
58 }
59 
60 bool autopilot_guided_goto_body_relative(float dx, float dy, float dz, float dyaw)
61 {
63  float psi = stateGetNedToBodyEulers_f()->psi;
64  float x = stateGetPositionNed_f()->x + cosf(-psi) * dx + sinf(-psi) * dy;
65  float y = stateGetPositionNed_f()->y - sinf(-psi) * dx + cosf(-psi) * dy;
66  float z = stateGetPositionNed_f()->z + dz;
67  float heading = psi + dyaw;
68  return autopilot_guided_goto_ned(x, y, z, heading);
69  }
70  return false;
71 }
72 
73 bool autopilot_guided_move_ned(float vx, float vy, float vz, float heading)
74 {
76  guidance_h_set_vel(vx, vy);
79  return true;
80  }
81  return false;
82 }
83 
84 /* Set guided mode setpoint
85  * Note: Offset position command in NED frame or body frame will only be implemented if
86  * local reference frame has been initialised.
87  * Flag definition:
88  bit 0: x,y as offset coordinates
89  bit 1: x,y in body coordinates
90  bit 2: z as offset coordinates
91  bit 3: yaw as offset coordinates
92  bit 4: free
93  bit 5: x,y as vel
94  bit 6: z as vel
95  bit 7: yaw as rate
96  */
97 void autopilot_guided_update(uint8_t flags, float x, float y, float z, float yaw)
98 {
99  // handle x,y
100  struct FloatVect2 setpoint = {.x = x, .y = y};
101  if (bit_is_set(flags, 5)) { // velocity setpoint
102  if (bit_is_set(flags, 1)) { // set velocity in body frame
103  guidance_h_set_body_vel(setpoint.x, setpoint.y);
104  } else {
105  guidance_h_set_vel(setpoint.x, setpoint.y);
106  }
107  } else { // position setpoint
108  if (!bit_is_set(flags, 0) && !bit_is_set(flags, 1)) { // set absolute position setpoint
109  guidance_h_set_pos(setpoint.x, setpoint.y);
110  } else {
112  if (bit_is_set(flags, 1)) { // set position as offset in body frame
113  float psi = stateGetNedToBodyEulers_f()->psi;
114 
115  setpoint.x = stateGetPositionNed_f()->x + cosf(-psi) * x + sinf(-psi) * y;
116  setpoint.y = stateGetPositionNed_f()->y - sinf(-psi) * x + cosf(-psi) * y;
117  } else { // set position as offset in NED frame
118  setpoint.x += stateGetPositionNed_f()->x;
119  setpoint.y += stateGetPositionNed_f()->y;
120  }
121  guidance_h_set_pos(setpoint.x, setpoint.y);
122  }
123  }
124  }
125 
126  //handle z
127  if (bit_is_set(flags, 6)) { // speed set-point
129  } else { // position set-point
130  if (bit_is_set(flags, 2)) { // set position as offset in NED frame
132  z += stateGetPositionNed_f()->z;
133  guidance_v_set_z(z);
134  }
135  } else {
136  guidance_v_set_z(z);
137  }
138  }
139 
140  //handle yaw
141  if (bit_is_set(flags, 7)) { // speed set-point
143  } else { // position set-point
144  if (bit_is_set(flags, 3)) { // set yaw as offset
145  yaw += stateGetNedToBodyEulers_f()->psi; // will be wrapped to [-pi,pi] later
146  }
148  }
149 }
150 
154  if (DL_GUIDED_SETPOINT_NED_ac_id(buf) != AC_ID || autopilot_get_mode() != AP_MODE_GUIDED) {
155  return;
156  }
157 
159  DL_GUIDED_SETPOINT_NED_flags(buf),
160  DL_GUIDED_SETPOINT_NED_x(buf),
161  DL_GUIDED_SETPOINT_NED_y(buf),
162  DL_GUIDED_SETPOINT_NED_z(buf),
163  DL_GUIDED_SETPOINT_NED_yaw(buf));
164 }
165 
166 #else
167 
168 bool autopilot_guided_goto_ned(float x, float y, float z, float heading)
169 {
170  (void) x;
171  (void) y;
172  (void) z;
173  (void) heading;
174  return false;
175 }
176 
177 bool autopilot_guided_goto_ned_relative(float dx, float dy, float dz, float dyaw)
178 {
179  (void) dx;
180  (void) dy;
181  (void) dz;
182  (void) dyaw;
183  return false;
184 }
185 
186 bool autopilot_guided_goto_body_relative(float dx, float dy, float dz, float dyaw)
187 {
188  (void) dx;
189  (void) dy;
190  (void) dz;
191  (void) dyaw;
192  return false;
193 }
194 
195 bool autopilot_guided_move_ned(float vx, float vy, float vz, float heading)
196 {
197  (void) vx;
198  (void) vy;
199  (void) vz;
200  (void) heading;
201  return false;
202 }
203 
204 void autopilot_guided_update(uint8_t flags, float x, float y, float z, float yaw)
205 {
206  (void) flags;
207  (void) x;
208  (void) y;
209  (void) z;
210  (void) yaw;
211 }
212 
216  (void) buf;
217 }
218 
219 #endif
220 
uint8_t autopilot_get_mode(void)
get autopilot mode
Definition: autopilot.c:217
Core autopilot interface common to all firmwares.
void autopilot_guided_update(uint8_t flags, float x, float y, float z, float yaw)
Set guided setpoints using flag mask in GUIDED mode.
bool autopilot_guided_move_ned(float vx, float vy, float vz, float heading)
Set velocity and heading setpoints in GUIDED mode.
void autopilot_guided_parse_GUIDED(uint8_t *buf)
Parse GUIDED_SETPOINT_NED messages from datalink.
bool autopilot_guided_goto_ned_relative(float dx, float dy, float dz, float dyaw)
Set position and heading setpoints wrt.
bool autopilot_guided_goto_body_relative(float dx, float dy, float dz, float dyaw)
Set position and heading setpoints wrt.
bool autopilot_guided_goto_ned(float x, float y, float z, float heading)
Set position and heading setpoints in GUIDED mode.
Autopilot guided mode interface.
float psi
in radians
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
Definition: state.h:710
static bool stateIsLocalCoordinateValid(void)
Test if local coordinates are valid.
Definition: state.h:508
float z
in meters
float x
in meters
float y
in meters
#define AP_MODE_GUIDED
void guidance_h_set_pos(float x, float y)
Set horizontal position setpoint.
Definition: guidance_h.c:452
void guidance_h_set_heading(float heading)
Set heading setpoint.
Definition: guidance_h.c:462
void guidance_h_set_vel(float vx, float vy)
Set horizontal acceleration setpoint.
Definition: guidance_h.c:477
void guidance_h_set_body_vel(float vx, float vy)
Set body relative horizontal velocity setpoint.
Definition: guidance_h.c:469
void guidance_h_set_heading_rate(float rate)
Set heading rate setpoint.
Definition: guidance_h.c:505
void guidance_v_set_vz(float vz)
Set z velocity setpoint.
Definition: guidance_v.c:378
void guidance_v_set_z(float z)
Set z position setpoint.
Definition: guidance_v.c:368
API to get/set the generic vehicle states.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
float heading
Definition: wedgebug.c:258