Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
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 void update_actuators(void);
155 #if defined ACTUATORS && defined INTER_MCU
156 void update_actuators(void)
157 {
158  if (fbw_new_actuators > 0) {
159  pprz_t trimmed_commands[COMMANDS_NB];
160  int i;
161  for (i = 0; i < COMMANDS_NB; i++) {trimmed_commands[i] = commands[i];}
162 
163 #ifdef COMMAND_ROLL
164  trimmed_commands[COMMAND_ROLL] += ClipAbs(command_roll_trim, MAX_PPRZ / 10);
165 #endif /* COMMAND_ROLL */
166 
167 #ifdef COMMAND_PITCH
168  trimmed_commands[COMMAND_PITCH] += ClipAbs(command_pitch_trim, MAX_PPRZ / 10);
169 #endif /* COMMAND_PITCH */
170 
171 #ifdef COMMAND_YAW
172  trimmed_commands[COMMAND_YAW] += ClipAbs(command_yaw_trim, MAX_PPRZ);
173 #endif /* COMMAND_YAW */
174 
175  SetActuatorsFromCommands(trimmed_commands, autopilot_get_mode());
176  fbw_new_actuators = 0;
177  }
178 }
179 #else
180 void update_actuators(void) {};
181 #endif /* ACTUATORS && INTER_MCU */
182 
183 
184 /********** INTER_MCU defines ************************************************/
185 #ifdef INTER_MCU
186 // pre-and post functions
187 void pre_inter_mcu_received_ap(void);
188 void post_inter_mcu_received_ap(void);
189 
194 #define FBW_MODE_INTER_MCU_FAILSAFE FBW_MODE_AUTO
195 
196 #if !OUTBACK_CHALLENGE_VERY_DANGEROUS_RULE_NO_AP_MUST_FAILSAFE && !OUTBACK_CHALLENGE_VERY_DANGEROUS_RULE_AP_CAN_FORCE_FAILSAFE
197 void pre_inter_mcu_received_ap(void) {};
198 void post_inter_mcu_received_ap(void) {};
199 #endif /* DEFAULT */
200 
201 
202 #if OUTBACK_CHALLENGE_DANGEROUS_RULE_RC_LOST_NO_AP
203 #undef FBW_MODE_INTER_MCU_FAILSAFE
204 #define FBW_MODE_INTER_MCU_FAILSAFE fbw_mode
205 #endif /* OUTBACK_CHALLENGE_DANGEROUS_RULE_RC_LOST_NO_AP */
206 
207 
208 #if OUTBACK_CHALLENGE_VERY_DANGEROUS_RULE_NO_AP_MUST_FAILSAFE
209 #warning OUTBACK_CHALLENGE_VERY_DANGEROUS_RULE_NO_AP_MUST_FAILSAFE loose ap is forced crash
210 void pre_inter_mcu_received_ap(void)
211 {
212  if (ap_ok) {
213  ap_has_been_ok = true;
214  }
215  if ((ap_has_been_ok) && (!ap_ok)) {
216  commands[COMMAND_FORCECRASH] = 9600;
217  }
218 }
219 void post_inter_mcu_received_ap(void) {};
220 #endif /* OUTBACK_CHALLENGE_VERY_DANGEROUS_RULE_NO_AP_MUST_FAILSAFE */
221 
222 
223 #if OUTBACK_CHALLENGE_VERY_DANGEROUS_RULE_AP_CAN_FORCE_FAILSAFE
224 #warning DANGER DANGER DANGER DANGER: Outback Challenge Rule FORCE-CRASH-RULE: DANGER DANGER:
225 #warning AP is now capable to FORCE your FBW in failsafe mode EVEN IF RC IS NOT LOST: Consider the consequences.
226 // OUTBACK: JURY REQUEST FLIGHT TERMINATION
227 void pre_inter_mcu_received_ap(void) {};
228 void post_inter_mcu_received_ap(void)
229 {
230  if (commands[COMMAND_FORCECRASH] >= 8000) {
231  set_failsafe_mode();
232  }
233 }
234 #endif /* OUTBACK_CHALLENGE_VERY_DANGEROUS_RULE_AP_CAN_FORCE_FAILSAFE */
235 
236 
240 static inline void set_failsafe_mode(void)
241 {
244  fbw_new_actuators = 1;
245 }
246 
250 void inter_mcu_periodic_handle(void)
251 {
252  inter_mcu_periodic_task();
253  if (fbw_mode == FBW_MODE_AUTO && !ap_ok) {
254  set_failsafe_mode();
255  }
256 
257 #if defined MCU_UART_LINK || defined MCU_CAN_LINK
258  inter_mcu_fill_fbw_state();
260 #endif /* defined MCU_UART_LINK || defined MCU_CAN_LINK */
261 }
262 
266 void inter_mcu_event_handle(void)
267 {
268 #if defined MCU_SPI_LINK | defined MCU_UART_LINK
270 #endif /* MCU_SPI_LINK */
271 
272  pre_inter_mcu_received_ap();
273 
274  if (inter_mcu_received_ap) {
275  inter_mcu_received_ap = false;
276  inter_mcu_event_task();
277 
278  PPRZ_MUTEX_LOCK(ap_state_mtx);
279  command_roll_trim = ap_state->command_roll_trim;
280  command_pitch_trim = ap_state->command_pitch_trim;
281  command_yaw_trim = ap_state->command_yaw_trim;
282  if (ap_ok && fbw_mode == FBW_MODE_FAILSAFE) {
283  fbw_mode = FBW_MODE_INTER_MCU_FAILSAFE;
284  }
285  if (fbw_mode == FBW_MODE_AUTO) {
286  SetCommands(ap_state->commands);
287  } else {
288 #if SET_AP_ONLY_COMMANDS
289  SetApOnlyCommands(ap_state->commands);
290 #endif /* SET_AP_ONLY_COMMANDS */
291  }
292  fbw_new_actuators = 1;
293  PPRZ_MUTEX_UNLOCK(ap_state_mtx);
294 
295 #ifdef SINGLE_MCU
296  inter_mcu_fill_fbw_state();
297 #endif /* SINGLE_MCU - The buffer is filled even if the last receive was not correct */
298  }
299 
300  post_inter_mcu_received_ap();
301 
303 
304 #ifdef MCU_SPI_LINK
305  if (link_mcu_received) {
306  link_mcu_received = false;
307  inter_mcu_fill_fbw_state();
308  link_mcu_restart();
309  }
310 #endif /* MCU_SPI_LINK */
311 }
312 #else /* no INTER_MCU */
315 #endif /* INTER_MCU */
316 
317 /********** PERIODIC MESSAGES ************************************************/
318 #if PERIODIC_TELEMETRY
319 static void send_commands(struct transport_tx *trans, struct link_device *dev)
320 {
321  pprz_msg_send_COMMANDS(trans, dev, AC_ID, COMMANDS_NB, commands);
322 }
323 
324 #ifdef RADIO_CONTROL
325 static void send_fbw_status(struct transport_tx *trans, struct link_device *dev)
326 {
327  pprz_msg_send_FBW_STATUS(trans, dev, AC_ID,
329 }
330 
331 static void send_rc(struct transport_tx *trans, struct link_device *dev)
332 {
333  pprz_msg_send_RC(trans, dev, AC_ID, RADIO_CONTROL_NB_CHANNEL, radio_control.values);
334 }
335 
336 #else /* no RADIO_CONTROL */
337 static void send_fbw_status(struct transport_tx *trans, struct link_device *dev)
338 {
339  uint8_t dummy = 0;
340  pprz_msg_send_FBW_STATUS(trans, dev, AC_ID,
341  &dummy, &dummy, &fbw_mode, &electrical.vsupply, &electrical.current);
342 }
343 #endif /* RADIO_CONTROL */
344 
345 #ifdef ACTUATORS
346 static void send_actuators(struct transport_tx *trans, struct link_device *dev)
347 {
348  pprz_msg_send_ACTUATORS(trans, dev, AC_ID , ACTUATORS_NB, actuators);
349 }
350 #endif /* ACTUATORS */
351 
353 {
354  periodic_telemetry_send_Fbw(DefaultPeriodic, &(DefaultChannel).trans_tx, &(DefaultDevice).device);
355 }
356 
357 #else
358 void periodic_telemetry_handle(void) {}
359 #endif /* PERIODIC_TELEMETRY */
360 
361 /********** INIT *************************************************************/
362 void init_fbw(void)
363 {
364  mcu_init();
365 
366 #if !(DISABLE_ELECTRICAL)
367  electrical_init();
368 #endif
369 
370 #ifdef ACTUATORS
371  actuators_init();
372  /* Load the failsafe defaults */
374  fbw_new_actuators = 1;
375 #endif /* ACTUATORS */
376 
377 #ifdef RADIO_CONTROL
379 #endif /* RADIO_CONTROL */
380 
381 #ifdef INTER_MCU
382  inter_mcu_init();
383 #endif /* INTER_MCU */
384 
385 #if defined MCU_SPI_LINK || defined MCU_CAN_LINK
386  link_mcu_init();
387 #endif /* MCU_SPI_LINK || MCU_CAN_LINK */
388 
389 #ifdef MCU_SPI_LINK
390  link_mcu_restart();
391 #endif /* MCU_SPI_LINK */
392 
394 
395  /**** start timers for periodic functions *****/
396  fbw_periodic_tid = sys_time_register_timer((1. / 60.), NULL);
398 
399 #ifndef SINGLE_MCU
400  mcu_int_enable();
401 #endif
402 
403 #if PERIODIC_TELEMETRY
406 
407 #ifdef ACTUATORS
409 #endif /* ACTUATORS */
410 
411 #ifdef RADIO_CONTROL
413 #endif /* RADIO_CONTROL */
414 
415 #endif /* PERIODIC_TELEMETRY */
416 
417 }
418 
419 /********** EVENT ************************************************************/
420 
421 void event_task_fbw(void)
422 {
424 
425  /* event functions for mcu peripherals: i2c, usb_serial.. */
426  mcu_event();
427 
429 
431 }
432 
433 /************* PERIODIC ******************************************************/
435 {
437 
439 
441 
443 }
444 
446 {
449  }
450 
451 #if !(DISABLE_ELECTRICAL)
454  }
455 #endif
456 }
radio_control.h
electrical.h
MAX_PPRZ
#define MAX_PPRZ
Definition: paparazzi.h:8
init_fbw
void init_fbw(void)
Definition: main_fbw.c:362
command_roll_trim
pprz_t command_roll_trim
Trim commands for roll, pitch and yaw.
Definition: main_fbw.c:68
radio_control_periodic_task
void radio_control_periodic_task(void)
Definition: radio_control.c:46
radio_control_periodic_handle
void radio_control_periodic_handle(void)
Definition: main_fbw.c:126
mcu_int_enable
#define mcu_int_enable()
Definition: mcu_arch.h:36
FBW_MODE_FAILSAFE
@ FBW_MODE_FAILSAFE
Definition: main_fbw.h:64
send_rc
static void send_rc(struct transport_tx *trans, struct link_device *dev)
Definition: main_fbw.c:331
fbw_mode
uint8_t fbw_mode
Definition: main_fbw.c:63
inter_mcu_received_ap
volatile bool inter_mcu_received_ap
Definition: inter_mcu.c:41
mcu_event
void mcu_event(void)
MCU event functions.
Definition: mcu.c:258
inter_mcu.h
RADIO_MODE
#define RADIO_MODE
Definition: intermcu_ap.h:44
send_commands
static void send_commands(struct transport_tx *trans, struct link_device *dev)
Definition: main_fbw.c:319
update_actuators
void update_actuators(void)
Definition: main_fbw.c:180
ap_has_been_ok
uint8_t ap_has_been_ok
Definition: main_fbw.c:74
PPRZ_MUTEX_UNLOCK
#define PPRZ_MUTEX_UNLOCK(_mtx)
Definition: pprz_mutex.h:47
commands_failsafe
const pprz_t commands_failsafe[COMMANDS_NB]
Definition: commands.c:31
nps_autopilot.h
Electrical::vsupply
float vsupply
supply voltage in V
Definition: electrical.h:45
paparazzi.h
SetCommands
#define SetCommands(t)
Definition: commands.h:41
radio_lost_mode
void radio_lost_mode(void)
Defines behavior when the RC is lost, default goes to AUTO.
Definition: main_fbw.c:88
RadioControl::frame_rate
uint8_t frame_rate
Definition: radio_control.h:67
command_yaw_trim
pprz_t command_yaw_trim
Definition: main_fbw.c:70
telemetry.h
uart.h
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
pprz_t
int16_t pprz_t
Definition: paparazzi.h:6
event_task_fbw
void event_task_fbw(void)
Definition: main_fbw.c:421
mcu.h
Arch independent mcu ( Micro Controller Unit ) utilities.
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
electrical_periodic
void electrical_periodic(void)
Definition: electrical.c:123
sys_time.h
Architecture independent timing functions.
uint8_t
unsigned char uint8_t
Definition: types.h:14
RADIO_CONTROL_NB_CHANNEL
#define RADIO_CONTROL_NB_CHANNEL
Definition: intermcu_ap.h:49
register_periodic_telemetry
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
RadioControl::status
uint8_t status
Definition: radio_control.h:64
fbw_new_actuators
volatile uint8_t fbw_new_actuators
Definition: main_fbw.c:72
fbw_periodic_tid
tid_t fbw_periodic_tid
id for periodic_task_fbw() timer
Definition: main_fbw.c:76
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:60
autopilot.h
command_pitch_trim
pprz_t command_pitch_trim
Definition: main_fbw.c:69
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
PPRZ_MUTEX_LOCK
#define PPRZ_MUTEX_LOCK(_mtx)
Definition: pprz_mutex.h:46
radio_control_event
void radio_control_event(void)
Definition: main_fbw.c:121
commands
static const ShellCommand commands[]
Definition: shell_arch.c:71
FBW_MODE_MANUAL
@ FBW_MODE_MANUAL
Definition: main_fbw.h:64
RadioControlEvent
#define RadioControlEvent(_received_frame_handler)
Definition: cc2500_paparazzi.h:37
handle_rc_frame
static void handle_rc_frame(void)
Definition: main_fbw.c:112
FBW_MODE_AUTO
@ FBW_MODE_AUTO
Definition: main_fbw.h:64
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
send_actuators
static void send_actuators(struct transport_tx *trans, struct link_device *dev)
Definition: demo_ahrs_actuators.c:155
periodic_telemetry_handle
void periodic_telemetry_handle(void)
Definition: main_fbw.c:352
FBW_MODE_OF_PPRZ
#define FBW_MODE_OF_PPRZ(mode)
Definition: main_fbw.h:37
ap_state
struct ap_state * ap_state
Definition: inter_mcu.c:37
fbw_datalink_event_handle
void fbw_datalink_event_handle(void)
Definition: main_fbw.c:150
commands.h
Hardware independent code for commands handling.
electrical
struct Electrical electrical
Definition: electrical.c:66
autopilot_get_mode
uint8_t autopilot_get_mode(void)
get autopilot mode
Definition: autopilot.c:184
inter_mcu_periodic_handle
void inter_mcu_periodic_handle(void)
Definition: main_fbw.c:313
periodic_task_fbw
void periodic_task_fbw(void)
Definition: main_fbw.c:434
main_fbw.h
handle_periodic_tasks_fbw
void handle_periodic_tasks_fbw(void)
Definition: main_fbw.c:445
i2c.h
send_fbw_status
static void send_fbw_status(struct transport_tx *trans, struct link_device *dev)
Definition: main_fbw.c:325
mcu_init
void mcu_init(void)
Microcontroller peripherals initialization.
Definition: mcu.c:87
inter_mcu_event_handle
void inter_mcu_event_handle(void)
Definition: main_fbw.c:314
RC_REALLY_LOST
#define RC_REALLY_LOST
Definition: radio_control.h:58
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
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:114
Electrical::current
float current
current in A
Definition: electrical.h:46
radio_control
struct RadioControl radio_control
Definition: radio_control.c:30
actuators.h
RadioControl::values
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:69
fbw_datalink_periodic_handle
void fbw_datalink_periodic_handle(void)
Definition: main_fbw.c:149