Paparazzi UAS  v4.2.2_stable-4-gcc32f65
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
main_fbw.c
Go to the documentation of this file.
1 /*
2  * Paparazzi $Id$
3  *
4  * Copyright (C) 2003-2010 The Paparazzi Team
5  *
6  * This file is part of Paparazzi.
7  *
8  * Paparazzi is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2, or (at your option)
11  * any later version.
12  *
13  * Paparazzi is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with Paparazzi; see the file COPYING. If not, write to
20  * the Free Software Foundation, 59 Temple Place - Suite 330,
21  * Boston, MA 02111-1307, USA.
22  *
23  */
24 
34 #include "generated/airframe.h"
35 
37 #include "mcu.h"
38 #include "mcu_periph/sys_time.h"
39 #include "commands.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 
62 
65 
66 /********** INIT *************************************************************/
67 void init_fbw( void ) {
68 
69  mcu_init();
70 
71 #if !(DISABLE_ELECTRICAL)
73 #endif
74 
75 #ifdef ACTUATORS
77  /* Load the failsafe defaults */
80 #endif
81 #ifdef RADIO_CONTROL
82  radio_control_init();
83 #endif
84 #ifdef INTER_MCU
85  inter_mcu_init();
86 #endif
87 #ifdef MCU_SPI_LINK
88  link_mcu_restart();
89 #endif
90 
92 
93  /**** start timers for periodic functions *****/
94  fbw_periodic_tid = sys_time_register_timer((1./60.), NULL);
96 
97 #ifndef SINGLE_MCU
99 #endif
100 }
101 
102 
103 static inline void set_failsafe_mode( void ) {
106  fbw_new_actuators = 1;
107 }
108 
109 
110 #ifdef RADIO_CONTROL
111 static inline void handle_rc_frame( void ) {
113  if (fbw_mode == FBW_MODE_MANUAL)
114  {
115  SetCommandsFromRC(commands, radio_control.values);
116  fbw_new_actuators = 1;
117  }
118 }
119 #endif
120 
121 
122 /********** EVENT ************************************************************/
123 
124 void event_task_fbw( void) {
125 #ifdef RADIO_CONTROL
126  RadioControlEvent(handle_rc_frame);
127 #endif
128 
129  i2c_event();
130 
131 #ifdef INTER_MCU
132 #if defined MCU_SPI_LINK | defined MCU_UART_LINK
134 #endif /* MCU_SPI_LINK */
135 
136 
137  if (inter_mcu_received_ap) {
139  inter_mcu_event_task();
140  command_roll_trim = ap_state->command_roll_trim;
141  command_pitch_trim = ap_state->command_pitch_trim;
142  command_yaw_trim = ap_state->command_yaw_trim;
143 #ifndef OUTBACK_CHALLENGE_DANGEROUS_RULE_RC_LOST_NO_AP
144  if (ap_ok && fbw_mode == FBW_MODE_FAILSAFE) {
146  }
147 #endif
148  if (fbw_mode == FBW_MODE_AUTO) {
149  SetCommands(ap_state->commands);
150  }
151 #ifdef SetApOnlyCommands
152  else
153  {
154  SetApOnlyCommands(ap_state->commands);
155  }
156 #endif
157  fbw_new_actuators = 1;
158 
159 #ifdef SINGLE_MCU
160  inter_mcu_fill_fbw_state();
161 #endif
162  }
163 
164 #if OUTBACK_CHALLENGE_VERY_DANGEROUS_RULE_AP_CAN_FORCE_FAILSAFE
165 #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.
166 
167  int crash = 0;
168  if (commands[COMMAND_FORCECRASH] >= 8000)
169  {
171  crash = 1;
172  }
173 
174 #endif
175 #ifdef ACTUATORS
176  if (fbw_new_actuators > 0)
177  {
178  pprz_t trimmed_commands[COMMANDS_NB];
179  int i;
180  for(i = 0; i < COMMANDS_NB; i++) trimmed_commands[i] = commands[i];
181 
182  #ifdef COMMAND_ROLL
183  trimmed_commands[COMMAND_ROLL] += ChopAbs(command_roll_trim, MAX_PPRZ/10);
184  #endif
185  #ifdef COMMAND_PITCH
186  trimmed_commands[COMMAND_PITCH] += ChopAbs(command_pitch_trim, MAX_PPRZ/10);
187  #endif
188  #ifdef COMMAND_YAW
189  trimmed_commands[COMMAND_YAW] += ChopAbs(command_yaw_trim, MAX_PPRZ);
190  #endif
191 
192  SetActuatorsFromCommands(trimmed_commands);
193  fbw_new_actuators = 0;
194  #if OUTBACK_CHALLENGE_VERY_DANGEROUS_RULE_AP_CAN_FORCE_FAILSAFE
195  if (crash == 1)
196  {
197  for (;;) {}
198  }
199  #endif
200 
201  }
202 #endif
203 
204 
205 #ifdef MCU_SPI_LINK
206  if (link_mcu_received) {
208  inter_mcu_fill_fbw_state();
209  link_mcu_restart();
210  }
211 #endif /* MCU_SPI_LINK */
212 #endif /* INTER_MCU */
213 
214 }
215 
216 
217 /************* PERIODIC ******************************************************/
218 void periodic_task_fbw( void ) {
219 
220 #ifdef RADIO_CONTROL
221  radio_control_periodic_task();
222  if (fbw_mode == FBW_MODE_MANUAL && radio_control.status == RC_REALLY_LOST) {
223 #ifdef OUTBACK_CHALLENGE_DANGEROUS_RULE_RC_LOST_NO_AP
224 #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!!
226 #else
228 #endif
229  }
230 #endif
231 
232 #ifdef INTER_MCU
233  inter_mcu_periodic_task();
234  if (fbw_mode == FBW_MODE_AUTO && !ap_ok)
235  {
237  }
238 #endif
239 
240 #ifdef MCU_UART_LINK
241  inter_mcu_fill_fbw_state();
243 #endif
244 
245 #ifdef DOWNLINK
247 #endif
248 
249 }
250 
252 
255 
256 #if !(DISABLE_ELECTRICAL)
259 #endif
260 
261 }
FBW ( FlyByWire ) process API.
void event_task_fbw(void)
Definition: main_fbw.c:124
void periodic_task_fbw(void)
Definition: main_fbw.c:218
tid_t fbw_periodic_tid
id for periodic_task_fbw() timer
Definition: main_fbw.c:63
volatile uint8_t fbw_new_actuators
Definition: main_fbw.c:61
void i2c_event(void)
Definition: i2c_arch.c:344
pprz_t command_roll_trim
Definition: commands.c:43
volatile bool_t inter_mcu_received_ap
Definition: inter_mcu.c:39
pprz_t command_pitch_trim
Definition: commands.c:44
int16_t pprz_t
Definition: paparazzi.h:6
void init_fbw(void)
Definition: main_fbw.c:67
uint8_t tid_t
sys_time timer id type
Definition: sys_time.h:43
#define FBW_MODE_OF_PPRZ(mode)
Definition: main_fbw.h:37
#define FALSE
Definition: imu_chimu.h:141
const pprz_t commands_failsafe[COMMANDS_NB]
Definition: commands.c:49
struct ap_state * ap_state
Definition: inter_mcu.c:35
#define mcu_int_enable()
Definition: mcu_arch.h:32
#define RadioControlEvent(_received_frame_handler)
Definition: dummy.h:30
void electrical_init(void)
Definition: electrical.c:39
static void set_failsafe_mode(void)
Definition: main_fbw.c:103
uint8_t fbw_mode
Definition: main_fbw.c:56
Architecture independent timing functions.
#define FBW_MODE_AUTO
Definition: main_fbw.h:35
void actuators_init(void)
struct RadioControl radio_control
Definition: radio_control.c:27
tid_t electrical_tid
id for electrical_periodic() timer
Definition: main_fbw.c:64
int sys_time_register_timer(float duration, sys_time_cb cb)
Register a new system timer.
Definition: sys_time.c:35
static bool_t sys_time_check_and_ack_timer(tid_t id)
Definition: sys_time.h:90
void electrical_periodic(void)
Definition: electrical.c:53
#define RADIO_MODE
Definition: spektrum_arch.h:63
arch independent mcu ( Micro Controller Unit ) utilities
#define SetCommands(t)
Storage of intermediate command values: these values come from the RC (MANUAL mode), from the autopilot (AUTO mode) or from control loops.
Definition: commands.h:45
unsigned char uint8_t
Definition: types.h:14
pprz_t commands[COMMANDS_NB]
Definition: commands.c:48
pprz_t command_yaw_trim
Definition: commands.c:45
#define FBW_MODE_FAILSAFE
Definition: main_fbw.h:36
#define MAX_PPRZ
Definition: paparazzi.h:8
void handle_periodic_tasks_fbw(void)
Definition: main_fbw.c:251
#define FBW_MODE_MANUAL
Fly by wire modes.
Definition: main_fbw.h:34
void mcu_init(void)
Definition: mcu.c:57