Paparazzi UAS  v6.0_unstable-92-g17422e4-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 "subsystems/commands.h"
35 #include "subsystems/actuators.h"
36 #if USE_MOTOR_MIXING
38 #endif
39 
40 #include "subsystems/electrical.h"
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);
101  electrical_tid = sys_time_register_timer(0.1, 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 }
void intermcu_send_status(uint8_t mode)
Definition: intermcu_fbw.c:139
void mcu_init(void)
Microcontroller peripherals initialization.
Definition: mcu.c:87
void InterMcuEvent(void(*frame_handler)(void))
Definition: intermcu_fbw.c:199
tid_t radio_control_tid
id for radio_control_periodic_task() timer
Definition: main_fbw.c:69
bool fbw_motors_on
Definition: main_fbw.c:60
void main_event(void)
Definition: main_fbw.c:284
#define RadioControlEvent(_received_frame_handler)
tid_t modules_tid
id for modules_periodic_task() timer
Definition: main_fbw.c:68
#define MIN_PPRZ
Definition: paparazzi.h:9
pprz_t intermcu_commands[COMMANDS_NB]
Definition: intermcu_fbw.c:63
tid_t electrical_tid
id for electrical_periodic() timer
Definition: main_fbw.c:77
uint8_t status
Definition: radio_control.h:64
#define LED_OFF(i)
Definition: led_hw.h:52
struct intermcu_t intermcu
Definition: intermcu_ap.c:41
static void fbw_on_ap_command(void)
Callback when receive commands from the AP.
Definition: main_fbw.c:276
bool autopilot_motors_on
Definition: intermcu_fbw.c:64
void intermcu_periodic(void)
Definition: intermcu_ap.c:90
enum intermcu_status status
Status of the INTERMCU.
Definition: intermcu.h:73
tid_t telemetry_tid
id for telemetry_periodic() timer
Definition: main_fbw.c:71
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:69
void telemetry_periodic(void)
Definition: main_fbw.c:128
void radio_control_init(void)
Definition: radio_control.c:32
const pprz_t commands_failsafe[COMMANDS_NB]
Definition: commands.c:31
fbw_mode_enum
Definition: main_fbw.h:64
Rotorcraft Inter-MCU on FlyByWire.
No interMCU communication anymore.
Definition: intermcu.h:46
Hardware independent API for actuators (servos, motor controllers).
void electrical_init(void)
Definition: electrical.c:100
void main_init(void)
Main initialization.
Definition: main_fbw.c:75
Some helper functions to check RC sticks.
#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
void handle_periodic_tasks(void)
Definition: main_fbw.c:109
Interface for electrical status: supply voltage, current, battery status, etc.
Architecture independent timing functions.
void motor_mixing_init(void)
Definition: motor_mixing.c:109
void radio_control_periodic_task(void)
Definition: radio_control.c:46
int8_t tid_t
sys_time timer id type
Definition: sys_time.h:56
tid_t main_periodic_tid
id for main_periodic() timer
Definition: main_fbw.c:67
Hardware independent code for commands handling.
struct RadioControl radio_control
Definition: radio_control.c:30
void intermcu_on_rc_frame(uint8_t fbw_mode)
Definition: intermcu_fbw.c:106
#define LED_TOGGLE(i)
Definition: led_hw.h:53
#define RC_REALLY_LOST
Definition: radio_control.h:58
void electrical_periodic(void)
Definition: electrical.c:123
void intermcu_init(void)
Definition: intermcu_ap.c:71
uint8_t fbw_mode
Definition: main_fbw.c:63
Fly By Wire:
Arch independent mcu ( Micro Controller Unit ) utilities.
#define SetCommands(t)
Definition: commands.h:41
#define RC_LOST_FBW_MODE
mode to enter when RC is lost while using a mode with RC input
Definition: main_fbw.h:41
static bool sys_time_check_and_ack_timer(tid_t id)
Check if timer has elapsed.
Definition: sys_time.h:119
#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
#define LED_PERIODIC()
Definition: led_hw.h:55
arch independent LED (Light Emitting Diodes) API
#define LED_ON(i)
Definition: led_hw.h:51
void mcu_event(void)
MCU event functions.
Definition: mcu.c:258
#define RADIO_KILL_SWITCH
Definition: intermcu_ap.h:45
Motor Mixing.
#define AP_LOST_FBW_MODE
mode to enter when AP is lost while using autopilot
Definition: main_fbw.h:51
static const ShellCommand commands[]
Definition: shell_arch.c:71
#define RC_LOST_IN_AUTO_FBW_MODE
mode to enter when AP is lost while using autopilot
Definition: main_fbw.h:46
static void fbw_safety_check(void)
Definition: main_fbw.c:138
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
static void fbw_on_rc_frame(void)
Callback when we received an RC frame.
Definition: main_fbw.c:230
tid_t sys_time_register_timer(float duration, sys_time_cb cb)
Register a new system timer.
Definition: sys_time.c:43
uint8_t autopilot_get_mode(void)
get autopilot mode
Definition: autopilot.c:184
void main_periodic(void)
Definition: main_fbw.c:174