Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
31
32#include "autopilot.h"
33
34#include "generated/modules.h"
35
36#include "mcu.h"
37#include "mcu_periph/uart.h"
38#include "mcu_periph/sys_time.h"
39#include "mcu_periph/gpio.h"
42#include "modules/core/abi.h"
44
46#include "generated/settings.h"
47
48#include "pprz_version.h"
49
51
52#ifndef AUTOPILOT_RC_ID
53#define AUTOPILOT_RC_ID ABI_BROADCAST
54#endif
57
58static void rc_cb(uint8_t __attribute__((unused)) sender_id,
59 struct RadioControl __attribute__((unused)) *rc)
60{
61 // TODO pass the RC struct to the on_rc_frame function
63}
64
65static void send_autopilot_version(struct transport_tx *trans, struct link_device *dev)
66{
67 static uint32_t ap_version = PPRZ_VERSION_INT;
68 static char *ver_desc = PPRZ_VERSION_DESC;
69 static uint32_t build_time = PPRZ_BUILD_TIME;
70 uint8_t sha1[20];
71 uint8_t uid[18];
72 get_pprz_git_sha1(sha1, sizeof(sha1));
73 uint8_t uid_len = mcu_get_uid(uid, sizeof(uid));
74 pprz_msg_send_AUTOPILOT_VERSION(trans, dev, AC_ID, &ap_version, strlen(ver_desc), ver_desc,
75 sha1, &build_time, uid_len, uid);
76}
77
78static void send_alive(struct transport_tx *trans, struct link_device *dev)
79{
80 pprz_msg_send_ALIVE(trans, dev, AC_ID, 16, MD5SUM);
81}
82
83static void send_attitude(struct transport_tx *trans, struct link_device *dev)
84{
86 pprz_msg_send_ATTITUDE(trans, dev, AC_ID, &(att->phi), &(att->psi), &(att->theta));
87};
88
89static void send_dl_value(struct transport_tx *trans, struct link_device *dev)
90{
91 PeriodicSendDlValue(trans, dev);
92}
93
94static void send_minimal_com(struct transport_tx *trans, struct link_device *dev)
95{
96 float lat = DegOfRad(stateGetPositionLla_f()->lat);
97 float lon = DegOfRad(stateGetPositionLla_f()->lon);
98 float hmsl = stateGetPositionUtm_f()->alt;
99 float gspeed = stateGetHorizontalSpeedNorm_f();
101 float climb = stateGetSpeedEnu_f()->z;
102 uint8_t throttle = (uint8_t)(100 * autopilot.throttle / MAX_PPRZ);
103#if USE_GPS
104 uint8_t gps_fix = gps.fix;
105#else
106 uint8_t gps_fix = 0;
107#endif
108 pprz_msg_send_MINIMAL_COM(trans, dev, AC_ID,
109 &lat, &lon, &hmsl, &gspeed, &course, &climb,
110 &electrical.vsupply, &throttle, &autopilot.mode,
111 &nav_block, &gps_fix, &autopilot.flight_time);
112}
113
115{
116#ifdef MODE_AUTO2
117 autopilot.mode_auto2 = MODE_AUTO2; // FIXME
118#endif
119 autopilot.flight_time = 0;
120 autopilot.throttle = 0;
121 autopilot.motors_on = false;
122 autopilot.kill_throttle = true;
123 autopilot.in_flight = false;
124 autopilot.ground_detected = false;
125 autopilot.detect_ground_once = false;
126 autopilot.use_rc = true;
127
128 // call firmware specific init
130
131 // call autopilot implementation init after guidance modules init
132 // (should be guaranteed by modules dependencies)
133 // it will set startup mode
134#if USE_GENERATED_AUTOPILOT
136#else
138#endif
139
140 // bind ABI messages
141 AbiBindMsgRADIO_CONTROL(AUTOPILOT_RC_ID, &rc_ev, rc_cb);
142
143 // register messages
149}
150
154{
155 // first check for failsafe case
157
158 // check if in flight
160
161#if FIXEDWING_FIRMWARE
162 if (autopilot.flight_time) {
163#else
164 if (autopilot_in_flight()) {
165#endif
166 // flight time is incremented every second
167 // after takeoff for fixedwing
168 // when in flight for other firmwares
169 RunOnceEvery(PERIODIC_FREQUENCY, autopilot.flight_time++);
170 }
171
172#if USE_GENERATED_AUTOPILOT
174#else
176#endif
177}
178
181void WEAK autopilot_event(void) {}
182
186{
187#if USE_GENERATED_AUTOPILOT
189#else
191#endif
192}
193
197
200bool autopilot_set_mode(uint8_t new_autopilot_mode)
201{
202 uint8_t mode = autopilot.mode;
203#if USE_GENERATED_AUTOPILOT
204 autopilot_generated_set_mode(new_autopilot_mode);
205#else
206 autopilot_static_set_mode(new_autopilot_mode);
207#endif
208 if (autopilot.mode != mode) {
210 return true;
211 } else {
212 return false;
213 }
214}
215
219{
220#if USE_GENERATED_AUTOPILOT
222#else
224#endif
225}
226
230{
231 return autopilot.mode;
232}
233
237{
238 autopilot.flight_time = 0;
239 autopilot.launch = false;
240}
241
244void autopilot_force_motors_on(bool motors_on)
245{
246#if USE_GENERATED_AUTOPILOT
248#else
250#endif
251 autopilot.kill_throttle = ! autopilot.motors_on;
252}
253
258bool autopilot_set_motors_on(bool motors_on)
259{
260 // Prevent unnessary preflight checks
261 if (autopilot.motors_on == motors_on) {
262 return true;
263 }
264
265#if PREFLIGHT_CHECKS
266 // When we fail the preflight checks abort
267 if (motors_on && !preflight_check()) {
268 // Bypass the preflight checks even if they fail but still preform them
269 if (!preflight_bypass) {
270 return false;
271 }
272 }
273#endif
274 autopilot_force_motors_on(motors_on);
275 return true;
276}
277
281bool autopilot_arming_motors_on(bool motors_on)
282{
283 // Prevent unnessary preflight checks
284 if (autopilot.motors_on == motors_on) {
285 return true;
286 }
287
288#if PREFLIGHT_CHECKS
289 // When we fail the preflight checks abort
290 if (motors_on && !preflight_check()) {
291 // Bypass the preflight checks even if they fail but still preform them
292 if (!preflight_bypass) {
293 return false;
294 }
295 }
296#endif
297 autopilot.motors_on = motors_on;
298 return true;
299}
300
304{
305 return autopilot.motors_on;
306}
307
313{
314 if (kill) {
316 autopilot.kill_throttle = true;
317 } else if (autopilot_set_motors_on(true)) {
318 autopilot.kill_throttle = !autopilot.motors_on;
319 }
320}
321
325{
326 return autopilot.kill_throttle;
327}
328
332void WEAK autopilot_check_in_flight(bool motors_on __attribute__((unused))) {}
333
338
341void autopilot_set_in_flight(bool in_flight)
342{
343 autopilot.in_flight = in_flight;
344 if (!in_flight) {
346 }
347}
348
352{
353 return autopilot.in_flight;
354}
355
359{
360 if (autopilot.kill_throttle) {
361 settings_store_flag = true;
363 }
364}
365
369{
370 if (autopilot.kill_throttle) {
371 settings_clear_flag = true;
373 }
374}
375
379{
381}
382
386void WEAK autopilot_send_mode(void) {}
387
Main include for ABI (AirBorneInterface).
struct abi_struct abi_event
Definition abi_common.h:101
static int16_t course[3]
void WEAK autopilot_event(void)
AP event call.
Definition autopilot.c:181
static void send_dl_value(struct transport_tx *trans, struct link_device *dev)
Definition autopilot.c:89
void autopilot_periodic(void)
AP periodic call.
Definition autopilot.c:153
bool autopilot_set_mode(uint8_t new_autopilot_mode)
set autopilot mode
Definition autopilot.c:200
void WEAK autopilot_send_mode(void)
send autopilot mode actual implementation is firmware dependent
Definition autopilot.c:386
uint8_t autopilot_get_mode(void)
get autopilot mode
Definition autopilot.c:229
void autopilot_set_in_flight(bool in_flight)
set in_flight flag
Definition autopilot.c:341
static void rc_cb(uint8_t sender_id, struct RadioControl *rc)
Definition autopilot.c:58
static void send_attitude(struct transport_tx *trans, struct link_device *dev)
Definition autopilot.c:83
static void send_autopilot_version(struct transport_tx *trans, struct link_device *dev)
Definition autopilot.c:65
bool autopilot_set_motors_on(bool motors_on)
turn motors on/off, eventually depending of the current mode set kill_throttle accordingly: kill_thro...
Definition autopilot.c:258
#define AUTOPILOT_RC_ID
Definition autopilot.c:53
struct pprz_autopilot autopilot
Global autopilot structure.
Definition autopilot.c:50
static void send_alive(struct transport_tx *trans, struct link_device *dev)
Definition autopilot.c:78
bool autopilot_get_motors_on(void)
get motors status
Definition autopilot.c:303
void autopilot_on_rc_frame(void)
RC frame handler.
Definition autopilot.c:185
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:281
void autopilot_SetModeHandler(float mode)
AP mode setting handler.
Definition autopilot.c:218
void autopilot_init(void)
Autopilot initialization function.
Definition autopilot.c:114
bool autopilot_in_flight(void)
get in_flight flag
Definition autopilot.c:351
void autopilot_force_motors_on(bool motors_on)
Force the motors on/off skipping preflight checks.
Definition autopilot.c:244
static void send_minimal_com(struct transport_tx *trans, struct link_device *dev)
Definition autopilot.c:94
void autopilot_set_kill_throttle(bool kill)
set kill throttle Kill always kills, also when motors_on is already false (fixedwing in flight).
Definition autopilot.c:312
void WEAK autopilot_failsafe_checks(void)
Failsafe checks.
Definition autopilot.c:196
void autopilot_send_version(void)
send autopilot version
Definition autopilot.c:378
void WEAK autopilot_reset_in_flight_counter(void)
reset in_flight counter actual implementation is firmware dependent
Definition autopilot.c:337
bool autopilot_throttle_killed(void)
get kill status
Definition autopilot.c:324
static abi_event rc_ev
Definition autopilot.c:56
void autopilot_store_settings(void)
store settings
Definition autopilot.c:358
void autopilot_clear_settings(void)
clear settings
Definition autopilot.c:368
void autopilot_reset_flight_time(void)
reset flight time and launch
Definition autopilot.c:236
void WEAK autopilot_check_in_flight(bool motors_on)
in flight check utility function actual implementation is firmware dependent
Definition autopilot.c:332
Core autopilot interface common to all firmwares.
PPRZ Autopilot structure definition.
Definition autopilot.h:62
Hardware independent code for commands handling.
uint8_t nav_block
static struct uart_periph * dev
struct Electrical electrical
Definition electrical.c:92
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:74
float phi
in radians
float theta
in radians
float psi
in radians
euler angles
uint8_t mcu_get_uid(uint8_t *uid, uint8_t max_len)
Get the 96-bit unique device ID of the STM32.
Definition mcu_arch.c:252
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition state.h:1325
static struct LlaCoor_f * stateGetPositionLla_f(void)
Get position in LLA coordinates (float).
Definition state.h:868
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition state.h:832
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition state.h:1087
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition state.h:1096
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
Definition state.h:1069
Arch independent mcu ( Micro Controller Unit ) utilities.
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
#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:47
int32_t settings_clear(void)
clear all persistent settings from flash
Definition settings.c:101
bool settings_store_flag
flag for setting feedback.
Definition settings.c:45
int32_t settings_store(void)
store settings marked as persistent to flash
Definition settings.c:81
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:65
Architecture independent timing functions.
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_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.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.