Paparazzi UAS  v5.12_stable-4-g9b43e9b
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) 2003-2010 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 
34 
35 #include "generated/airframe.h"
36 
37 #include "mcu.h"
38 #include "mcu_periph/sys_time.h"
39 #include "subsystems/commands.h"
40 #include "subsystems/actuators.h"
41 #include "subsystems/electrical.h"
43 #include "autopilot.h"
44 #include "paparazzi.h"
45 #include "mcu_periph/i2c.h"
46 #include "mcu_periph/uart.h"
47 
48 #ifdef USE_NPS
49 #include "nps_autopilot.h"
50 #endif
51 
52 #if PERIODIC_TELEMETRY
54 #endif
55 
56 #ifdef FBW_DATALINK
58 #endif
59 
60 #include "inter_mcu.h"
61 #include "link_mcu.h"
62 
64 
71 
73 
75 
78 
79 /********** RADIO CONTROL DEFINES ************************************************/
80 #ifdef RADIO_CONTROL
81 
84 void radio_lost_mode(void);
85 
86 #if !OUTBACK_CHALLENGE_DANGEROUS_RULE_RC_LOST_NO_AP && !OUTBACK_CHALLENGE_DANGEROUS_RULE_RC_LOST_NO_AP_IRREVERSIBLE
87 // default case
88 void radio_lost_mode(void)
89 {
91 }
92 #endif /* default */
93 
94 #if OUTBACK_CHALLENGE_DANGEROUS_RULE_RC_LOST_NO_AP
95 #warning WARNING DANGER: OUTBACK_CHALLENGE RULE RC_LOST_NO_AP defined. If you loose RC you will NOT go to automatically go to AUTO2 Anymore!!
96 static inline void set_failsafe_mode(void);
97 void radio_lost_mode(void)
98 {
99  set_failsafe_mode();
100 }
101 #endif /* OUTBACK_CHALLENGE_DANGEROUS_RULE_RC_LOST_NO_AP */
102 
103 #if OUTBACK_CHALLENGE_DANGEROUS_RULE_RC_LOST_NO_AP_IRREVERSIBLE
104 #warning WARNING DANGER: OUTBACK_CHALLENGE_DANGEROUS_RULE_RC_LOST_NO_AP_IRREVERSIBLE defined. If you ever temporarly lost RC while in manual, you will failsafe forever even if RC is restored
105 void radio_lost_mode(void)
106 {
107  commands[COMMAND_FORCECRASH] = 9600;
108 }
109 #endif /* OUTBACK_CHALLENGE_DANGEROUS_RULE_RC_LOST_NO_AP_IRREVERSIBLE */
110 
111 
112 static inline void handle_rc_frame(void)
113 {
115  if (fbw_mode == FBW_MODE_MANUAL) {
116  SetCommandsFromRC(commands, radio_control.values);
117  fbw_new_actuators = 1;
118  }
119 }
120 
122 {
124 }
125 
127 {
130  radio_lost_mode();
131  }
132 }
133 #else /* no RADIO_CONTROL */
134 void radio_control_event(void) {}
135 void radio_control_periodic_handle(void) {}
136 #endif /* RADIO_CONTROL */
137 
138 /********** FBW_DATALINK defines ************************************************/
139 #ifdef FBW_DATALINK
141 {
143 }
144 void fbw_datalink_event_handle(void)
145 {
147 }
148 #else /* no FBW_DATALINK */
151 #endif /* FBW_DATALINK */
152 
153 /********** ACTUATORS defines ************************************************/
154 #if defined ACTUATORS && defined INTER_MCU
155 void update_actuators(void);
156 
157 void update_actuators(void)
158 {
159  if (fbw_new_actuators > 0) {
160  pprz_t trimmed_commands[COMMANDS_NB];
161  int i;
162  for (i = 0; i < COMMANDS_NB; i++) {trimmed_commands[i] = commands[i];}
163 
164 #ifdef COMMAND_ROLL
165  trimmed_commands[COMMAND_ROLL] += ChopAbs(command_roll_trim, MAX_PPRZ / 10);
166 #endif /* COMMAND_ROLL */
167 
168 #ifdef COMMAND_PITCH
169  trimmed_commands[COMMAND_PITCH] += ChopAbs(command_pitch_trim, MAX_PPRZ / 10);
170 #endif /* COMMAND_PITCH */
171 
172 #ifdef COMMAND_YAW
173  trimmed_commands[COMMAND_YAW] += ChopAbs(command_yaw_trim, MAX_PPRZ);
174 #endif /* COMMAND_YAW */
175 
176  SetActuatorsFromCommands(trimmed_commands, autopilot_get_mode());
177  fbw_new_actuators = 0;
178  }
179 }
180 #else
181 void update_actuators(void) {};
182 #endif /* ACTUATORS && INTER_MCU */
183 
184 
185 /********** INTER_MCU defines ************************************************/
186 #ifdef INTER_MCU
187 // pre-and post functions
188 void pre_inter_mcu_received_ap(void);
189 void post_inter_mcu_received_ap(void);
190 
195 #define FBW_MODE_INTER_MCU_FAILSAFE FBW_MODE_AUTO
196 
197 #if !OUTBACK_CHALLENGE_VERY_DANGEROUS_RULE_NO_AP_MUST_FAILSAFE && !OUTBACK_CHALLENGE_VERY_DANGEROUS_RULE_AP_CAN_FORCE_FAILSAFE
198 void pre_inter_mcu_received_ap(void) {};
199 void post_inter_mcu_received_ap(void) {};
200 #endif /* DEFAULT */
201 
202 
203 #if OUTBACK_CHALLENGE_DANGEROUS_RULE_RC_LOST_NO_AP
204 #undef FBW_MODE_INTER_MCU_FAILSAFE
205 #define FBW_MODE_INTER_MCU_FAILSAFE fbw_mode
206 #endif /* OUTBACK_CHALLENGE_DANGEROUS_RULE_RC_LOST_NO_AP */
207 
208 
209 #if OUTBACK_CHALLENGE_VERY_DANGEROUS_RULE_NO_AP_MUST_FAILSAFE
210 #warning OUTBACK_CHALLENGE_VERY_DANGEROUS_RULE_NO_AP_MUST_FAILSAFE loose ap is forced crash
211 void pre_inter_mcu_received_ap(void)
212 {
213  if (ap_ok) {
214  ap_has_been_ok = true;
215  }
216  if ((ap_has_been_ok) && (!ap_ok)) {
217  commands[COMMAND_FORCECRASH] = 9600;
218  }
219 }
220 void post_inter_mcu_received_ap(void) {};
221 #endif /* OUTBACK_CHALLENGE_VERY_DANGEROUS_RULE_NO_AP_MUST_FAILSAFE */
222 
223 
224 #if OUTBACK_CHALLENGE_VERY_DANGEROUS_RULE_AP_CAN_FORCE_FAILSAFE
225 #warning DANGER DANGER DANGER DANGER: Outback Challenge Rule FORCE-CRASH-RULE: DANGER DANGER:
226 #warning AP is now capable to FORCE your FBW in failsafe mode EVEN IF RC IS NOT LOST: Consider the consequences.
227 // OUTBACK: JURY REQUEST FLIGHT TERMINATION
228 void pre_inter_mcu_received_ap(void) {};
229 void post_inter_mcu_received_ap(void)
230 {
231  if (commands[COMMAND_FORCECRASH] >= 8000) {
232  set_failsafe_mode();
233  }
234 }
235 #endif /* OUTBACK_CHALLENGE_VERY_DANGEROUS_RULE_AP_CAN_FORCE_FAILSAFE */
236 
237 
241 static inline void set_failsafe_mode(void)
242 {
245  fbw_new_actuators = 1;
246 }
247 
251 void inter_mcu_periodic_handle(void)
252 {
253  inter_mcu_periodic_task();
254  if (fbw_mode == FBW_MODE_AUTO && !ap_ok) {
255  set_failsafe_mode();
256  }
257 
258 #if defined MCU_UART_LINK || defined MCU_CAN_LINK
259  inter_mcu_fill_fbw_state();
261 #endif /* defined MCU_UART_LINK || defined MCU_CAN_LINK */
262 }
263 
267 void inter_mcu_event_handle(void)
268 {
269 #if defined MCU_SPI_LINK | defined MCU_UART_LINK
271 #endif /* MCU_SPI_LINK */
272 
273  pre_inter_mcu_received_ap();
274 
275  if (inter_mcu_received_ap) {
276  inter_mcu_received_ap = false;
277  inter_mcu_event_task();
278 
279  PPRZ_MUTEX_LOCK(ap_state_mtx);
280  command_roll_trim = ap_state->command_roll_trim;
281  command_pitch_trim = ap_state->command_pitch_trim;
282  command_yaw_trim = ap_state->command_yaw_trim;
283  if (ap_ok && fbw_mode == FBW_MODE_FAILSAFE) {
284  fbw_mode = FBW_MODE_INTER_MCU_FAILSAFE;
285  }
286  if (fbw_mode == FBW_MODE_AUTO) {
287  SetCommands(ap_state->commands);
288  } else {
289 #if SET_AP_ONLY_COMMANDS
290  SetApOnlyCommands(ap_state->commands);
291 #endif /* SET_AP_ONLY_COMMANDS */
292  }
293  fbw_new_actuators = 1;
294  PPRZ_MUTEX_UNLOCK(ap_state_mtx);
295 
296 #ifdef SINGLE_MCU
297  inter_mcu_fill_fbw_state();
298 #endif /* SINGLE_MCU - The buffer is filled even if the last receive was not correct */
299  }
300 
301  post_inter_mcu_received_ap();
302 
304 
305 #ifdef MCU_SPI_LINK
306  if (link_mcu_received) {
307  link_mcu_received = false;
308  inter_mcu_fill_fbw_state();
309  link_mcu_restart();
310  }
311 #endif /* MCU_SPI_LINK */
312 }
313 #else /* no INTER_MCU */
316 #endif /* INTER_MCU */
317 
318 /********** PERIODIC MESSAGES ************************************************/
319 #if PERIODIC_TELEMETRY
320 static void send_commands(struct transport_tx *trans, struct link_device *dev)
321 {
322  pprz_msg_send_COMMANDS(trans, dev, AC_ID, COMMANDS_NB, commands);
323 }
324 
325 #ifdef RADIO_CONTROL
326 static void send_fbw_status(struct transport_tx *trans, struct link_device *dev)
327 {
328  pprz_msg_send_FBW_STATUS(trans, dev, AC_ID,
330 }
331 
332 static void send_rc(struct transport_tx *trans, struct link_device *dev)
333 {
334  pprz_msg_send_RC(trans, dev, AC_ID, RADIO_CONTROL_NB_CHANNEL, radio_control.values);
335 }
336 
337 #else /* no RADIO_CONTROL */
338 static void send_fbw_status(struct transport_tx *trans, struct link_device *dev)
339 {
340  uint8_t dummy = 0;
341  pprz_msg_send_FBW_STATUS(trans, dev, AC_ID,
342  &dummy, &dummy, &fbw_mode, &electrical.vsupply, &electrical.current);
343 }
344 #endif /* RADIO_CONTROL */
345 
346 #ifdef ACTUATORS
347 static void send_actuators(struct transport_tx *trans, struct link_device *dev)
348 {
349  pprz_msg_send_ACTUATORS(trans, dev, AC_ID , ACTUATORS_NB, actuators);
350 }
351 #endif /* ACTUATORS */
352 
354 {
355  periodic_telemetry_send_Fbw(DefaultPeriodic, &(DefaultChannel).trans_tx, &(DefaultDevice).device);
356 }
357 
358 #else
359 void periodic_telemetry_handle(void) {}
360 #endif /* PERIODIC_TELEMETRY */
361 
362 /********** INIT *************************************************************/
363 void init_fbw(void)
364 {
365  mcu_init();
366 
367 #if !(DISABLE_ELECTRICAL)
368  electrical_init();
369 #endif
370 
371 #ifdef ACTUATORS
372  actuators_init();
373  /* Load the failsafe defaults */
375  fbw_new_actuators = 1;
376 #endif /* ACTUATORS */
377 
378 #ifdef RADIO_CONTROL
380 #endif /* RADIO_CONTROL */
381 
382 #ifdef INTER_MCU
383  inter_mcu_init();
384 #endif /* INTER_MCU */
385 
386 #if defined MCU_SPI_LINK || defined MCU_CAN_LINK
387  link_mcu_init();
388 #endif /* MCU_SPI_LINK || MCU_CAN_LINK */
389 
390 #ifdef MCU_SPI_LINK
391  link_mcu_restart();
392 #endif /* MCU_SPI_LINK */
393 
395 
396  /**** start timers for periodic functions *****/
397  fbw_periodic_tid = sys_time_register_timer((1. / 60.), NULL);
399 
400 #ifndef SINGLE_MCU
401  mcu_int_enable();
402 #endif
403 
404 #if PERIODIC_TELEMETRY
407 
408 #ifdef ACTUATORS
410 #endif /* ACTUATORS */
411 
412 #ifdef RADIO_CONTROL
414 #endif /* RADIO_CONTROL */
415 
416 #endif /* PERIODIC_TELEMETRY */
417 
418 }
419 
420 /********** EVENT ************************************************************/
421 
422 void event_task_fbw(void)
423 {
425 
426  /* event functions for mcu peripherals: i2c, usb_serial.. */
427  mcu_event();
428 
430 
432 }
433 
434 /************* PERIODIC ******************************************************/
436 {
438 
440 
442 
444 }
445 
447 {
450  }
451 
452 #if !(DISABLE_ELECTRICAL)
455  }
456 #endif
457 }
int32_t current
current in milliamps
Definition: electrical.h:49
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
void mcu_init(void)
Microcontroller peripherals initialization.
Definition: mcu.c:76
uint8_t ap_has_been_ok
Definition: main_fbw.c:74
Communication between fbw and ap processes.
void radio_control_event(void)
Definition: main_fbw.c:121
Periodic telemetry system header (includes downlink utility and generated code).
pprz_t command_pitch_trim
Definition: main_fbw.c:69
tid_t electrical_tid
id for electrical_periodic() timer
Definition: main_fbw.c:77
uint8_t status
Definition: radio_control.h:53
int16_t pprz_t
Definition: paparazzi.h:6
#define PPRZ_MUTEX_LOCK(_mtx)
Definition: pprz_mutex.h:46
volatile bool inter_mcu_received_ap
Definition: inter_mcu.c:41
void event_task_fbw(void)
Definition: main_fbw.c:422
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:58
#define mcu_int_enable()
Definition: mcu_arch.h:36
void periodic_task_fbw(void)
Definition: main_fbw.c:435
static void send_actuators(struct transport_tx *trans, struct link_device *dev)
void radio_control_init(void)
Definition: radio_control.c:32
const pprz_t commands_failsafe[COMMANDS_NB]
Definition: commands.c:31
struct ap_state * ap_state
Definition: inter_mcu.c:37
tid_t fbw_periodic_tid
id for periodic_task_fbw() timer
Definition: main_fbw.c:76
#define RadioControlEvent(_received_frame_handler)
Definition: dummy.h:28
Hardware independent API for actuators (servos, motor controllers).
volatile uint8_t fbw_new_actuators
Definition: main_fbw.c:72
void electrical_init(void)
Definition: electrical.c:99
void handle_periodic_tasks_fbw(void)
Definition: main_fbw.c:446
void update_actuators(void)
Definition: main_fbw.c:181
Interface for electrical status: supply voltage, current, battery status, etc.
void init_fbw(void)
Definition: main_fbw.c:363
Architecture independent timing functions.
pprz_t command_roll_trim
Trim commands for roll, pitch and yaw.
Definition: main_fbw.c:68
void fbw_datalink_periodic_handle(void)
Definition: main_fbw.c:149
void radio_control_periodic_task(void)
Definition: radio_control.c:46
void radio_lost_mode(void)
Defines behavior when the RC is lost, default goes to AUTO.
Definition: main_fbw.c:88
void radio_control_periodic_handle(void)
Definition: main_fbw.c:126
int8_t tid_t
sys_time timer id type
Definition: sys_time.h:60
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
Hardware independent code for commands handling.
struct RadioControl radio_control
Definition: radio_control.c:30
pprz_t command_yaw_trim
Definition: main_fbw.c:70
#define RADIO_CONTROL_NB_CHANNEL
Definition: spektrum_arch.h:34
#define RC_REALLY_LOST
Definition: radio_control.h:50
void fbw_datalink_event_handle(void)
Definition: main_fbw.c:150
void electrical_periodic(void)
Definition: electrical.c:121
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
Core autopilot interface common to all firmwares.
#define RADIO_MODE
Definition: spektrum_arch.h:59
static void send_rc(struct transport_tx *trans, struct link_device *dev)
Definition: main_fbw.c:332
#define FBW_MODE_OF_PPRZ(mode)
Definition: main_fbw.h:37
void inter_mcu_periodic_handle(void)
Definition: main_fbw.c:314
uint8_t fbw_mode
Definition: main_fbw.c:63
Arch independent mcu ( Micro Controller Unit ) utilities.
#define SetCommands(t)
Definition: commands.h:41
unsigned char uint8_t
Definition: types.h:14
pprz_t commands[COMMANDS_NB]
Storage of intermediate command values.
Definition: commands.c:30
static void send_commands(struct transport_tx *trans, struct link_device *dev)
Definition: main_fbw.c:320
static void send_fbw_status(struct transport_tx *trans, struct link_device *dev)
Definition: main_fbw.c:326
static bool sys_time_check_and_ack_timer(tid_t id)
Check if timer has elapsed.
Definition: sys_time.h:114
uint16_t vsupply
supply voltage in decivolts
Definition: electrical.h:48
void inter_mcu_event_handle(void)
Definition: main_fbw.c:315
void mcu_event(void)
MCU event functions.
Definition: mcu.c:231
struct Electrical electrical
Definition: electrical.c:65
#define MAX_PPRZ
Definition: paparazzi.h:8
FBW ( FlyByWire ) process API.
static void handle_rc_frame(void)
Definition: main_fbw.c:112
#define PPRZ_MUTEX_UNLOCK(_mtx)
Definition: pprz_mutex.h:47
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
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:179
void periodic_telemetry_handle(void)
Definition: main_fbw.c:353
uint8_t frame_rate
Definition: radio_control.h:56
Architecture independent I2C (Inter-Integrated Circuit Bus) API.