Paparazzi UAS  v7.0_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"
43 
44 #include "modules/core/settings.h"
45 #include "generated/settings.h"
46 
47 #include "pprz_version.h"
48 
50 
51 #ifndef AUTOPILOT_RC_ID
52 #define AUTOPILOT_RC_ID ABI_BROADCAST
53 #endif
54 PRINT_CONFIG_VAR(AUTOPILOT_RC_ID)
56 
57 static void rc_cb(uint8_t __attribute__((unused)) sender_id,
58  struct RadioControl __attribute__((unused)) *rc)
59 {
60  // TODO pass the RC struct to the on_rc_frame function
62 }
63 
64 static void send_autopilot_version(struct transport_tx *trans, struct link_device *dev)
65 {
66  static uint32_t ap_version = PPRZ_VERSION_INT;
67  static char *ver_desc = PPRZ_VERSION_DESC;
68  pprz_msg_send_AUTOPILOT_VERSION(trans, dev, AC_ID, &ap_version, strlen(ver_desc), ver_desc);
69 }
70 
71 static void send_alive(struct transport_tx *trans, struct link_device *dev)
72 {
73  pprz_msg_send_ALIVE(trans, dev, AC_ID, 16, MD5SUM);
74 }
75 
76 static void send_attitude(struct transport_tx *trans, struct link_device *dev)
77 {
78  struct FloatEulers *att = stateGetNedToBodyEulers_f();
79  pprz_msg_send_ATTITUDE(trans, dev, AC_ID, &(att->phi), &(att->psi), &(att->theta));
80 };
81 
82 static void send_dl_value(struct transport_tx *trans, struct link_device *dev)
83 {
84  PeriodicSendDlValue(trans, dev);
85 }
86 
87 static void send_minimal_com(struct transport_tx *trans, struct link_device *dev)
88 {
89  float lat = DegOfRad(stateGetPositionLla_f()->lat);
90  float lon = DegOfRad(stateGetPositionLla_f()->lon);
91  float hmsl = stateGetPositionUtm_f()->alt;
92  float gspeed = stateGetHorizontalSpeedNorm_f();
94  float climb = stateGetSpeedEnu_f()->z;
95  uint8_t throttle = (uint8_t)(100 * autopilot.throttle / MAX_PPRZ);
96 #if USE_GPS
97  uint8_t gps_fix = gps.fix;
98 #else
99  uint8_t gps_fix = 0;
100 #endif
101  pprz_msg_send_MINIMAL_COM(trans, dev, AC_ID,
102  &lat, &lon, &hmsl, &gspeed, &course, &climb,
103  &electrical.vsupply, &throttle, &autopilot.mode,
104  &nav_block, &gps_fix, &autopilot.flight_time);
105 }
106 
107 void autopilot_init(void)
108 {
109 #ifdef MODE_AUTO2
110  autopilot.mode_auto2 = MODE_AUTO2; // FIXME
111 #endif
113  autopilot.throttle = 0;
114  autopilot.motors_on = false;
115  autopilot.kill_throttle = true;
116  autopilot.in_flight = false;
117  autopilot.ground_detected = false;
119  autopilot.use_rc = true;
120 
121  // call firmware specific init
123 
124  // call autopilot implementation init after guidance modules init
125  // (should be guaranteed by modules dependencies)
126  // it will set startup mode
127 #if USE_GENERATED_AUTOPILOT
129 #else
131 #endif
132 
133  // bind ABI messages
134  AbiBindMsgRADIO_CONTROL(AUTOPILOT_RC_ID, &rc_ev, rc_cb);
135 
136  // register messages
137  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AUTOPILOT_VERSION, send_autopilot_version);
142 }
143 
147 {
148  // first check for failsafe case
150 
151  // check if in flight
153 
154 #if FIXEDWING_FIRMWARE
155  if (autopilot.flight_time) {
156 #else
157  if (autopilot_in_flight()) {
158 #endif
159  // flight time is incremented every second
160  // after takeoff for fixedwing
161  // when in flight for other firmwares
162  RunOnceEvery(PERIODIC_FREQUENCY, autopilot.flight_time++);
163  }
164 
165 #if USE_GENERATED_AUTOPILOT
167 #else
169 #endif
170 }
171 
174 void WEAK autopilot_event(void) {}
175 
179 {
180 #if USE_GENERATED_AUTOPILOT
182 #else
184 #endif
185 }
186 
189 void WEAK autopilot_failsafe_checks(void) {}
190 
193 bool autopilot_set_mode(uint8_t new_autopilot_mode)
194 {
196 #if USE_GENERATED_AUTOPILOT
197  autopilot_generated_set_mode(new_autopilot_mode);
198 #else
199  autopilot_static_set_mode(new_autopilot_mode);
200 #endif
201  if (autopilot.mode != mode) {
203  return true;
204  } else {
205  return false;
206  }
207 }
208 
212 {
213 #if USE_GENERATED_AUTOPILOT
215 #else
217 #endif
218 }
219 
223 {
224  return autopilot.mode;
225 }
226 
230 {
232  autopilot.launch = false;
233 }
234 
237 void autopilot_force_motors_on(bool motors_on)
238 {
239 #if USE_GENERATED_AUTOPILOT
241 #else
243 #endif
245 }
246 
250 bool autopilot_set_motors_on(bool motors_on)
251 {
252  // Prevent unnessary preflight checks
253  if (autopilot.motors_on == motors_on) {
254  return true;
255  }
256 
257 #if PREFLIGHT_CHECKS
258  // When we fail the preflight checks abort
259  if (motors_on && !preflight_check()) {
260  // Bypass the preflight checks even if they fail but still preform them
261  if (!preflight_bypass) {
262  return false;
263  }
264  }
265 #endif
266  autopilot_force_motors_on(motors_on);
267  return true;
268 }
269 
273 bool autopilot_arming_motors_on(bool motors_on)
274 {
275  // Prevent unnessary preflight checks
276  if (autopilot.motors_on == motors_on) {
277  return true;
278  }
279 
280 #if PREFLIGHT_CHECKS
281  // When we fail the preflight checks abort
282  if (motors_on && !preflight_check()) {
283  // Bypass the preflight checks even if they fail but still preform them
284  if (!preflight_bypass) {
285  return false;
286  }
287  }
288 #endif
289  autopilot.motors_on = motors_on;
290  return true;
291 }
292 
296 {
297  return autopilot.motors_on;
298 }
299 
303 {
304  if (kill) {
306  } else {
308  }
309 }
310 
314 {
315  return autopilot.kill_throttle;
316 }
317 
321 void WEAK autopilot_check_in_flight(bool motors_on __attribute__((unused))) {}
322 
327 
330 void autopilot_set_in_flight(bool in_flight)
331 {
332  autopilot.in_flight = in_flight;
333  if (!in_flight) {
335  }
336 }
337 
341 {
342  return autopilot.in_flight;
343 }
344 
348 {
349  if (autopilot.kill_throttle) {
350  settings_store_flag = true;
351  settings_store();
352  }
353 }
354 
358 {
359  if (autopilot.kill_throttle) {
360  settings_clear_flag = true;
361  settings_clear();
362  }
363 }
364 
368 {
369  send_autopilot_version(&(DefaultChannel).trans_tx, &(DefaultDevice).device);
370 }
371 
375 void WEAK autopilot_send_mode(void) {}
376 
Main include for ABI (AirBorneInterface).
Event structure to store callbacks in a linked list.
Definition: abi_common.h:67
static int16_t course[3]
Definition: airspeed_uADC.c:58
void WEAK autopilot_event(void)
AP event call.
Definition: autopilot.c:174
static void send_dl_value(struct transport_tx *trans, struct link_device *dev)
Definition: autopilot.c:82
void autopilot_periodic(void)
AP periodic call.
Definition: autopilot.c:146
bool autopilot_set_mode(uint8_t new_autopilot_mode)
set autopilot mode
Definition: autopilot.c:193
void WEAK autopilot_send_mode(void)
send autopilot mode actual implementation is firmware dependent
Definition: autopilot.c:375
uint8_t autopilot_get_mode(void)
get autopilot mode
Definition: autopilot.c:222
void autopilot_set_in_flight(bool in_flight)
set in_flight flag
Definition: autopilot.c:330
static void rc_cb(uint8_t sender_id, struct RadioControl *rc)
Definition: autopilot.c:57
static void send_attitude(struct transport_tx *trans, struct link_device *dev)
Definition: autopilot.c:76
static void send_autopilot_version(struct transport_tx *trans, struct link_device *dev)
Definition: autopilot.c:64
bool 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:250
#define AUTOPILOT_RC_ID
Definition: autopilot.c:52
struct pprz_autopilot autopilot
Global autopilot structure.
Definition: autopilot.c:49
static void send_alive(struct transport_tx *trans, struct link_device *dev)
Definition: autopilot.c:71
bool autopilot_get_motors_on(void)
get motors status
Definition: autopilot.c:295
void autopilot_on_rc_frame(void)
RC frame handler.
Definition: autopilot.c:178
bool autopilot_arming_motors_on(bool motors_on)
turn motors on/off during arming, not done automatically prevents takeoff with preflight checks
Definition: autopilot.c:273
void autopilot_SetModeHandler(float mode)
AP mode setting handler.
Definition: autopilot.c:211
void autopilot_init(void)
Autopilot initialization function.
Definition: autopilot.c:107
bool autopilot_in_flight(void)
get in_flight flag
Definition: autopilot.c:340
void autopilot_force_motors_on(bool motors_on)
Force the motors on/off skipping preflight checks.
Definition: autopilot.c:237
static void send_minimal_com(struct transport_tx *trans, struct link_device *dev)
Definition: autopilot.c:87
void autopilot_set_kill_throttle(bool kill)
set kill throttle
Definition: autopilot.c:302
void WEAK autopilot_failsafe_checks(void)
Failsafe checks.
Definition: autopilot.c:189
void autopilot_send_version(void)
send autopilot version
Definition: autopilot.c:367
void WEAK autopilot_reset_in_flight_counter(void)
reset in_flight counter actual implementation is firmware dependent
Definition: autopilot.c:326
bool autopilot_throttle_killed(void)
get kill status
Definition: autopilot.c:313
static abi_event rc_ev
Definition: autopilot.c:55
void autopilot_store_settings(void)
store settings
Definition: autopilot.c:347
void autopilot_clear_settings(void)
clear settings
Definition: autopilot.c:357
void autopilot_reset_flight_time(void)
reset flight time and launch
Definition: autopilot.c:229
void WEAK autopilot_check_in_flight(bool motors_on)
in flight check utility function actual implementation is firmware dependent
Definition: autopilot.c:321
Core autopilot interface common to all firmwares.
bool motors_on
motor status
Definition: autopilot.h:68
bool ground_detected
automatic detection of landing
Definition: autopilot.h:73
uint8_t mode_auto2
FIXME hide this in a private part ?
Definition: autopilot.h:64
bool launch
request launch
Definition: autopilot.h:71
pprz_t throttle
throttle level as will be displayed in GCS
Definition: autopilot.h:66
bool use_rc
enable/disable RC input
Definition: autopilot.h:72
bool detect_ground_once
enable automatic detection of ground (one shot)
Definition: autopilot.h:74
bool kill_throttle
allow autopilot to use throttle
Definition: autopilot.h:69
uint8_t mode
current autopilot mode
Definition: autopilot.h:63
bool in_flight
in flight status
Definition: autopilot.h:70
uint16_t flight_time
flight time in seconds
Definition: autopilot.h:65
PPRZ Autopilot structure definition.
Definition: autopilot.h:62
Hardware independent code for commands handling.
uint8_t nav_block
struct Electrical electrical
Definition: electrical.c:92
float vsupply
supply voltage in V
Definition: electrical.h:45
void autopilot_firmware_init(void)
Init function.
void autopilot_generated_SetModeHandler(float mode)
AP mode setting handler.
void autopilot_generated_periodic(void)
void autopilot_generated_set_mode(uint8_t new_autopilot_mode)
void autopilot_generated_init(void)
void autopilot_generated_on_rc_frame(void)
void autopilot_generated_set_motors_on(bool motors_on)
void autopilot_static_init(void)
Static autopilot API.
void autopilot_static_periodic(void)
void autopilot_static_SetModeHandler(float new_autopilot_mode)
void autopilot_static_set_motors_on(bool motors_on)
void autopilot_static_on_rc_frame(void)
Function to be called when a message from FBW is available.
void autopilot_static_set_mode(uint8_t new_autopilot_mode)
Some architecture independent helper functions for GPIOs.
struct GpsState gps
global GPS state
Definition: gps.c:69
uint8_t fix
status of fix
Definition: gps.h:106
float phi
in radians
float theta
in radians
float psi
in radians
euler angles
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:692
static struct LlaCoor_f * stateGetPositionLla_f(void)
Get position in LLA coordinates (float).
Definition: state.h:728
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:935
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:944
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
Definition: state.h:917
#define MAX_PPRZ
Definition: paparazzi.h:8
float alt
in meters (above WGS84 reference ellipsoid or above MSL)
float z
in meters
bool preflight_bypass
bool preflight_check(void)
Perform all the preflight checks.
#define MODE_AUTO2
bool settings_clear_flag
Definition: settings.c:43
int32_t settings_clear(void)
clear all persistent settings from flash
Definition: settings.c:80
bool settings_store_flag
flag for setting feedback.
Definition: settings.c:41
int32_t settings_store(void)
store settings marked as persistent to flash
Definition: settings.c:60
Persistent settings interface.
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
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
Architecture independent timing functions.
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98