Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
autopilot.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2012 The Paparazzi Team
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
30 
32 #include "subsystems/gps.h"
33 #include "subsystems/commands.h"
37 #include "led.h"
38 
41 
45 
48 
49 bool_t autopilot_rc;
51 
54 
55 #define AUTOPILOT_IN_FLIGHT_TIME 40
56 
57 #ifndef AUTOPILOT_DISABLE_AHRS_KILL
58 #include "subsystems/ahrs.h"
59 static inline int ahrs_is_aligned(void) {
60  return (ahrs.status == AHRS_RUNNING);
61 }
62 #else
63 PRINT_CONFIG_MSG("Using AUTOPILOT_DISABLE_AHRS_KILL")
64 static inline int ahrs_is_aligned(void) {
65  return TRUE;
66 }
67 #endif
68 
69 #if USE_KILL_SWITCH_FOR_MOTOR_ARMING
71 #elif USE_THROTTLE_FOR_MOTOR_ARMING
73 #else
74 #include "autopilot_arming_yaw.h"
75 #endif
76 
77 #ifndef MODE_STARTUP
78 #define MODE_STARTUP AP_MODE_KILL
79 PRINT_CONFIG_MSG("Using default AP_MODE_KILL as MODE_STARTUP")
80 #endif
81 
82 void autopilot_init(void) {
83  /* mode is finally set at end of init if MODE_STARTUP is not KILL */
95 #ifdef POWER_SWITCH_LED
96  LED_ON(POWER_SWITCH_LED); // POWER OFF
97 #endif
98 
100 
101  nav_init();
102  guidance_h_init();
103  guidance_v_init();
105 
106  /* set startup mode, propagats through to guidance h/v */
108 }
109 
110 
111 static inline void autopilot_check_in_flight_no_rc( bool_t motors_on ) {
112  if (autopilot_in_flight) {
113  if (autopilot_in_flight_counter > 0) {
114  if (stabilization_cmd[COMMAND_THRUST] == 0) {
116  if (autopilot_in_flight_counter == 0) {
118  }
119  }
120  else { /* !THROTTLE_STICK_DOWN */
122  }
123  }
124  }
125  else { /* not in flight */
127  motors_on) {
128  if (stabilization_cmd[COMMAND_THRUST] > 0) {
132  }
133  else { /* THROTTLE_STICK_DOWN */
135  }
136  }
137  }
138 }
139 
140 
141 void autopilot_periodic(void) {
142 
143  RunOnceEvery(NAV_PRESCALER, nav_periodic_task());
144 #if FAILSAFE_GROUND_DETECT
145 INFO("Using FAILSAFE_GROUND_DETECT")
149  }
150 #endif
151 
152  /* set failsafe commands, if in FAILSAFE or KILL mode */
153 #if !FAILSAFE_GROUND_DETECT
154  if (autopilot_mode == AP_MODE_KILL ||
156 #else
157  if (autopilot_mode == AP_MODE_KILL) {
158 #endif
160  }
161  else {
165  }
166 
167  // when we dont have RC, check in flight by looking at throttle
168  if (radio_control.status != RC_OK) {
170  }
171 }
172 
173 
174 void autopilot_set_mode(uint8_t new_autopilot_mode) {
175 
176  /* force kill mode as long as AHRS is not aligned */
177  if (!ahrs_is_aligned())
178  new_autopilot_mode = AP_MODE_KILL;
179 
180  if (new_autopilot_mode != autopilot_mode) {
181  /* horizontal mode */
182  switch (new_autopilot_mode) {
183  case AP_MODE_FAILSAFE:
184 #ifndef KILL_AS_FAILSAFE
187  break;
188 #endif
189  case AP_MODE_KILL:
194  break;
195  case AP_MODE_RC_DIRECT:
197  break;
198  case AP_MODE_RATE_DIRECT:
199  case AP_MODE_RATE_Z_HOLD:
201  break;
207  break;
208  case AP_MODE_FORWARD:
210  break;
213  break;
215  case AP_MODE_HOVER_CLIMB:
218  break;
219  case AP_MODE_NAV:
221  break;
222  default:
223  break;
224  }
225  /* vertical mode */
226  switch (new_autopilot_mode) {
227  case AP_MODE_FAILSAFE:
228 #ifndef KILL_AS_FAILSAFE
231  break;
232 #endif
233  case AP_MODE_KILL:
235  break;
236  case AP_MODE_RC_DIRECT:
237  case AP_MODE_RATE_DIRECT:
241  case AP_MODE_FORWARD:
243  break;
247  break;
249  case AP_MODE_HOVER_CLIMB:
251  break;
252  case AP_MODE_RATE_Z_HOLD:
256  break;
257  case AP_MODE_NAV:
259  break;
260  default:
261  break;
262  }
263  autopilot_mode = new_autopilot_mode;
264  }
265 
266 }
267 
268 
269 static inline void autopilot_check_in_flight( bool_t motors_on ) {
270  if (autopilot_in_flight) {
271  if (autopilot_in_flight_counter > 0) {
272  if (THROTTLE_STICK_DOWN()) {
274  if (autopilot_in_flight_counter == 0) {
276  }
277  }
278  else { /* !THROTTLE_STICK_DOWN */
280  }
281  }
282  }
283  else { /* not in flight */
285  motors_on) {
286  if (!THROTTLE_STICK_DOWN()) {
290  }
291  else { /* THROTTLE_STICK_DOWN */
293  }
294  }
295  }
296 }
297 
298 
299 void autopilot_set_motors_on(bool_t motors_on) {
300  if (ahrs_is_aligned() && motors_on)
302  else
306 }
307 
308 
310 
311  if (kill_switch_is_on())
313  else {
314  uint8_t new_autopilot_mode = 0;
315  AP_MODE_OF_PPRZ(radio_control.values[RADIO_MODE], new_autopilot_mode);
316  /* don't enter NAV mode if GPS is lost (this also prevents mode oscillations) */
317  if (!(new_autopilot_mode == AP_MODE_NAV
318 #if USE_GPS
319  && GpsIsLost()
320 #endif
321  ))
322  autopilot_set_mode(new_autopilot_mode);
323  }
324 
325  /* if not in FAILSAFE mode check motor and in_flight status, read RC */
327 
328  /* if there are some commands that should always be set from RC, do it */
329 #ifdef SetAutoCommandsFromRC
330  SetAutoCommandsFromRC(commands, radio_control.values);
331 #endif
332 
333  /* if not in NAV_MODE set commands from the rc */
334 #ifdef SetCommandsFromRC
335  if (autopilot_mode != AP_MODE_NAV) {
336  SetCommandsFromRC(commands, radio_control.values);
337  }
338 #endif
339 
340  /* an arming sequence is used to start/stop motors */
343 
345 
348  }
349 
350 }
static void autopilot_check_in_flight(bool_t motors_on)
Definition: autopilot.c:269
unsigned short uint16_t
Definition: types.h:16
#define AP_MODE_KILL
Definition: autopilot.h:40
#define AP_MODE_ATTITUDE_DIRECT
Definition: autopilot.h:42
static void autopilot_arming_check_motors_on(void)
State machine to check if motors should be turned ON or OFF using the kill switch.
Attitude and Heading Reference System interface.
#define GUIDANCE_V_MODE_NAV
Definition: guidance_v.h:42
#define GUIDANCE_H_MODE_RC_DIRECT
Definition: guidance_h.h:49
#define GUIDANCE_H_MODE_CARE_FREE
Definition: guidance_h.h:50
void autopilot_init(void)
Autopilot inititalization.
Definition: autopilot.c:48
uint8_t autopilot_mode
Definition: autopilot.c:39
bool_t autopilot_motors_on
Definition: autopilot.c:46
#define AP_MODE_HOVER_DIRECT
Definition: autopilot.h:48
uint16_t autopilot_flight_time
flight time in seconds.
Definition: autopilot.c:37
void guidance_v_run(bool_t in_flight)
Definition: guidance_v.c:162
static bool_t kill_switch_is_on(void)
void guidance_v_init(void)
Definition: guidance_v.c:97
#define AP_MODE_RATE_RC_CLIMB
Definition: autopilot.h:43
#define AP_MODE_FORWARD
Definition: autopilot.h:54
#define AP_MODE_RATE_Z_HOLD
Definition: autopilot.h:46
static void autopilot_arming_set(bool_t motors_on)
struct Ahrs ahrs
global AHRS state
Definition: ahrs.c:30
void guidance_v_mode_changed(uint8_t new_mode)
Definition: guidance_v.c:126
#define GUIDANCE_H_MODE_ATTITUDE
Definition: guidance_h.h:46
#define AP_MODE_ATTITUDE_Z_HOLD
Definition: autopilot.h:47
#define LED_ON(i)
Definition: led_hw.h:28
bool_t autopilot_detect_ground_once
Definition: autopilot.c:53
Autopilot modes.
void stabilization_init(void)
Definition: stabilization.c:30
#define AP_MODE_HOVER_CLIMB
Definition: autopilot.h:49
uint8_t autopilot_mode_auto2
Definition: autopilot.c:40
#define THROTTLE_STICK_DOWN()
#define GUIDANCE_V_MODE_KILL
Definition: guidance_v.h:37
#define FALSE
Definition: imu_chimu.h:141
const pprz_t commands_failsafe[COMMANDS_NB]
Definition: commands.c:31
#define AP_MODE_RATE_DIRECT
Definition: autopilot.h:41
Automatically arm the motors when applying throttle.
#define SetRotorcraftCommands(_cmd, _in_flight,_motor_on)
Set Rotorcraft commands.
Definition: autopilot.h:125
#define MODE_AUTO2
Definition: autopilot.h:85
bool_t GpsIsLost(void)
Definition: gps.h:114
uint32_t autopilot_in_flight_counter
Definition: autopilot.c:43
#define AP_MODE_RC_DIRECT
Definition: autopilot.h:52
#define GUIDANCE_H_MODE_HOVER
Definition: guidance_h.h:47
void autopilot_set_motors_on(bool_t motors_on)
Definition: autopilot.c:299
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:55
void guidance_h_read_rc(bool_t in_flight)
Definition: guidance_h.c:172
#define AP_MODE_CARE_FREE_DIRECT
Definition: autopilot.h:53
Arm the motors using a switch.
#define POWER_SWITCH_LED
Definition: booz_1.0.h:51
bool_t autopilot_power_switch
Definition: autopilot.c:50
Device independent GPS code (interface)
bool_t autopilot_in_flight
Definition: autopilot.c:42
uint8_t status
status of the AHRS, AHRS_UNINIT or AHRS_RUNNING
Definition: ahrs.h:45
#define GUIDANCE_V_MODE_CLIMB
Definition: guidance_v.h:40
unsigned long uint32_t
Definition: types.h:18
void stabilization_attitude_set_failsafe_setpoint(void)
static void autopilot_arming_init(void)
bool_t autopilot_rc
Definition: autopilot.c:49
bool_t kill_throttle
Definition: autopilot.c:32
void guidance_h_run(bool_t in_flight)
Definition: guidance_h.c:213
bool_t autopilot_detect_ground
Definition: autopilot.c:52
void autopilot_on_rc_frame(void)
Definition: autopilot.c:309
#define GUIDANCE_V_MODE_HOVER
Definition: guidance_v.h:41
Hardware independent code for commands handling.
struct RadioControl radio_control
Definition: radio_control.c:25
#define GUIDANCE_H_MODE_NAV
Definition: guidance_h.h:48
#define AP_MODE_NAV
Definition: autopilot.h:51
#define GUIDANCE_H_MODE_KILL
Definition: guidance_h.h:44
Arm the motors by with max yaw stick.
void autopilot_periodic(void)
Definition: autopilot.c:141
#define TRUE
Definition: imu_chimu.h:144
#define RC_OK
Definition: radio_control.h:45
uint8_t status
Definition: radio_control.h:50
#define AP_MODE_OF_PPRZ(_rc, _mode)
Definition: autopilot.h:92
#define RADIO_MODE
Definition: spektrum_arch.h:61
#define SetCommands(t)
Definition: commands.h:41
unsigned char uint8_t
Definition: types.h:14
pprz_t commands[COMMANDS_NB]
Storage of intermediate command values.
Definition: commands.c:30
void guidance_h_mode_changed(uint8_t new_mode)
Definition: guidance_h.c:120
General stabilization interface for rotorcrafts.
void guidance_v_read_rc(void)
Definition: guidance_v.c:115
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:28
#define AP_MODE_HOVER_Z_HOLD
Definition: autopilot.h:50
static int ahrs_is_aligned(void)
Definition: autopilot.c:59
#define AP_MODE_ATTITUDE_RC_CLIMB
Definition: autopilot.h:44
arch independent LED (Light Emitting Diodes) API
int32_t guidance_v_zd_sp
vertical speed setpoint in meter/s (input).
Definition: guidance_v.c:74
#define AP_MODE_FAILSAFE
Definition: autopilot.h:39
#define MODE_STARTUP
Definition: autopilot.c:78
#define AUTOPILOT_IN_FLIGHT_TIME
Definition: autopilot.c:55
#define GUIDANCE_V_MODE_RC_CLIMB
Definition: guidance_v.h:39
#define GUIDANCE_H_MODE_FORWARD
Definition: guidance_h.h:51
#define GUIDANCE_V_MODE_RC_DIRECT
Definition: guidance_v.h:38
#define GUIDANCE_H_MODE_RATE
Definition: guidance_h.h:45
void autopilot_set_mode(uint8_t new_autopilot_mode)
Definition: autopilot.c:174
#define AP_MODE_ATTITUDE_CLIMB
Definition: autopilot.h:45
#define SPEED_BFP_OF_REAL(_af)
static void autopilot_check_in_flight_no_rc(bool_t motors_on)
Definition: autopilot.c:111
void guidance_h_init(void)
Definition: guidance_h.c:93
#define AHRS_RUNNING
Definition: ahrs.h:36