Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
28
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
42bool fbw_motors_on = false;
43
44/* if PRINT_CONFIG is defined, print some config options */
45PRINT_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 */
54PRINT_CONFIG_VAR(TELEMETRY_FREQUENCY)
55
56
63
67#ifndef MAIN_FBW_RC_ID
68#define MAIN_FBW_RC_ID ABI_BROADCAST
69#endif
72static void rc_cb(uint8_t sender_id, struct RadioControl *rc);
73
74
78void 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);
97 telemetry_tid = sys_time_register_timer((1. / TELEMETRY_FREQUENCY), NULL);
98}
99
100
104
105/* Checks the different safety cases and sets the correct FBW mode */
106static 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 */
142static void main_task_periodic(void)
143{
144 /* Safety check and set FBW mode */
146
147 // TODO make module out of led blink?
148#ifdef FBW_MODE_LED
149 static uint16_t dv = 0;
151 if (!(dv++ % (PERIODIC_FREQUENCY / 20))) { LED_TOGGLE(FBW_MODE_LED);}
152 } else if (fbw_mode == FBW_MODE_MANUAL) {
153 if (!(dv++ % (PERIODIC_FREQUENCY))) { LED_TOGGLE(FBW_MODE_LED);}
154 } else if (fbw_mode == FBW_MODE_AUTO) {
155 LED_ON(FBW_MODE_LED);
156 }
157#endif // FWB_MODE_LED
158
159 /* Set failsafe commands */
161 fbw_motors_on = false;
162 SetCommands(commands_failsafe);
163 }
164
165 /* If in auto copy autopilot motors on and commands from intermcu */
166 if (fbw_mode == FBW_MODE_AUTO) {
168 SetCommands(intermcu_commands);
169 }
170
171 /* in MANUAL, commands are updated in RC callback */
172}
173
175{
177 modules_radio_control_periodic_task();
178 }
179
182 modules_actuators_periodic_task();
183 modules_mcu_periodic_task();
184 modules_core_periodic_task();
185 }
186
189 }
190
192 modules_datalink_periodic_task();
193 }
194}
195
196
198// Event
199
201static void rc_cb(uint8_t sender_id __attribute__((unused)), struct RadioControl *rc __attribute__((unused)))
202{
203 /* get autopilot fbw mode as set by RADIO_MODE 3-way switch */
205
206#ifdef RADIO_KILL_SWITCH
207 static bool kill_state_init = false; // require a kill == off before enabling engines with kill == on
208 if (radio_control.values[RADIO_KILL_SWITCH] < (MIN_PPRZ / 2)) {
210 kill_state_init = true;
211 } else {
212 if (kill_state_init)
214 else
216 }
217#else
219#endif
220
221 } else {
223 }
224
225 /* Failsafe check if intermcu is lost while AP was in control */
226 if ((intermcu.status == INTERMCU_LOST) &&
227 (fbw_mode == FBW_MODE_AUTO)) {
229 }
230
231 /* If the FBW is in control */
232 if (fbw_mode == FBW_MODE_MANUAL) {
233 fbw_motors_on = true;
234 SetCommands(commands_failsafe);
235#ifdef SetCommandsFromRC
236 SetCommandsFromRC(commands, radio_control.values);
237#else
238#warning "FBW: needs commands from RC in order to be useful."
239#endif
240 }
241}
242
244{
245 if (DL_EMERGENCY_CMD_ac_id(buf) == AC_ID && DL_EMERGENCY_CMD_cmd(buf) == 0) {
247 }
248}
249
251{
252 modules_mcu_event_task();
254 modules_radio_control_event_task();
255 modules_actuators_event_task();
256 modules_datalink_event_task();
257}
Main include for ABI (AirBorneInterface).
struct abi_struct abi_event
Definition abi_common.h:101
static void rc_cb(uint8_t sender_id, struct RadioControl *rc)
Definition autopilot.c:58
static abi_event rc_ev
Definition autopilot.c:56
#define LED_ON(i)
Definition led_hw.h:51
#define LED_TOGGLE(i)
Definition led_hw.h:53
const pprz_t commands_failsafe[COMMANDS_NB]
Definition commands.c:31
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
pprz_t intermcu_commands[COMMANDS_NB]
bool intermcu_ap_motors_on
void main_fbw_parse_EMERGENCY_CMD(uint8_t *buf)
Definition main_fbw.c:243
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:201
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
static void fbw_safety_check(void)
Periodic tasks.
Definition main_fbw.c:106
void main_fbw_event(void)
Definition main_fbw.c:250
uint8_t fbw_mode
Fly by wire modes.
Definition main_fbw.c:41
void main_fbw_periodic(void)
Definition main_fbw.c:174
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
tid_t periodic_tid
id for periodic task timer
tid_t electrical_tid
id for electrical_periodic() timer
tid_t telemetry_tid
id for periodic telemetry
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
#define MIN_PPRZ
Definition paparazzi.h:9
struct RadioControl radio_control
#define RC_REALLY_LOST
static const ShellCommand commands[]
Definition shell_arch.c:77
tid_t sys_time_register_timer(float duration, sys_time_cb cb)
Register a new system timer.
Definition sys_time.c:172
static bool sys_time_check_and_ack_timer(tid_t id)
Check if timer has elapsed.
Definition sys_time.h:226
int8_t tid_t
sys_time timer id type
Definition sys_time.h:68
Periodic telemetry system header (includes downlink utility and generated code).
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.