Paparazzi UAS  v5.14.0_stable-0-g3f680d1
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 
33 #include "subsystems/commands.h"
34 #include "subsystems/actuators.h"
35 #include "subsystems/electrical.h"
36 #include "subsystems/settings.h"
39 
43 
44 #if USE_STABILIZATION_RATE
46 #endif
47 
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 #if USE_KILL_SWITCH_FOR_MOTOR_ARMING
65 PRINT_CONFIG_MSG("Using kill switch for motor arming")
66 #elif USE_THROTTLE_FOR_MOTOR_ARMING
68 PRINT_CONFIG_MSG("Using throttle for motor arming")
69 #else
70 #include "autopilot_arming_yaw.h"
71 PRINT_CONFIG_MSG("Using 2 sec yaw for motor arming")
72 #endif
73 
74 /* Geofence exceptions */
76 
78 #ifndef FAILSAFE_MODE_TOO_FAR_FROM_HOME
79 #define FAILSAFE_MODE_TOO_FAR_FROM_HOME AP_MODE_FAILSAFE
80 #endif
81 
82 
83 #ifndef MODE_STARTUP
84 #define MODE_STARTUP AP_MODE_KILL
85 PRINT_CONFIG_MSG("Using default AP_MODE_KILL as MODE_STARTUP")
86 #endif
87 
88 #ifndef UNLOCKED_HOME_MODE
89 #if MODE_AUTO1 == AP_MODE_HOME
90 #define UNLOCKED_HOME_MODE TRUE
91 PRINT_CONFIG_MSG("Enabled UNLOCKED_HOME_MODE since MODE_AUTO1 is AP_MODE_HOME")
92 #elif MODE_AUTO2 == AP_MODE_HOME
93 #define UNLOCKED_HOME_MODE TRUE
94 PRINT_CONFIG_MSG("Enabled UNLOCKED_HOME_MODE since MODE_AUTO2 is AP_MODE_HOME")
95 #else
96 #define UNLOCKED_HOME_MODE FALSE
97 #endif
98 #endif
99 
100 #if MODE_MANUAL == AP_MODE_NAV
101 #error "MODE_MANUAL mustn't be AP_MODE_NAV"
102 #endif
103 
104 
106 {
107  /* Mode is finally set by autopilot_static_set_mode if MODE_STARTUP is not KILL.
108  * For autopilot_static_set_mode to do anything, the requested mode needs to differ
109  * from previous mode, so we set it to a safe KILL first.
110  */
112 
113  /* set startup mode, propagates through to guidance h/v */
115 
116  /* init arming */
118 }
119 
120 
121 #define NAV_PRESCALER (PERIODIC_FREQUENCY / NAV_FREQ)
123 {
124 
125  RunOnceEvery(NAV_PRESCALER, compute_dist2_to_home());
126 
131  } else {
133  }
134  }
135  }
136 
137  if (autopilot.mode == AP_MODE_HOME) {
138  RunOnceEvery(NAV_PRESCALER, nav_home());
139  } else {
140  // otherwise always call nav_periodic_task so that carrot is always updated in GCS for other modes
141  RunOnceEvery(NAV_PRESCALER, nav_periodic_task());
142  }
143 
144 
145  /* If in FAILSAFE mode and either already not in_flight anymore
146  * or just "detected" ground, go to KILL mode.
147  */
149  if (!autopilot_in_flight()) {
151  }
152 
153 #if FAILSAFE_GROUND_DETECT
154  INFO("Using FAILSAFE_GROUND_DETECT: KILL")
157  }
158 #endif
159  }
160 
161  /* Reset ground detection _after_ running flight plan
162  */
163  if (!autopilot_in_flight()) {
164  autopilot.ground_detected = false;
166  }
167 
168  /* Set fixed "failsafe" commands from airframe file if in KILL mode.
169  * If in FAILSAFE mode, run normal loops with failsafe attitude and
170  * downwards velocity setpoints.
171  */
172  if (autopilot.mode == AP_MODE_KILL) {
174  } else {
178  }
179 
180 }
181 
187 {
188  if (mode == AP_MODE_KILL || mode == AP_MODE_FAILSAFE || mode == AP_MODE_HOME) {
189  // safety modes are always accessible via settings
191  } else {
192  if (radio_control.status != RC_OK &&
193  (mode == AP_MODE_NAV || mode == AP_MODE_GUIDED || mode == AP_MODE_FLIP || mode == AP_MODE_MODULE)) {
194  // without RC, only nav-like modes are accessible
196  }
197  }
198  // with RC, other modes can only be changed from the RC
199 }
200 
201 
202 void autopilot_static_set_mode(uint8_t new_autopilot_mode)
203 {
204 
205  /* force startup mode (default is kill) as long as AHRS is not aligned */
206  if (!ap_ahrs_is_aligned()) {
207  new_autopilot_mode = MODE_STARTUP;
208  }
209 
210  if (new_autopilot_mode != autopilot.mode) {
211  /* horizontal mode */
212  switch (new_autopilot_mode) {
213  case AP_MODE_FAILSAFE:
214 #ifndef KILL_AS_FAILSAFE
217  break;
218 #endif
219  case AP_MODE_KILL:
222  break;
223  case AP_MODE_RC_DIRECT:
225  break;
227  case AP_MODE_RATE_DIRECT:
228  case AP_MODE_RATE_Z_HOLD:
229 #if USE_STABILIZATION_RATE
231 #else
232  return;
233 #endif
234  break;
240  break;
241  case AP_MODE_FORWARD:
243  break;
246  break;
248  case AP_MODE_HOVER_CLIMB:
251  break;
252  case AP_MODE_HOME:
253  case AP_MODE_NAV:
255  break;
256  case AP_MODE_MODULE:
257 #ifdef GUIDANCE_H_MODE_MODULE_SETTING
259 #endif
260  break;
261  case AP_MODE_FLIP:
263  break;
264  case AP_MODE_GUIDED:
266  break;
267  default:
268  break;
269  }
270  /* vertical mode */
271  switch (new_autopilot_mode) {
272  case AP_MODE_FAILSAFE:
273 #ifndef KILL_AS_FAILSAFE
276  break;
277 #endif
278  case AP_MODE_KILL:
280  stabilization_cmd[COMMAND_THRUST] = 0;
282  break;
283  case AP_MODE_RC_DIRECT:
284  case AP_MODE_RATE_DIRECT:
288  case AP_MODE_FORWARD:
290  break;
294  break;
296  case AP_MODE_HOVER_CLIMB:
298  break;
299  case AP_MODE_RATE_Z_HOLD:
303  break;
304  case AP_MODE_HOME:
305  case AP_MODE_NAV:
307  break;
308  case AP_MODE_MODULE:
309 #ifdef GUIDANCE_V_MODE_MODULE_SETTING
311 #endif
312  break;
313  case AP_MODE_FLIP:
315  break;
316  case AP_MODE_GUIDED:
318  break;
319  default:
320  break;
321  }
322  //if switching to rate mode but rate mode is not defined, the function returned
323  autopilot.mode = new_autopilot_mode;
324  }
325 }
326 
327 
328 void autopilot_static_set_motors_on(bool motors_on)
329 {
330  if (autopilot.mode != AP_MODE_KILL && ap_ahrs_is_aligned() && motors_on) {
331  autopilot.motors_on = true;
332  } else {
333  autopilot.motors_on = false;
334  }
336 }
337 
339 {
340 
341  if (kill_switch_is_on()) {
343  } else {
344 #ifdef RADIO_AUTO_MODE
345  INFO("Using RADIO_AUTO_MODE to switch between AUTO1 and AUTO2.")
346  uint8_t new_autopilot_mode = ap_mode_of_two_switches();
347 #else
348 #ifdef RADIO_MODE_2x3
349  uint8_t new_autopilot_mode = ap_mode_of_3x2way_switch();
350 #else
351  uint8_t new_autopilot_mode = ap_mode_of_3way_switch();
352 #endif
353 #endif
354 
355  /* don't enter NAV mode if GPS is lost (this also prevents mode oscillations) */
356  if (!(new_autopilot_mode == AP_MODE_NAV && GpsIsLost())) {
357  /* always allow to switch to manual */
358  if (new_autopilot_mode == MODE_MANUAL) {
359  autopilot_static_set_mode(new_autopilot_mode);
360  }
361  /* if in HOME mode, don't allow switching to non-manual modes */
362  else if ((autopilot.mode != AP_MODE_HOME)
364  /* Allowed to leave home mode when UNLOCKED_HOME_MODE */
366 #endif
367  ) {
368  autopilot_static_set_mode(new_autopilot_mode);
369  }
370  }
371  }
372 
373  /* an arming sequence is used to start/stop motors.
374  * only allow arming if ahrs is aligned
375  */
376  if (ap_ahrs_is_aligned()) {
379  } else {
381  }
382 
383  /* if not in FAILSAFE or HOME mode, read RC and set commands accordingly */
385 
386  /* if there are some commands that should always be set from RC, do it */
387 #ifdef SetAutoCommandsFromRC
388  SetAutoCommandsFromRC(commands, radio_control.values);
389 #endif
390 
391  /* if not in NAV_MODE set commands from the rc */
392 #ifdef SetCommandsFromRC
393  if (autopilot.mode != AP_MODE_NAV) {
394  SetCommandsFromRC(commands, radio_control.values);
395  }
396 #endif
397 
400  }
401 
402 }
403 
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:63
#define GUIDANCE_V_MODE_NAV
Definition: guidance_v.h:40
void guidance_h_read_rc(bool in_flight)
Definition: guidance_h.c:292
#define GUIDANCE_H_MODE_CARE_FREE
Definition: guidance_h.h:59
#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:351
#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:65
#define GUIDANCE_H_MODE_HOVER
Definition: guidance_h.h:56
void guidance_v_mode_changed(uint8_t new_mode)
Definition: guidance_v.c:227
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:295
static bool kill_switch_is_on(void)
#define GUIDANCE_H_MODE_ATTITUDE
Definition: guidance_h.h:55
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:69
float dist2_to_home
squared distance to home waypoint
Definition: navigation.c:82
#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:62
void autopilot_static_set_mode(uint8_t new_autopilot_mode)
Rate stabilization for rotorcrafts.
#define AP_MODE_ATTITUDE_RC_CLIMB
#define AP_ARMING_STATUS_AHRS_NOT_ALLIGNED
bool too_far_from_home
Definition: navigation.c:83
bool ap_ahrs_is_aligned(void)
Display descent speed in failsafe mode if needed.
Automatically arm the motors when applying throttle.
#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:70
Some helper functions to check RC sticks.
#define MODE_STARTUP
Arm the motors using a switch.
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:256
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:212
#define GUIDANCE_H_MODE_FORWARD
Definition: guidance_h.h:60
#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:473
uint8_t arming_status
Definition: autopilot.h:62
Arm the motors by with max yaw stick.
#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
arming status
Definition: autopilot.h:63
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:57
General stabilization interface for rotorcrafts.
void guidance_v_read_rc(void)
Definition: guidance_v.c:205
#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:199
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:126
#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:81
void autopilot_set_in_flight(bool in_flight)
set in_flight flag
Definition: autopilot.c:246
bool kill_throttle
allow autopilot to use throttle
Definition: autopilot.h:64
#define GUIDANCE_V_MODE_RC_DIRECT
Definition: guidance_v.h:36
bool ground_detected
automatic detection of landing
Definition: autopilot.h:69
#define AP_MODE_CARE_FREE_DIRECT
#define AP_MODE_FAILSAFE
#define GUIDANCE_H_MODE_RATE
Definition: guidance_h.h:54
#define AP_MODE_ATTITUDE_Z_HOLD
#define GUIDANCE_V_MODE_GUIDED
Definition: guidance_v.h:43
uint8_t mode
current autopilot mode
Definition: autopilot.h:59
#define SetRotorcraftCommands(_cmd, _in_flight, _motors_on)
#define GUIDANCE_H_MODE_KILL
Definition: guidance_h.h:53
#define NAV_PRESCALER
#define GUIDANCE_H_MODE_RC_DIRECT
Definition: guidance_h.h:58