/
main_fbw.c
322 lines (262 loc) · 7.99 KB
/
main_fbw.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
/*
* Copyright (C) 2003-2010 The Paparazzi Team
*
* This file is part of Paparazzi.
*
* Paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* Paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*/
/**
* @file firmwares/fixedwing/main_fbw.c
*
* FBW ( FlyByWire ) process
*
* This process is responsible for decoding radio control, generating actuators
* signals either from the radio control or from the commands provided by the
* AP (autopilot) process. It also performs a telemetry task and a low level monitoring
* ( for parameters like the supply )
*/
#include "generated/airframe.h"
#include "firmwares/fixedwing/main_fbw.h"
#include "mcu.h"
#include "mcu_periph/sys_time.h"
#include "subsystems/commands.h"
#include "subsystems/actuators.h"
#include "subsystems/electrical.h"
#include "subsystems/radio_control.h"
#include "firmwares/fixedwing/autopilot.h"
#include "paparazzi.h"
#include "mcu_periph/i2c.h"
#if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h"
#endif
#ifdef MCU_SPI_LINK
#include "link_mcu_spi.h"
#endif
#ifdef MCU_UART_LINK
#include "link_mcu_usart.h"
#endif
uint8_t fbw_mode;
#include "inter_mcu.h"
#ifdef USE_NPS
#include "nps_autopilot.h"
#endif
/** Trim commands for roll, pitch and yaw.
* These are updated from the trim commands in ap_state via inter_mcu
*/
pprz_t command_roll_trim;
pprz_t command_pitch_trim;
pprz_t command_yaw_trim;
volatile uint8_t fbw_new_actuators = 0;
tid_t fbw_periodic_tid; ///< id for periodic_task_fbw() timer
tid_t electrical_tid; ///< id for electrical_periodic() timer
/********** PERIODIC MESSAGES ************************************************/
#if PERIODIC_TELEMETRY
static void send_commands(void) {
DOWNLINK_SEND_COMMANDS(DefaultChannel, DefaultDevice, COMMANDS_NB, commands);
}
#ifdef RADIO_CONTROL
static void send_fbw_status(void) {
DOWNLINK_SEND_FBW_STATUS(DefaultChannel, DefaultDevice,
&(radio_control.status), &(radio_control.frame_rate), &fbw_mode, &electrical.vsupply, &electrical.current);
}
static void send_rc(void) {
DOWNLINK_SEND_RC(DefaultChannel, DefaultDevice, RADIO_CONTROL_NB_CHANNEL, radio_control.values);
}
#else
static void send_fbw_status(void) {
uint8_t dummy = 0;
DOWNLINK_SEND_FBW_STATUS(DefaultChannel, DefaultDevice,
&dummy, &dummy, &fbw_mode, &electrical.vsupply, &electrical.current);
}
#endif
#ifdef ACTUATORS
static void send_actuators(void) {
DOWNLINK_SEND_ACTUATORS(DefaultChannel, DefaultDevice , ACTUATORS_NB, actuators);
}
#endif
#endif
/********** INIT *************************************************************/
void init_fbw( void ) {
mcu_init();
#if !(DISABLE_ELECTRICAL)
electrical_init();
#endif
#ifdef ACTUATORS
actuators_init();
/* Load the failsafe defaults */
SetCommands(commands_failsafe);
fbw_new_actuators = 1;
#endif
#ifdef RADIO_CONTROL
radio_control_init();
#endif
#ifdef INTER_MCU
inter_mcu_init();
#endif
#ifdef MCU_SPI_LINK
link_mcu_init();
link_mcu_restart();
#endif
fbw_mode = FBW_MODE_FAILSAFE;
/**** start timers for periodic functions *****/
fbw_periodic_tid = sys_time_register_timer((1./60.), NULL);
electrical_tid = sys_time_register_timer(0.1, NULL);
#ifndef SINGLE_MCU
mcu_int_enable();
#endif
#if PERIODIC_TELEMETRY
register_periodic_telemetry(&telemetry_Fbw, "FBW_STATUS", send_fbw_status);
register_periodic_telemetry(&telemetry_Fbw, "COMMANDS", send_commands);
#ifdef ACTUATORS
register_periodic_telemetry(&telemetry_Fbw, "ACTUATORS", send_actuators);
#endif
#ifdef RADIO_CONTROL
register_periodic_telemetry(&telemetry_Fbw, "RC", send_rc);
#endif
#endif
}
static inline void set_failsafe_mode( void ) {
fbw_mode = FBW_MODE_FAILSAFE;
SetCommands(commands_failsafe);
fbw_new_actuators = 1;
}
#ifdef RADIO_CONTROL
static inline void handle_rc_frame( void ) {
fbw_mode = FBW_MODE_OF_PPRZ(radio_control.values[RADIO_MODE]);
if (fbw_mode == FBW_MODE_MANUAL)
{
SetCommandsFromRC(commands, radio_control.values);
fbw_new_actuators = 1;
}
}
#endif
/********** EVENT ************************************************************/
void event_task_fbw( void) {
#ifdef RADIO_CONTROL
RadioControlEvent(handle_rc_frame);
#endif
#if USE_I2C0 || USE_I2C1 || USE_I2C2
i2c_event();
#endif
#ifdef INTER_MCU
#if defined MCU_SPI_LINK | defined MCU_UART_LINK
link_mcu_event_task();
#endif /* MCU_SPI_LINK */
if (inter_mcu_received_ap) {
inter_mcu_received_ap = FALSE;
inter_mcu_event_task();
command_roll_trim = ap_state->command_roll_trim;
command_pitch_trim = ap_state->command_pitch_trim;
command_yaw_trim = ap_state->command_yaw_trim;
#ifndef OUTBACK_CHALLENGE_DANGEROUS_RULE_RC_LOST_NO_AP
if (ap_ok && fbw_mode == FBW_MODE_FAILSAFE) {
fbw_mode = FBW_MODE_AUTO;
}
#endif
if (fbw_mode == FBW_MODE_AUTO) {
SetCommands(ap_state->commands);
}
#ifdef SetApOnlyCommands
else
{
SetApOnlyCommands(ap_state->commands);
}
#endif
fbw_new_actuators = 1;
#ifdef SINGLE_MCU
inter_mcu_fill_fbw_state();
#endif /**Else the buffer is filled even if the last receive was not correct */
}
#if OUTBACK_CHALLENGE_VERY_DANGEROUS_RULE_AP_CAN_FORCE_FAILSAFE
#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.
int crash = 0;
if (commands[COMMAND_FORCECRASH] >= 8000)
{
set_failsafe_mode();
crash = 1;
}
#endif
#ifdef ACTUATORS
if (fbw_new_actuators > 0)
{
pprz_t trimmed_commands[COMMANDS_NB];
int i;
for(i = 0; i < COMMANDS_NB; i++) trimmed_commands[i] = commands[i];
#ifdef COMMAND_ROLL
trimmed_commands[COMMAND_ROLL] += ChopAbs(command_roll_trim, MAX_PPRZ/10);
#endif
#ifdef COMMAND_PITCH
trimmed_commands[COMMAND_PITCH] += ChopAbs(command_pitch_trim, MAX_PPRZ/10);
#endif
#ifdef COMMAND_YAW
trimmed_commands[COMMAND_YAW] += ChopAbs(command_yaw_trim, MAX_PPRZ);
#endif
SetActuatorsFromCommands(trimmed_commands, autopilot_mode);
fbw_new_actuators = 0;
#if OUTBACK_CHALLENGE_VERY_DANGEROUS_RULE_AP_CAN_FORCE_FAILSAFE
if (crash == 1)
{
for (;;) {}
}
#endif
}
#endif
#ifdef MCU_SPI_LINK
if (link_mcu_received) {
link_mcu_received = FALSE;
inter_mcu_fill_fbw_state(); /** Prepares the next message for AP */
link_mcu_restart(); /** Prepares the next SPI communication */
}
#endif /* MCU_SPI_LINK */
#endif /* INTER_MCU */
}
/************* PERIODIC ******************************************************/
void periodic_task_fbw( void ) {
#ifdef RADIO_CONTROL
radio_control_periodic_task();
if (fbw_mode == FBW_MODE_MANUAL && radio_control.status == RC_REALLY_LOST) {
#ifdef OUTBACK_CHALLENGE_DANGEROUS_RULE_RC_LOST_NO_AP
#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!!
set_failsafe_mode();
#else
fbw_mode = FBW_MODE_AUTO;
#endif
}
#endif
#ifdef INTER_MCU
inter_mcu_periodic_task();
if (fbw_mode == FBW_MODE_AUTO && !ap_ok)
{
set_failsafe_mode();
}
#endif
#ifdef MCU_UART_LINK
inter_mcu_fill_fbw_state();
link_mcu_periodic_task();
#endif
#if PERIODIC_TELEMETRY
periodic_telemetry_send_Fbw();
#endif
}
void handle_periodic_tasks_fbw(void) {
if (sys_time_check_and_ack_timer(fbw_periodic_tid))
periodic_task_fbw();
#if !(DISABLE_ELECTRICAL)
if (sys_time_check_and_ack_timer(electrical_tid))
electrical_periodic();
#endif
}