Paparazzi UAS  v5.15_devel-112-g521f3cf
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 
95 
96  intermcu_init();
97 
98  // Register the timers for the periodic functions
99  main_periodic_tid = sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL);
100  modules_tid = sys_time_register_timer(1. / MODULES_FREQUENCY, NULL);
101  radio_control_tid = sys_time_register_timer((1. / 60.), NULL);
102  electrical_tid = sys_time_register_timer(0.1, NULL);
103  telemetry_tid = sys_time_register_timer((1. / 10.), NULL);
104 }
105 
106 
108 // PERIODIC
109 
111 {
113  main_periodic();
114  }
116  modules_periodic_task();
117  }
120  }
123  }
126  }
127 }
128 
130 {
131  /* Send status to AP */
133 
134  /* Handle Modems */
135  // TODO
136 }
137 
138 /* Checks the different safety cases and sets the correct FBW mode */
139 static void fbw_safety_check(void)
140 {
141  /* Safety logic */
142  bool ap_lost = (intermcu.status == INTERMCU_LOST);
143  bool rc_lost = (radio_control.status == RC_REALLY_LOST);
144 
145  // Both the RC and AP are lost
146  if (rc_lost && ap_lost) {
148  }
149  // RC is valid but lost AP
150  else if (!rc_lost && ap_lost) {
151  // Only crucial when AP was in control
152  if (fbw_mode == FBW_MODE_AUTO) {
154  }
155  }
156  // RC is lost but AP is valid
157  else if (rc_lost && !ap_lost) {
158 
159  // Lost RC while flying in manual trough FBW
160  if (fbw_mode == FBW_MODE_MANUAL) {
162  }
163  // Allways keep failsafe when RC is lost
164  else if (fbw_mode == FBW_MODE_FAILSAFE) {
165  // No change: failsafe stays failsafe
166  }
167  // Lost RC while in working Auto mode
168  else {
170  }
171  }
172 }
173 
174 /* Sets the actual actuator commands */
175 void main_periodic(void)
176 {
177  /* Inter-MCU watchdog */
179 
180  /* Safety check and set FBW mode */
182 
183 #ifdef BOARD_PX4IO
184  //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
185  //to prevent this situation:
186  if (intermcu.stable_px4_baud != PPRZ_BAUD) {
188  fbw_motors_on = false;
189  //signal to user whether fbw can be flashed:
190 #ifdef FBW_MODE_LED
191  LED_OFF(FBW_MODE_LED); // causes really fast blinking
192 #endif
193  }
194 #endif
195 
196  // TODO make module out of led blink?
197 #ifdef FBW_MODE_LED
198  static uint16_t dv = 0;
199  if (fbw_mode == FBW_MODE_FAILSAFE) {
200  if (!(dv++ % (PERIODIC_FREQUENCY / 20))) { LED_TOGGLE(FBW_MODE_LED);}
201  } else if (fbw_mode == FBW_MODE_MANUAL) {
202  if (!(dv++ % (PERIODIC_FREQUENCY))) { LED_TOGGLE(FBW_MODE_LED);}
203  } else if (fbw_mode == FBW_MODE_AUTO) {
204  LED_ON(FBW_MODE_LED);
205  }
206 #endif // FWB_MODE_LED
207 
208  /* Set failsafe commands */
209  if (fbw_mode == FBW_MODE_FAILSAFE) {
210  fbw_motors_on = false;
212  }
213 
214  /* If in auto copy autopilot motors on */
215  if (fbw_mode == FBW_MODE_AUTO) {
217  }
218 
219  /* Set actuators */
220  SetActuatorsFromCommands(commands, autopilot_get_mode());
221 
222  /* Periodic blinking */
223  RunOnceEvery(10, LED_PERIODIC());
224 }
225 
226 
228 // Event
229 
231 static void fbw_on_rc_frame(void)
232 {
233  /* get autopilot fbw mode as set by RADIO_MODE 3-way switch */
235 
236 #ifdef RADIO_KILL_SWITCH
237  static bool kill_state_init = false; // require a kill == off before enabling engines with kill == on
240  kill_state_init = true;
241  } else {
242  if (kill_state_init)
244  else
246  }
247 #else
249 #endif
250 
251  } else {
253  }
254 
255  /* Failsafe check if intermcu is lost while AP was in control */
256  if ((intermcu.status == INTERMCU_LOST) &&
257  (fbw_mode == FBW_MODE_AUTO)) {
259  }
260 
261  /* If the FBW is in control */
262  if (fbw_mode == FBW_MODE_MANUAL) {
263  fbw_motors_on = true;
265 #ifdef SetCommandsFromRC
266  SetCommandsFromRC(commands, radio_control.values);
267 #else
268 #warning "FBW: needs commands from RC in order to be useful."
269 #endif
270  }
271 
272  /* Forward radiocontrol to AP */
274 }
275 
277 static void fbw_on_ap_command(void)
278 {
279  // Only set the command from AP when we are in AUTO mode
280  if (fbw_mode == FBW_MODE_AUTO) {
282  }
283 }
284 
285 void main_event(void)
286 {
287  /* Event functions for mcu peripherals: i2c, usb_serial.. */
288  mcu_event();
289 
290  /* Handle RC */
292 
293  /* InterMCU (gives autopilot commands as output) */
295 
296  /* FBW modules */
297  modules_event_task();
298 }
unsigned short uint16_t
Definition: types.h:16
void intermcu_send_status(uint8_t mode)
Definition: intermcu_fbw.c:139
void mcu_init(void)
Microcontroller peripherals initialization.
Definition: mcu.c:83
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:285
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:277
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
#define mcu_int_enable()
Definition: mcu_arch.h:36
void telemetry_periodic(void)
Definition: main_fbw.c:129
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.
#define RadioControlEvent(_received_frame_handler)
Definition: dummy.h:28
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:110
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:60
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
pprz_t commands[COMMANDS_NB]
Storage of intermediate command values.
Definition: commands.c:30
#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:114
#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:248
#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
#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:139
static void fbw_on_rc_frame(void)
Callback when we received an RC frame.
Definition: main_fbw.c:231
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:175