Paparazzi UAS  v5.17_devel-24-g2ae834f
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 {
178  if (mode == AP_MODE_KILL || mode == AP_MODE_FAILSAFE || mode == AP_MODE_HOME) {
179  // safety modes are always accessible via settings
181  } else {
182  if (radio_control.status != RC_OK &&
183  (mode == AP_MODE_NAV || mode == AP_MODE_GUIDED || mode == AP_MODE_FLIP || mode == AP_MODE_MODULE)) {
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 mode, don't allow switching to non-manual modes */
352  else if ((autopilot.mode != AP_MODE_HOME)
354  /* Allowed to leave home mode when UNLOCKED_HOME_MODE */
356 #endif
357  ) {
358  autopilot_static_set_mode(new_autopilot_mode);
359  }
360  }
361  }
362 
363  /* an arming sequence is used to start/stop motors.
364  * only allow arming if ahrs is aligned
365  */
366  if (ap_ahrs_is_aligned()) {
369  } else {
371  }
372 
373  /* if not in FAILSAFE or HOME mode, read RC and set commands accordingly */
375 
376  /* if there are some commands that should always be set from RC, do it */
377 #ifdef SetAutoCommandsFromRC
378  SetAutoCommandsFromRC(commands, radio_control.values);
379 #endif
380 
381  /* if not in NAV_MODE set commands from the rc */
382 #ifdef SetCommandsFromRC
383  if (autopilot.mode != AP_MODE_NAV) {
384  SetCommandsFromRC(commands, radio_control.values);
385  }
386 #endif
387 
390  }
391 
392 }
393 
static void autopilot_arming_set(bool 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.
#define GUIDANCE_H_MODE_GUIDED
Definition: guidance_h.h:66
#define GUIDANCE_V_MODE_NAV
Definition: guidance_v.h:40
void guidance_h_read_rc(bool in_flight)
Definition: guidance_h.c:302
#define GUIDANCE_H_MODE_CARE_FREE
Definition: guidance_h.h:62
#define AP_MODE_RATE_DIRECT
#define AP_MODE_KILL
Static autopilot modes.
uint8_t ap_mode_of_3way_switch(void)
get autopilot mode as set by RADIO_MODE 3-way switch
Dummy stabilization for rotorcrafts.
General attitude stabilization interface for rotorcrafts.
void guidance_h_run(bool in_flight)
Definition: guidance_h.c:361
#define AP_MODE_HOVER_DIRECT
#define AP_MODE_RATE_Z_HOLD
#define FAILSAFE_DESCENT_SPEED
Set descent speed in failsafe mode.
uint8_t status
Definition: radio_control.h:64
bool in_flight
in flight status
Definition: autopilot.h:70
#define GUIDANCE_H_MODE_HOVER
Definition: guidance_h.h:59
void guidance_v_mode_changed(uint8_t new_mode)
Definition: guidance_v.c:231
void autopilot_static_SetModeHandler(float new_autopilot_mode)
#define AP_MODE_RATE_RC_CLIMB
Autopilot guided mode interface.
#define AP_MODE_HOME
void guidance_v_run(bool in_flight)
Definition: guidance_v.c:299
static bool kill_switch_is_on(void)
#define GUIDANCE_H_MODE_ATTITUDE
Definition: guidance_h.h:58
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:69
float dist2_to_home
squared distance to home waypoint
Definition: navigation.c:86
#define GUIDANCE_V_MODE_MODULE_SETTING
#define GUIDANCE_V_MODE_KILL
Definition: guidance_v.h:35
struct pprz_autopilot autopilot
Global autopilot structure.
Definition: autopilot.c:50
#define AP_MODE_MODULE
#define FALSE
Definition: std.h:5
const pprz_t commands_failsafe[COMMANDS_NB]
Definition: commands.c:31
#define GUIDANCE_H_MODE_FLIP
Definition: guidance_h.h:65
void autopilot_static_set_mode(uint8_t new_autopilot_mode)
Rate stabilization for rotorcrafts.
#define AP_MODE_ATTITUDE_RC_CLIMB
bool too_far_from_home
Definition: navigation.c:87
bool ap_ahrs_is_aligned(void)
Display descent speed in failsafe mode if needed.
#define MODE_MANUAL
Default RC mode.
Hardware independent API for actuators (servos, motor controllers).
#define AP_MODE_ATTITUDE_CLIMB
#define UNLOCKED_HOME_MODE
bool detect_ground_once
enable automatic detection of ground (one shot)
Definition: autopilot.h:75
#define MODE_STARTUP
void autopilot_static_on_rc_frame(void)
Function to be called when a message from FBW is available.
Interface for electrical status: supply voltage, current, battery status, etc.
void autopilot_static_periodic(void)
bool autopilot_in_flight(void)
get in_flight flag
Definition: autopilot.c:257
Device independent GPS code (interface)
#define AP_MODE_HOVER_Z_HOLD
#define GUIDANCE_V_MODE_CLIMB
Definition: guidance_v.h:38
#define AP_MODE_GUIDED
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
void stabilization_attitude_set_failsafe_setpoint(void)
static void autopilot_arming_init(void)
#define AP_MODE_FLIP
#define AP_MODE_RC_DIRECT
#define GUIDANCE_V_MODE_HOVER
Definition: guidance_v.h:39
Hardware independent code for commands handling.
struct RadioControl radio_control
Definition: radio_control.c:30
void guidance_h_mode_changed(uint8_t new_mode)
Definition: guidance_h.c:222
#define GUIDANCE_H_MODE_FORWARD
Definition: guidance_h.h:63
#define GUIDANCE_H_MODE_MODULE_SETTING
void compute_dist2_to_home(void)
Computes squared distance to the HOME waypoint potentially sets too_far_from_home.
Definition: navigation.c:478
uint8_t arming_status
arming status
Definition: autopilot.h:67
#define RC_OK
Definition: radio_control.h:56
void autopilot_static_init(void)
Static autopilot API.
Core autopilot interface common to all firmwares.
Rotorcraft navigation functions.
#define AP_MODE_FORWARD
#define SetCommands(t)
Definition: commands.h:41
#define AP_MODE_NAV
bool motors_on
motor status
Definition: autopilot.h:68
unsigned char uint8_t
Definition: types.h:14
pprz_t commands[COMMANDS_NB]
Storage of intermediate command values.
Definition: commands.c:30
void autopilot_static_set_motors_on(bool motors_on)
Persistent settings interface.
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
#define AP_MODE_HOVER_CLIMB
#define GUIDANCE_H_MODE_NAV
Definition: guidance_h.h:60
General stabilization interface for rotorcrafts.
void guidance_v_read_rc(void)
Definition: guidance_v.c:209
#define FAILSAFE_MODE_TOO_FAR_FROM_HOME
Mode that is set when the plane is really too far from home.
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
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:32
#define GUIDANCE_V_MODE_FLIP
Definition: guidance_v.h:42
int32_t guidance_v_zd_sp
vertical speed setpoint in meter/s (input).
Definition: guidance_v.c:130
#define SPEED_BFP_OF_REAL(_af)
#define AP_MODE_ATTITUDE_DIRECT
#define GpsIsLost()
Definition: gps.h:45
#define GUIDANCE_V_MODE_RC_CLIMB
Definition: guidance_v.h:37
float failsafe_mode_dist2
maximum squared distance to home wp before going to failsafe mode
Definition: navigation.c:85
void autopilot_set_in_flight(bool in_flight)
set in_flight flag
Definition: autopilot.c:247
bool kill_throttle
allow autopilot to use throttle
Definition: autopilot.h:69
#define GUIDANCE_V_MODE_RC_DIRECT
Definition: guidance_v.h:36
bool ground_detected
automatic detection of landing
Definition: autopilot.h:74
#define AP_MODE_CARE_FREE_DIRECT
#define AP_MODE_FAILSAFE
#define GUIDANCE_H_MODE_RATE
Definition: guidance_h.h:57
#define AP_MODE_ATTITUDE_Z_HOLD
#define GUIDANCE_V_MODE_GUIDED
Definition: guidance_v.h:43
#define AP_ARMING_STATUS_AHRS_NOT_ALLIGNED
uint8_t mode
current autopilot mode
Definition: autopilot.h:63
Arming procedure for rotorcraft Several options can be selected:
#define SetRotorcraftCommands(_cmd, _in_flight, _motors_on)
#define GUIDANCE_H_MODE_KILL
Definition: guidance_h.h:56
#define NAV_PRESCALER
pprz_t throttle
throttle level as will be displayed in GCS
Definition: autopilot.h:66
#define GUIDANCE_H_MODE_RC_DIRECT
Definition: guidance_h.h:61