Paparazzi UAS  v5.8.2_stable-0-g6260b7c
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
main_fbw.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2015 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 
29 #include <inttypes.h>
30 #include "mcu.h"
31 #include "mcu_periph/sys_time.h"
32 #include "led.h"
33 
34 #include "subsystems/commands.h"
35 #include "subsystems/actuators.h"
36 #if USE_MOTOR_MIXING
38 #endif
39 
40 
41 #include "subsystems/electrical.h"
42 
44 
46 
49 
50 //#include "generated/modules.h"
51 
55 
56 /* MODULES_FREQUENCY is defined in generated/modules.h
57  * according to main_freq parameter set for modules in airframe file
58  */
59 //PRINT_CONFIG_VAR(MODULES_FREQUENCY)
60 
62 //tid_t modules_tid; ///< id for modules_periodic_task() timer
66 
67 #ifndef SITL
68 int main(void)
69 {
70  main_init();
71 
72  while (1) {
74  main_event();
75  }
76 
77  return 0;
78 }
79 #endif /* SITL */
80 
82 {
83  // fbw_init
85 
86  mcu_init();
87 
89 
90  actuators_init();
91 #if USE_MOTOR_MIXING
93 #endif
94 
96 
97  // TODO
98  //modules_init();
99 
100  mcu_int_enable();
101 
102  intermcu_init();
103 
104  // register the timers for the periodic functions
106 // modules_tid = sys_time_register_timer(1. / MODULES_FREQUENCY, NULL);
107  radio_control_tid = sys_time_register_timer((1. / 60.), NULL);
109  telemetry_tid = sys_time_register_timer((1. / 20.), NULL);
110 }
111 
112 
114 // PERIODIC
115 
117 {
119  main_periodic();
120  }
121  //if (sys_time_check_and_ack_timer(modules_tid)) {
122  // TODO
123  //modules_periodic_task();
124  //}
127  }
130  }
133  }
134 }
135 
137 {
138  /* Send status to AP */
140 
141  /* Handle Modems */
142  // TODO
143  // Read Telemetry
144 }
145 
147 {
148  /* Inter-MCU watchdog */
150 
151  /* Safety logic */
152  bool_t ap_lost = (inter_mcu.status == INTERMCU_LOST);
153  bool_t rc_lost = (radio_control.status == RC_REALLY_LOST);
154  if (rc_lost) {
155  if (ap_lost) {
156  // Both are lost
158  } else {
159  if (fbw_mode == FBW_MODE_MANUAL) {
161  } else {
162  if (fbw_mode == FBW_MODE_FAILSAFE) {
163  // No change: failsafe stays failsafe
164  } else {
165  // Lost RC while in working Auto mode
167  }
168  }
169  }
170  } else { // rc_is_good
171  if (fbw_mode == FBW_MODE_AUTO) {
172  if (ap_lost) {
174  }
175  }
176  }
177 
178  /* set failsafe commands */
179  if (fbw_mode == FBW_MODE_FAILSAFE) {
181  }
182 
183  /* set actuators */
184  SetActuatorsFromCommands(commands, autopilot_mode);
185 
186  /* blink */
187  RunOnceEvery(10, LED_PERIODIC());
188 }
189 
190 
192 // Event
193 
194 static void autopilot_on_rc_frame(void)
195 {
196  /* get autopilot fbw mode as set by RADIO_MODE 3-way switch */
199  } else {
201  }
202 
203  /* Trying to switch to auto when AP is lost */
204  if ((inter_mcu.status == INTERMCU_LOST) &&
205  (fbw_mode == FBW_MODE_AUTO)) {
207  }
208 
209  /* if manual */
210  if (fbw_mode == FBW_MODE_MANUAL) {
211 #ifdef SetCommandsFromRC
212  SetCommandsFromRC(commands, radio_control.values);
213 #else
214 #warning "FBW: needs commands from RC in order to be useful."
215 #endif
216  }
217 
218  /* Forward radiocontrol to AP */
220 }
221 
222 static void autopilot_on_ap_command(void)
223 {
224  if (fbw_mode != FBW_MODE_MANUAL) {
226  }
227 }
228 
230 {
231  /* event functions for mcu peripherals: i2c, usb_serial.. */
232  mcu_event();
233 
234  // Handle RC
236 
237  // InterMCU
239 
240  // TODO Modules
241  //modules_event_task();
242 }
void intermcu_send_status(uint8_t mode)
Definition: intermcu_fbw.c:68
void mcu_init(void)
Microcontroller peripherals initialization.
Definition: mcu.c:71
void InterMcuEvent(void(*frame_handler)(void))
Definition: intermcu_fbw.c:102
tid_t radio_control_tid
id for radio_control_periodic_task() timer
Definition: main_fbw.c:63
static void autopilot_on_rc_frame(void)
Definition: main_fbw.c:194
#define MIN_PPRZ
Definition: paparazzi.h:9
uint8_t autopilot_mode
Definition: autopilot.c:65
pprz_t intermcu_commands[COMMANDS_NB]
Definition: intermcu_fbw.c:44
tid_t electrical_tid
id for electrical_periodic() timer
Definition: main_fbw.c:76
uint8_t status
Definition: radio_control.h:53
int main(void)
Definition: main_fbw.c:68
STATIC_INLINE void telemetry_periodic(void)
Definition: main_fbw.c:136
void intermcu_periodic(void)
Definition: intermcu_ap.c:49
enum intermcu_status status
Definition: intermcu.h:44
uint8_t tid_t
sys_time timer id type
Definition: sys_time.h:57
tid_t telemetry_tid
id for telemetry_periodic() timer
Definition: main_fbw.c:65
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:58
void radio_control_init(void)
Definition: radio_control.c:32
const pprz_t commands_failsafe[COMMANDS_NB]
Definition: commands.c:31
Rotorcraft Inter-MCU on FlyByWire.
#define RadioControlEvent(_received_frame_handler)
Definition: dummy.h:28
Hardware independent API for actuators (servos, motor controllers).
void electrical_init(void)
Definition: electrical.c:99
Interface for electrical status: supply voltage, current, battery status, etc.
Architecture independent timing functions.
void motor_mixing_init(void)
Definition: motor_mixing.c:96
void radio_control_periodic_task(void)
Definition: radio_control.c:46
struct intermcu_t inter_mcu
Definition: intermcu_ap.c:41
tid_t main_periodic_tid
id for main_periodic() timer
Definition: main_fbw.c:61
Hardware independent code for commands handling.
struct RadioControl radio_control
Definition: radio_control.c:30
static void autopilot_on_ap_command(void)
Definition: main_fbw.c:222
void intermcu_on_rc_frame(void)
Definition: intermcu_fbw.c:62
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:50
Some helper functions to check RC sticks.
void electrical_periodic(void)
Definition: electrical.c:121
void intermcu_init(void)
Definition: intermcu_ap.c:44
STATIC_INLINE void main_periodic(void)
Definition: main_fbw.c:146
uint8_t fbw_mode
Definition: main_fbw.c:56
Fly By Wire:
Arch independent mcu ( Micro Controller Unit ) utilities.
#define mcu_int_enable()
Definition: mcu_arch.h:35
#define SetCommands(t)
Definition: commands.h:41
pprz_t commands[COMMANDS_NB]
Storage of intermediate command values.
Definition: commands.c:30
#define RC_LOST_FBW_MODE
mode to enter when RC is lost while using a mode with RC input
Definition: main_fbw.h:47
#define RADIO_FBW_MODE
Switching between FBW and autopilot is done with RADIO_FBW_MODE: default is to re-use RADIO_MODE...
Definition: main_fbw.h:65
#define LED_PERIODIC()
Definition: led_hw.h:47
STATIC_INLINE void main_init(void)
Definition: main_fbw.c:81
#define STATIC_INLINE
Definition: main.h:34
#define PERIODIC_FREQUENCY
Definition: imu_aspirin2.c:47
arch independent LED (Light Emitting Diodes) API
void mcu_event(void)
MCU event functions.
Definition: mcu.c:192
STATIC_INLINE void main_event(void)
Definition: main_fbw.c:229
Motor Mixing.
#define AP_LOST_FBW_MODE
mode to enter when AP is lost while using autopilot
Definition: main_fbw.h:57
#define RC_LOST_IN_AUTO_FBW_MODE
mode to enter when AP is lost while using autopilot
Definition: main_fbw.h:52
fbw_mode_enum
Fly by wire modes.
Definition: main_fbw.c:53
STATIC_INLINE void handle_periodic_tasks(void)
Definition: main_fbw.c:116