Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
autopilot.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2003 Pascal Brisset, Antoine Drouin
3  * Copyright (C) 2008-2012 The Paparazzi Team
4  * Copyright (C) 2016-2017 Gautier Hattenberger <gautier.hattenberger@enac.fr>
5  *
6  * This file is part of paparazzi.
7  *
8  * paparazzi is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2, or (at your option)
11  * any later version.
12  *
13  * paparazzi is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with paparazzi; see the file COPYING. If not, see
20  * <http://www.gnu.org/licenses/>.
21  */
22 
32 #include "autopilot.h"
33 
34 #include "generated/modules.h"
35 
36 #include "mcu_periph/uart.h"
37 #include "mcu_periph/sys_time.h"
38 #include "mcu_periph/gpio.h"
39 #include "modules/core/commands.h"
41 #include "modules/core/abi.h"
42 
43 #include "modules/core/settings.h"
44 #include "generated/settings.h"
45 
46 #include "pprz_version.h"
47 
49 
50 #ifndef AUTOPILOT_RC_ID
51 #define AUTOPILOT_RC_ID ABI_BROADCAST
52 #endif
53 PRINT_CONFIG_VAR(AUTOPILOT_RC_ID)
55 
56 static void rc_cb(uint8_t __attribute__((unused)) sender_id,
57  struct RadioControl __attribute__((unused)) *rc)
58 {
59  // TODO pass the RC struct to the on_rc_frame function
61 }
62 
63 static void send_autopilot_version(struct transport_tx *trans, struct link_device *dev)
64 {
65  static uint32_t ap_version = PPRZ_VERSION_INT;
66  static char *ver_desc = PPRZ_VERSION_DESC;
67  pprz_msg_send_AUTOPILOT_VERSION(trans, dev, AC_ID, &ap_version, strlen(ver_desc), ver_desc);
68 }
69 
70 static void send_alive(struct transport_tx *trans, struct link_device *dev)
71 {
72  pprz_msg_send_ALIVE(trans, dev, AC_ID, 16, MD5SUM);
73 }
74 
75 static void send_attitude(struct transport_tx *trans, struct link_device *dev)
76 {
77  struct FloatEulers *att = stateGetNedToBodyEulers_f();
78  pprz_msg_send_ATTITUDE(trans, dev, AC_ID, &(att->phi), &(att->psi), &(att->theta));
79 };
80 
81 static void send_dl_value(struct transport_tx *trans, struct link_device *dev)
82 {
83  PeriodicSendDlValue(trans, dev);
84 }
85 
86 static void send_minimal_com(struct transport_tx *trans, struct link_device *dev)
87 {
88  float lat = DegOfRad(stateGetPositionLla_f()->lat);
89  float lon = DegOfRad(stateGetPositionLla_f()->lon);
90  float hmsl = stateGetPositionUtm_f()->alt;
91  float gspeed = stateGetHorizontalSpeedNorm_f();
93  float climb = stateGetSpeedEnu_f()->z;
94  uint8_t throttle = (uint8_t)(100 * autopilot.throttle / MAX_PPRZ);
95 #if USE_GPS
96  uint8_t gps_fix = gps.fix;
97 #else
98  uint8_t gps_fix = 0;
99 #endif
100  pprz_msg_send_MINIMAL_COM(trans, dev, AC_ID,
101  &lat, &lon, &hmsl, &gspeed, &course, &climb,
102  &electrical.vsupply, &throttle, &autopilot.mode,
103  &nav_block, &gps_fix, &autopilot.flight_time);
104 }
105 
106 void autopilot_init(void)
107 {
108 #ifdef MODE_AUTO2
109  autopilot.mode_auto2 = MODE_AUTO2; // FIXME
110 #endif
112  autopilot.throttle = 0;
113  autopilot.motors_on = false;
114  autopilot.kill_throttle = true;
115  autopilot.in_flight = false;
116  autopilot.ground_detected = false;
118  autopilot.use_rc = true;
119 
120  // call firmware specific init
122 
123  // call autopilot implementation init after guidance modules init
124  // (should be guaranteed by modules dependencies)
125  // it will set startup mode
126 #if USE_GENERATED_AUTOPILOT
128 #else
130 #endif
131 
132  // bind ABI messages
133  AbiBindMsgRADIO_CONTROL(AUTOPILOT_RC_ID, &rc_ev, rc_cb);
134 
135  // register messages
136  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AUTOPILOT_VERSION, send_autopilot_version);
141 }
142 
146 {
147  // first check for failsafe case
149 
150  // check if in flight
152 
153 #if FIXEDWING_FIRMWARE
154  if (autopilot.flight_time) {
155 #else
156  if (autopilot_in_flight()) {
157 #endif
158  // flight time is incremented every second
159  // after takeoff for fixedwing
160  // when in flight for other firmwares
161  RunOnceEvery(PERIODIC_FREQUENCY, autopilot.flight_time++);
162  }
163 
164 #if USE_GENERATED_AUTOPILOT
166 #else
168 #endif
169 }
170 
173 void WEAK autopilot_event(void) {}
174 
178 {
179 #if USE_GENERATED_AUTOPILOT
181 #else
183 #endif
184 }
185 
188 void WEAK autopilot_failsafe_checks(void) {}
189 
192 bool autopilot_set_mode(uint8_t new_autopilot_mode)
193 {
195 #if USE_GENERATED_AUTOPILOT
196  autopilot_generated_set_mode(new_autopilot_mode);
197 #else
198  autopilot_static_set_mode(new_autopilot_mode);
199 #endif
200  return (autopilot.mode != mode);
201 }
202 
206 {
207 #if USE_GENERATED_AUTOPILOT
209 #else
211 #endif
212 }
213 
217 {
218  return autopilot.mode;
219 }
220 
224 {
226  autopilot.launch = false;
227 }
228 
232 void autopilot_set_motors_on(bool motors_on)
233 {
234 #if USE_GENERATED_AUTOPILOT
236 #else
238 #endif
240 }
241 
245 {
246  return autopilot.motors_on;
247 }
248 
252 {
253  if (kill) {
255  } else {
257  }
258 }
259 
263 {
264  return autopilot.kill_throttle;
265 }
266 
270 void WEAK autopilot_check_in_flight(bool motors_on __attribute__((unused))) {}
271 
276 
279 void autopilot_set_in_flight(bool in_flight)
280 {
281  autopilot.in_flight = in_flight;
282  if (!in_flight) {
284  }
285 }
286 
290 {
291  return autopilot.in_flight;
292 }
293 
297 {
298  if (autopilot.kill_throttle) {
299  settings_store_flag = true;
300  settings_store();
301  }
302 }
303 
307 {
308  if (autopilot.kill_throttle) {
309  settings_clear_flag = true;
310  settings_clear();
311  }
312 }
313 
317 {
318  send_autopilot_version(&(DefaultChannel).trans_tx, &(DefaultDevice).device);
319 }
320 
324 void WEAK autopilot_send_mode(void) {}
325 
autopilot_get_motors_on
bool autopilot_get_motors_on(void)
get motors status
Definition: autopilot.c:244
autopilot_clear_settings
void autopilot_clear_settings(void)
clear settings
Definition: autopilot.c:306
autopilot_set_in_flight
void autopilot_set_in_flight(bool in_flight)
set in_flight flag
Definition: autopilot.c:279
autopilot_reset_in_flight_counter
void WEAK autopilot_reset_in_flight_counter(void)
reset in_flight counter actual implementation is firmware dependent
Definition: autopilot.c:275
MAX_PPRZ
#define MAX_PPRZ
Definition: paparazzi.h:8
uint32_t
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
stateGetHorizontalSpeedDir_f
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:944
autopilot_generated_periodic
void autopilot_generated_periodic(void)
Definition: autopilot_generated.c:50
settings_clear_flag
bool settings_clear_flag
Definition: settings.c:43
autopilot_generated_init
void autopilot_generated_init(void)
Definition: autopilot_generated.c:41
MODE_AUTO2
#define MODE_AUTO2
Definition: autopilot_static.h:66
settings.h
abi.h
autopilot_reset_flight_time
void autopilot_reset_flight_time(void)
reset flight time and launch
Definition: autopilot.c:223
autopilot_static_set_motors_on
void autopilot_static_set_motors_on(bool motors_on)
Definition: autopilot_static.c:151
autopilot_generated_on_rc_frame
void autopilot_generated_on_rc_frame(void)
Definition: autopilot_generated.c:92
pprz_autopilot::flight_time
uint16_t flight_time
flight time in seconds
Definition: autopilot.h:65
pprz_autopilot::mode
uint8_t mode
current autopilot mode
Definition: autopilot.h:63
autopilot_generated_SetModeHandler
void autopilot_generated_SetModeHandler(float mode)
AP mode setting handler.
Definition: autopilot_generated.c:71
autopilot_init
void autopilot_init(void)
Autopilot initialization function.
Definition: autopilot.c:106
abi_struct
Event structure to store callbacks in a linked list.
Definition: abi_common.h:66
stateGetNedToBodyEulers_f
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
autopilot_static_init
void autopilot_static_init(void)
Static autopilot API.
Definition: autopilot_static.c:81
pprz_autopilot::use_rc
bool use_rc
enable/disable RC input
Definition: autopilot.h:72
autopilot_SetModeHandler
void autopilot_SetModeHandler(float mode)
AP mode setting handler.
Definition: autopilot.c:205
pprz_autopilot
PPRZ Autopilot structure definition.
Definition: autopilot.h:62
rc_ev
static abi_event rc_ev
Definition: autopilot.c:54
autopilot_event
void WEAK autopilot_event(void)
AP event call.
Definition: autopilot.c:173
stateGetPositionUtm_f
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:692
send_autopilot_version
static void send_autopilot_version(struct transport_tx *trans, struct link_device *dev)
Definition: autopilot.c:63
Electrical::vsupply
float vsupply
supply voltage in V
Definition: electrical.h:45
EnuCoor_f::z
float z
in meters
Definition: pprz_geodetic_float.h:75
FloatEulers::theta
float theta
in radians
Definition: pprz_algebra_float.h:86
autopilot_in_flight
bool autopilot_in_flight(void)
get in_flight flag
Definition: autopilot.c:289
telemetry.h
autopilot_send_version
void autopilot_send_version(void)
send autopilot version
Definition: autopilot.c:316
uart.h
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
pprz_autopilot::motors_on
bool motors_on
motor status
Definition: autopilot.h:68
GpsState::fix
uint8_t fix
status of fix
Definition: gps.h:107
FloatEulers::phi
float phi
in radians
Definition: pprz_algebra_float.h:85
autopilot
struct pprz_autopilot autopilot
Global autopilot structure.
Definition: autopilot.c:48
autopilot_throttle_killed
bool autopilot_throttle_killed(void)
get kill status
Definition: autopilot.c:262
pprz_autopilot::in_flight
bool in_flight
in flight status
Definition: autopilot.h:70
UtmCoor_f::alt
float alt
in meters (above WGS84 reference ellipsoid or above MSL)
Definition: pprz_geodetic_float.h:84
pprz_autopilot::throttle
pprz_t throttle
throttle level as will be displayed in GCS
Definition: autopilot.h:66
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
nav_block
uint8_t nav_block
Definition: common_flight_plan.c:35
settings_store_flag
bool settings_store_flag
flag for setting feedback.
Definition: settings.c:41
sys_time.h
Architecture independent timing functions.
register_periodic_telemetry
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:51
AUTOPILOT_RC_ID
#define AUTOPILOT_RC_ID
Definition: autopilot.c:51
autopilot_generated_set_mode
void autopilot_generated_set_mode(uint8_t new_autopilot_mode)
Definition: autopilot_generated.c:77
pprz_autopilot::ground_detected
bool ground_detected
automatic detection of landing
Definition: autopilot.h:73
autopilot_send_mode
void WEAK autopilot_send_mode(void)
send autopilot mode actual implementation is firmware dependent
Definition: autopilot.c:324
RadioControl
Definition: radio_control.h:60
autopilot.h
settings_clear
int32_t settings_clear(void)
clear all persistent settings from flash
Definition: settings.c:80
autopilot_firmware_init
void autopilot_firmware_init(void)
Init function.
Definition: autopilot_firmware.c:152
autopilot_set_motors_on
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:232
autopilot_set_kill_throttle
void autopilot_set_kill_throttle(bool kill)
set kill throttle
Definition: autopilot.c:251
rc_cb
static void rc_cb(uint8_t sender_id, struct RadioControl *rc)
Definition: autopilot.c:56
autopilot_generated_set_motors_on
void autopilot_generated_set_motors_on(bool motors_on)
Definition: autopilot_generated.c:85
course
static int16_t course[3]
Definition: airspeed_uADC.c:58
autopilot_periodic
void autopilot_periodic(void)
AP periodic call.
Definition: autopilot.c:145
autopilot_static_set_mode
void autopilot_static_set_mode(uint8_t new_autopilot_mode)
Definition: autopilot_static.c:139
stateGetHorizontalSpeedNorm_f
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:935
autopilot_store_settings
void autopilot_store_settings(void)
store settings
Definition: autopilot.c:296
send_alive
static void send_alive(struct transport_tx *trans, struct link_device *dev)
Definition: autopilot.c:70
autopilot_static_periodic
void autopilot_static_periodic(void)
Definition: autopilot_static.c:93
gpio.h
stateGetSpeedEnu_f
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
Definition: state.h:917
autopilot_set_mode
bool autopilot_set_mode(uint8_t new_autopilot_mode)
set autopilot mode
Definition: autopilot.c:192
autopilot_check_in_flight
void WEAK autopilot_check_in_flight(bool motors_on)
in flight check utility function actual implementation is firmware dependent
Definition: autopilot.c:270
pprz_autopilot::mode_auto2
uint8_t mode_auto2
FIXME hide this in a private part ?
Definition: autopilot.h:64
autopilot_static_SetModeHandler
void autopilot_static_SetModeHandler(float new_autopilot_mode)
Definition: autopilot_static.c:146
send_attitude
static void send_attitude(struct transport_tx *trans, struct link_device *dev)
Definition: autopilot.c:75
send_minimal_com
static void send_minimal_com(struct transport_tx *trans, struct link_device *dev)
Definition: autopilot.c:86
FloatEulers
euler angles
Definition: pprz_algebra_float.h:84
mode
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
commands.h
Hardware independent code for commands handling.
electrical
struct Electrical electrical
Definition: electrical.c:66
autopilot_get_mode
uint8_t autopilot_get_mode(void)
get autopilot mode
Definition: autopilot.c:216
FloatEulers::psi
float psi
in radians
Definition: pprz_algebra_float.h:87
stateGetPositionLla_f
static struct LlaCoor_f * stateGetPositionLla_f(void)
Get position in LLA coordinates (float).
Definition: state.h:728
autopilot_on_rc_frame
void autopilot_on_rc_frame(void)
RC frame handler.
Definition: autopilot.c:177
settings_store
int32_t settings_store(void)
store settings marked as persistent to flash
Definition: settings.c:60
send_dl_value
static void send_dl_value(struct transport_tx *trans, struct link_device *dev)
Definition: autopilot.c:81
autopilot_static_on_rc_frame
void autopilot_static_on_rc_frame(void)
Function to be called when a message from FBW is available.
Definition: autopilot_static.c:100
gps
struct GpsState gps
global GPS state
Definition: gps.c:69
pprz_autopilot::kill_throttle
bool kill_throttle
allow autopilot to use throttle
Definition: autopilot.h:69
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
pprz_autopilot::launch
bool launch
request launch
Definition: autopilot.h:71
pprz_autopilot::detect_ground_once
bool detect_ground_once
enable automatic detection of ground (one shot)
Definition: autopilot.h:74
autopilot_failsafe_checks
void WEAK autopilot_failsafe_checks(void)
Failsafe checks.
Definition: autopilot.c:188