Paparazzi UAS  v6.3_unstable
Paparazzi is a free software Unmanned Aircraft System.
autopilot_static.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2003 Pascal Brisset, Antoine Drouin
3  * Copyright (C) 2017 Gautier Hattenberger <gautier.hattenberger@enac.fr>
4  *
5  * This file is part of paparazzi.
6  *
7  * paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with paparazzi; see the file COPYING. If not, see
19  * <http://www.gnu.org/licenses/>.
20  */
21 
29 #include "autopilot.h"
31 
32 #include "state.h"
35 #include "generated/flight_plan.h"
36 
37 /* Geofence exceptions */
39 
40 #if USE_GPS
41 #include "modules/gps/gps.h"
42 #endif
43 
45 
46 static inline uint8_t pprz_mode_update(void);
47 
49 #ifndef RC_LOST_MODE
50 #define RC_LOST_MODE AP_MODE_HOME
51 #endif
52 
53 
55 #ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
56 #include "modules/core/abi.h"
57 volatile uint8_t new_ins_attitude = 0;
58 static abi_event new_att_ev;
59 static void new_att_cb(uint8_t sender_id __attribute__((unused)),
60  uint32_t stamp __attribute__((unused)),
61  struct Int32Rates *gyro __attribute__((unused)))
62 {
63  new_ins_attitude = 1;
64 }
65 
68 void autopilot_event(void)
69 {
70  if (new_ins_attitude > 0) {
71  attitude_loop();
72  new_ins_attitude = 0;
73  }
74 }
75 #endif
76 
77 
82 {
85 
87 #ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
88  AbiBindMsgIMU_GYRO(ABI_BROADCAST, &new_att_ev, &new_att_cb);
89 #endif
90 
91 }
92 
94 {
95 }
96 
101 {
102  // Send back uncontrolled channels.
103 #ifdef SetAutoCommandsFromRC
104  SetAutoCommandsFromRC(commands, radio_control.values);
105 #elif defined RADIO_YAW && defined COMMAND_YAW
106  command_set(COMMAND_YAW, radio_control_get(RADIO_YAW));
107 #endif
108 
109 #if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
116 
119 #if H_CTL_YAW_LOOP && defined RADIO_YAW
121  h_ctl_yaw_rate_setpoint = FLOAT_OF_PPRZ(radio_control_get(RADIO_YAW), 0., AUTO1_MAX_YAW_RATE);
122 #endif
123  // Note that old SetApOnlyCommands is no longer needed without a separated FBW
124  } else if (autopilot_get_mode() == AP_MODE_MANUAL) {
125  // Set commands from RC in MANUAL mode
126  SetCommandsFromRC(commands, radio_control.values);
128  }
135  }
137 #endif
138 }
139 
140 void autopilot_static_set_mode(uint8_t new_autopilot_mode)
141 {
142  if (new_autopilot_mode != autopilot.mode) {
143  autopilot.mode = new_autopilot_mode;
144  }
145 }
146 
147 void autopilot_static_SetModeHandler(float new_autopilot_mode)
148 {
149  autopilot_static_set_mode(new_autopilot_mode);
150 }
151 
152 void autopilot_static_set_motors_on(bool motors_on)
153 {
154  // it doesn't make real sense on fixedwing, as you can still use throttle
155  // in MAN and AUTO1 modes while have motor killed for AUTO2
156  // only needed for consistency with other firmwares
157  autopilot.motors_on = motors_on;
158 }
159 
163 void navigation_task(void)
164 {
165 
167  if (autopilot_get_mode() == AP_MODE_HOME) {
168  nav_home();
170  nav_without_gps();
171  } else {
173  }
174 
175 #ifdef TCAS
176  callTCAS();
177 #endif
178 
179 #if DOWNLINK && !defined PERIOD_NAVIGATION_Ap_0 // If not sent periodically (in default 0 mode)
180  SEND_NAVIGATION(&(DefaultChannel).trans_tx, &(DefaultDevice).device);
181 #endif
182 
183  /* The nav task computes only nav_altitude. However, we are interested
184  by desired_altitude (= nav_alt+alt_shift) in any case.
185  So we always run the altitude control loop */
188  }
189 
193 #ifdef H_CTL_RATE_LOOP
194  /* Be sure to be in attitude mode, not roll */
195  h_ctl_auto1_rate = false;
196 #endif
198  h_ctl_course_loop(); /* aka compute nav_desired_roll */
199  }
200 
201  }
202 }
203 
217 {
218  uint8_t mode_changed = false;
219 
220 #if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
221  /* check normal mode from RC channel(s)
222  */
223  if (!RadioControlIsLost()) {
224  bool pprz_mode_changed = pprz_mode_update();
225  mode_changed |= pprz_mode_changed;
226 #if defined RADIO_CALIB && defined RADIO_CONTROL_SETTINGS
227  bool calib_mode_changed = RcSettingsModeUpdate(radio_control.values);
228  rc_settings(calib_mode_changed || pprz_mode_changed);
229  mode_changed |= calib_mode_changed;
230 #endif
231  }
232 
233  /* RC lost while in use is true if we lost RC in MANUAL or AUTO1
234  *
235  * RC_LOST_MODE defaults to AP_MODE_HOME, but it can also be set to NAV_MODE_NAV or MANUAL when the RC receiver is well configured to send failsafe commands
236  */
237  if (RadioControlIsLost() &&
240  mode_changed |= autopilot_set_mode(RC_LOST_MODE);
241  }
242 
243  /* Check RC kill switch
244  */
245 #ifdef RADIO_KILL_SWITCH
247  autopilot_set_kill_throttle(true); // not a mode change, just set kill_throttle
248  }
249 #endif
250 
251  /* the SITL check is a hack to prevent "automatic" launch in NPS
252  */
253 #ifndef SITL
254  if (!autopilot.flight_time) {
256  autopilot.launch = true; // set launch to true from RC throttel up
257  }
258  }
259 #endif
260 
261 #endif // RADIO_CONTROL
262 
263 #if USE_GPS && (defined FAILSAFE_DELAY_WITHOUT_GPS)
264  /* This section is used for the failsafe of GPS
265  */
266  static uint8_t last_pprz_mode;
267  static bool gps_lost = false;
268 
271  if (autopilot.launch) {
272  // check GPS timeout
273  if (sys_time.nb_sec - gps.last_3dfix_time > FAILSAFE_DELAY_WITHOUT_GPS) {
276  last_pprz_mode = autopilot_get_mode();
278  gps_lost = true;
279  }
280  } else if (gps_lost) { /* GPS is ok */
282  mode_changed |= autopilot_set_mode(last_pprz_mode);
283  gps_lost = false;
284  }
285  }
286 #endif /* GPS && FAILSAFE_DELAY_WITHOUT_GPS */
287 
288  /* If in-flight, with good GPS but too far, then activate HOME mode
289  * In MANUAL with good RC, FBW will allow to override. */
290  if (autopilot_get_mode() != AP_MODE_HOME &&
292  autopilot.launch) {
294  mode_changed |= autopilot_set_mode(AP_MODE_HOME);
295  }
296  }
297 
298  // send new mode if needed
299  if (mode_changed) {
301  }
302 }
303 
304 
305 void attitude_loop(void)
306 {
307  // Call vertical climb loop if mode is at least AUTO2
309 #if CTRL_VERTICAL_LANDING
312  } else {
313 #endif
317  } else {
320  }
321  }
322 #if CTRL_VERTICAL_LANDING
323  }
324 #endif
325 
326 #if defined V_CTL_THROTTLE_IDLE
327  Bound(v_ctl_throttle_setpoint, TRIM_PPRZ(V_CTL_THROTTLE_IDLE * MAX_PPRZ), MAX_PPRZ);
328 #endif
329 
330 #ifdef V_CTL_POWER_CTL_BAT_NOMINAL
331  if (electrical.vsupply > 0.f) {
332  v_ctl_throttle_setpoint *= V_CTL_POWER_CTL_BAT_NOMINAL / electrical.vsupply;
334  }
335 #endif
336 
337  // Copy the pitch setpoint from the guidance to the stabilization control
339  Bound(h_ctl_pitch_setpoint, H_CTL_PITCH_MIN_SETPOINT, H_CTL_PITCH_MAX_SETPOINT);
342  }
343  }
344 
345  // Call attitude control and set commands if mode is at least AUTO1
347  h_ctl_attitude_loop(); /* Set h_ctl_aileron_setpoint & h_ctl_elevator_setpoint */
352  AP_COMMAND_SET_YAW(h_ctl_rudder_setpoint);
353  AP_COMMAND_SET_CL(h_ctl_flaps_setpoint);
354  }
355 
356 }
357 
358 
361 #if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
362 static inline uint8_t pprz_mode_update(void)
363 {
364  if ((autopilot_get_mode() != AP_MODE_HOME &&
366 #ifdef UNLOCKED_HOME_MODE
367  || true
368 #endif
369  ) {
370 #ifndef RADIO_AUTO_MODE
372  bool b = autopilot_set_mode(nm);
373  return b;
374  //return autopilot_set_mode(AP_MODE_OF_PULSE(radio_control_get(RADIO_MODE)));
375 #else
376  INFO("Using RADIO_AUTO_MODE to switch between AUTO1 and AUTO2.")
377  /* If RADIO_AUTO_MODE is enabled mode swithing will be seperated between
378  * two switches/channels
379  * RADIO_MODE will switch between AP_MODE_MANUAL and any AP_MODE_AUTO mode
380  * selected by RADIO_AUTO_MODE.
381  *
382  * This is mainly a cludge for entry level radios with no three-way switch
383  * but two available two-way switches which can be used.
384  */
386  /* RADIO_MODE in MANUAL position */
388  } else {
389  /* RADIO_MODE not in MANUAL position.
390  * Select AUTO mode bassed on RADIO_AUTO_MODE channel
391  */
393  }
394 #endif // RADIO_AUTO_MODE
395  } else {
396  return false;
397  }
398 }
399 #else // not RADIO_CONTROL
400 static inline uint8_t pprz_mode_update(void)
401 {
402  return false;
403 }
404 #endif
405 
406 
Main include for ABI (AirBorneInterface).
#define ABI_BROADCAST
Broadcast address.
Definition: abi_common.h:57
Event structure to store callbacks in a linked list.
Definition: abi_common.h:66
void WEAK autopilot_event(void)
AP event call.
Definition: autopilot.c:173
bool autopilot_set_mode(uint8_t new_autopilot_mode)
set autopilot mode
Definition: autopilot.c:192
void WEAK autopilot_send_mode(void)
send autopilot mode actual implementation is firmware dependent
Definition: autopilot.c:324
uint8_t autopilot_get_mode(void)
get autopilot mode
Definition: autopilot.c:216
struct pprz_autopilot autopilot
Global autopilot structure.
Definition: autopilot.c:48
void autopilot_set_kill_throttle(bool kill)
set kill throttle
Definition: autopilot.c:251
Core autopilot interface common to all firmwares.
bool motors_on
motor status
Definition: autopilot.h:68
bool launch
request launch
Definition: autopilot.h:71
pprz_t throttle
throttle level as will be displayed in GCS
Definition: autopilot.h:66
bool kill_throttle
allow autopilot to use throttle
Definition: autopilot.h:69
uint8_t mode
current autopilot mode
Definition: autopilot.h:63
uint16_t flight_time
flight time in seconds
Definition: autopilot.h:65
pprz_t commands[COMMANDS_NB]
Definition: commands.c:30
void common_nav_periodic_task()
Definition: common_nav.c:153
struct Electrical electrical
Definition: electrical.c:66
float vsupply
supply voltage in V
Definition: electrical.h:45
pprz_t v_ctl_throttle_setpoint
Definition: energy_ctrl.c:131
void v_ctl_climb_loop(void)
Auto-throttle inner loop.
Definition: energy_ctrl.c:324
float v_ctl_pitch_setpoint
Definition: energy_ctrl.c:133
void v_ctl_throttle_slew(void)
Computes slewed throttle from throttle setpoint called at 20Hz.
Definition: energy_ctrl.c:437
pprz_t v_ctl_throttle_slewed
Definition: energy_ctrl.c:132
void v_ctl_altitude_loop(void)
outer loop
Definition: energy_ctrl.c:286
uint8_t v_ctl_mode
Definition: energy_ctrl.c:74
uint8_t lateral_mode
#define LATERAL_MODE_MANUAL
#define LATERAL_MODE_COURSE
void attitude_loop(void)
void autopilot_static_init(void)
Static autopilot API.
void navigation_task(void)
Compute desired_course.
void autopilot_static_periodic(void)
static uint8_t pprz_mode_update(void)
Update paparazzi mode from RC.
void autopilot_static_SetModeHandler(float new_autopilot_mode)
void autopilot_static_set_motors_on(bool motors_on)
#define RC_LOST_MODE
mode to enter when RC is lost in AP_MODE_MANUAL or AP_MODE_AUTO1
void autopilot_failsafe_checks(void)
Failsafe checks.
void autopilot_static_on_rc_frame(void)
Function to be called when a message from FBW is available.
void autopilot_static_set_mode(uint8_t new_autopilot_mode)
Fixedwing autopilot modes (static implementation).
#define AP_MODE_HOME
#define AP_MODE_MANUAL
AP modes.
#define AP_MODE_GPS_OUT_OF_ORDER
#define AP_MODE_AUTO2
#define AP_MODE_AUTO1
#define AP_COMMAND_SET_THROTTLE(_throttle)
#define THRESHOLD2
#define AP_COMMAND_SET_CL(_cl)
#define AP_COMMAND_SET_PITCH(_pitch)
#define THROTTLE_THRESHOLD_TAKEOFF
Takeoff detection threshold from throttle.
#define AP_COMMAND_SET_ROLL(_roll)
AP command setter macros for usual commands.
#define FLOAT_OF_PPRZ(pprz, center, travel)
pprz_t to float with saturation
#define AP_COMMAND_SET_YAW(_yaw)
#define AP_MODE_OF_PULSE(pprz)
Get mode from pulse.
Fixed wing horizontal control.
struct GpsState gps
global GPS state
Definition: gps.c:69
Device independent GPS code (interface)
uint32_t last_3dfix_time
cpu time in sec at last valid 3D fix
Definition: gps.h:118
angular rates
void v_ctl_landing_loop(void)
Definition: guidance_v.c:357
#define V_CTL_MODE_AUTO_THROTTLE
#define V_CTL_MODE_LANDING
#define V_CTL_MODE_AUTO_ALT
#define V_CTL_MODE_AUTO_CLIMB
volatile uint8_t new_ins_attitude
void nav_home(void)
Home mode navigation (circle around HOME)
Definition: nav.c:425
pprz_t nav_throttle_setpoint
Definition: nav.c:310
void nav_without_gps(void)
Failsafe navigation without position estimation.
Definition: nav.c:571
float nav_pitch
Definition: nav.c:311
void nav_periodic_task(void)
Navigation main: call to the code generated from the XML flight plan.
Definition: nav.c:446
Fixedwing Navigation library.
#define SEND_NAVIGATION(_trans, _dev)
Definition: nav.h:249
Optional exceptions triggeringg HOME_MODE 1) GEOFENCE_DATALINK_LOST_TIME: go to HOME mode if datalink...
static bool higher_than_max_altitude(void)
Definition: nav_geofence.h:66
static bool datalink_lost(void)
Definition: nav_geofence.h:45
#define TRIM_UPPRZ(pprz)
Definition: paparazzi.h:14
#define MAX_PPRZ
Definition: paparazzi.h:8
#define TRIM_PPRZ(pprz)
Definition: paparazzi.h:11
#define MIN_PPRZ
Definition: paparazzi.h:9
struct RadioControl radio_control
Definition: radio_control.c:33
#define RadioControlIsLost()
Definition: radio_control.h:76
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:67
static pprz_t radio_control_get(uint8_t idx)
Get a radio control channel value.
Definition: radio_control.h:94
#define RADIO_KILL_SWITCH
Definition: rc_intermcu.h:40
void rc_settings(bool mode_changed)
Includes generated code from tuning_rc.xml.
Definition: rc_settings.c:47
#define UNLOCKED_HOME_MODE
bool too_far_from_home
Definition: navigation.c:92
void h_ctl_course_loop(void)
float h_ctl_pitch_setpoint
pprz_t h_ctl_elevator_setpoint
float h_ctl_roll_setpoint
pprz_t h_ctl_aileron_setpoint
bool h_ctl_auto1_rate
void h_ctl_attitude_loop(void)
API to get/set the generic vehicle states.
volatile uint32_t nb_sec
full seconds since startup
Definition: sys_time.h:72
void callTCAS(void)
Definition: tcas.c:65
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
float b
Definition: wedgebug.c:202