Paparazzi UAS  v5.15_devel-99-g2ff7410
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  autopilot.throttle = commands[COMMAND_THRUST];
180 
181 }
182 
188 {
189  if (mode == AP_MODE_KILL || mode == AP_MODE_FAILSAFE || mode == AP_MODE_HOME) {
190  // safety modes are always accessible via settings
192  } else {
193  if (radio_control.status != RC_OK &&
194  (mode == AP_MODE_NAV || mode == AP_MODE_GUIDED || mode == AP_MODE_FLIP || mode == AP_MODE_MODULE)) {
195  // without RC, only nav-like modes are accessible
197  }
198  }
199  // with RC, other modes can only be changed from the RC
200 }
201 
202 
203 void autopilot_static_set_mode(uint8_t new_autopilot_mode)
204 {
205 
206  /* force startup mode (default is kill) as long as AHRS is not aligned */
207  if (!ap_ahrs_is_aligned()) {
208  new_autopilot_mode = MODE_STARTUP;
209  }
210 
211  if (new_autopilot_mode != autopilot.mode) {
212  /* horizontal mode */
213  switch (new_autopilot_mode) {
214  case AP_MODE_FAILSAFE:
215 #ifndef KILL_AS_FAILSAFE
218  break;
219 #endif
220  case AP_MODE_KILL:
223  break;
224  case AP_MODE_RC_DIRECT:
226  break;
228  case AP_MODE_RATE_DIRECT:
229  case AP_MODE_RATE_Z_HOLD:
230 #if USE_STABILIZATION_RATE
232 #else
233  return;
234 #endif
235  break;
241  break;
242  case AP_MODE_FORWARD:
244  break;
247  break;
249  case AP_MODE_HOVER_CLIMB:
252  break;
253  case AP_MODE_HOME:
254  case AP_MODE_NAV:
256  break;
257  case AP_MODE_MODULE:
258 #ifdef GUIDANCE_H_MODE_MODULE_SETTING
260 #endif
261  break;
262  case AP_MODE_FLIP:
264  break;
265  case AP_MODE_GUIDED:
267  break;
268  default:
269  break;
270  }
271  /* vertical mode */
272  switch (new_autopilot_mode) {
273  case AP_MODE_FAILSAFE:
274 #ifndef KILL_AS_FAILSAFE
277  break;
278 #endif
279  case AP_MODE_KILL:
281  stabilization_cmd[COMMAND_THRUST] = 0;
283  break;
284  case AP_MODE_RC_DIRECT:
285  case AP_MODE_RATE_DIRECT:
289  case AP_MODE_FORWARD:
291  break;
295  break;
297  case AP_MODE_HOVER_CLIMB:
299  break;
300  case AP_MODE_RATE_Z_HOLD:
304  break;
305  case AP_MODE_HOME:
306  case AP_MODE_NAV:
308  break;
309  case AP_MODE_MODULE:
310 #ifdef GUIDANCE_V_MODE_MODULE_SETTING
312 #endif
313  break;
314  case AP_MODE_FLIP:
316  break;
317  case AP_MODE_GUIDED:
319  break;
320  default:
321  break;
322  }
323  //if switching to rate mode but rate mode is not defined, the function returned
324  autopilot.mode = new_autopilot_mode;
325  }
326 }
327 
328 
329 void autopilot_static_set_motors_on(bool motors_on)
330 {
331  if (autopilot.mode != AP_MODE_KILL && ap_ahrs_is_aligned() && motors_on) {
332  autopilot.motors_on = true;
333  } else {
334  autopilot.motors_on = false;
335  }
337 }
338 
340 {
341 
342  if (kill_switch_is_on()) {
344  } else {
345 #ifdef RADIO_AUTO_MODE
346  INFO("Using RADIO_AUTO_MODE to switch between AUTO1 and AUTO2.")
347  uint8_t new_autopilot_mode = ap_mode_of_two_switches();
348 #else
349 #ifdef RADIO_MODE_2x3
350  uint8_t new_autopilot_mode = ap_mode_of_3x2way_switch();
351 #else
352  uint8_t new_autopilot_mode = ap_mode_of_3way_switch();
353 #endif
354 #endif
355 
356  /* don't enter NAV mode if GPS is lost (this also prevents mode oscillations) */
357  if (!(new_autopilot_mode == AP_MODE_NAV && GpsIsLost())) {
358  /* always allow to switch to manual */
359  if (new_autopilot_mode == MODE_MANUAL) {
360  autopilot_static_set_mode(new_autopilot_mode);
361  }
362  /* if in HOME mode, don't allow switching to non-manual modes */
363  else if ((autopilot.mode != AP_MODE_HOME)
365  /* Allowed to leave home mode when UNLOCKED_HOME_MODE */
367 #endif
368  ) {
369  autopilot_static_set_mode(new_autopilot_mode);
370  }
371  }
372  }
373 
374  /* an arming sequence is used to start/stop motors.
375  * only allow arming if ahrs is aligned
376  */
377  if (ap_ahrs_is_aligned()) {
380  } else {
382  }
383 
384  /* if not in FAILSAFE or HOME mode, read RC and set commands accordingly */
386 
387  /* if there are some commands that should always be set from RC, do it */
388 #ifdef SetAutoCommandsFromRC
389  SetAutoCommandsFromRC(commands, radio_control.values);
390 #endif
391 
392  /* if not in NAV_MODE set commands from the rc */
393 #ifdef SetCommandsFromRC
394  if (autopilot.mode != AP_MODE_NAV) {
395  SetCommandsFromRC(commands, radio_control.values);
396  }
397 #endif
398 
401  }
402 
403 }
404 
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:292
#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: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: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: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:65
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:75
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: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:212
#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:473
uint8_t arming_status
arming status
Definition: autopilot.h:67
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
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:81
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
uint8_t mode
current autopilot mode
Definition: autopilot.h:63
#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