Paparazzi UAS  v5.8.2_stable-0-g6260b7c
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 #include "generated/airframe.h"
35 
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"
44 #include "paparazzi.h"
45 #include "mcu_periph/i2c.h"
46 #include "mcu_periph/uart.h"
47 
48 #if PERIODIC_TELEMETRY
50 #endif
51 
52 #ifdef FBW_DATALINK
54 #endif
55 
57 
58 #include "inter_mcu.h"
59 #include "link_mcu.h"
60 
61 #ifdef USE_NPS
62 #include "nps_autopilot.h"
63 #endif
64 
71 
72 
74 
77 
78 /********** PERIODIC MESSAGES ************************************************/
79 #if PERIODIC_TELEMETRY
80 static void send_commands(struct transport_tx *trans, struct link_device *dev)
81 {
82  pprz_msg_send_COMMANDS(trans, dev, AC_ID, COMMANDS_NB, commands);
83 }
84 
85 #ifdef RADIO_CONTROL
86 static void send_fbw_status(struct transport_tx *trans, struct link_device *dev)
87 {
88  pprz_msg_send_FBW_STATUS(trans, dev, AC_ID,
90 }
91 
92 static void send_rc(struct transport_tx *trans, struct link_device *dev)
93 {
94  pprz_msg_send_RC(trans, dev, AC_ID, RADIO_CONTROL_NB_CHANNEL, radio_control.values);
95 }
96 
97 #else
98 static void send_fbw_status(struct transport_tx *trans, struct link_device *dev)
99 {
100  uint8_t dummy = 0;
101  pprz_msg_send_FBW_STATUS(trans, dev, AC_ID,
102  &dummy, &dummy, &fbw_mode, &electrical.vsupply, &electrical.current);
103 }
104 #endif
105 
106 #ifdef ACTUATORS
107 static void send_actuators(struct transport_tx *trans, struct link_device *dev)
108 {
109  pprz_msg_send_ACTUATORS(trans, dev, AC_ID , ACTUATORS_NB, actuators);
110 }
111 #endif
112 
113 #endif
114 
115 /********** INIT *************************************************************/
116 void init_fbw(void)
117 {
118 
119  mcu_init();
120 
121 #if !(DISABLE_ELECTRICAL)
122  electrical_init();
123 #endif
124 
125 #ifdef ACTUATORS
126  actuators_init();
127  /* Load the failsafe defaults */
129  fbw_new_actuators = 1;
130 #endif
131 #ifdef RADIO_CONTROL
133 #endif
134 #ifdef INTER_MCU
135  inter_mcu_init();
136 #endif
137 #if defined MCU_SPI_LINK || defined MCU_CAN_LINK
138  link_mcu_init();
139 #endif
140 #ifdef MCU_SPI_LINK
141  link_mcu_restart();
142 #endif
143 
145 
146  /**** start timers for periodic functions *****/
147  fbw_periodic_tid = sys_time_register_timer((1. / 60.), NULL);
149 
150 #ifndef SINGLE_MCU
151  mcu_int_enable();
152 #endif
153 
154 #if PERIODIC_TELEMETRY
155  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_FBW_STATUS, send_fbw_status);
157 #ifdef ACTUATORS
159 #endif
160 #ifdef RADIO_CONTROL
162 #endif
163 #endif
164 
165 }
166 
167 
168 static inline void set_failsafe_mode(void)
169 {
172  fbw_new_actuators = 1;
173 }
174 
175 
176 #ifdef RADIO_CONTROL
177 static inline void handle_rc_frame(void)
178 {
180  if (fbw_mode == FBW_MODE_MANUAL) {
181  SetCommandsFromRC(commands, radio_control.values);
182  fbw_new_actuators = 1;
183  }
184 }
185 #endif
186 
188 /********** EVENT ************************************************************/
189 
190 void event_task_fbw(void)
191 {
192 #ifdef RADIO_CONTROL
194 #endif
195 
196  /* event functions for mcu peripherals: i2c, usb_serial.. */
197  mcu_event();
198 
199 #ifdef INTER_MCU
200 #if defined MCU_SPI_LINK | defined MCU_UART_LINK
202 #endif /* MCU_SPI_LINK */
203 
204 
205 #if OUTBACK_CHALLENGE_VERY_DANGEROUS_RULE_NO_AP_MUST_FAILSAFE
206 #warning OUTBACK_CHALLENGE_VERY_DANGEROUS_RULE_NO_AP_MUST_FAILSAFE loose ap is forced crash
207  if (ap_ok) {
208  ap_has_been_ok = TRUE;
209  }
210 
211  if ((ap_has_been_ok) && (!ap_ok)) {
212  commands[COMMAND_FORCECRASH] = 9600;
213  }
214 #endif
215 
216  if (inter_mcu_received_ap) {
218  inter_mcu_event_task();
219  command_roll_trim = ap_state->command_roll_trim;
220  command_pitch_trim = ap_state->command_pitch_trim;
221  command_yaw_trim = ap_state->command_yaw_trim;
222 #if OUTBACK_CHALLENGE_DANGEROUS_RULE_RC_LOST_NO_AP
223  // LOST-RC: do NOT go to autonomous
224  // auto = stay in auto
225  // manual = stay in manual
226 #else
227  if (ap_ok && fbw_mode == FBW_MODE_FAILSAFE) {
229  }
230 #endif
231  if (fbw_mode == FBW_MODE_AUTO) {
232  SetCommands(ap_state->commands);
233  }
234 #ifdef SetApOnlyCommands
235  else {
236  SetApOnlyCommands(ap_state->commands);
237  }
238 #endif
239  fbw_new_actuators = 1;
240 
241 #ifdef SINGLE_MCU
242  inter_mcu_fill_fbw_state();
243 #endif
244  }
245 
246 #if OUTBACK_CHALLENGE_VERY_DANGEROUS_RULE_AP_CAN_FORCE_FAILSAFE
247 #warning DANGER DANGER DANGER DANGER: Outback Challenge Rule FORCE-CRASH-RULE: DANGER DANGER: AP is now capable to FORCE your FBW in failsafe mode EVEN IF RC IS NOT LOST: Consider the consequences.
248  // OUTBACK: JURY REQUEST FLIGHT TERMINATION
249  int crash = 0;
250  if (commands[COMMAND_FORCECRASH] >= 8000) {
252  crash = 1;
253  }
254 
255 #endif
256 #ifdef ACTUATORS
257  if (fbw_new_actuators > 0) {
258  pprz_t trimmed_commands[COMMANDS_NB];
259  int i;
260  for (i = 0; i < COMMANDS_NB; i++) { trimmed_commands[i] = commands[i]; }
261 
262 #ifdef COMMAND_ROLL
263  trimmed_commands[COMMAND_ROLL] += ChopAbs(command_roll_trim, MAX_PPRZ / 10);
264 #endif
265 #ifdef COMMAND_PITCH
266  trimmed_commands[COMMAND_PITCH] += ChopAbs(command_pitch_trim, MAX_PPRZ / 10);
267 #endif
268 #ifdef COMMAND_YAW
269  trimmed_commands[COMMAND_YAW] += ChopAbs(command_yaw_trim, MAX_PPRZ);
270 #endif
271 
272  SetActuatorsFromCommands(trimmed_commands, autopilot_mode);
273  fbw_new_actuators = 0;
274 #if OUTBACK_CHALLENGE_VERY_DANGEROUS_RULE_AP_CAN_FORCE_FAILSAFE
275  if (crash == 1) {
276  for (;;) {
277 #if FBW_DATALINK
279 #endif
280  }
281  }
282 #endif
283 
284  }
285 #endif
286 
287 
288 #ifdef MCU_SPI_LINK
289  if (link_mcu_received) {
291  inter_mcu_fill_fbw_state();
292  link_mcu_restart();
293  }
294 #endif /* MCU_SPI_LINK */
295 #endif /* INTER_MCU */
296 
297 #ifdef FBW_DATALINK
299 #endif
300 }
301 
302 
303 /************* PERIODIC ******************************************************/
305 {
306 
307 #ifdef FBW_DATALINK
309 #endif
310 
311 #ifdef RADIO_CONTROL
314 #if OUTBACK_CHALLENGE_DANGEROUS_RULE_RC_LOST_NO_AP
315 #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!!
317 #if OUTBACK_CHALLENGE_DANGEROUS_RULE_RC_LOST_NO_AP_IRREVERSIBLE
318 #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
319  commands[COMMAND_FORCECRASH] = 9600;
320 #endif
321 #else
323 #endif
324  }
325 #endif
326 
327 #ifdef INTER_MCU
328  inter_mcu_periodic_task();
329  if (fbw_mode == FBW_MODE_AUTO && !ap_ok) {
331  }
332 #endif
333 
334 #if defined MCU_UART_LINK || defined MCU_CAN_LINK
335  inter_mcu_fill_fbw_state();
337 #endif
338 
339 #if PERIODIC_TELEMETRY
340  periodic_telemetry_send_Fbw(DefaultPeriodic, &(DefaultChannel).trans_tx, &(DefaultDevice).device);
341 #endif
342 
343 }
344 
346 {
347 
350  }
351 
352 #if !(DISABLE_ELECTRICAL)
355  }
356 #endif
357 
358 }
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:71
uint8_t ap_has_been_ok
Definition: main_fbw.c:187
Communication between fbw and ap processes.
Generic transmission transport header.
Definition: transport.h:89
uint8_t autopilot_mode
Definition: autopilot.c:65
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:76
uint8_t status
Definition: radio_control.h:53
volatile bool_t inter_mcu_received_ap
Definition: inter_mcu.c:41
int16_t pprz_t
Definition: paparazzi.h:6
uint8_t tid_t
sys_time timer id type
Definition: sys_time.h:57
void event_task_fbw(void)
Definition: main_fbw.c:190
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:58
void periodic_task_fbw(void)
Definition: main_fbw.c:304
static void send_actuators(struct transport_tx *trans, struct link_device *dev)
void radio_control_init(void)
Definition: radio_control.c:32
#define FALSE
Definition: std.h:5
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:75
#define RadioControlEvent(_received_frame_handler)
Definition: dummy.h:28
Hardware independent API for actuators (servos, motor controllers).
#define TRUE
Definition: std.h:4
volatile uint8_t fbw_new_actuators
Definition: main_fbw.c:73
void electrical_init(void)
Definition: electrical.c:99
void handle_periodic_tasks_fbw(void)
Definition: main_fbw.c:345
Interface for electrical status: supply voltage, current, battery status, etc.
void init_fbw(void)
Definition: main_fbw.c:116
Architecture independent timing functions.
pprz_t command_roll_trim
Trim commands for roll, pitch and yaw.
Definition: main_fbw.c:68
void radio_control_periodic_task(void)
Definition: radio_control.c:46
#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
static void set_failsafe_mode(void)
Definition: main_fbw.c:168
pprz_t command_yaw_trim
Definition: main_fbw.c:70
int sys_time_register_timer(float duration, sys_time_cb cb)
Register a new system timer.
Definition: sys_time.c:37
static bool_t sys_time_check_and_ack_timer(tid_t id)
Check if timer has elapsed.
Definition: sys_time.h:111
#define RC_REALLY_LOST
Definition: radio_control.h:50
#define RADIO_CONTROL_NB_CHANNEL
Definition: spektrum_arch.h:34
void electrical_periodic(void)
Definition: electrical.c:121
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:69
#define RADIO_MODE
Definition: spektrum_arch.h:59
#define FBW_MODE_OF_PPRZ(mode)
Definition: main_fbw.h:38
uint8_t fbw_mode
Definition: main_fbw.c:56
Arch independent mcu ( Micro Controller Unit ) utilities.
#define mcu_int_enable()
Definition: mcu_arch.h:35
#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)
uint16_t vsupply
supply voltage in decivolts
Definition: electrical.h:48
void mcu_event(void)
MCU event functions.
Definition: mcu.c:192
struct Electrical electrical
Definition: electrical.c:65
#define MAX_PPRZ
Definition: paparazzi.h:8
FBW ( FlyByWire ) process API.
static void send_rc(struct transport_tx *trans, struct link_device *dev)
Definition: autopilot.c:237
static void handle_rc_frame(void)
Definition: main_fbw.c:177
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
uint8_t frame_rate
Definition: radio_control.h:56
Architecture independent I2C (Inter-Integrated Circuit Bus) API.
Fixedwing autopilot modes.