Paparazzi UAS  v6.2_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_INT32(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
110 
116 
119 #if H_CTL_YAW_LOOP && defined RADIO_YAW
120 
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);
127  }
134  }
136 #endif
137 }
138 
139 void autopilot_static_set_mode(uint8_t new_autopilot_mode)
140 {
141  if (new_autopilot_mode != autopilot.mode) {
142  autopilot.mode = new_autopilot_mode;
143  }
144 }
145 
146 void autopilot_static_SetModeHandler(float new_autopilot_mode)
147 {
148  autopilot_static_set_mode(new_autopilot_mode);
149 }
150 
151 void autopilot_static_set_motors_on(bool motors_on)
152 {
153  // it doesn't make real sense on fixedwing, as you can still use throttle
154  // in MAN and AUTO1 modes while have motor killed for AUTO2
155  // only needed for consistency with other firmwares
156  autopilot.motors_on = motors_on;
157 }
158 
162 void navigation_task(void)
163 {
164 
166  if (autopilot_get_mode() == AP_MODE_HOME) {
167  nav_home();
169  nav_without_gps();
170  } else {
172  }
173 
174 #ifdef TCAS
175  callTCAS();
176 #endif
177 
178 #if DOWNLINK && !defined PERIOD_NAVIGATION_Ap_0 // If not sent periodically (in default 0 mode)
179  SEND_NAVIGATION(&(DefaultChannel).trans_tx, &(DefaultDevice).device);
180 #endif
181 
182  /* The nav task computes only nav_altitude. However, we are interested
183  by desired_altitude (= nav_alt+alt_shift) in any case.
184  So we always run the altitude control loop */
187  }
188 
192 #ifdef H_CTL_RATE_LOOP
193  /* Be sure to be in attitude mode, not roll */
194  h_ctl_auto1_rate = false;
195 #endif
197  h_ctl_course_loop(); /* aka compute nav_desired_roll */
198  }
199 
200  }
201 }
202 
216 {
217  uint8_t mode_changed = false;
218 
219 #if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
220  /* check normal mode from RC channel(s)
221  */
222  if (!RadioControlIsLost()) {
223  bool pprz_mode_changed = pprz_mode_update();
224  mode_changed |= pprz_mode_changed;
225 #if defined RADIO_CALIB && defined RADIO_CONTROL_SETTINGS
226  bool calib_mode_changed = RcSettingsModeUpdate(radio_control.values);
227  rc_settings(calib_mode_changed || pprz_mode_changed);
228  mode_changed |= calib_mode_changed;
229 #endif
230  }
231 
232  /* RC lost while in use is true if we lost RC in MANUAL or AUTO1
233  *
234  * 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
235  */
236  if (RadioControlIsLost() &&
239  mode_changed |= autopilot_set_mode(RC_LOST_MODE);
240  }
241 
242  /* Check RC kill switch
243  */
244 #ifdef RADIO_KILL_SWITCH
246  autopilot_set_kill_throttle(true); // not a mode change, just set kill_throttle
247  }
248 #endif
249 
250  /* the SITL check is a hack to prevent "automatic" launch in NPS
251  */
252 #ifndef SITL
253  if (!autopilot.flight_time) {
255  autopilot.launch = true; // set launch to true from RC throttel up
256  }
257  }
258 #endif
259 
260 #endif // RADIO_CONTROL
261 
262 #if USE_GPS && (defined FAILSAFE_DELAY_WITHOUT_GPS)
263  /* This section is used for the failsafe of GPS
264  */
265  static uint8_t last_pprz_mode;
266  static bool gps_lost = false;
267 
270  if (autopilot.launch) {
271  // check GPS timeout
272  if (sys_time.nb_sec - gps.last_3dfix_time > FAILSAFE_DELAY_WITHOUT_GPS) {
275  last_pprz_mode = autopilot_get_mode();
277  gps_lost = true;
278  }
279  } else if (gps_lost) { /* GPS is ok */
281  mode_changed |= autopilot_set_mode(last_pprz_mode);
282  gps_lost = false;
283  }
284  }
285 #endif /* GPS && FAILSAFE_DELAY_WITHOUT_GPS */
286 
287  /* If in-flight, with good GPS but too far, then activate HOME mode
288  * In MANUAL with good RC, FBW will allow to override. */
289  if (autopilot_get_mode() != AP_MODE_HOME &&
291  autopilot.launch) {
293  mode_changed |= autopilot_set_mode(AP_MODE_HOME);
294  }
295  }
296 
297  // send new mode if needed
298  if (mode_changed) {
300  }
301 }
302 
303 
304 void attitude_loop(void)
305 {
306  // Call vertical climb loop if mode is at least AUTO2
308 #if CTRL_VERTICAL_LANDING
311  } else {
312 #endif
316  } else {
319  }
320  }
321 #if CTRL_VERTICAL_LANDING
322  }
323 #endif
324 
325 #if defined V_CTL_THROTTLE_IDLE
326  Bound(v_ctl_throttle_setpoint, TRIM_PPRZ(V_CTL_THROTTLE_IDLE * MAX_PPRZ), MAX_PPRZ);
327 #endif
328 
329 #ifdef V_CTL_POWER_CTL_BAT_NOMINAL
330  if (electrical.vsupply > 0.f) {
331  v_ctl_throttle_setpoint *= V_CTL_POWER_CTL_BAT_NOMINAL / electrical.vsupply;
333  }
334 #endif
335 
336  // Copy the pitch setpoint from the guidance to the stabilization control
338  Bound(h_ctl_pitch_setpoint, H_CTL_PITCH_MIN_SETPOINT, H_CTL_PITCH_MAX_SETPOINT);
341  }
342  }
343 
344  // Call attitude control and set commands if mode is at least AUTO1
346  h_ctl_attitude_loop(); /* Set h_ctl_aileron_setpoint & h_ctl_elevator_setpoint */
351  AP_COMMAND_SET_YAW(h_ctl_rudder_setpoint);
352  AP_COMMAND_SET_CL(h_ctl_flaps_setpoint);
353  }
354 
355 }
356 
357 
360 #if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
361 static inline uint8_t pprz_mode_update(void)
362 {
363  if ((autopilot_get_mode() != AP_MODE_HOME &&
365 #ifdef UNLOCKED_HOME_MODE
366  || true
367 #endif
368  ) {
369 #ifndef RADIO_AUTO_MODE
371  bool b = autopilot_set_mode(nm);
372  return b;
373  //return autopilot_set_mode(AP_MODE_OF_PULSE(radio_control_get(RADIO_MODE)));
374 #else
375  INFO("Using RADIO_AUTO_MODE to switch between AUTO1 and AUTO2.")
376  /* If RADIO_AUTO_MODE is enabled mode swithing will be seperated between
377  * two switches/channels
378  * RADIO_MODE will switch between AP_MODE_MANUAL and any AP_MODE_AUTO mode
379  * selected by RADIO_AUTO_MODE.
380  *
381  * This is mainly a cludge for entry level radios with no three-way switch
382  * but two available two-way switches which can be used.
383  */
385  /* RADIO_MODE in MANUAL position */
387  } else {
388  /* RADIO_MODE not in MANUAL position.
389  * Select AUTO mode bassed on RADIO_AUTO_MODE channel
390  */
392  }
393 #endif // RADIO_AUTO_MODE
394  } else {
395  return false;
396  }
397 }
398 #else // not RADIO_CONTROL
399 static inline uint8_t pprz_mode_update(void)
400 {
401  return false;
402 }
403 #endif
404 
405 
pprz_mode_update
static uint8_t pprz_mode_update(void)
Update paparazzi mode from RC.
Definition: autopilot_static.c:361
MAX_PPRZ
#define MAX_PPRZ
Definition: paparazzi.h:8
uint32_t
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
radio_control_get
static pprz_t radio_control_get(uint8_t idx)
Get a radio control channel value.
Definition: radio_control.h:94
common_nav_periodic_task
void common_nav_periodic_task()
Definition: common_nav.c:153
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
FLOAT_OF_PPRZ
#define FLOAT_OF_PPRZ(pprz, center, travel)
pprz_t to float with saturation
Definition: autopilot_utils.h:47
v_ctl_mode
uint8_t v_ctl_mode
Definition: energy_ctrl.c:74
V_CTL_MODE_AUTO_ALT
#define V_CTL_MODE_AUTO_ALT
Definition: guidance_common.h:39
Int32Rates
angular rates
Definition: pprz_algebra_int.h:179
b
float b
Definition: wedgebug.c:202
abi.h
AP_MODE_AUTO2
#define AP_MODE_AUTO2
Definition: autopilot_static.h:38
AP_COMMAND_SET_PITCH
#define AP_COMMAND_SET_PITCH(_pitch)
Definition: autopilot_utils.h:66
autopilot_static_set_motors_on
void autopilot_static_set_motors_on(bool motors_on)
Definition: autopilot_static.c:151
LATERAL_MODE_MANUAL
#define LATERAL_MODE_MANUAL
Definition: autopilot_firmware.h:36
RADIO_KILL_SWITCH
#define RADIO_KILL_SWITCH
Definition: rc_intermcu.h:40
pprz_autopilot::flight_time
uint16_t flight_time
flight time in seconds
Definition: autopilot.h:65
pprz_autopilot::mode
uint8_t mode
current autopilot mode
Definition: autopilot.h:63
nav_geofence.h
abi_struct
Event structure to store callbacks in a linked list.
Definition: abi_common.h:66
autopilot_failsafe_checks
void autopilot_failsafe_checks(void)
Failsafe checks.
Definition: autopilot_static.c:215
autopilot_static_init
void autopilot_static_init(void)
Static autopilot API.
Definition: autopilot_static.c:81
stabilization_attitude.h
commands
pprz_t commands[COMMANDS_NB]
Definition: commands.c:30
autopilot_event
void WEAK autopilot_event(void)
AP event call.
Definition: autopilot.c:173
AP_MODE_OF_PULSE
#define AP_MODE_OF_PULSE(pprz)
Get mode from pulse.
Definition: autopilot_utils.h:37
nav_home
void nav_home(void)
Home mode navigation (circle around HOME)
Definition: nav.c:425
Electrical::vsupply
float vsupply
supply voltage in V
Definition: electrical.h:45
h_ctl_roll_setpoint
float h_ctl_roll_setpoint
Definition: stabilization_adaptive.c:157
UNLOCKED_HOME_MODE
#define UNLOCKED_HOME_MODE
Definition: autopilot_static.c:79
datalink_lost
static bool datalink_lost(void)
Definition: nav_geofence.h:45
autopilot_static.h
nav_pitch
float nav_pitch
Definition: nav.c:311
pprz_autopilot::motors_on
bool motors_on
motor status
Definition: autopilot.h:68
h_ctl_course_loop
void h_ctl_course_loop(void)
Definition: stabilization_adaptive.c:374
gps.h
Device independent GPS code (interface)
callTCAS
void callTCAS(void)
Definition: tcas.c:65
autopilot
struct pprz_autopilot autopilot
Global autopilot structure.
Definition: autopilot.c:48
lateral_mode
uint8_t lateral_mode
Definition: autopilot_firmware.c:38
AP_COMMAND_SET_YAW
#define AP_COMMAND_SET_YAW(_yaw)
Definition: autopilot_utils.h:72
TRIM_UPPRZ
#define TRIM_UPPRZ(pprz)
Definition: paparazzi.h:14
h_ctl_attitude_loop
void h_ctl_attitude_loop(void)
Definition: stabilization_adaptive.c:413
too_far_from_home
bool too_far_from_home
Definition: navigation.c:92
higher_than_max_altitude
static bool higher_than_max_altitude(void)
Definition: nav_geofence.h:66
MIN_PPRZ
#define MIN_PPRZ
Definition: paparazzi.h:9
autopilot_send_mode
void WEAK autopilot_send_mode(void)
send autopilot mode actual implementation is firmware dependent
Definition: autopilot.c:324
v_ctl_throttle_slewed
pprz_t v_ctl_throttle_slewed
Definition: energy_ctrl.c:132
GpsState::last_3dfix_time
uint32_t last_3dfix_time
cpu time in sec at last valid 3D fix
Definition: gps.h:115
AP_COMMAND_SET_THROTTLE
#define AP_COMMAND_SET_THROTTLE(_throttle)
Definition: autopilot_utils.h:76
AP_COMMAND_SET_CL
#define AP_COMMAND_SET_CL(_cl)
Definition: autopilot_utils.h:85
attitude_loop
void attitude_loop(void)
Definition: autopilot_static.c:304
autopilot.h
AP_COMMAND_SET_ROLL
#define AP_COMMAND_SET_ROLL(_roll)
AP command setter macros for usual commands.
Definition: autopilot_utils.h:63
v_ctl_altitude_loop
void v_ctl_altitude_loop(void)
outer loop
Definition: energy_ctrl.c:298
navigation_task
void navigation_task(void)
Compute desired_course.
Definition: autopilot_static.c:162
autopilot_set_kill_throttle
void autopilot_set_kill_throttle(bool kill)
set kill throttle
Definition: autopilot.c:251
V_CTL_MODE_AUTO_THROTTLE
#define V_CTL_MODE_AUTO_THROTTLE
Definition: guidance_common.h:37
THROTTLE_THRESHOLD_TAKEOFF
#define THROTTLE_THRESHOLD_TAKEOFF
Takeoff detection threshold from throttle.
Definition: autopilot_utils.h:51
v_ctl_throttle_setpoint
pprz_t v_ctl_throttle_setpoint
Definition: energy_ctrl.c:131
nav.h
autopilot_static_set_mode
void autopilot_static_set_mode(uint8_t new_autopilot_mode)
Definition: autopilot_static.c:139
h_ctl_elevator_setpoint
pprz_t h_ctl_elevator_setpoint
Definition: stabilization_adaptive.c:178
AP_MODE_GPS_OUT_OF_ORDER
#define AP_MODE_GPS_OUT_OF_ORDER
Definition: autopilot_static.h:40
SEND_NAVIGATION
#define SEND_NAVIGATION(_trans, _dev)
Definition: nav.h:249
autopilot_static_periodic
void autopilot_static_periodic(void)
Definition: autopilot_static.c:93
v_ctl_throttle_slew
void v_ctl_throttle_slew(void)
Computes slewed throttle from throttle setpoint called at 20Hz.
Definition: energy_ctrl.c:452
v_ctl_climb_loop
void v_ctl_climb_loop(void)
Auto-throttle inner loop.
Definition: energy_ctrl.c:336
sys_time
Definition: sys_time.h:71
LATERAL_MODE_COURSE
#define LATERAL_MODE_COURSE
Definition: autopilot_firmware.h:39
autopilot_set_mode
bool autopilot_set_mode(uint8_t new_autopilot_mode)
set autopilot mode
Definition: autopilot.c:192
RC_LOST_MODE
#define RC_LOST_MODE
mode to enter when RC is lost in AP_MODE_MANUAL or AP_MODE_AUTO1
Definition: autopilot_static.c:50
THRESHOLD2
#define THRESHOLD2
Definition: autopilot_utils.h:57
AP_MODE_HOME
#define AP_MODE_HOME
Definition: autopilot_static.h:39
autopilot_static_SetModeHandler
void autopilot_static_SetModeHandler(float new_autopilot_mode)
Definition: autopilot_static.c:146
V_CTL_MODE_LANDING
#define V_CTL_MODE_LANDING
Definition: guidance_common.h:40
nav_periodic_task
void nav_periodic_task(void)
Navigation main: call to the code generated from the XML flight plan.
Definition: nav.c:446
AP_MODE_MANUAL
#define AP_MODE_MANUAL
AP modes.
Definition: autopilot_static.h:36
h_ctl_pitch_setpoint
float h_ctl_pitch_setpoint
Definition: stabilization_adaptive.c:170
h_ctl_aileron_setpoint
pprz_t h_ctl_aileron_setpoint
Definition: stabilization_adaptive.c:164
electrical
struct Electrical electrical
Definition: electrical.c:66
TRIM_PPRZ
#define TRIM_PPRZ(pprz)
Definition: paparazzi.h:11
autopilot_get_mode
uint8_t autopilot_get_mode(void)
get autopilot mode
Definition: autopilot.c:216
state.h
sys_time::nb_sec
volatile uint32_t nb_sec
full seconds since startup
Definition: sys_time.h:72
v_ctl_pitch_setpoint
float v_ctl_pitch_setpoint
Definition: energy_ctrl.c:133
nav_throttle_setpoint
pprz_t nav_throttle_setpoint
Definition: nav.c:310
v_ctl_landing_loop
void v_ctl_landing_loop(void)
Definition: guidance_v.c:357
autopilot_static_on_rc_frame
void autopilot_static_on_rc_frame(void)
Function to be called when a message from FBW is available.
Definition: autopilot_static.c:100
V_CTL_MODE_AUTO_CLIMB
#define V_CTL_MODE_AUTO_CLIMB
Definition: guidance_common.h:38
ABI_BROADCAST
#define ABI_BROADCAST
Broadcast address.
Definition: abi_common.h:57
new_ins_attitude
volatile uint8_t new_ins_attitude
gps
struct GpsState gps
global GPS state
Definition: gps.c:69
pprz_autopilot::kill_throttle
bool kill_throttle
allow autopilot to use throttle
Definition: autopilot.h:69
rc_settings
void rc_settings(bool mode_changed)
Includes generated code from tuning_rc.xml.
Definition: rc_settings.c:47
pprz_autopilot::launch
bool launch
request launch
Definition: autopilot.h:71
RadioControlIsLost
#define RadioControlIsLost()
Definition: radio_control.h:76
nav_without_gps
void nav_without_gps(void)
Failsafe navigation without position estimation.
Definition: nav.c:571
radio_control
struct RadioControl radio_control
Definition: radio_control.c:33
AP_MODE_AUTO1
#define AP_MODE_AUTO1
Definition: autopilot_static.h:37
RadioControl::values
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:67
h_ctl_auto1_rate
bool h_ctl_auto1_rate
Definition: stabilization_adaptive.c:91