Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures 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 "fbw_downlink.h"
45 #include "paparazzi.h"
46 #include "mcu_periph/i2c.h"
47 
48 #ifdef MCU_SPI_LINK
49 #include "link_mcu_spi.h"
50 #endif
51 
52 #ifdef MCU_UART_LINK
53 #include "link_mcu_usart.h"
54 #endif
55 
57 
58 #include "inter_mcu.h"
59 
60 
67 
68 
70 
73 
74 /********** INIT *************************************************************/
75 void init_fbw( void ) {
76 
77  mcu_init();
78 
79 #if !(DISABLE_ELECTRICAL)
81 #endif
82 
83 #ifdef ACTUATORS
85  /* Load the failsafe defaults */
88 #endif
89 #ifdef RADIO_CONTROL
91 #endif
92 #ifdef INTER_MCU
93  inter_mcu_init();
94 #endif
95 #ifdef MCU_SPI_LINK
96  link_mcu_init();
97  link_mcu_restart();
98 #endif
99 
101 
102  /**** start timers for periodic functions *****/
103  fbw_periodic_tid = sys_time_register_timer((1./60.), NULL);
105 
106 #ifndef SINGLE_MCU
107  mcu_int_enable();
108 #endif
109 }
110 
111 
112 static inline void set_failsafe_mode( void ) {
115  fbw_new_actuators = 1;
116 }
117 
118 
119 #ifdef RADIO_CONTROL
120 static inline void handle_rc_frame( void ) {
122  if (fbw_mode == FBW_MODE_MANUAL)
123  {
124  SetCommandsFromRC(commands, radio_control.values);
125  fbw_new_actuators = 1;
126  }
127 }
128 #endif
129 
130 
131 /********** EVENT ************************************************************/
132 
133 void event_task_fbw( void) {
134 #ifdef RADIO_CONTROL
136 #endif
137 
138  i2c_event();
139 
140 #ifdef INTER_MCU
141 #if defined MCU_SPI_LINK | defined MCU_UART_LINK
143 #endif /* MCU_SPI_LINK */
144 
145 
146  if (inter_mcu_received_ap) {
148  inter_mcu_event_task();
149  command_roll_trim = ap_state->command_roll_trim;
150  command_pitch_trim = ap_state->command_pitch_trim;
151  command_yaw_trim = ap_state->command_yaw_trim;
152 #ifndef OUTBACK_CHALLENGE_DANGEROUS_RULE_RC_LOST_NO_AP
153  if (ap_ok && fbw_mode == FBW_MODE_FAILSAFE) {
155  }
156 #endif
157  if (fbw_mode == FBW_MODE_AUTO) {
158  SetCommands(ap_state->commands);
159  }
160 #ifdef SetApOnlyCommands
161  else
162  {
163  SetApOnlyCommands(ap_state->commands);
164  }
165 #endif
166  fbw_new_actuators = 1;
167 
168 #ifdef SINGLE_MCU
169  inter_mcu_fill_fbw_state();
170 #endif
171  }
172 
173 #if OUTBACK_CHALLENGE_VERY_DANGEROUS_RULE_AP_CAN_FORCE_FAILSAFE
174 #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.
175 
176  int crash = 0;
177  if (commands[COMMAND_FORCECRASH] >= 8000)
178  {
180  crash = 1;
181  }
182 
183 #endif
184 #ifdef ACTUATORS
185  if (fbw_new_actuators > 0)
186  {
187  pprz_t trimmed_commands[COMMANDS_NB];
188  int i;
189  for(i = 0; i < COMMANDS_NB; i++) trimmed_commands[i] = commands[i];
190 
191  #ifdef COMMAND_ROLL
192  trimmed_commands[COMMAND_ROLL] += ChopAbs(command_roll_trim, MAX_PPRZ/10);
193  #endif
194  #ifdef COMMAND_PITCH
195  trimmed_commands[COMMAND_PITCH] += ChopAbs(command_pitch_trim, MAX_PPRZ/10);
196  #endif
197  #ifdef COMMAND_YAW
198  trimmed_commands[COMMAND_YAW] += ChopAbs(command_yaw_trim, MAX_PPRZ);
199  #endif
200 
201  SetActuatorsFromCommands(trimmed_commands, autopilot_mode);
202  fbw_new_actuators = 0;
203  #if OUTBACK_CHALLENGE_VERY_DANGEROUS_RULE_AP_CAN_FORCE_FAILSAFE
204  if (crash == 1)
205  {
206  for (;;) {}
207  }
208  #endif
209 
210  }
211 #endif
212 
213 
214 #ifdef MCU_SPI_LINK
215  if (link_mcu_received) {
217  inter_mcu_fill_fbw_state();
218  link_mcu_restart();
219  }
220 #endif /* MCU_SPI_LINK */
221 #endif /* INTER_MCU */
222 
223 }
224 
225 
226 /************* PERIODIC ******************************************************/
227 void periodic_task_fbw( void ) {
228 
229 #ifdef RADIO_CONTROL
232 #ifdef OUTBACK_CHALLENGE_DANGEROUS_RULE_RC_LOST_NO_AP
233 #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!!
235 #else
237 #endif
238  }
239 #endif
240 
241 #ifdef INTER_MCU
242  inter_mcu_periodic_task();
243  if (fbw_mode == FBW_MODE_AUTO && !ap_ok)
244  {
246  }
247 #endif
248 
249 #ifdef MCU_UART_LINK
250  inter_mcu_fill_fbw_state();
252 #endif
253 
254 #ifdef DOWNLINK
256 #endif
257 
258 }
259 
261 
264 
265 #if !(DISABLE_ELECTRICAL)
268 #endif
269 
270 }
FBW ( FlyByWire ) process API.
void event_task_fbw(void)
Definition: main_fbw.c:133
void periodic_task_fbw(void)
Definition: main_fbw.c:227
void mcu_init(void)
Microcontroller peripherals initialization.
Definition: mcu.c:61
void electrical_init(void)
static void radio_control_periodic_task(void)
Definition: radio_control.h:75
tid_t fbw_periodic_tid
id for periodic_task_fbw() timer
Definition: main_fbw.c:71
volatile uint8_t fbw_new_actuators
Definition: main_fbw.c:69
uint8_t autopilot_mode
Definition: autopilot.c:39
volatile bool_t inter_mcu_received_ap
Definition: inter_mcu.c:37
pprz_t command_roll_trim
Trim commands for roll, pitch and yaw.
Definition: main_fbw.c:64
int16_t pprz_t
Definition: paparazzi.h:6
void init_fbw(void)
Definition: main_fbw.c:75
uint8_t tid_t
sys_time timer id type
Definition: sys_time.h:57
#define FBW_MODE_OF_PPRZ(mode)
Definition: main_fbw.h:38
#define FALSE
Definition: imu_chimu.h:141
const pprz_t commands_failsafe[COMMANDS_NB]
Definition: commands.c:31
struct ap_state * ap_state
Definition: inter_mcu.c:33
#define mcu_int_enable()
Definition: mcu_arch.h:37
#define RadioControlEvent(_received_frame_handler)
Definition: dummy.h:28
static void set_failsafe_mode(void)
Definition: main_fbw.c:112
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:55
uint8_t fbw_mode
Definition: main_fbw.c:56
Architecture independent timing functions.
#define FBW_MODE_AUTO
Definition: main_fbw.h:36
pprz_t command_pitch_trim
Definition: main_fbw.c:65
static void radio_control_init(void)
Definition: radio_control.h:62
#define SetActuatorsFromCommands(commands, AP_MODE)
Definition: actuators_at.h:34
Hardware independent code for commands handling.
struct RadioControl radio_control
Definition: radio_control.c:25
Hardware independent API for actuators (servos, motor controllers).
tid_t electrical_tid
id for electrical_periodic() timer
Definition: main_fbw.c:72
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:47
uint8_t status
Definition: radio_control.h:50
#define RADIO_MODE
Definition: spektrum_arch.h:61
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 handle_rc_frame(void)
Definition: main_fbw.c:120
pprz_t command_yaw_trim
Definition: main_fbw.c:66
void electrical_periodic(void)
#define FBW_MODE_FAILSAFE
Definition: main_fbw.h:37
void actuators_init(void)
Definition: actuators_at.c:32
void i2c_event(void)
Definition: i2c_arch.c:364
#define MAX_PPRZ
Definition: paparazzi.h:8
void handle_periodic_tasks_fbw(void)
Definition: main_fbw.c:260
#define FBW_MODE_MANUAL
Fly by wire modes.
Definition: main_fbw.h:35
Architecture independent I2C (Inter-Integrated Circuit Bus) API.
Fixedwing autopilot modes.