Paparazzi UAS  v6.1.0_stable
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  *
4  * This file is part of Paparazzi.
5  *
6  * Paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * Paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with Paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  *
21  */
22 
29 #include <inttypes.h>
30 #include "mcu.h"
31 #include "led.h"
32 #include "mcu_periph/sys_time.h"
33 
34 #include "modules/core/commands.h"
36 #if USE_MOTOR_MIXING
38 #endif
39 
45 
46 #define MODULES_C
47 #include "generated/modules.h"
48 
49 /* So one can use these in command_laws section */
50 #define And(x, y) ((x) && (y))
51 #define Or(x, y) ((x) || (y))
52 #define Min(x,y) (x < y ? x : y)
53 #define Max(x,y) (x > y ? x : y)
54 #define LessThan(_x, _y) ((_x) < (_y))
55 #define MoreThan(_x, _y) ((_x) > (_y))
56 
57 
60 bool fbw_motors_on = false;
61 
62 /* MODULES_FREQUENCY is defined in generated/modules.h
63  * according to main_freq parameter set for modules in airframe file
64  */
65 PRINT_CONFIG_VAR(MODULES_FREQUENCY)
66 
72 
73 
75 void main_init(void)
76 {
77  // Set startup mode to Failsafe
79 
80  mcu_init();
81 
82  actuators_init();
83 
85 
86 #if USE_MOTOR_MIXING
88 #endif
89 
91 
92  modules_init();
93 
94 
95  intermcu_init();
96 
97  // Register the timers for the periodic functions
98  main_periodic_tid = sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL);
99  modules_tid = sys_time_register_timer(1. / MODULES_FREQUENCY, NULL);
100  radio_control_tid = sys_time_register_timer((1. / 60.), NULL);
102  telemetry_tid = sys_time_register_timer((1. / 10.), NULL);
103 }
104 
105 
107 // PERIODIC
108 
110 {
112  main_periodic();
113  }
115  modules_periodic_task();
116  }
119  }
122  }
125  }
126 }
127 
129 {
130  /* Send status to AP */
132 
133  /* Handle Modems */
134  // TODO
135 }
136 
137 /* Checks the different safety cases and sets the correct FBW mode */
138 static void fbw_safety_check(void)
139 {
140  /* Safety logic */
141  bool ap_lost = (intermcu.status == INTERMCU_LOST);
142  bool rc_lost = (radio_control.status == RC_REALLY_LOST);
143 
144  // Both the RC and AP are lost
145  if (rc_lost && ap_lost) {
147  }
148  // RC is valid but lost AP
149  else if (!rc_lost && ap_lost) {
150  // Only crucial when AP was in control
151  if (fbw_mode == FBW_MODE_AUTO) {
153  }
154  }
155  // RC is lost but AP is valid
156  else if (rc_lost && !ap_lost) {
157 
158  // Lost RC while flying in manual trough FBW
159  if (fbw_mode == FBW_MODE_MANUAL) {
161  }
162  // Allways keep failsafe when RC is lost
163  else if (fbw_mode == FBW_MODE_FAILSAFE) {
164  // No change: failsafe stays failsafe
165  }
166  // Lost RC while in working Auto mode
167  else {
169  }
170  }
171 }
172 
173 /* Sets the actual actuator commands */
174 void main_periodic(void)
175 {
176  /* Inter-MCU watchdog */
178 
179  /* Safety check and set FBW mode */
181 
182 #ifdef BOARD_PX4IO
183  //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
184  //to prevent this situation:
185  if (intermcu.stable_px4_baud != PPRZ_BAUD) {
187  fbw_motors_on = false;
188  //signal to user whether fbw can be flashed:
189 #ifdef FBW_MODE_LED
190  LED_OFF(FBW_MODE_LED); // causes really fast blinking
191 #endif
192  }
193 #endif
194 
195  // TODO make module out of led blink?
196 #ifdef FBW_MODE_LED
197  static uint16_t dv = 0;
198  if (fbw_mode == FBW_MODE_FAILSAFE) {
199  if (!(dv++ % (PERIODIC_FREQUENCY / 20))) { LED_TOGGLE(FBW_MODE_LED);}
200  } else if (fbw_mode == FBW_MODE_MANUAL) {
201  if (!(dv++ % (PERIODIC_FREQUENCY))) { LED_TOGGLE(FBW_MODE_LED);}
202  } else if (fbw_mode == FBW_MODE_AUTO) {
203  LED_ON(FBW_MODE_LED);
204  }
205 #endif // FWB_MODE_LED
206 
207  /* Set failsafe commands */
208  if (fbw_mode == FBW_MODE_FAILSAFE) {
209  fbw_motors_on = false;
211  }
212 
213  /* If in auto copy autopilot motors on */
214  if (fbw_mode == FBW_MODE_AUTO) {
216  }
217 
218  /* Set actuators */
219  SetActuatorsFromCommands(commands, autopilot_get_mode());
220 
221  /* Periodic blinking */
222  RunOnceEvery(10, LED_PERIODIC());
223 }
224 
225 
227 // Event
228 
230 static void fbw_on_rc_frame(void)
231 {
232  /* get autopilot fbw mode as set by RADIO_MODE 3-way switch */
234 
235 #ifdef RADIO_KILL_SWITCH
236  static bool kill_state_init = false; // require a kill == off before enabling engines with kill == on
239  kill_state_init = true;
240  } else {
241  if (kill_state_init)
243  else
245  }
246 #else
248 #endif
249 
250  } else {
252  }
253 
254  /* Failsafe check if intermcu is lost while AP was in control */
255  if ((intermcu.status == INTERMCU_LOST) &&
256  (fbw_mode == FBW_MODE_AUTO)) {
258  }
259 
260  /* If the FBW is in control */
261  if (fbw_mode == FBW_MODE_MANUAL) {
262  fbw_motors_on = true;
264 #ifdef SetCommandsFromRC
265  SetCommandsFromRC(commands, radio_control.values);
266 #else
267 #warning "FBW: needs commands from RC in order to be useful."
268 #endif
269  }
270 
271  /* Forward radiocontrol to AP */
273 }
274 
276 static void fbw_on_ap_command(void)
277 {
278  // Only set the command from AP when we are in AUTO mode
279  if (fbw_mode == FBW_MODE_AUTO) {
281  }
282 }
283 
284 void main_event(void)
285 {
286  /* Event functions for mcu peripherals: i2c, usb_serial.. */
287  mcu_event();
288 
289  /* Handle RC */
291 
292  /* InterMCU (gives autopilot commands as output) */
294 
295  /* FBW modules */
296  modules_event_task();
297 }
radio_control.h
electrical.h
radio_control_periodic_task
void radio_control_periodic_task(void)
Definition: radio_control.c:46
motor_mixing.h
LED_OFF
#define LED_OFF(i)
Definition: led_hw.h:52
FBW_MODE_FAILSAFE
@ FBW_MODE_FAILSAFE
Definition: main_fbw.h:64
RADIO_FBW_MODE
#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:61
telemetry_periodic
void telemetry_periodic(void)
Definition: main_fbw.c:128
intermcu_fbw.h
Rotorcraft Inter-MCU on FlyByWire.
autopilot_motors_on
bool autopilot_motors_on
Definition: intermcu_fbw.c:64
fbw_mode
uint8_t fbw_mode
Definition: main_fbw.c:63
intermcu_send_status
void intermcu_send_status(uint8_t mode)
Definition: intermcu_fbw.c:139
mcu_event
void mcu_event(void)
MCU event functions.
Definition: mcu.c:258
intermcu_commands
pprz_t intermcu_commands[COMMANDS_NB]
Definition: intermcu_fbw.c:63
InterMcuEvent
void InterMcuEvent(void(*frame_handler)(void))
Definition: intermcu_fbw.c:199
LED_ON
#define LED_ON(i)
Definition: led_hw.h:51
intermcu_t::status
enum intermcu_status status
Status of the INTERMCU.
Definition: intermcu.h:73
commands_failsafe
const pprz_t commands_failsafe[COMMANDS_NB]
Definition: commands.c:31
LED_TOGGLE
#define LED_TOGGLE(i)
Definition: led_hw.h:53
main_event
void main_event(void)
Definition: main_fbw.c:284
motor_mixing_init
void motor_mixing_init(void)
Definition: motor_mixing.c:109
main_periodic_tid
tid_t main_periodic_tid
id for main_periodic() timer
Definition: main_fbw.c:67
SetCommands
#define SetCommands(t)
Definition: commands.h:41
main_fbw.h
INTERMCU_LOST
@ INTERMCU_LOST
No interMCU communication anymore.
Definition: intermcu.h:46
fbw_on_rc_frame
static void fbw_on_rc_frame(void)
Callback when we received an RC frame.
Definition: main_fbw.c:230
autopilot_rc_helpers.h
intermcu_periodic
void intermcu_periodic(void)
Definition: intermcu_ap.c:90
radio_control_tid
tid_t radio_control_tid
id for radio_control_periodic_task() timer
Definition: main_fbw.c:69
mcu.h
Arch independent mcu ( Micro Controller Unit ) utilities.
intermcu_on_rc_frame
void intermcu_on_rc_frame(uint8_t fbw_mode)
Definition: intermcu_fbw.c:106
electrical_periodic
void electrical_periodic(void)
Definition: electrical.c:123
PPRZ_BAUD
@ PPRZ_BAUD
Definition: intermcu.h:53
sys_time.h
Architecture independent timing functions.
LED_PERIODIC
#define LED_PERIODIC()
Definition: led_hw.h:55
MIN_PPRZ
#define MIN_PPRZ
Definition: paparazzi.h:9
telemetry_tid
tid_t telemetry_tid
id for telemetry_periodic() timer
Definition: main_fbw.c:71
RC_LOST_IN_AUTO_FBW_MODE
#define RC_LOST_IN_AUTO_FBW_MODE
mode to enter when AP is lost while using autopilot
Definition: main_fbw.h:46
fbw_safety_check
static void fbw_safety_check(void)
Definition: main_fbw.c:138
RadioControl::status
uint8_t status
Definition: radio_control.h:64
electrical_init
void electrical_init(void)
Definition: electrical.c:100
tid_t
int8_t tid_t
sys_time timer id type
Definition: sys_time.h:56
led.h
arch independent LED (Light Emitting Diodes) API
electrical_tid
tid_t electrical_tid
id for electrical_periodic() timer
Definition: main_fbw.c:77
radio_control_init
void radio_control_init(void)
Definition: radio_control.c:32
commands
static const ShellCommand commands[]
Definition: shell_arch.c:71
FBW_MODE_AUTO_ONLY
#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:56
handle_periodic_tasks
void handle_periodic_tasks(void)
Definition: main_fbw.c:109
FBW_MODE_MANUAL
@ FBW_MODE_MANUAL
Definition: main_fbw.h:64
RadioControlEvent
#define RadioControlEvent(_received_frame_handler)
Definition: cc2500_paparazzi.h:37
fbw_motors_on
bool fbw_motors_on
Definition: main_fbw.c:60
RC_LOST_FBW_MODE
#define RC_LOST_FBW_MODE
mode to enter when RC is lost while using a mode with RC input
Definition: main_fbw.h:41
AP_LOST_FBW_MODE
#define AP_LOST_FBW_MODE
mode to enter when AP is lost while using autopilot
Definition: main_fbw.h:51
actuators.h
FBW_MODE_AUTO
@ FBW_MODE_AUTO
Definition: main_fbw.h:64
main_periodic
void main_periodic(void)
Definition: main_fbw.c:174
sys_time_register_timer
tid_t sys_time_register_timer(float duration, sys_time_cb cb)
Register a new system timer.
Definition: sys_time.c:43
main_init
void main_init(void)
Main initialization.
Definition: main_fbw.c:75
commands.h
Hardware independent code for commands handling.
autopilot_get_mode
uint8_t autopilot_get_mode(void)
get autopilot mode
Definition: autopilot.c:211
intermcu_init
void intermcu_init(void)
Definition: intermcu_ap.c:71
RADIO_KILL_SWITCH
#define RADIO_KILL_SWITCH
Definition: intermcu_ap.h:45
intermcu
struct intermcu_t intermcu
Definition: intermcu_ap.c:41
uint16_t
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
mcu_init
void mcu_init(void)
Microcontroller peripherals initialization.
Definition: mcu.c:87
RC_REALLY_LOST
#define RC_REALLY_LOST
Definition: radio_control.h:58
fbw_on_ap_command
static void fbw_on_ap_command(void)
Callback when receive commands from the AP.
Definition: main_fbw.c:276
sys_time_check_and_ack_timer
static bool sys_time_check_and_ack_timer(tid_t id)
Check if timer has elapsed.
Definition: sys_time.h:119
modules_tid
tid_t modules_tid
id for modules_periodic_task() timer
Definition: main_fbw.c:68
fbw_mode_enum
fbw_mode_enum
Definition: main_fbw.h:64
radio_control
struct RadioControl radio_control
Definition: radio_control.c:30
RadioControl::values
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:69