Paparazzi UAS  v4.0.4_stable-3-gf39211a
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros 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 
23 
29 #include "led.h"
30 
33 
37 
40 
41 bool_t autopilot_rc;
43 
46 
47 #define AUTOPILOT_IN_FLIGHT_TIME 40
48 
49 #ifndef AUTOPILOT_DISABLE_AHRS_KILL
50 #include "subsystems/ahrs.h"
51 static inline int ahrs_is_aligned(void) {
52  return (ahrs.status == AHRS_RUNNING);
53 }
54 #else
55 static inline int ahrs_is_aligned(void) {
56  return TRUE;
57 }
58 #endif
59 
60 #if USE_KILL_SWITCH_FOR_MOTOR_ARMING
62 #elif USE_THROTTLE_FOR_MOTOR_ARMING
64 #else
65 #include "autopilot_arming_yaw.h"
66 #endif
67 
68 void autopilot_init(void) {
80 #ifdef POWER_SWITCH_LED
81  LED_ON(POWER_SWITCH_LED); // POWER OFF
82 #endif
84 }
85 
86 
87 void autopilot_periodic(void) {
88 
89  RunOnceEvery(NAV_PRESCALER, nav_periodic_task());
90 #ifdef FAILSAFE_GROUND_DETECT
94  }
95 #endif
96 
97  /* set failsafe commands, if in FAILSAFE or KILL mode */
98 #ifndef FAILSAFE_GROUND_DETECT
101 #else
102  if (autopilot_mode == AP_MODE_KILL) {
103 #endif
106  }
107  else {
112  }
113 
114 }
115 
116 
117 void autopilot_set_mode(uint8_t new_autopilot_mode) {
118 
119  /* force kill mode as long as AHRS is not aligned */
120  if (!ahrs_is_aligned())
121  new_autopilot_mode = AP_MODE_KILL;
122 
123  if (new_autopilot_mode != autopilot_mode) {
124  /* horizontal mode */
125  switch (new_autopilot_mode) {
126  case AP_MODE_FAILSAFE:
127 #ifndef KILL_AS_FAILSAFE
131  break;
132 #endif
133  case AP_MODE_KILL:
138  break;
139  case AP_MODE_RC_DIRECT:
141  break;
142  case AP_MODE_RATE_DIRECT:
143  case AP_MODE_RATE_Z_HOLD:
145  break;
150  break;
152  case AP_MODE_HOVER_CLIMB:
155  break;
156  case AP_MODE_NAV:
158  break;
159  default:
160  break;
161  }
162  /* vertical mode */
163  switch (new_autopilot_mode) {
164  case AP_MODE_FAILSAFE:
165 #ifndef KILL_AS_FAILSAFE
168  break;
169 #endif
170  case AP_MODE_KILL:
172  break;
173  case AP_MODE_RC_DIRECT:
174  case AP_MODE_RATE_DIRECT:
178  break;
182  break;
184  case AP_MODE_HOVER_CLIMB:
186  break;
187  case AP_MODE_RATE_Z_HOLD:
191  break;
192  case AP_MODE_NAV:
194  break;
195  default:
196  break;
197  }
198  autopilot_mode = new_autopilot_mode;
199  }
200 
201 }
202 
203 
204 static inline void autopilot_check_in_flight( bool_t motors_on ) {
205  if (autopilot_in_flight) {
206  if (autopilot_in_flight_counter > 0) {
207  if (THROTTLE_STICK_DOWN()) {
209  if (autopilot_in_flight_counter == 0) {
211  }
212  }
213  else { /* !THROTTLE_STICK_DOWN */
215  }
216  }
217  }
218  else { /* not in flight */
220  motors_on) {
221  if (!THROTTLE_STICK_DOWN()) {
225  }
226  else { /* THROTTLE_STICK_DOWN */
228  }
229  }
230  }
231 }
232 
233 
234 void autopilot_set_motors_on(bool_t motors_on) {
235  if (ahrs_is_aligned() && motors_on)
237  else
241 }
242 
243 
245 
246  if (kill_switch_is_on())
248  else {
249  uint8_t new_autopilot_mode = 0;
250  AP_MODE_OF_PPRZ(radio_control.values[RADIO_MODE], new_autopilot_mode);
251  autopilot_set_mode(new_autopilot_mode);
252  }
253 
254  /* if not in FAILSAFE mode check motor and in_flight status, read RC */
256 
257  /* an arming sequence is used to start/stop motors */
260 
262 
265  }
266 
267 }
unsigned short uint16_t
Definition: types.h:16
uint8_t autopilot_mode
Definition: autopilot.c:31
#define AP_MODE_KILL
Definition: autopilot.h:33
#define AP_MODE_ATTITUDE_DIRECT
Definition: autopilot.h:35
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:53
#define GUIDANCE_H_MODE_RC_DIRECT
Definition: guidance_h.h:40
#define AP_MODE_HOVER_DIRECT
Definition: autopilot.h:41
void guidance_v_run(bool_t in_flight)
Definition: guidance_v.c:165
static bool_t kill_switch_is_on(void)
bool_t autopilot_in_flight
Definition: autopilot.c:34
#define AP_MODE_RATE_RC_CLIMB
Definition: autopilot.h:36
uint16_t autopilot_flight_time
Definition: autopilot.c:36
#define AP_MODE_RATE_Z_HOLD
Definition: autopilot.h:39
bool_t autopilot_detect_ground
Definition: autopilot.c:44
static void autopilot_arming_set(bool_t motors_on)
struct Ahrs ahrs
global AHRS state (fixed point version)
Definition: ahrs.c:24
#define AUTOPILOT_IN_FLIGHT_TIME
Definition: autopilot.c:47
void guidance_v_mode_changed(uint8_t new_mode)
Definition: guidance_v.c:129
#define GUIDANCE_H_MODE_ATTITUDE
Definition: guidance_h.h:37
#define AP_MODE_ATTITUDE_Z_HOLD
Definition: autopilot.h:40
#define LED_ON(i)
Definition: led_hw.h:28
#define AP_MODE_HOVER_CLIMB
Definition: autopilot.h:42
bool_t autopilot_power_switch
Definition: autopilot.c:42
void autopilot_init(void)
Definition: autopilot.c:68
float theta
in radians
uint32_t autopilot_in_flight_counter
Definition: autopilot.c:35
#define THROTTLE_STICK_DOWN()
#define GUIDANCE_V_MODE_KILL
Definition: guidance_v.h:48
#define FALSE
Definition: imu_chimu.h:141
const pprz_t commands_failsafe[COMMANDS_NB]
Definition: commands.c:43
#define AP_MODE_RATE_DIRECT
Definition: autopilot.h:34
bool_t autopilot_motors_on
Definition: autopilot.c:38
#define MODE_AUTO2
Definition: autopilot.h:77
struct FloatEulers stab_att_sp_euler
with INT32_ANGLE_FRAC
#define AP_MODE_RC_DIRECT
Definition: autopilot.h:45
#define GUIDANCE_H_MODE_HOVER
Definition: guidance_h.h:38
void guidance_h_read_rc(bool_t in_flight)
Definition: guidance_h.c:143
float phi
in radians
#define POWER_SWITCH_LED
Definition: booz_1.0.h:51
uint8_t status
status of the AHRS, AHRS_UNINIT or AHRS_RUNNING
Definition: ahrs.h:54
bool_t autopilot_detect_ground_once
Definition: autopilot.c:45
#define GUIDANCE_V_MODE_CLIMB
Definition: guidance_v.h:51
unsigned long uint32_t
Definition: types.h:18
void autopilot_set_mode(uint8_t new_autopilot_mode)
Definition: autopilot.c:117
static void autopilot_arming_init(void)
static int ahrs_is_aligned(void)
Definition: autopilot.c:51
uint8_t autopilot_mode_auto2
Definition: autopilot.c:32
void guidance_h_run(bool_t in_flight)
Definition: guidance_h.c:178
#define GUIDANCE_V_MODE_HOVER
Definition: guidance_v.h:52
void autopilot_set_motors_on(bool_t motors_on)
Definition: autopilot.c:234
struct RadioControl radio_control
Definition: radio_control.c:27
#define GUIDANCE_H_MODE_NAV
Definition: guidance_h.h:39
#define AP_MODE_NAV
Definition: autopilot.h:44
#define GUIDANCE_H_MODE_KILL
Definition: guidance_h.h:35
bool_t kill_throttle
Definition: autopilot.c:39
#define TRUE
Definition: imu_chimu.h:144
#define AP_MODE_OF_PPRZ(_rc, _mode)
Definition: autopilot.h:84
#define RADIO_MODE
Definition: spektrum_arch.h:63
#define SetCommands(t)
Storage of intermediate command values: these values come from the RC (MANUAL mode), from the autopilot (AUTO mode) or from control loops.
Definition: commands.h:44
unsigned char uint8_t
Definition: types.h:14
void guidance_h_mode_changed(uint8_t new_mode)
Definition: guidance_h.c:109
General stabilization interface for rotorcrafts.
void guidance_v_read_rc(void)
Definition: guidance_v.c:118
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:28
bool_t autopilot_rc
Definition: autopilot.c:41
#define AP_MODE_HOVER_Z_HOLD
Definition: autopilot.h:43
void autopilot_periodic(void)
Definition: autopilot.c:87
void autopilot_on_rc_frame(void)
Definition: autopilot.c:244
#define AP_MODE_ATTITUDE_RC_CLIMB
Definition: autopilot.h:37
arch independent LED (Light Emitting Diodes) API
int32_t guidance_v_zd_sp
vertical speed setpoint in meter/s (input).
Definition: guidance_v.c:77
#define AP_MODE_FAILSAFE
Definition: autopilot.h:32
#define GUIDANCE_V_MODE_RC_CLIMB
Definition: guidance_v.h:50
static void autopilot_check_in_flight(bool_t motors_on)
Definition: autopilot.c:204
#define GUIDANCE_V_MODE_RC_DIRECT
Definition: guidance_v.h:49
#define GUIDANCE_H_MODE_RATE
Definition: guidance_h.h:36
#define AP_MODE_ATTITUDE_CLIMB
Definition: autopilot.h:38
#define SPEED_BFP_OF_REAL(_af)
#define AHRS_RUNNING
Definition: ahrs.h:34