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.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
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 
29 #ifndef AUTOPILOT_H
30 #define AUTOPILOT_H
31 
32 #include "std.h"
33 
34 #include "led.h"
35 
36 #include "generated/airframe.h"
37 #include "subsystems/ins.h"
38 
39 #define AP_MODE_FAILSAFE 0
40 #define AP_MODE_KILL 1
41 #define AP_MODE_RATE_DIRECT 2
42 #define AP_MODE_ATTITUDE_DIRECT 3
43 #define AP_MODE_RATE_RC_CLIMB 4
44 #define AP_MODE_ATTITUDE_RC_CLIMB 5
45 #define AP_MODE_ATTITUDE_CLIMB 6
46 #define AP_MODE_RATE_Z_HOLD 7
47 #define AP_MODE_ATTITUDE_Z_HOLD 8
48 #define AP_MODE_HOVER_DIRECT 9
49 #define AP_MODE_HOVER_CLIMB 10
50 #define AP_MODE_HOVER_Z_HOLD 11
51 #define AP_MODE_NAV 12
52 #define AP_MODE_RC_DIRECT 13 // Safety Pilot Direct Commands for helicopter direct control: appropriately chosen as mode "13"
53 #define AP_MODE_CARE_FREE_DIRECT 14
54 #define AP_MODE_FORWARD 15
55 
56 extern uint8_t autopilot_mode;
58 extern bool_t autopilot_motors_on;
59 extern bool_t autopilot_in_flight;
60 extern bool_t kill_throttle;
61 extern bool_t autopilot_rc;
62 
63 extern bool_t autopilot_power_switch;
64 
65 extern void autopilot_init(void);
66 extern void autopilot_periodic(void);
67 extern void autopilot_on_rc_frame(void);
68 extern void autopilot_set_mode(uint8_t new_autopilot_mode);
69 extern void autopilot_set_motors_on(bool_t motors_on);
70 
71 extern bool_t autopilot_detect_ground;
72 extern bool_t autopilot_detect_ground_once;
73 
75 
78 #ifndef MODE_MANUAL
79 #define MODE_MANUAL AP_MODE_RATE_DIRECT
80 #endif
81 #ifndef MODE_AUTO1
82 #define MODE_AUTO1 AP_MODE_ATTITUDE_DIRECT
83 #endif
84 #ifndef MODE_AUTO2
85 #define MODE_AUTO2 AP_MODE_ATTITUDE_Z_HOLD
86 #endif
87 
88 
89 #define THRESHOLD_1_PPRZ (MIN_PPRZ / 2)
90 #define THRESHOLD_2_PPRZ (MAX_PPRZ / 2)
91 
92 #define AP_MODE_OF_PPRZ(_rc, _mode) { \
93  if (_rc > THRESHOLD_2_PPRZ) \
94  _mode = autopilot_mode_auto2; \
95  else if (_rc > THRESHOLD_1_PPRZ) \
96  _mode = MODE_AUTO1; \
97  else \
98  _mode = MODE_MANUAL; \
99  }
100 
101 #define autopilot_KillThrottle(_kill) { \
102  if (_kill) \
103  autopilot_set_motors_on(FALSE); \
104  else \
105  autopilot_set_motors_on(TRUE); \
106  }
107 
108 #ifdef POWER_SWITCH_LED
109 #define autopilot_SetPowerSwitch(_v) { \
110  autopilot_power_switch = _v; \
111  if (_v) { LED_OFF(POWER_SWITCH_LED); } \
112  else { LED_ON(POWER_SWITCH_LED); } \
113  }
114 #else
115 #define autopilot_SetPowerSwitch(_v) { \
116  autopilot_power_switch = _v; \
117  }
118 #endif
119 
124 #ifndef ROTORCRAFT_COMMANDS_YAW_ALWAYS_ENABLED
125 #define SetRotorcraftCommands(_cmd, _in_flight, _motor_on) { \
126  if (!(_in_flight)) { _cmd[COMMAND_YAW] = 0; } \
127  if (!(_motor_on)) { _cmd[COMMAND_THRUST] = 0; } \
128  commands[COMMAND_ROLL] = _cmd[COMMAND_ROLL]; \
129  commands[COMMAND_PITCH] = _cmd[COMMAND_PITCH]; \
130  commands[COMMAND_YAW] = _cmd[COMMAND_YAW]; \
131  commands[COMMAND_THRUST] = _cmd[COMMAND_THRUST]; \
132 }
133 #else
134 #define SetRotorcraftCommands(_cmd, _in_flight, _motor_on) { \
135  if (!(_motor_on)) { _cmd[COMMAND_THRUST] = 0; } \
136  commands[COMMAND_ROLL] = _cmd[COMMAND_ROLL]; \
137  commands[COMMAND_PITCH] = _cmd[COMMAND_PITCH]; \
138  commands[COMMAND_YAW] = _cmd[COMMAND_YAW]; \
139  commands[COMMAND_THRUST] = _cmd[COMMAND_THRUST]; \
140 }
141 #endif
142 
144 #ifndef THRESHOLD_GROUND_DETECT
145 #define THRESHOLD_GROUND_DETECT 25.0
146 #endif
147 
149 static inline void DetectGroundEvent(void) {
151  struct NedCoor_f* accel = stateGetAccelNed_f();
152  if (accel->z < -THRESHOLD_GROUND_DETECT ||
153  accel->z > THRESHOLD_GROUND_DETECT) {
156  }
157  }
158 }
159 
160 #endif /* AUTOPILOT_H */
unsigned short uint16_t
Definition: types.h:16
void autopilot_periodic(void)
Definition: autopilot.c:141
bool_t autopilot_power_switch
Definition: autopilot.c:50
uint16_t autopilot_flight_time
flight time in seconds.
Definition: autopilot.c:37
void autopilot_set_mode(uint8_t new_autopilot_mode)
Definition: autopilot.c:174
bool_t autopilot_rc
Definition: autopilot.c:49
Integrated Navigation System interface.
bool_t autopilot_in_flight
Definition: autopilot.c:42
bool_t autopilot_detect_ground
Definition: autopilot.c:52
#define FALSE
Definition: imu_chimu.h:141
bool_t autopilot_motors_on
Definition: autopilot.c:46
vector in North East Down coordinates Units: meters
#define THRESHOLD_GROUND_DETECT
Z-acceleration threshold to detect ground in m/s^2.
Definition: autopilot.h:145
bool_t autopilot_detect_ground_once
Definition: autopilot.c:53
static struct NedCoor_f * stateGetAccelNed_f(void)
Get acceleration in NED coordinates (float).
Definition: state.h:938
bool_t kill_throttle
Definition: autopilot.c:32
float z
in meters
void autopilot_init(void)
Autopilot inititalization.
Definition: autopilot.c:48
#define TRUE
Definition: imu_chimu.h:144
unsigned char uint8_t
Definition: types.h:14
void autopilot_on_rc_frame(void)
Definition: autopilot.c:309
arch independent LED (Light Emitting Diodes) API
#define AP_MODE_FAILSAFE
Definition: autopilot.h:39
void autopilot_set_motors_on(bool_t motors_on)
Definition: autopilot.c:299
uint8_t autopilot_mode_auto2
Definition: autopilot.c:40
uint8_t autopilot_mode
Definition: autopilot.c:39
static void DetectGroundEvent(void)
Ground detection based on vertical acceleration.
Definition: autopilot.h:149