Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
autopilot_static.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2012 The Paparazzi Team
3  * Copyright (C) 2016 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, write to
19  * the Free Software Foundation, 59 Temple Place - Suite 330,
20  * Boston, MA 02111-1307, USA.
21  */
22 
30 #include "autopilot.h"
31 #include "autopilot_arming.h"
32 
34 #include "subsystems/commands.h"
35 #include "subsystems/actuators.h"
36 #include "subsystems/electrical.h"
37 #include "subsystems/settings.h"
40 
44 
45 #if USE_STABILIZATION_RATE
47 #endif
48 
50 
51 #include "generated/settings.h"
52 
53 #if USE_GPS
54 #include "subsystems/gps.h"
55 #else
56 #if NO_GPS_NEEDED_FOR_NAV
57 #define GpsIsLost() FALSE
58 #else
59 #define GpsIsLost() TRUE
60 #endif
61 #endif
62 
63 /* Geofence exceptions */
65 
67 #ifndef FAILSAFE_MODE_TOO_FAR_FROM_HOME
68 #define FAILSAFE_MODE_TOO_FAR_FROM_HOME AP_MODE_FAILSAFE
69 #endif
70 
71 
72 #ifndef MODE_STARTUP
73 #define MODE_STARTUP AP_MODE_KILL
74 PRINT_CONFIG_MSG("Using default AP_MODE_KILL as MODE_STARTUP")
75 #endif
76 
77 #ifndef UNLOCKED_HOME_MODE
78 #if MODE_AUTO1 == AP_MODE_HOME
79 #define UNLOCKED_HOME_MODE TRUE
80 PRINT_CONFIG_MSG("Enabled UNLOCKED_HOME_MODE since MODE_AUTO1 is AP_MODE_HOME")
81 #elif MODE_AUTO2 == AP_MODE_HOME
82 #define UNLOCKED_HOME_MODE TRUE
83 PRINT_CONFIG_MSG("Enabled UNLOCKED_HOME_MODE since MODE_AUTO2 is AP_MODE_HOME")
84 #else
85 #define UNLOCKED_HOME_MODE FALSE
86 #endif
87 #endif
88 
89 #if MODE_MANUAL == AP_MODE_NAV
90 #error "MODE_MANUAL mustn't be AP_MODE_NAV"
91 #endif
92 
93 
95 {
96  /* Mode is finally set by autopilot_static_set_mode if MODE_STARTUP is not KILL.
97  * For autopilot_static_set_mode to do anything, the requested mode needs to differ
98  * from previous mode, so we set it to a safe KILL first.
99  */
101 
102  /* set startup mode, propagates through to guidance h/v */
104 
105  /* init arming */
107 }
108 
109 
110 #define NAV_PRESCALER (PERIODIC_FREQUENCY / NAV_FREQ)
112 {
113 
114  RunOnceEvery(NAV_PRESCALER, compute_dist2_to_home());
115 
120  } else {
122  }
123  }
124  }
125 
126  if (autopilot.mode == AP_MODE_HOME) {
127  RunOnceEvery(NAV_PRESCALER, nav_home());
128  } else {
129  // otherwise always call nav_periodic_task so that carrot is always updated in GCS for other modes
130  RunOnceEvery(NAV_PRESCALER, nav_periodic_task());
131  }
132 
133 
134  /* If in FAILSAFE mode and either already not in_flight anymore
135  * or just "detected" ground, go to KILL mode.
136  */
138  if (!autopilot_in_flight()) {
140  }
141 
142 #if FAILSAFE_GROUND_DETECT
143  INFO("Using FAILSAFE_GROUND_DETECT: KILL")
146  }
147 #endif
148  }
149 
150  /* Reset ground detection _after_ running flight plan
151  */
152  if (!autopilot_in_flight()) {
153  autopilot.ground_detected = false;
155  }
156 
157  /* Set fixed "failsafe" commands from airframe file if in KILL mode.
158  * If in FAILSAFE mode, run normal loops with failsafe attitude and
159  * downwards velocity setpoints.
160  */
161  if (autopilot.mode == AP_MODE_KILL) {
163  } else {
167  }
168  autopilot.throttle = commands[COMMAND_THRUST];
169 
170 }
171 
177 {
179  // safety modes are always accessible via settings
181  } else {
182  if (radio_control.status != RC_OK &&
184  // without RC, only nav-like modes are accessible
186  }
187  }
188  // with RC, other modes can only be changed from the RC
189 }
190 
191 
192 void autopilot_static_set_mode(uint8_t new_autopilot_mode)
193 {
194 
195  /* force startup mode (default is kill) as long as AHRS is not aligned */
196  if (!ap_ahrs_is_aligned()) {
197  new_autopilot_mode = MODE_STARTUP;
198  }
199 
200  if (new_autopilot_mode != autopilot.mode) {
201  /* horizontal mode */
202  switch (new_autopilot_mode) {
203  case AP_MODE_FAILSAFE:
204 #ifndef KILL_AS_FAILSAFE
207  break;
208 #endif
209  case AP_MODE_KILL:
212  break;
213  case AP_MODE_RC_DIRECT:
215  break;
217  case AP_MODE_RATE_DIRECT:
218  case AP_MODE_RATE_Z_HOLD:
219 #if USE_STABILIZATION_RATE
221 #else
222  return;
223 #endif
224  break;
230  break;
231  case AP_MODE_FORWARD:
233  break;
236  break;
238  case AP_MODE_HOVER_CLIMB:
241  break;
242  case AP_MODE_HOME:
243  case AP_MODE_NAV:
245  break;
246  case AP_MODE_MODULE:
247 #ifdef GUIDANCE_H_MODE_MODULE_SETTING
249 #endif
250  break;
251  case AP_MODE_FLIP:
253  break;
254  case AP_MODE_GUIDED:
256  break;
257  default:
258  break;
259  }
260  /* vertical mode */
261  switch (new_autopilot_mode) {
262  case AP_MODE_FAILSAFE:
263 #ifndef KILL_AS_FAILSAFE
266  break;
267 #endif
268  case AP_MODE_KILL:
270  stabilization_cmd[COMMAND_THRUST] = 0;
272  break;
273  case AP_MODE_RC_DIRECT:
274  case AP_MODE_RATE_DIRECT:
278  case AP_MODE_FORWARD:
280  break;
284  break;
286  case AP_MODE_HOVER_CLIMB:
288  break;
289  case AP_MODE_RATE_Z_HOLD:
293  break;
294  case AP_MODE_HOME:
295  case AP_MODE_NAV:
297  break;
298  case AP_MODE_MODULE:
299 #ifdef GUIDANCE_V_MODE_MODULE_SETTING
301 #endif
302  break;
303  case AP_MODE_FLIP:
305  break;
306  case AP_MODE_GUIDED:
308  break;
309  default:
310  break;
311  }
312  //if switching to rate mode but rate mode is not defined, the function returned
313  autopilot.mode = new_autopilot_mode;
314  }
315 }
316 
317 
318 void autopilot_static_set_motors_on(bool motors_on)
319 {
320  if (autopilot.mode != AP_MODE_KILL && ap_ahrs_is_aligned() && motors_on) {
321  autopilot.motors_on = true;
322  } else {
323  autopilot.motors_on = false;
324  }
326 }
327 
329 {
330 
331  if (kill_switch_is_on()) {
333  } else {
334 #ifdef RADIO_AUTO_MODE
335  INFO("Using RADIO_AUTO_MODE to switch between AUTO1 and AUTO2.")
336  uint8_t new_autopilot_mode = ap_mode_of_two_switches();
337 #else
338 #ifdef RADIO_MODE_2x3
339  uint8_t new_autopilot_mode = ap_mode_of_3x2way_switch();
340 #else
341  uint8_t new_autopilot_mode = ap_mode_of_3way_switch();
342 #endif
343 #endif
344 
345  /* don't enter NAV mode if GPS is lost (this also prevents mode oscillations) */
346  if (!(new_autopilot_mode == AP_MODE_NAV && GpsIsLost())) {
347  /* always allow to switch to manual */
348  if (new_autopilot_mode == MODE_MANUAL) {
349  autopilot_static_set_mode(new_autopilot_mode);
350  }
351  /* if in HOME or FAILSAFE mode, don't allow switching to non-manual modes */
352  else if (((autopilot.mode != AP_MODE_HOME) && (autopilot.mode != AP_MODE_FAILSAFE))
353 
355  /* Allowed to leave home mode when UNLOCKED_HOME_MODE */
357 #endif
358  ) {
359  autopilot_static_set_mode(new_autopilot_mode);
360  }
361  }
362  }
363 
364  /* an arming sequence is used to start/stop motors.
365  * only allow arming if ahrs is aligned
366  */
367  if (ap_ahrs_is_aligned()) {
370  } else {
372  }
373 
374  /* if not in FAILSAFE or HOME mode, read RC and set commands accordingly */
376 
377  /* if there are some commands that should always be set from RC, do it */
378 #ifdef SetAutoCommandsFromRC
379  SetAutoCommandsFromRC(commands, radio_control.values);
380 #endif
381 
382  /* if not in NAV_MODE set commands from the rc */
383 #ifdef SetCommandsFromRC
384  if (autopilot.mode != AP_MODE_NAV) {
385  SetCommandsFromRC(commands, radio_control.values);
386  }
387 #endif
388 
391  }
392 
393 }
GUIDANCE_H_MODE_MODULE_SETTING
#define GUIDANCE_H_MODE_MODULE_SETTING
Definition: ctrl_module_innerloop_demo.h:41
radio_control.h
autopilot_set_in_flight
void autopilot_set_in_flight(bool in_flight)
set in_flight flag
Definition: autopilot.c:247
electrical.h
AP_MODE_ATTITUDE_DIRECT
#define AP_MODE_ATTITUDE_DIRECT
Definition: autopilot_static.h:39
stabilization_attitude.h
autopilot_arming.h
settings.h
AP_MODE_HOVER_CLIMB
#define AP_MODE_HOVER_CLIMB
Definition: autopilot_static.h:46
AP_MODE_MODULE
#define AP_MODE_MODULE
Definition: autopilot_static.h:52
autopilot_static_set_motors_on
void autopilot_static_set_motors_on(bool motors_on)
Definition: autopilot_static.c:206
GUIDANCE_H_MODE_ATTITUDE
#define GUIDANCE_H_MODE_ATTITUDE
Definition: guidance_h.h:58
dist2_to_home
float dist2_to_home
squared distance to home waypoint
Definition: navigation.c:86
guidance_v_mode_changed
void guidance_v_mode_changed(uint8_t new_mode)
Definition: guidance_v.c:231
compute_dist2_to_home
void compute_dist2_to_home(void)
Computes squared distance to the HOME waypoint potentially sets too_far_from_home.
Definition: navigation.c:478
MODE_STARTUP
#define MODE_STARTUP
Definition: autopilot_static.c:73
guidance_h_mode_changed
void guidance_h_mode_changed(uint8_t new_mode)
Definition: guidance_h.c:222
pprz_autopilot::mode
uint8_t mode
current autopilot mode
Definition: autopilot.h:63
guidance_v_read_rc
void guidance_v_read_rc(void)
Definition: guidance_v.c:209
GUIDANCE_H_MODE_GUIDED
#define GUIDANCE_H_MODE_GUIDED
Definition: guidance_h.h:66
AP_MODE_FAILSAFE
#define AP_MODE_FAILSAFE
Definition: autopilot_static.h:36
nav_geofence.h
GUIDANCE_V_MODE_CLIMB
#define GUIDANCE_V_MODE_CLIMB
Definition: guidance_v.h:38
ap_ahrs_is_aligned
bool ap_ahrs_is_aligned(void)
Display descent speed in failsafe mode if needed.
Definition: autopilot_utils.c:41
AP_MODE_ATTITUDE_CLIMB
#define AP_MODE_ATTITUDE_CLIMB
Definition: autopilot_static.h:42
SPEED_BFP_OF_REAL
#define SPEED_BFP_OF_REAL(_af)
Definition: pprz_algebra_int.h:218
autopilot_static_init
void autopilot_static_init(void)
Static autopilot API.
Definition: autopilot_static.c:90
GUIDANCE_H_MODE_RC_DIRECT
#define GUIDANCE_H_MODE_RC_DIRECT
Definition: guidance_h.h:61
AP_MODE_ATTITUDE_Z_HOLD
#define AP_MODE_ATTITUDE_Z_HOLD
Definition: autopilot_static.h:44
AP_MODE_RATE_Z_HOLD
#define AP_MODE_RATE_Z_HOLD
Definition: autopilot_static.h:43
GpsIsLost
#define GpsIsLost()
Definition: gps.h:45
commands_failsafe
const pprz_t commands_failsafe[COMMANDS_NB]
Definition: commands.c:31
nav_home
void nav_home(void)
Home mode navigation (circle around HOME)
Definition: nav.c:422
AP_MODE_KILL
#define AP_MODE_KILL
Static autopilot modes.
Definition: autopilot_static.h:35
GUIDANCE_H_MODE_FLIP
#define GUIDANCE_H_MODE_FLIP
Definition: guidance_h.h:65
AP_MODE_FORWARD
#define AP_MODE_FORWARD
Definition: autopilot_static.h:51
SetCommands
#define SetCommands(t)
Definition: commands.h:41
AP_MODE_CARE_FREE_DIRECT
#define AP_MODE_CARE_FREE_DIRECT
Definition: autopilot_static.h:50
UNLOCKED_HOME_MODE
#define UNLOCKED_HOME_MODE
Definition: autopilot_static.c:79
AP_MODE_GUIDED
#define AP_MODE_GUIDED
Definition: autopilot_static.h:54
datalink_lost
static bool datalink_lost(void)
Definition: nav_geofence.h:45
autopilot_in_flight
bool autopilot_in_flight(void)
get in_flight flag
Definition: autopilot.c:257
AP_MODE_HOVER_Z_HOLD
#define AP_MODE_HOVER_Z_HOLD
Definition: autopilot_static.h:47
autopilot_guided.h
AP_MODE_NAV
#define AP_MODE_NAV
Definition: autopilot_static.h:48
stabilization_attitude_set_failsafe_setpoint
void stabilization_attitude_set_failsafe_setpoint(void)
Definition: stabilization_attitude_euler_float.c:147
pprz_autopilot::motors_on
bool motors_on
motor status
Definition: autopilot.h:68
gps.h
Device independent GPS code (interface)
autopilot
struct pprz_autopilot autopilot
Global autopilot structure.
Definition: autopilot.c:50
GUIDANCE_H_MODE_HOVER
#define GUIDANCE_H_MODE_HOVER
Definition: guidance_h.h:59
GUIDANCE_V_MODE_RC_CLIMB
#define GUIDANCE_V_MODE_RC_CLIMB
Definition: guidance_v.h:37
pprz_autopilot::in_flight
bool in_flight
in flight status
Definition: autopilot.h:70
guidance_h_run
void guidance_h_run(bool in_flight)
Definition: guidance_h.c:361
GUIDANCE_H_MODE_FORWARD
#define GUIDANCE_H_MODE_FORWARD
Definition: guidance_h.h:63
GUIDANCE_V_MODE_KILL
#define GUIDANCE_V_MODE_KILL
Definition: guidance_v.h:35
GUIDANCE_V_MODE_RC_DIRECT
#define GUIDANCE_V_MODE_RC_DIRECT
Definition: guidance_v.h:36
pprz_autopilot::throttle
pprz_t throttle
throttle level as will be displayed in GCS
Definition: autopilot.h:66
guidance_v_zd_sp
int32_t guidance_v_zd_sp
vertical speed setpoint in meter/s (input).
Definition: guidance_v.c:130
AP_MODE_HOVER_DIRECT
#define AP_MODE_HOVER_DIRECT
Definition: autopilot_static.h:45
too_far_from_home
bool too_far_from_home
Definition: navigation.c:87
higher_than_max_altitude
static bool higher_than_max_altitude(void)
Definition: nav_geofence.h:66
uint8_t
unsigned char uint8_t
Definition: types.h:14
ap_mode_of_3way_switch
uint8_t ap_mode_of_3way_switch(void)
get autopilot mode as set by RADIO_MODE 3-way switch
Definition: autopilot_utils.c:90
autopilot_arming_init
static void autopilot_arming_init(void)
Definition: autopilot_arming_switch.h:48
pprz_autopilot::ground_detected
bool ground_detected
automatic detection of landing
Definition: autopilot.h:74
RadioControl::status
uint8_t status
Definition: radio_control.h:64
guidance.h
failsafe_mode_dist2
float failsafe_mode_dist2
maximum squared distance to home wp before going to failsafe mode
Definition: navigation.c:85
AP_MODE_ATTITUDE_RC_CLIMB
#define AP_MODE_ATTITUDE_RC_CLIMB
Definition: autopilot_static.h:41
guidance_v_run
void guidance_v_run(bool in_flight)
Definition: guidance_v.c:299
autopilot.h
GUIDANCE_V_MODE_HOVER
#define GUIDANCE_V_MODE_HOVER
Definition: guidance_v.h:39
NAV_PRESCALER
#define NAV_PRESCALER
Definition: autopilot_static.c:110
commands
static const ShellCommand commands[]
Definition: shell_arch.c:71
GUIDANCE_H_MODE_KILL
#define GUIDANCE_H_MODE_KILL
Definition: guidance_h.h:56
autopilot_set_motors_on
void autopilot_set_motors_on(bool motors_on)
turn motors on/off, eventually depending of the current mode set kill_throttle accordingly FIXME is i...
Definition: autopilot.c:200
stabilization_rate.h
GUIDANCE_V_MODE_FLIP
#define GUIDANCE_V_MODE_FLIP
Definition: guidance_v.h:42
PRINT_CONFIG_MSG
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
SetRotorcraftCommands
#define SetRotorcraftCommands(_cmd, _in_flight, _motors_on)
Definition: autopilot_utils.h:64
GUIDANCE_H_MODE_RATE
#define GUIDANCE_H_MODE_RATE
Definition: guidance_h.h:57
autopilot_static_set_mode
void autopilot_static_set_mode(uint8_t new_autopilot_mode)
Definition: autopilot_static.c:194
FAILSAFE_MODE_TOO_FAR_FROM_HOME
#define FAILSAFE_MODE_TOO_FAR_FROM_HOME
Mode that is set when the plane is really too far from home.
Definition: autopilot_static.c:68
AP_MODE_RC_DIRECT
#define AP_MODE_RC_DIRECT
Definition: autopilot_static.h:49
autopilot_static_periodic
void autopilot_static_periodic(void)
Definition: autopilot_static.c:104
kill_switch_is_on
static bool kill_switch_is_on(void)
Definition: autopilot_rc_helpers.h:97
AP_ARMING_STATUS_AHRS_NOT_ALLIGNED
#define AP_ARMING_STATUS_AHRS_NOT_ALLIGNED
Definition: autopilot_arming_common.h:48
GUIDANCE_V_MODE_MODULE_SETTING
#define GUIDANCE_V_MODE_MODULE_SETTING
Definition: ctrl_module_innerloop_demo.h:44
GUIDANCE_V_MODE_NAV
#define GUIDANCE_V_MODE_NAV
Definition: guidance_v.h:40
RC_OK
#define RC_OK
Definition: radio_control.h:56
guidance_h_read_rc
void guidance_h_read_rc(bool in_flight)
Definition: guidance_h.c:302
navigation.h
GUIDANCE_V_MODE_GUIDED
#define GUIDANCE_V_MODE_GUIDED
Definition: guidance_v.h:43
stabilization_cmd
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:32
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:201
nav_periodic_task
void nav_periodic_task(void)
Navigation main: call to the code generated from the XML flight plan.
Definition: nav.c:443
autopilot_arming_check_motors_on
static void autopilot_arming_check_motors_on(void)
State machine to check if motors should be turned ON or OFF using the kill switch.
Definition: autopilot_arming_switch.h:79
autopilot_arming_set
static void autopilot_arming_set(bool motors_on)
Definition: autopilot_arming_switch.h:54
mode
static uint8_t mode
mode holds the current sonar mode mode = 0 used at high altitude, uses 16 wave patterns mode = 1 used...
Definition: sonar_bebop.c:69
commands.h
Hardware independent code for commands handling.
stabilization.h
FAILSAFE_DESCENT_SPEED
#define FAILSAFE_DESCENT_SPEED
Set descent speed in failsafe mode.
Definition: autopilot_utils.h:37
AP_MODE_FLIP
#define AP_MODE_FLIP
Definition: autopilot_static.h:53
FALSE
#define FALSE
Definition: std.h:5
GUIDANCE_H_MODE_NAV
#define GUIDANCE_H_MODE_NAV
Definition: guidance_h.h:60
MODE_MANUAL
#define MODE_MANUAL
Default RC mode.
Definition: autopilot_static.h:60
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:112
AP_MODE_RATE_DIRECT
#define AP_MODE_RATE_DIRECT
Definition: autopilot_static.h:38
pprz_autopilot::kill_throttle
bool kill_throttle
allow autopilot to use throttle
Definition: autopilot.h:69
pprz_autopilot::arming_status
uint8_t arming_status
arming status
Definition: autopilot.h:67
stabilization_none.h
GUIDANCE_H_MODE_CARE_FREE
#define GUIDANCE_H_MODE_CARE_FREE
Definition: guidance_h.h:62
AP_MODE_RATE_RC_CLIMB
#define AP_MODE_RATE_RC_CLIMB
Definition: autopilot_static.h:40
pprz_autopilot::detect_ground_once
bool detect_ground_once
enable automatic detection of ground (one shot)
Definition: autopilot.h:75
radio_control
struct RadioControl radio_control
Definition: radio_control.c:30
actuators.h
RadioControl::values
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:69