Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
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 "paparazzi.h"
45 #include "mcu_periph/i2c.h"
46 
47 #if PERIODIC_TELEMETRY
49 #endif
50 
51 #ifdef MCU_SPI_LINK
52 #include "link_mcu_spi.h"
53 #endif
54 
55 #ifdef MCU_UART_LINK
56 #include "link_mcu_usart.h"
57 #endif
58 
60 
61 #include "inter_mcu.h"
62 
63 #ifdef USE_NPS
64 #include "nps_autopilot.h"
65 #endif
66 
73 
74 
76 
79 
80 /********** PERIODIC MESSAGES ************************************************/
81 #if PERIODIC_TELEMETRY
82 static void send_commands(void) {
83  DOWNLINK_SEND_COMMANDS(DefaultChannel, DefaultDevice, COMMANDS_NB, commands);
84 }
85 
86 #ifdef RADIO_CONTROL
87 static void send_fbw_status(void) {
88  DOWNLINK_SEND_FBW_STATUS(DefaultChannel, DefaultDevice,
90 }
91 
92 static void send_rc(void) {
94 }
95 
96 #else
97 static void send_fbw_status(void) {
98  uint8_t dummy = 0;
99  DOWNLINK_SEND_FBW_STATUS(DefaultChannel, DefaultDevice,
100  &dummy, &dummy, &fbw_mode, &electrical.vsupply, &electrical.current);
101 }
102 #endif
103 
104 #ifdef ACTUATORS
105 static void send_actuators(void) {
106  DOWNLINK_SEND_ACTUATORS(DefaultChannel, DefaultDevice , ACTUATORS_NB, actuators);
107 }
108 #endif
109 
110 #endif
111 
112 /********** INIT *************************************************************/
113 void init_fbw( void ) {
114 
115  mcu_init();
116 
117 #if !(DISABLE_ELECTRICAL)
118  electrical_init();
119 #endif
120 
121 #ifdef ACTUATORS
122  actuators_init();
123  /* Load the failsafe defaults */
125  fbw_new_actuators = 1;
126 #endif
127 #ifdef RADIO_CONTROL
129 #endif
130 #ifdef INTER_MCU
131  inter_mcu_init();
132 #endif
133 #ifdef MCU_SPI_LINK
134  link_mcu_init();
135  link_mcu_restart();
136 #endif
137 
139 
140  /**** start timers for periodic functions *****/
141  fbw_periodic_tid = sys_time_register_timer((1./60.), NULL);
143 
144 #ifndef SINGLE_MCU
145  mcu_int_enable();
146 #endif
147 
148 #if PERIODIC_TELEMETRY
149  register_periodic_telemetry(&telemetry_Fbw, "FBW_STATUS", send_fbw_status);
150  register_periodic_telemetry(&telemetry_Fbw, "COMMANDS", send_commands);
151 #ifdef ACTUATORS
152  register_periodic_telemetry(&telemetry_Fbw, "ACTUATORS", send_actuators);
153 #endif
154 #ifdef RADIO_CONTROL
155  register_periodic_telemetry(&telemetry_Fbw, "RC", send_rc);
156 #endif
157 #endif
158 
159 }
160 
161 
162 static inline void set_failsafe_mode( void ) {
165  fbw_new_actuators = 1;
166 }
167 
168 
169 #ifdef RADIO_CONTROL
170 static inline void handle_rc_frame( void ) {
172  if (fbw_mode == FBW_MODE_MANUAL)
173  {
174  SetCommandsFromRC(commands, radio_control.values);
175  fbw_new_actuators = 1;
176  }
177 }
178 #endif
179 
180 
181 /********** EVENT ************************************************************/
182 
183 void event_task_fbw( void) {
184 #ifdef RADIO_CONTROL
186 #endif
187 
188  i2c_event();
189 
190 #ifdef INTER_MCU
191 #if defined MCU_SPI_LINK | defined MCU_UART_LINK
193 #endif /* MCU_SPI_LINK */
194 
195 
196  if (inter_mcu_received_ap) {
198  inter_mcu_event_task();
199  command_roll_trim = ap_state->command_roll_trim;
200  command_pitch_trim = ap_state->command_pitch_trim;
201  command_yaw_trim = ap_state->command_yaw_trim;
202 #ifndef OUTBACK_CHALLENGE_DANGEROUS_RULE_RC_LOST_NO_AP
203  if (ap_ok && fbw_mode == FBW_MODE_FAILSAFE) {
205  }
206 #endif
207  if (fbw_mode == FBW_MODE_AUTO) {
208  SetCommands(ap_state->commands);
209  }
210 #ifdef SetApOnlyCommands
211  else
212  {
213  SetApOnlyCommands(ap_state->commands);
214  }
215 #endif
216  fbw_new_actuators = 1;
217 
218 #ifdef SINGLE_MCU
219  inter_mcu_fill_fbw_state();
220 #endif
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: AP is now capable to FORCE your FBW in failsafe mode EVEN IF RC IS NOT LOST: Consider the consequences.
225 
226  int crash = 0;
227  if (commands[COMMAND_FORCECRASH] >= 8000)
228  {
230  crash = 1;
231  }
232 
233 #endif
234 #ifdef ACTUATORS
235  if (fbw_new_actuators > 0)
236  {
237  pprz_t trimmed_commands[COMMANDS_NB];
238  int i;
239  for(i = 0; i < COMMANDS_NB; i++) trimmed_commands[i] = commands[i];
240 
241  #ifdef COMMAND_ROLL
242  trimmed_commands[COMMAND_ROLL] += ChopAbs(command_roll_trim, MAX_PPRZ/10);
243  #endif
244  #ifdef COMMAND_PITCH
245  trimmed_commands[COMMAND_PITCH] += ChopAbs(command_pitch_trim, MAX_PPRZ/10);
246  #endif
247  #ifdef COMMAND_YAW
248  trimmed_commands[COMMAND_YAW] += ChopAbs(command_yaw_trim, MAX_PPRZ);
249  #endif
250 
251  SetActuatorsFromCommands(trimmed_commands, autopilot_mode);
252  fbw_new_actuators = 0;
253  #if OUTBACK_CHALLENGE_VERY_DANGEROUS_RULE_AP_CAN_FORCE_FAILSAFE
254  if (crash == 1)
255  {
256  for (;;) {}
257  }
258  #endif
259 
260  }
261 #endif
262 
263 
264 #ifdef MCU_SPI_LINK
265  if (link_mcu_received) {
267  inter_mcu_fill_fbw_state();
268  link_mcu_restart();
269  }
270 #endif /* MCU_SPI_LINK */
271 #endif /* INTER_MCU */
272 
273 }
274 
275 
276 /************* PERIODIC ******************************************************/
277 void periodic_task_fbw( void ) {
278 
279 #ifdef RADIO_CONTROL
282 #ifdef OUTBACK_CHALLENGE_DANGEROUS_RULE_RC_LOST_NO_AP
283 #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!!
285 #else
287 #endif
288  }
289 #endif
290 
291 #ifdef INTER_MCU
292  inter_mcu_periodic_task();
293  if (fbw_mode == FBW_MODE_AUTO && !ap_ok)
294  {
296  }
297 #endif
298 
299 #ifdef MCU_UART_LINK
300  inter_mcu_fill_fbw_state();
302 #endif
303 
304 #if PERIODIC_TELEMETRY
305  periodic_telemetry_send_Fbw();
306 #endif
307 
308 }
309 
311 
314 
315 #if !(DISABLE_ELECTRICAL)
318 #endif
319 
320 }
FBW ( FlyByWire ) process API.
void event_task_fbw(void)
Definition: main_fbw.c:183
void periodic_task_fbw(void)
Definition: main_fbw.c:277
void mcu_init(void)
Microcontroller peripherals initialization.
Definition: mcu.c:61
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:77
volatile uint8_t fbw_new_actuators
Definition: main_fbw.c:75
uint8_t autopilot_mode
Definition: autopilot.c:47
Periodic telemetry system header (includes downlink utility and generated code).
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:70
int16_t pprz_t
Definition: paparazzi.h:6
void init_fbw(void)
Definition: main_fbw.c:113
uint16_t vsupply
supply voltage in decivolts
Definition: electrical.h:48
uint8_t tid_t
sys_time timer id type
Definition: sys_time.h:57
static void send_rc(void)
Definition: autopilot.c:194
#define FBW_MODE_OF_PPRZ(mode)
Definition: main_fbw.h:38
bool_t register_periodic_telemetry(struct pprz_telemetry *_pt, const char *_msg, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c: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:162
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:55
uint8_t fbw_mode
Definition: main_fbw.c:59
Interface for electrical status: supply voltage, current, battery status, etc.
Architecture independent timing functions.
#define FBW_MODE_AUTO
Definition: main_fbw.h:36
pprz_t command_pitch_trim
Definition: main_fbw.c:71
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).
uint8_t frame_rate
Definition: radio_control.h:53
tid_t electrical_tid
id for electrical_periodic() timer
Definition: main_fbw.c:78
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:69
int32_t current
current in milliamps
Definition: electrical.h:49
Arch independent mcu ( Micro Controller Unit ) utilities.
#define SetCommands(t)
Definition: commands.h:41
#define RADIO_CONTROL_NB_CHANNEL
Definition: spektrum_arch.h:34
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:170
pprz_t command_yaw_trim
Definition: main_fbw.c:72
#define FBW_MODE_FAILSAFE
Definition: main_fbw.h:37
struct Electrical electrical
void actuators_init(void)
Definition: actuators_at.c:32
void i2c_event(void)
Definition: i2c_arch.c:368
#define MAX_PPRZ
Definition: paparazzi.h:8
void electrical_periodic(void)
void handle_periodic_tasks_fbw(void)
Definition: main_fbw.c:310
#define FBW_MODE_MANUAL
Fly by wire modes.
Definition: main_fbw.h:35
void electrical_init(void)
Architecture independent I2C (Inter-Integrated Circuit Bus) API.
Fixedwing autopilot modes.