Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
main_fbw.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2015 The Paparazzi Team
3  * Copyright (C) 2022 Gautier Hattenberger <gautier.hattenberger@enac.fr>
4  *
5  * This file is part of Paparazzi.
6  *
7  * Paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * Paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with paparazzi; see the file COPYING. If not, see
19  * <http://www.gnu.org/licenses/>.
20  *
21  */
22 
29 #define MODULES_C
30 
31 #define ABI_C
32 
33 #include "main_fbw.h"
34 #include "generated/airframe.h"
35 #include "generated/modules.h"
36 #include "modules/core/abi.h"
39 
42 bool fbw_motors_on = false;
43 
44 /* if PRINT_CONFIG is defined, print some config options */
45 PRINT_CONFIG_VAR(PERIODIC_FREQUENCY)
46 /* SYS_TIME_FREQUENCY/PERIODIC_FREQUENCY should be an integer, otherwise the timer will not be correct */
47 #if !(SYS_TIME_FREQUENCY/PERIODIC_FREQUENCY*PERIODIC_FREQUENCY == SYS_TIME_FREQUENCY)
48 #warning "The SYS_TIME_FREQUENCY can not be divided by PERIODIC_FREQUENCY. Make sure this is the case for correct timing."
49 #endif
50 
51 /* TELEMETRY_FREQUENCY is defined in generated/periodic_telemetry.h
52  * defaults to 60Hz or set by TELEMETRY_FREQUENCY configure option in airframe file
53  */
54 PRINT_CONFIG_VAR(TELEMETRY_FREQUENCY)
55 
56 
63 
67 #ifndef MAIN_FBW_RC_ID
68 #define MAIN_FBW_RC_ID ABI_BROADCAST
69 #endif
70 PRINT_CONFIG_VAR(MAIN_FBW_RC_ID)
72 static void rc_cb(uint8_t sender_id, struct RadioControl *rc);
73 
74 
78 void main_fbw_init(void)
79 {
80  // mcu init done in main
81 
82  modules_core_init();
83  modules_radio_control_init();
84  modules_actuators_init();
85  modules_datalink_init();
86 
87  // Set startup mode to Failsafe
89 
90  // Bind to RC event
91  AbiBindMsgRADIO_CONTROL(MAIN_FBW_RC_ID, &rc_ev, rc_cb);
92 
93  // Register the timers for the periodic functions
94  periodic_tid = sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL);
95  radio_control_tid = sys_time_register_timer((1. / 60.), NULL);
97  telemetry_tid = sys_time_register_timer((1. / TELEMETRY_FREQUENCY), NULL);
98 }
99 
100 
105 /* Checks the different safety cases and sets the correct FBW mode */
106 static void fbw_safety_check(void)
107 {
108  /* Safety logic */
109  bool ap_lost = (intermcu.status == INTERMCU_LOST);
110  bool rc_lost = (radio_control.status == RC_REALLY_LOST);
111 
112  // Both the RC and AP are lost
113  if (rc_lost && ap_lost) {
115  }
116  // RC is valid but lost AP
117  else if (!rc_lost && ap_lost) {
118  // Only crucial when AP was in control
119  if (fbw_mode == FBW_MODE_AUTO) {
121  }
122  }
123  // RC is lost but AP is valid
124  else if (rc_lost && !ap_lost) {
125 
126  // Lost RC while flying in manual trough FBW
127  if (fbw_mode == FBW_MODE_MANUAL) {
129  }
130  // Allways keep failsafe when RC is lost
131  else if (fbw_mode == FBW_MODE_FAILSAFE) {
132  // No change: failsafe stays failsafe
133  }
134  // Lost RC while in working Auto mode
135  else {
137  }
138  }
139 }
140 
141 /* Sets the actual actuator commands */
142 static void main_task_periodic(void)
143 {
144  /* Safety check and set FBW mode */
146 
147 #ifdef BOARD_PX4IO
148  //due to a baud rate issue on PX4, for a few seconds the baud is 1500000 however this may result in package loss, causing the motors to spin at random
149  //to prevent this situation:
150  if (intermcu.stable_px4_baud != PPRZ_BAUD) {
152  fbw_motors_on = false;
153  //signal to user whether fbw can be flashed:
154 #ifdef FBW_MODE_LED
155  LED_OFF(FBW_MODE_LED); // causes really fast blinking
156 #endif
157  }
158 #endif
159 
160  // TODO make module out of led blink?
161 #ifdef FBW_MODE_LED
162  static uint16_t dv = 0;
163  if (fbw_mode == FBW_MODE_FAILSAFE) {
164  if (!(dv++ % (PERIODIC_FREQUENCY / 20))) { LED_TOGGLE(FBW_MODE_LED);}
165  } else if (fbw_mode == FBW_MODE_MANUAL) {
166  if (!(dv++ % (PERIODIC_FREQUENCY))) { LED_TOGGLE(FBW_MODE_LED);}
167  } else if (fbw_mode == FBW_MODE_AUTO) {
168  LED_ON(FBW_MODE_LED);
169  }
170 #endif // FWB_MODE_LED
171 
172  /* Set failsafe commands */
173  if (fbw_mode == FBW_MODE_FAILSAFE) {
174  fbw_motors_on = false;
175  SetCommands(commands_failsafe);
176  }
177 
178  /* If in auto copy autopilot motors on and commands from intermcu */
179  if (fbw_mode == FBW_MODE_AUTO) {
181  SetCommands(intermcu_commands);
182  }
183 
184  /* in MANUAL, commands are updated in RC callback */
185 }
186 
188 {
190  modules_radio_control_periodic_task();
191  }
192 
195  modules_actuators_periodic_task();
196  modules_mcu_periodic_task();
197  modules_core_periodic_task();
198  }
199 
202  }
203 
205  modules_datalink_periodic_task();
206  }
207 }
208 
209 
211 // Event
212 
214 static void rc_cb(uint8_t sender_id __attribute__((unused)), struct RadioControl *rc __attribute__((unused)))
215 {
216  /* get autopilot fbw mode as set by RADIO_MODE 3-way switch */
218 
219 #ifdef RADIO_KILL_SWITCH
220  static bool kill_state_init = false; // require a kill == off before enabling engines with kill == on
223  kill_state_init = true;
224  } else {
225  if (kill_state_init)
227  else
229  }
230 #else
232 #endif
233 
234  } else {
236  }
237 
238  /* Failsafe check if intermcu is lost while AP was in control */
239  if ((intermcu.status == INTERMCU_LOST) &&
240  (fbw_mode == FBW_MODE_AUTO)) {
242  }
243 
244  /* If the FBW is in control */
245  if (fbw_mode == FBW_MODE_MANUAL) {
246  fbw_motors_on = true;
247  SetCommands(commands_failsafe);
248 #ifdef SetCommandsFromRC
249  SetCommandsFromRC(commands, radio_control.values);
250 #else
251 #warning "FBW: needs commands from RC in order to be useful."
252 #endif
253  }
254 }
255 
257 {
258  if (DL_EMERGENCY_CMD_ac_id(buf) == AC_ID && DL_EMERGENCY_CMD_cmd(buf) == 0) {
260  }
261 }
262 
263 void main_fbw_event(void)
264 {
265  modules_mcu_event_task();
266  intermcu_event();
267  modules_radio_control_event_task();
268  modules_actuators_event_task();
269  modules_datalink_event_task();
270 }
Main include for ABI (AirBorneInterface).
Event structure to store callbacks in a linked list.
Definition: abi_common.h:67
#define LED_ON(i)
Definition: led_hw.h:51
#define LED_OFF(i)
Definition: led_hw.h:52
#define LED_TOGGLE(i)
Definition: led_hw.h:53
const pprz_t commands_failsafe[COMMANDS_NB]
Definition: commands.c:31
pprz_t commands[COMMANDS_NB]
Definition: commands.c:30
void electrical_periodic(void)
Definition: electrical.c:166
void intermcu_event(void)
Definition: intermcu_ap.c:95
@ INTERMCU_LOST
No interMCU communication anymore.
Definition: intermcu.h:43
struct intermcu_t intermcu
Definition: intermcu_ap.c:45
enum intermcu_status status
Status of the INTERMCU.
Definition: intermcu.h:72
pprz_t intermcu_commands[COMMANDS_NB]
Definition: intermcu_fbw.c:66
bool intermcu_ap_motors_on
Definition: intermcu_fbw.c:67
void main_fbw_parse_EMERGENCY_CMD(uint8_t *buf)
Definition: main_fbw.c:256
tid_t periodic_tid
IDs for timers.
Definition: main_fbw.c:59
void main_fbw_init(void)
Main initialization.
Definition: main_fbw.c:78
static void rc_cb(uint8_t sender_id, struct RadioControl *rc)
Callback when we received an RC frame.
Definition: main_fbw.c:214
tid_t electrical_tid
id for electrical_periodic() timer
Definition: main_fbw.c:61
tid_t radio_control_tid
id for radio_control_periodic() timer
Definition: main_fbw.c:60
bool fbw_motors_on
Definition: main_fbw.c:42
#define MAIN_FBW_RC_ID
ABI RC binding.
Definition: main_fbw.c:68
static void main_task_periodic(void)
Definition: main_fbw.c:142
tid_t telemetry_tid
id for telemetry_periodic() timer
Definition: main_fbw.c:62
static void fbw_safety_check(void)
Periodic tasks.
Definition: main_fbw.c:106
void main_fbw_event(void)
Definition: main_fbw.c:263
uint8_t fbw_mode
Fly by wire modes.
Definition: main_fbw.c:41
void main_fbw_periodic(void)
Definition: main_fbw.c:187
static abi_event rc_ev
Definition: main_fbw.c:71
Fly By Wire:
#define FBW_MODE_AUTO_ONLY
holds whether the aircraft can only be flown with the AP and not RC-Direct/FBW-mode
Definition: main_fbw.h:60
#define FBW_MODE_FAILSAFE
Definition: main_fbw.h:70
#define RC_LOST_IN_AUTO_FBW_MODE
mode to enter when AP is lost while using autopilot
Definition: main_fbw.h:50
#define AP_LOST_FBW_MODE
mode to enter when AP is lost while using autopilot
Definition: main_fbw.h:55
#define RC_LOST_FBW_MODE
mode to enter when RC is lost while using a mode with RC input switching to AUTO allows a recover wit...
Definition: main_fbw.h:45
#define FBW_MODE_MANUAL
Definition: main_fbw.h:68
#define FBW_MODE_AUTO
Definition: main_fbw.h:69
#define RADIO_FBW_MODE
Switching between FBW and autopilot is done with RADIO_FBW_MODE: default is to re-use RADIO_MODE.
Definition: main_fbw.h:65
#define MIN_PPRZ
Definition: paparazzi.h:9
struct RadioControl radio_control
Definition: radio_control.c:33
#define RC_REALLY_LOST
Definition: radio_control.h:51
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:67
uint8_t status
Definition: radio_control.h:61
#define RADIO_KILL_SWITCH
Definition: rc_intermcu.h:40
tid_t sys_time_register_timer(float duration, sys_time_cb cb)
Register a new system timer.
Definition: sys_time.c:43
static bool sys_time_check_and_ack_timer(tid_t id)
Check if timer has elapsed.
Definition: sys_time.h:123
int8_t tid_t
sys_time timer id type
Definition: sys_time.h:60
Periodic telemetry system header (includes downlink utility and generated code).
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98