Paparazzi UAS  v6.1.0_stable
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 
34 #include "state.h"
37 #include "generated/flight_plan.h"
38 
39 /* Geofence exceptions */
41 
42 #if USE_GPS
43 #include "modules/gps/gps.h"
44 #endif
45 static bool gps_lost;
46 
48 
49 #if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
51 #endif
52 
53 static inline uint8_t pprz_mode_update(void);
54 static inline uint8_t mcu1_status_update(void);
55 static inline void copy_from_to_fbw(void);
56 
58 #ifndef RC_LOST_MODE
59 #define RC_LOST_MODE AP_MODE_HOME
60 #endif
61 
62 
64 #ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
65 #include "modules/core/abi.h"
66 volatile uint8_t new_ins_attitude = 0;
67 static abi_event new_att_ev;
68 static void new_att_cb(uint8_t sender_id __attribute__((unused)),
69  uint32_t stamp __attribute__((unused)),
70  struct Int32Rates *gyro __attribute__((unused)))
71 {
72  new_ins_attitude = 1;
73 }
74 
77 void autopilot_event(void)
78 {
79  if (new_ins_attitude > 0) {
80  attitude_loop();
81  new_ins_attitude = 0;
82  }
83 }
84 #endif
85 
86 
91 {
93 
95  gps_lost = false;
96 
98 #ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
99  AbiBindMsgIMU_GYRO_INT32(ABI_BROADCAST, &new_att_ev, &new_att_cb);
100 #endif
101 
102 }
103 
105 {
106 }
107 
112 {
113  uint8_t mode_changed = false;
115 
116  /* rc_lost_while_in_use is true if we lost RC in MANUAL or AUTO1 */
117  uint8_t rc_lost_while_in_use = bit_is_set(imcu_get_status(), STATUS_RADIO_REALLY_LOST) &&
119 
120  /* 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 */
121  if (rc_lost_while_in_use) { // Always: no exceptions!
122  mode_changed = autopilot_set_mode(RC_LOST_MODE);
123  }
124 
125 #ifdef RADIO_KILL_SWITCH
126  if (imcu_get_radio(RADIO_KILL_SWITCH) < MIN_PPRZ / 2) {
128  }
129 #endif
130 
131  /* If in-flight, with good GPS but too far, then activate HOME mode
132  * In MANUAL with good RC, FBW will allow to override. */
135  mode_changed = autopilot_set_mode(AP_MODE_HOME);
136  }
137  }
138  if (bit_is_set(imcu_get_status(), AVERAGED_CHANNELS_SENT)) {
139  bool pprz_mode_changed = pprz_mode_update();
140  mode_changed |= pprz_mode_changed;
141 #if defined RADIO_CALIB && defined RADIO_CONTROL_SETTINGS
142  PPRZ_MUTEX_LOCK(fbw_state_mtx);
143  bool calib_mode_changed = RcSettingsModeUpdate(fbw_state->channels);
144  PPRZ_MUTEX_UNLOCK(fbw_state_mtx);
145  rc_settings(calib_mode_changed || pprz_mode_changed);
146  mode_changed |= calib_mode_changed;
147 #endif
148  }
149  mode_changed |= mcu1_status_update();
150  if (mode_changed) { autopilot_send_mode(); }
151 
152 #if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
153 
158  h_ctl_roll_setpoint = FLOAT_OF_PPRZ(imcu_get_radio(RADIO_ROLL), 0., AUTO1_MAX_ROLL);
159 
161  h_ctl_pitch_setpoint = FLOAT_OF_PPRZ(imcu_get_radio(RADIO_PITCH), 0., AUTO1_MAX_PITCH);
162 #if H_CTL_YAW_LOOP && defined RADIO_YAW
163 
164  h_ctl_yaw_rate_setpoint = FLOAT_OF_PPRZ(imcu_get_radio(RADIO_YAW), 0., AUTO1_MAX_YAW_RATE);
165 #endif
166  }
171  v_ctl_throttle_setpoint = imcu_get_radio(RADIO_THROTTLE);
172  }
175  mcu1_ppm_cpt = imcu_get_ppm_cpt();
176 #endif // RADIO_CONTROL
177 
178  // update electrical from FBW
179  imcu_get_electrical(&ap_electrical);
180 
181 #ifdef RADIO_CONTROL
182  /* the SITL check is a hack to prevent "automatic" launch in NPS */
183 #ifndef SITL
184  if (!autopilot.flight_time) {
186  autopilot.launch = true;
187  }
188  }
189 #endif
190 #endif
191 }
192 
193 void autopilot_static_set_mode(uint8_t new_autopilot_mode)
194 {
195  if (new_autopilot_mode != autopilot.mode) {
196  autopilot.mode = new_autopilot_mode;
197  }
198 }
199 
200 void autopilot_static_SetModeHandler(float new_autopilot_mode)
201 {
202  autopilot_static_set_mode(new_autopilot_mode);
203 }
204 
205 void autopilot_static_set_motors_on(bool motors_on)
206 {
207  // it doesn't make real sense on fixedwing, as you can still use throttle
208  // in MAN and AUTO1 modes while have motor killed for AUTO2
209  // only needed for consistency with other firmwares
210  autopilot.motors_on = motors_on;
211 }
212 
213 #ifdef FAILSAFE_DELAY_WITHOUT_GPS
214 #define GpsTimeoutError (sys_time.nb_sec - gps.last_3dfix_time > FAILSAFE_DELAY_WITHOUT_GPS)
215 #endif
216 
220 void navigation_task(void)
221 {
222 #if defined FAILSAFE_DELAY_WITHOUT_GPS
223 
224  static uint8_t last_pprz_mode;
225 
228  if (autopilot.launch) {
229  if (GpsTimeoutError) {
231  last_pprz_mode = autopilot_get_mode();
234  gps_lost = true;
235  }
236  } else if (gps_lost) { /* GPS is ok */
238  autopilot_set_mode(last_pprz_mode);
239  gps_lost = false;
241  }
242  }
243 #endif /* GPS && FAILSAFE_DELAY_WITHOUT_GPS */
244 
246  if (autopilot_get_mode() == AP_MODE_HOME) {
247  nav_home();
249  nav_without_gps();
250  } else {
252  }
253 
254 #ifdef TCAS
255  callTCAS();
256 #endif
257 
258 #if DOWNLINK && !defined PERIOD_NAVIGATION_Ap_0 // If not sent periodically (in default 0 mode)
259  SEND_NAVIGATION(&(DefaultChannel).trans_tx, &(DefaultDevice).device);
260 #endif
261 
262  /* The nav task computes only nav_altitude. However, we are interested
263  by desired_altitude (= nav_alt+alt_shift) in any case.
264  So we always run the altitude control loop */
267  }
268 
271 #ifdef H_CTL_RATE_LOOP
272  /* Be sure to be in attitude mode, not roll */
273  h_ctl_auto1_rate = false;
274 #endif
276  h_ctl_course_loop(); /* aka compute nav_desired_roll */
277  }
278 
279  }
280 }
281 
282 
283 void attitude_loop(void)
284 {
285 
287 #if CTRL_VERTICAL_LANDING
290  } else {
291 #endif
295  } else {
298  } /* v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB */
299  } /* v_ctl_mode == V_CTL_MODE_AUTO_THROTTLE */
300 #if CTRL_VERTICAL_LANDING
301  } /* v_ctl_mode == V_CTL_MODE_LANDING */
302 #endif
303 
304 #if defined V_CTL_THROTTLE_IDLE
305  Bound(v_ctl_throttle_setpoint, TRIM_PPRZ(V_CTL_THROTTLE_IDLE * MAX_PPRZ), MAX_PPRZ);
306 #endif
307 
308 #ifdef V_CTL_POWER_CTL_BAT_NOMINAL
309  if (ap_electrical.vsupply > 0.) {
310  v_ctl_throttle_setpoint *= V_CTL_POWER_CTL_BAT_NOMINAL / ap_electrical.vsupply;
312  }
313 #endif
314 
315  // Copy the pitch setpoint from the guidance to the stabilization control
317  Bound(h_ctl_pitch_setpoint, H_CTL_PITCH_MIN_SETPOINT, H_CTL_PITCH_MAX_SETPOINT);
320  }
321  }
322 
323  h_ctl_attitude_loop(); /* Set h_ctl_aileron_setpoint & h_ctl_elevator_setpoint */
325  PPRZ_MUTEX_LOCK(ap_state_mtx);
329  AP_COMMAND_SET_YAW(h_ctl_rudder_setpoint);
330  AP_COMMAND_SET_CL(h_ctl_flaps_setpoint);
331  PPRZ_MUTEX_UNLOCK(ap_state_mtx);
332 
333 #if defined MCU_SPI_LINK || defined MCU_UART_LINK || defined MCU_CAN_LINK
334  link_mcu_send();
335 #elif defined INTER_MCU && defined SINGLE_MCU
336 
337  inter_mcu_received_ap = true;
338 #endif
339 
340 }
341 
342 /******************** Interaction with FBW *****************************/
343 
346 #if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
347 static inline uint8_t pprz_mode_update(void)
348 {
349  if ((autopilot_get_mode() != AP_MODE_HOME &&
351 #ifdef UNLOCKED_HOME_MODE
352  || true
353 #endif
354  ) {
355 #ifndef RADIO_AUTO_MODE
356  return autopilot_set_mode(AP_MODE_OF_PULSE(imcu_get_radio(RADIO_MODE)));
357 #else
358  INFO("Using RADIO_AUTO_MODE to switch between AUTO1 and AUTO2.")
359  /* If RADIO_AUTO_MODE is enabled mode swithing will be seperated between two switches/channels
360  * RADIO_MODE will switch between AP_MODE_MANUAL and any AP_MODE_AUTO mode selected by RADIO_AUTO_MODE.
361  *
362  * This is mainly a cludge for entry level radios with no three-way switch but two available two-way switches which can be used.
363  */
364  if (AP_MODE_OF_PULSE(imcu_get_radio(RADIO_MODE)) == AP_MODE_MANUAL) {
365  /* RADIO_MODE in MANUAL position */
367  } else {
368  /* RADIO_MODE not in MANUAL position.
369  * Select AUTO mode bassed on RADIO_AUTO_MODE channel
370  */
371  return autopilot_set_mode((imcu_get_radio(RADIO_AUTO_MODE) > THRESHOLD2) ? AP_MODE_AUTO2 : AP_MODE_AUTO1);
372  }
373 #endif // RADIO_AUTO_MODE
374  } else {
375  return false;
376  }
377 }
378 #else // not RADIO_CONTROL
379 static inline uint8_t pprz_mode_update(void)
380 {
381  return false;
382 }
383 #endif
384 
385 static inline uint8_t mcu1_status_update(void)
386 {
387  uint8_t new_status = imcu_get_status();
388  if (mcu1_status != new_status) {
389  bool changed = ((mcu1_status & MASK_FBW_CHANGED) != (new_status & MASK_FBW_CHANGED));
390  mcu1_status = new_status;
391  return changed;
392  }
393  return false;
394 }
395 
396 
399 static inline void copy_from_to_fbw(void)
400 {
401  PPRZ_MUTEX_LOCK(fbw_state_mtx);
402  PPRZ_MUTEX_LOCK(ap_state_mtx);
403 #ifdef SetAutoCommandsFromRC
404  SetAutoCommandsFromRC(ap_state->commands, fbw_state->channels);
405 #elif defined RADIO_YAW && defined COMMAND_YAW
406  ap_state->commands[COMMAND_YAW] = fbw_state->channels[RADIO_YAW];
407 #endif
408  PPRZ_MUTEX_UNLOCK(ap_state_mtx);
409  PPRZ_MUTEX_UNLOCK(fbw_state_mtx);
410 }
pprz_mode_update
static uint8_t pprz_mode_update(void)
Update paparazzi mode.
Definition: autopilot_static.c:347
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
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:48
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
mcu1_ppm_cpt
static uint8_t mcu1_ppm_cpt
Definition: autopilot_static.c:50
RADIO_ROLL
#define RADIO_ROLL
Definition: intermcu_ap.h:41
Int32Rates
angular rates
Definition: pprz_algebra_int.h:179
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:67
autopilot_static_set_motors_on
void autopilot_static_set_motors_on(bool motors_on)
Definition: autopilot_static.c:205
LATERAL_MODE_MANUAL
#define LATERAL_MODE_MANUAL
Definition: autopilot_firmware.h:36
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
inter_mcu_received_ap
volatile bool inter_mcu_received_ap
Definition: inter_mcu.c:41
nav_geofence.h
abi_struct
Event structure to store callbacks in a linked list.
Definition: abi_common.h:65
inter_mcu.h
fbw_state
struct fbw_state * fbw_state
Definition: inter_mcu.c:36
RADIO_MODE
#define RADIO_MODE
Definition: intermcu_ap.h:44
autopilot_static_init
void autopilot_static_init(void)
Static autopilot API.
Definition: autopilot_static.c:90
stabilization_attitude.h
PPRZ_MUTEX_UNLOCK
#define PPRZ_MUTEX_UNLOCK(_mtx)
Definition: pprz_mutex.h:47
autopilot_event
void WEAK autopilot_event(void)
AP event call.
Definition: autopilot.c:172
AP_MODE_OF_PULSE
#define AP_MODE_OF_PULSE(pprz)
Get mode from pulse.
Definition: autopilot_utils.h:38
nav_home
void nav_home(void)
Home mode navigation (circle around HOME)
Definition: nav.c:423
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
copy_from_to_fbw
static void copy_from_to_fbw(void)
Send back uncontrolled channels.
Definition: autopilot_static.c:399
gps_lost
static bool gps_lost
Definition: autopilot_static.c:45
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
ap_electrical
struct Electrical ap_electrical
Definition: autopilot_firmware.c:36
nav_pitch
float nav_pitch
Definition: nav.c:309
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:50
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:73
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:88
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:333
v_ctl_throttle_slewed
pprz_t v_ctl_throttle_slewed
Definition: energy_ctrl.c:132
AP_COMMAND_SET_THROTTLE
#define AP_COMMAND_SET_THROTTLE(_throttle)
Definition: autopilot_utils.h:77
AP_COMMAND_SET_CL
#define AP_COMMAND_SET_CL(_cl)
Definition: autopilot_utils.h:86
mcu1_status
uint8_t mcu1_status
Second MCU status (FBW part)
Definition: autopilot_firmware.c:39
attitude_loop
void attitude_loop(void)
Definition: autopilot_static.c:283
autopilot.h
AP_COMMAND_SET_ROLL
#define AP_COMMAND_SET_ROLL(_roll)
AP command setter macros for usual commands.
Definition: autopilot_utils.h:64
v_ctl_altitude_loop
void v_ctl_altitude_loop(void)
outer loop
Definition: energy_ctrl.c:298
PPRZ_MUTEX_LOCK
#define PPRZ_MUTEX_LOCK(_mtx)
Definition: pprz_mutex.h:46
navigation_task
void navigation_task(void)
Compute desired_course.
Definition: autopilot_static.c:220
autopilot_set_kill_throttle
void autopilot_set_kill_throttle(bool kill)
set kill throttle
Definition: autopilot.c:246
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:52
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:193
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:245
autopilot_static_periodic
void autopilot_static_periodic(void)
Definition: autopilot_static.c:104
RADIO_THROTTLE
#define RADIO_THROTTLE
Definition: intermcu_ap.h:40
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
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:187
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:59
THRESHOLD2
#define THRESHOLD2
Definition: autopilot_utils.h:58
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:200
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:444
RADIO_YAW
#define RADIO_YAW
Definition: intermcu_ap.h:43
AP_MODE_MANUAL
#define AP_MODE_MANUAL
AP modes.
Definition: autopilot_static.h:36
mcu1_status_update
static uint8_t mcu1_status_update(void)
Definition: autopilot_static.c:385
ap_state
struct ap_state * ap_state
Definition: inter_mcu.c:37
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
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:211
state.h
v_ctl_pitch_setpoint
float v_ctl_pitch_setpoint
Definition: energy_ctrl.c:133
RADIO_PITCH
#define RADIO_PITCH
Definition: intermcu_ap.h:42
RADIO_KILL_SWITCH
#define RADIO_KILL_SWITCH
Definition: intermcu_ap.h:45
nav_throttle_setpoint
pprz_t nav_throttle_setpoint
Definition: nav.c:308
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:111
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:56
new_ins_attitude
volatile uint8_t new_ins_attitude
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:48
pprz_autopilot::launch
bool launch
request launch
Definition: autopilot.h:71
nav_without_gps
void nav_without_gps(void)
Failsafe navigation without position estimation.
Definition: nav.c:569
AP_MODE_AUTO1
#define AP_MODE_AUTO1
Definition: autopilot_static.h:37
h_ctl_auto1_rate
bool h_ctl_auto1_rate
Definition: stabilization_adaptive.c:91