Paparazzi UAS  v4.0.4_stable-3-gf39211a
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.h"
50 #endif
51 
53 
54 #include "inter_mcu.h"
55 
56 
58 
61 
62 /********** INIT *************************************************************/
63 void init_fbw( void ) {
64 
65  mcu_init();
66 
68 
69 #ifdef ACTUATORS
71  /* Load the failsafe defaults */
74 #endif
75 #ifdef RADIO_CONTROL
76  radio_control_init();
77 #endif
78 #ifdef INTER_MCU
79  inter_mcu_init();
80 #endif
81 #ifdef MCU_SPI_LINK
82  link_mcu_restart();
83 #endif
84 
86 
87  /**** start timers for periodic functions *****/
88  fbw_periodic_tid = sys_time_register_timer((1./60.), NULL);
90 
91 #ifndef SINGLE_MCU
93 #endif
94 }
95 
96 
97 static inline void set_failsafe_mode( void ) {
100  fbw_new_actuators = 1;
101 }
102 
103 
104 #ifdef RADIO_CONTROL
105 static inline void handle_rc_frame( void ) {
107  if (fbw_mode == FBW_MODE_MANUAL)
108  {
109  SetCommandsFromRC(commands, radio_control.values);
110  fbw_new_actuators = 1;
111  }
112 }
113 #endif
114 
115 
116 /********** EVENT ************************************************************/
117 
118 void event_task_fbw( void) {
119 #ifdef RADIO_CONTROL
120  RadioControlEvent(handle_rc_frame);
121 #endif
122 
123  i2c_event();
124 
125 #ifdef INTER_MCU
126 #ifdef MCU_SPI_LINK
128 #endif /* MCU_SPI_LINK */
129 
130 
131  if (inter_mcu_received_ap) {
133  inter_mcu_event_task();
134  command_roll_trim = ap_state->command_roll_trim;
135  command_pitch_trim = ap_state->command_pitch_trim;
136  if (ap_ok && fbw_mode == FBW_MODE_FAILSAFE) {
138  }
139  if (fbw_mode == FBW_MODE_AUTO) {
140  SetCommands(ap_state->commands);
141  }
142 #ifdef SetApOnlyCommands
143  else
144  {
145  SetApOnlyCommands(ap_state->commands);
146  }
147 #endif
148  fbw_new_actuators = 1;
149 
150 #ifdef SINGLE_MCU
151  inter_mcu_fill_fbw_state();
152 #endif
153  }
154 
155 #ifdef ACTUATORS
156  if (fbw_new_actuators > 0)
157  {
158  pprz_t trimmed_commands[COMMANDS_NB];
159  int i;
160  for(i = 0; i < COMMANDS_NB; i++) trimmed_commands[i] = commands[i];
161 
162  #ifdef COMMAND_ROLL
163  trimmed_commands[COMMAND_ROLL] += ChopAbs(command_roll_trim, MAX_PPRZ/10);
164  #endif
165  #ifdef COMMAND_PITCH
166  trimmed_commands[COMMAND_PITCH] += ChopAbs(command_pitch_trim, MAX_PPRZ/10);
167  #endif
168 
169  SetActuatorsFromCommands(trimmed_commands);
170  fbw_new_actuators = 0;
171  }
172 #endif
173 
174 
175 #ifdef MCU_SPI_LINK
176  if (link_mcu_received) {
178  inter_mcu_fill_fbw_state();
179  link_mcu_restart();
180  }
181 #endif /* MCU_SPI_LINK */
182 #endif /* INTER_MCU */
183 
184 }
185 
186 
187 /************* PERIODIC ******************************************************/
188 void periodic_task_fbw( void ) {
189 
190 #ifdef RADIO_CONTROL
191  radio_control_periodic_task();
192  if (fbw_mode == FBW_MODE_MANUAL && radio_control.status == RC_REALLY_LOST) {
194  }
195 #endif
196 
197 #ifdef INTER_MCU
198  inter_mcu_periodic_task();
199  if (fbw_mode == FBW_MODE_AUTO && !ap_ok)
200  {
202  }
203 #endif
204 
205 #ifdef DOWNLINK
207 #endif
208 
209 }
210 
212 
215 
218 
219 }
FBW ( FlyByWire ) process API.
void event_task_fbw(void)
Definition: main_fbw.c:118
void periodic_task_fbw(void)
Definition: main_fbw.c:188
tid_t fbw_periodic_tid
id for periodic_task_fbw() timer
Definition: main_fbw.c:59
volatile uint8_t fbw_new_actuators
Definition: main_fbw.c:57
void i2c_event(void)
Definition: i2c_arch.c:344
pprz_t command_roll_trim
Definition: commands.c:39
volatile bool_t inter_mcu_received_ap
Definition: inter_mcu.c:39
pprz_t command_pitch_trim
Definition: commands.c:40
int16_t pprz_t
Definition: paparazzi.h:6
void init_fbw(void)
Definition: main_fbw.c:63
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:43
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:97
uint8_t fbw_mode
Definition: main_fbw.c:52
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:60
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:44
unsigned char uint8_t
Definition: types.h:14
pprz_t commands[COMMANDS_NB]
Definition: commands.c:42
#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:211
#define FBW_MODE_MANUAL
Fly by wire modes.
Definition: main_fbw.h:34
void mcu_init(void)
Definition: mcu.c:57