Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
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_KILL 0
40 #define AP_MODE_FAILSAFE 1
41 #define AP_MODE_HOME 2
42 #define AP_MODE_RATE_DIRECT 3
43 #define AP_MODE_ATTITUDE_DIRECT 4
44 #define AP_MODE_RATE_RC_CLIMB 5
45 #define AP_MODE_ATTITUDE_RC_CLIMB 6
46 #define AP_MODE_ATTITUDE_CLIMB 7
47 #define AP_MODE_RATE_Z_HOLD 8
48 #define AP_MODE_ATTITUDE_Z_HOLD 9
49 #define AP_MODE_HOVER_DIRECT 10
50 #define AP_MODE_HOVER_CLIMB 11
51 #define AP_MODE_HOVER_Z_HOLD 12
52 #define AP_MODE_NAV 13
53 #define AP_MODE_RC_DIRECT 14 // Safety Pilot Direct Commands for helicopter direct control
54 #define AP_MODE_CARE_FREE_DIRECT 15
55 #define AP_MODE_FORWARD 16
56 
57 extern uint8_t autopilot_mode;
59 extern bool_t autopilot_motors_on;
60 extern bool_t autopilot_in_flight;
61 extern bool_t kill_throttle;
62 extern bool_t autopilot_rc;
63 
64 extern bool_t autopilot_power_switch;
65 
66 extern void autopilot_init(void);
67 extern void autopilot_periodic(void);
68 extern void autopilot_on_rc_frame(void);
69 extern void autopilot_set_mode(uint8_t new_autopilot_mode);
70 extern void autopilot_set_motors_on(bool_t motors_on);
71 extern void autopilot_check_in_flight(bool_t motors_on);
72 
73 extern bool_t autopilot_ground_detected;
74 extern bool_t autopilot_detect_ground_once;
75 
77 
80 #ifndef MODE_MANUAL
81 #define MODE_MANUAL AP_MODE_ATTITUDE_DIRECT
82 #endif
83 #ifndef MODE_AUTO1
84 #define MODE_AUTO1 AP_MODE_HOVER_Z_HOLD
85 #endif
86 #ifndef MODE_AUTO2
87 #define MODE_AUTO2 AP_MODE_NAV
88 #endif
89 
90 
91 #define THRESHOLD_1_PPRZ (MIN_PPRZ / 2)
92 #define THRESHOLD_2_PPRZ (MAX_PPRZ / 2)
93 
94 #define AP_MODE_OF_PPRZ(_rc, _mode) { \
95  if (_rc > THRESHOLD_2_PPRZ) \
96  _mode = autopilot_mode_auto2; \
97  else if (_rc > THRESHOLD_1_PPRZ) \
98  _mode = MODE_AUTO1; \
99  else \
100  _mode = MODE_MANUAL; \
101  }
102 
103 #define autopilot_KillThrottle(_kill) { \
104  if (_kill) \
105  autopilot_set_motors_on(FALSE); \
106  else \
107  autopilot_set_motors_on(TRUE); \
108  }
109 
110 #ifdef POWER_SWITCH_GPIO
111 #include "mcu_periph/gpio.h"
112 #define autopilot_SetPowerSwitch(_v) { \
113  autopilot_power_switch = _v; \
114  if (_v) { gpio_set(POWER_SWITCH_GPIO); } \
115  else { gpio_clear(POWER_SWITCH_GPIO); } \
116  }
117 #else
118 #define autopilot_SetPowerSwitch(_v) { \
119  autopilot_power_switch = _v; \
120  }
121 #endif
122 
127 #ifndef ROTORCRAFT_COMMANDS_YAW_ALWAYS_ENABLED
128 #define SetRotorcraftCommands(_cmd, _in_flight, _motor_on) { \
129  if (!(_in_flight)) { _cmd[COMMAND_YAW] = 0; } \
130  if (!(_motor_on)) { _cmd[COMMAND_THRUST] = 0; } \
131  commands[COMMAND_ROLL] = _cmd[COMMAND_ROLL]; \
132  commands[COMMAND_PITCH] = _cmd[COMMAND_PITCH]; \
133  commands[COMMAND_YAW] = _cmd[COMMAND_YAW]; \
134  commands[COMMAND_THRUST] = _cmd[COMMAND_THRUST]; \
135 }
136 #else
137 #define SetRotorcraftCommands(_cmd, _in_flight, _motor_on) { \
138  if (!(_motor_on)) { _cmd[COMMAND_THRUST] = 0; } \
139  commands[COMMAND_ROLL] = _cmd[COMMAND_ROLL]; \
140  commands[COMMAND_PITCH] = _cmd[COMMAND_PITCH]; \
141  commands[COMMAND_YAW] = _cmd[COMMAND_YAW]; \
142  commands[COMMAND_THRUST] = _cmd[COMMAND_THRUST]; \
143 }
144 #endif
145 
147 #ifndef THRESHOLD_GROUND_DETECT
148 #define THRESHOLD_GROUND_DETECT 25.0
149 #endif
150 
152 static inline void DetectGroundEvent(void) {
154  struct NedCoor_f* accel = stateGetAccelNed_f();
155  if (accel->z < -THRESHOLD_GROUND_DETECT ||
156  accel->z > THRESHOLD_GROUND_DETECT) {
159  }
160  }
161 }
162 
163 #endif /* AUTOPILOT_H */
unsigned short uint16_t
Definition: types.h:16
void autopilot_periodic(void)
Definition: autopilot.c:279
bool_t autopilot_power_switch
Definition: autopilot.c:58
uint16_t autopilot_flight_time
flight time in seconds.
Definition: autopilot.c:47
void autopilot_set_mode(uint8_t new_autopilot_mode)
Definition: autopilot.c:338
bool_t autopilot_rc
Definition: autopilot.c:57
Some architecture independent helper functions for GPIOs.
Integrated Navigation System interface.
bool_t autopilot_in_flight
Definition: autopilot.c:50
#define FALSE
Definition: imu_chimu.h:141
bool_t autopilot_motors_on
Definition: autopilot.c:54
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:148
bool_t autopilot_detect_ground_once
Definition: autopilot.c:61
static struct NedCoor_f * stateGetAccelNed_f(void)
Get acceleration in NED coordinates (float).
Definition: state.h:938
bool_t kill_throttle
Definition: autopilot.c:41
float z
in meters
void autopilot_init(void)
Autopilot inititalization.
Definition: autopilot.c:142
void autopilot_check_in_flight(bool_t motors_on)
Definition: autopilot.c:436
#define TRUE
Definition: imu_chimu.h:144
unsigned char uint8_t
Definition: types.h:14
void autopilot_on_rc_frame(void)
Definition: autopilot.c:484
arch independent LED (Light Emitting Diodes) API
#define AP_MODE_FAILSAFE
Definition: autopilot.h:40
void autopilot_set_motors_on(bool_t motors_on)
Definition: autopilot.c:474
uint8_t autopilot_mode_auto2
Definition: autopilot.c:48
bool_t autopilot_ground_detected
Definition: autopilot.c:60
uint8_t autopilot_mode
Definition: autopilot.c:47
static void DetectGroundEvent(void)
Ground detection based on vertical acceleration.
Definition: autopilot.h:152