Paparazzi UAS  v5.18.0_stable
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"
40 #include "subsystems/commands.h"
41 #include "subsystems/actuators.h"
42 //#include "subsystems/electrical.h"
44 
45 #include "subsystems/settings.h"
46 #include "generated/settings.h"
47 
48 #include "pprz_version.h"
49 
51 
52 
53 static void send_autopilot_version(struct transport_tx *trans, struct link_device *dev)
54 {
55  static uint32_t ap_version = PPRZ_VERSION_INT;
56  static char *ver_desc = PPRZ_VERSION_DESC;
57  pprz_msg_send_AUTOPILOT_VERSION(trans, dev, AC_ID, &ap_version, strlen(ver_desc), ver_desc);
58 }
59 
60 static void send_alive(struct transport_tx *trans, struct link_device *dev)
61 {
62  pprz_msg_send_ALIVE(trans, dev, AC_ID, 16, MD5SUM);
63 }
64 
65 static void send_attitude(struct transport_tx *trans, struct link_device *dev)
66 {
67  struct FloatEulers *att = stateGetNedToBodyEulers_f();
68  pprz_msg_send_ATTITUDE(trans, dev, AC_ID, &(att->phi), &(att->psi), &(att->theta));
69 };
70 
71 static void send_dl_value(struct transport_tx *trans, struct link_device *dev)
72 {
73  PeriodicSendDlValue(trans, dev);
74 }
75 
76 #ifdef RADIO_CONTROL
77 static void send_rc(struct transport_tx *trans, struct link_device *dev)
78 {
79  pprz_msg_send_RC(trans, dev, AC_ID, RADIO_CONTROL_NB_CHANNEL, radio_control.values);
80 }
81 #endif
82 
83 #ifdef ACTUATORS
84 static void send_actuators(struct transport_tx *trans, struct link_device *dev)
85 {
86  pprz_msg_send_ACTUATORS(trans, dev, AC_ID , ACTUATORS_NB, actuators);
87 }
88 #endif
89 
90 
91 void autopilot_init(void)
92 {
93 #ifdef MODE_AUTO2
94  autopilot.mode_auto2 = MODE_AUTO2; // FIXME
95 #endif
97  autopilot.throttle = 0;
98  autopilot.motors_on = false;
99  autopilot.kill_throttle = true;
100  autopilot.in_flight = false;
101  autopilot.ground_detected = false;
103  autopilot.use_rc = true;
104  autopilot.power_switch = false;
105 #ifdef POWER_SWITCH_GPIO
107 #ifdef POWER_SWITCH_ENABLE
108  autopilot_set_power_switch(POWER_SWITCH_ENABLE); // set initial status
109 #else
110  gpio_clear(POWER_SWITCH_GPIO); // by default POWER OFF
111 #endif
112 #endif
113 
114  // call firmware specific init
116 
117  // static / generated AP init part is called later by main program
118  // and will set the correct initial mode
119 
120  // register messages
121  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AUTOPILOT_VERSION, send_autopilot_version);
125 #ifdef ACTUATORS
127 #endif
128 #ifdef RADIO_CONTROL
130 #endif
131 }
132 
136 {
137 #if USE_GENERATED_AUTOPILOT
139 #else
141 #endif
142 }
143 
146 void WEAK autopilot_event(void) {}
147 
151 {
152 #if USE_GENERATED_AUTOPILOT
154 #else
156 #endif
157 }
158 
161 bool autopilot_set_mode(uint8_t new_autopilot_mode)
162 {
163 #if USE_GENERATED_AUTOPILOT
164  autopilot_generated_set_mode(new_autopilot_mode);
165 #else
166  autopilot_static_set_mode(new_autopilot_mode);
167 #endif
168  return (autopilot.mode != new_autopilot_mode);
169 }
170 
174 {
175 #if USE_GENERATED_AUTOPILOT
177 #else
179 #endif
180 }
181 
185 {
186  return autopilot.mode;
187 }
188 
192 {
194  autopilot.launch = false;
195 }
196 
200 void autopilot_set_motors_on(bool motors_on)
201 {
202 #if USE_GENERATED_AUTOPILOT
204 #else
206 #endif
208 }
209 
213 {
214  return autopilot.motors_on;
215 }
216 
220 {
221  if (kill) {
223  } else {
225  }
226 }
227 
231 {
232  return autopilot.kill_throttle;
233 }
234 
238 void WEAK autopilot_check_in_flight(bool motors_on __attribute__((unused))) {}
239 
244 
247 void autopilot_set_in_flight(bool in_flight)
248 {
249  autopilot.in_flight = in_flight;
250  if (!in_flight) {
252  }
253 }
254 
258 {
259  return autopilot.in_flight;
260 }
261 
264 void autopilot_set_power_switch(bool power_switch)
265 {
266 #ifdef POWER_SWITCH_GPIO
267  if (power_switch) {
269  } else {
271  }
272 #endif
273  autopilot.power_switch = power_switch;
274 }
275 
279 {
280  if (autopilot.kill_throttle) {
281  settings_store_flag = true;
282  settings_store();
283  }
284 }
285 
289 {
290  if (autopilot.kill_throttle) {
291  settings_clear_flag = true;
292  settings_clear();
293  }
294 }
295 
299 {
300  send_autopilot_version(&(DefaultChannel).trans_tx, &(DefaultDevice).device);
301 }
302 
306 void WEAK autopilot_send_mode(void) {}
307 
autopilot_get_motors_on
bool autopilot_get_motors_on(void)
get motors status
Definition: autopilot.c:212
radio_control.h
autopilot_clear_settings
void autopilot_clear_settings(void)
clear settings
Definition: autopilot.c:288
autopilot_set_in_flight
void autopilot_set_in_flight(bool in_flight)
set in_flight flag
Definition: autopilot.c:247
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:243
autopilot_generated_periodic
void autopilot_generated_periodic(void)
Definition: autopilot_generated.c:50
spektrum
static struct spektrum_t spektrum
Definition: spektrum.c:83
settings_clear_flag
bool settings_clear_flag
Definition: settings.c:40
MODE_AUTO2
#define MODE_AUTO2
Definition: autopilot_static.h:66
settings.h
autopilot_reset_flight_time
void autopilot_reset_flight_time(void)
reset flight time and launch
Definition: autopilot.c:191
autopilot_static_set_motors_on
void autopilot_static_set_motors_on(bool motors_on)
Definition: autopilot_static.c:206
autopilot_generated_on_rc_frame
void autopilot_generated_on_rc_frame(void)
Definition: autopilot_generated.c:104
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
USE_AD_TIM4
#define USE_AD_TIM4
Definition: apogee_1.0.h:162
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:91
stateGetNedToBodyEulers_f
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
POWER_SWITCH_GPIO
#define POWER_SWITCH_GPIO
Definition: board.h:153
gpio_setup_output
void gpio_setup_output(ioportid_t port, uint16_t gpios)
Setup one or more pins of the given GPIO port as outputs.
Definition: gpio_arch.c:33
pprz_autopilot::use_rc
bool use_rc
enable/disable RC input
Definition: autopilot.h:72
uint32_t
unsigned long uint32_t
Definition: types.h:18
autopilot_SetModeHandler
void autopilot_SetModeHandler(float mode)
AP mode setting handler.
Definition: autopilot.c:173
pprz_autopilot
PPRZ Autopilot structure definition.
Definition: autopilot.h:62
autopilot_event
void WEAK autopilot_event(void)
AP event call.
Definition: autopilot.c:146
send_autopilot_version
static void send_autopilot_version(struct transport_tx *trans, struct link_device *dev)
Definition: autopilot.c:53
PWM_USE_TIM2
#define PWM_USE_TIM2
Definition: apogee_1.0.h:226
USE_AD_TIM2
#define USE_AD_TIM2
Definition: matek_f405_wing_v1.h:196
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:257
gpio_clear
static void gpio_clear(ioportid_t port, uint16_t pin)
Clear a gpio output to low level.
Definition: gpio_arch.h:108
telemetry.h
autopilot_set_power_switch
void autopilot_set_power_switch(bool power_switch)
set power switch
Definition: autopilot.c:264
PWM_USE_TIM1
#define PWM_USE_TIM1
Definition: cc3d.h:148
autopilot_send_version
void autopilot_send_version(void)
send autopilot version
Definition: autopilot.c:298
uart.h
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
PWM_USE_TIM3
#define PWM_USE_TIM3
Definition: apogee_1.0.h:227
pprz_autopilot::motors_on
bool motors_on
motor status
Definition: autopilot.h:68
FloatEulers::phi
float phi
in radians
Definition: pprz_algebra_float.h:85
autopilot
struct pprz_autopilot autopilot
Global autopilot structure.
Definition: autopilot.c:50
autopilot_throttle_killed
bool autopilot_throttle_killed(void)
get kill status
Definition: autopilot.c:230
PWM_USE_TIM12
#define PWM_USE_TIM12
Definition: matek_f405_wing_v1.h:299
pprz_autopilot::in_flight
bool in_flight
in flight status
Definition: autopilot.h:70
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
config
static const struct usb_config_descriptor config
Definition: usb_ser_hw.c:200
settings_store_flag
bool settings_store_flag
flag for setting feedback.
Definition: settings.c:38
sys_time.h
Architecture independent timing functions.
uint8_t
unsigned char uint8_t
Definition: types.h:14
RADIO_CONTROL_NB_CHANNEL
#define RADIO_CONTROL_NB_CHANNEL
Definition: intermcu_ap.h:49
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:46
autopilot_generated_set_mode
void autopilot_generated_set_mode(uint8_t new_autopilot_mode)
Definition: autopilot_generated.c:77
UART1_RX
#define UART1_RX
Definition: board.h:62
pprz_autopilot::ground_detected
bool ground_detected
automatic detection of landing
Definition: autopilot.h:74
autopilot_send_mode
void WEAK autopilot_send_mode(void)
send autopilot mode actual implementation is firmware dependent
Definition: autopilot.c:306
autopilot.h
USE_AD_TIM1
#define USE_AD_TIM1
Definition: cjmcu.h:138
settings_clear
int32_t settings_clear(void)
clear all persistent settings from flash
Definition: settings.c:77
autopilot_firmware_init
void autopilot_firmware_init(void)
Init function.
Definition: autopilot_firmware.c:114
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:200
autopilot_set_kill_throttle
void autopilot_set_kill_throttle(bool kill)
set kill throttle
Definition: autopilot.c:219
PWM
#define PWM
Definition: LPC21xx.h:91
autopilot_generated_set_motors_on
void autopilot_generated_set_motors_on(bool motors_on)
Definition: autopilot_generated.c:85
autopilot_periodic
void autopilot_periodic(void)
AP periodic call.
Definition: autopilot.c:135
autopilot_static_set_mode
void autopilot_static_set_mode(uint8_t new_autopilot_mode)
Definition: autopilot_static.c:194
pwm
timer subsystem PPM input on SERVO6 pin actuators pwm(if PWM_USE_TIM1) TIM8 radio_control/ppm(if USE_PPM_TIM8)(non-advanced timers using RCC_APB2) TIM2 adc(if USE_AD_TIM2
autopilot_store_settings
void autopilot_store_settings(void)
store settings
Definition: autopilot.c:278
send_alive
static void send_alive(struct transport_tx *trans, struct link_device *dev)
Definition: autopilot.c:60
autopilot_static_periodic
void autopilot_static_periodic(void)
Definition: autopilot_static.c:104
USE_PPM_TIM8
#define USE_PPM_TIM8
Definition: navstik_1.0.h:114
gpio.h
autopilot_set_mode
bool autopilot_set_mode(uint8_t new_autopilot_mode)
set autopilot mode
Definition: autopilot.c:161
USE_PPM_TIM1
#define USE_PPM_TIM1
Definition: apogee_1.0.h:342
send_actuators
static void send_actuators(struct transport_tx *trans, struct link_device *dev)
Definition: demo_ahrs_actuators.c:155
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:238
pprz_autopilot::mode_auto2
uint8_t mode_auto2
FIXME hide this in a private part ?
Definition: autopilot.h:64
type
timer subsystem type(config options) --------------------------------------------(advanced timers using RCC_APB1) TIM1 adc(if USE_AD_TIM1) radio_control/ppm(if USE_PPM_TIM1
autopilot_static_SetModeHandler
void autopilot_static_SetModeHandler(float new_autopilot_mode)
Definition: autopilot_static.c:201
ppm
timer subsystem PPM input on SERVO6 pin actuators default radio_control ppm(if USE_PPM_TIM2, PPM input on UART1_RX pin) actuators/pwm(if PWM_USE_TIM2) TIM3 actuators/pwm(if PWM_USE_TIM3
send_attitude
static void send_attitude(struct transport_tx *trans, struct link_device *dev)
Definition: autopilot.c:65
USE_PPM_TIM2
#define USE_PPM_TIM2
Definition: cc3d.h:107
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.
autopilot_get_mode
uint8_t autopilot_get_mode(void)
get autopilot mode
Definition: autopilot.c:184
FloatEulers::psi
float psi
in radians
Definition: pprz_algebra_float.h:87
autopilot_on_rc_frame
void autopilot_on_rc_frame(void)
RC frame handler.
Definition: autopilot.c:150
send_rc
static void send_rc(struct transport_tx *trans, struct link_device *dev)
Definition: autopilot.c:77
pprz_autopilot::power_switch
bool power_switch
enable/disable power from power switch (if any)
Definition: autopilot.h:73
settings_store
int32_t settings_store(void)
store settings marked as persistent to flash
Definition: settings.c:57
send_dl_value
static void send_dl_value(struct transport_tx *trans, struct link_device *dev)
Definition: autopilot.c:71
gpio_set
static void gpio_set(ioportid_t port, uint16_t pin)
Set a gpio output to high level.
Definition: gpio_arch.h:98
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:112
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:75
radio_control
struct RadioControl radio_control
Definition: radio_control.c:30
actuators.h
RadioControl::values
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:69