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.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-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 
29 #define MODULES_C
30 
31 #define ABI_C
32 
33 #include <inttypes.h>
34 #include "mcu.h"
35 #include "mcu_periph/sys_time.h"
36 #include "mcu_periph/i2c.h"
37 #include "led.h"
38 
41 #include "subsystems/settings.h"
43 #if DATALINK == UDP
45 #endif
46 
47 #include "subsystems/commands.h"
48 #include "subsystems/actuators.h"
49 #if USE_MOTOR_MIXING
51 #endif
52 
53 #include "subsystems/imu.h"
54 #include "subsystems/gps.h"
55 #include "subsystems/air_data.h"
56 
57 #if USE_BARO_BOARD
59 PRINT_CONFIG_MSG_VALUE("USE_BARO_BOARD is TRUE, reading onboard baro: ", BARO_BOARD)
60 #endif
61 
62 #include "subsystems/electrical.h"
63 
65 
67 
70 
71 #include "subsystems/ahrs.h"
73 #include "subsystems/ins.h"
74 
75 #include "state.h"
76 
78 
79 #ifdef SITL
80 #include "nps_autopilot.h"
81 #endif
82 
83 #include "generated/modules.h"
84 #include "subsystems/abi.h"
85 
86 
87 /* if PRINT_CONFIG is defined, print some config options */
89 
90 /* TELEMETRY_FREQUENCY is defined in generated/periodic_telemetry.h
91  * defaults to 60Hz or set by TELEMETRY_FREQUENCY configure option in airframe file
92  */
93 PRINT_CONFIG_VAR(TELEMETRY_FREQUENCY)
94 
95 /* MODULES_FREQUENCY is defined in generated/modules.h
96  * according to main_freq parameter set for modules in airframe file
97  */
98 PRINT_CONFIG_VAR(MODULES_FREQUENCY)
99 
100 #ifndef BARO_PERIODIC_FREQUENCY
101 #define BARO_PERIODIC_FREQUENCY 50
102 #endif
104 
105 #if USE_AHRS && USE_IMU && (defined AHRS_PROPAGATE_FREQUENCY)
106 #if (AHRS_PROPAGATE_FREQUENCY > PERIODIC_FREQUENCY)
107 #warning "PERIODIC_FREQUENCY should be least equal or greater than AHRS_PROPAGATE_FREQUENCY"
108 INFO_VALUE("it is recommended to configure in your airframe PERIODIC_FREQUENCY to at least ",AHRS_PROPAGATE_FREQUENCY)
109 #endif
110 #endif
111 
112 static inline void on_gyro_event( void );
113 static inline void on_accel_event( void );
114 static inline void on_gps_event( void );
115 static inline void on_mag_event( void );
116 
117 
124 #if USE_BARO_BOARD
125 tid_t baro_tid;
126 #endif
127 
128 #ifndef SITL
129 int main( void ) {
130  main_init();
131 
132  while(1) {
134  main_event();
135  }
136  return 0;
137 }
138 #endif /* SITL */
139 
140 STATIC_INLINE void main_init( void ) {
141 
142  mcu_init();
143 
144  electrical_init();
145 
146  stateInit();
147 
148  actuators_init();
149 #if USE_MOTOR_MIXING
151 #endif
152 
154 
155  air_data_init();
156 #if USE_BARO_BOARD
157  baro_init();
158 #endif
159  imu_init();
160 #if USE_IMU_FLOAT
161  imu_float_init();
162 #endif
164  ahrs_init();
165 
166  ins_init();
167 
168 #if USE_GPS
169  gps_init();
170 #endif
171 
172  autopilot_init();
173 
174  modules_init();
175 
176  settings_init();
177 
178  mcu_int_enable();
179 
180 #if DATALINK == XBEE
181  xbee_init();
182 #endif
183 
184 #if DATALINK == UDP
185  udp_init();
186 #endif
187 
188  // register the timers for the periodic functions
189  main_periodic_tid = sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL);
190  modules_tid = sys_time_register_timer(1./MODULES_FREQUENCY, NULL);
191  radio_control_tid = sys_time_register_timer((1./60.), NULL);
192  failsafe_tid = sys_time_register_timer(0.05, NULL);
193  electrical_tid = sys_time_register_timer(0.1, NULL);
194  telemetry_tid = sys_time_register_timer((1./TELEMETRY_FREQUENCY), NULL);
195 #if USE_BARO_BOARD
197 #endif
198 }
199 
201  if (sys_time_check_and_ack_timer(main_periodic_tid))
202  main_periodic();
203  if (sys_time_check_and_ack_timer(modules_tid))
204  modules_periodic_task();
205  if (sys_time_check_and_ack_timer(radio_control_tid))
207  if (sys_time_check_and_ack_timer(failsafe_tid))
208  failsafe_check();
209  if (sys_time_check_and_ack_timer(electrical_tid))
211  if (sys_time_check_and_ack_timer(telemetry_tid))
213 #if USE_BARO_BOARD
214  if (sys_time_check_and_ack_timer(baro_tid))
215  baro_periodic();
216 #endif
217 }
218 
220 
221  imu_periodic();
222 
223  /* run control loops */
225  /* set actuators */
226  //actuators_set(autopilot_motors_on);
228 
229  if (autopilot_in_flight) {
230  RunOnceEvery(PERIODIC_FREQUENCY, { autopilot_flight_time++; datalink_time++; });
231  }
232 
233  RunOnceEvery(10, LED_PERIODIC());
234 }
235 
237  periodic_telemetry_send_Main();
238 }
239 
241 #ifndef RC_LOST_MODE
242 #define RC_LOST_MODE AP_MODE_FAILSAFE
243 #endif
244 
251  {
253  }
254 
255 #if FAILSAFE_ON_BAT_CRITICAL
256  if (autopilot_mode != AP_MODE_KILL &&
258  {
260  }
261 #endif
262 
263 #if USE_GPS
265  if (autopilot_mode == AP_MODE_NAV &&
267 #if NO_GPS_LOST_WITH_RC_VALID
269 #endif
270  GpsIsLost())
271  {
273  }
274 
275  if (autopilot_mode == AP_MODE_HOME &&
277  {
279  }
280 #endif
281 
283 }
284 
285 STATIC_INLINE void main_event( void ) {
286 
287  i2c_event();
288 
289  DatalinkEvent();
290 
291  if (autopilot_rc) {
293  }
294 
296 
297 #if USE_BARO_BOARD
298  BaroEvent();
299 #endif
300 
301 #if USE_GPS
303 #endif
304 
305 #if FAILSAFE_GROUND_DETECT || KILL_ON_GROUND_DETECT
307 #endif
308 
309  modules_event_task();
310 
311 }
312 
313 static inline void on_accel_event( void ) {
315 
316  if (ahrs.status != AHRS_UNINIT) {
318  }
319 }
320 
321 static inline void on_gyro_event( void ) {
322 
323  ImuScaleGyro(imu);
324 
325  if (ahrs.status == AHRS_UNINIT) {
328  ahrs_align();
329  }
330  else {
331  ahrs_propagate();
332 #ifdef SITL
333  if (nps_bypass_ahrs) sim_overwrite_ahrs();
334 #endif
335  ins_propagate();
336  }
337 #ifdef USE_VEHICLE_INTERFACE
339 #endif
340 }
341 
342 static inline void on_gps_event(void) {
343  ahrs_update_gps();
344  ins_update_gps();
345 #ifdef USE_VEHICLE_INTERFACE
346  if (gps.fix == GPS_FIX_3D)
348 #endif
349 }
350 
351 static inline void on_mag_event(void) {
352  ImuScaleMag(imu);
353 
354 #if USE_MAGNETOMETER
355  if (ahrs.status == AHRS_RUNNING) {
356  ahrs_update_mag();
357  }
358 #endif
359 
360 #ifdef USE_VEHICLE_INTERFACE
362 #endif
363 }
Interface to align the AHRS via low-passed measurements at startup.
#define AP_MODE_KILL
Definition: autopilot.h:39
void mcu_init(void)
Microcontroller peripherals initialization.
Definition: mcu.c:61
static void on_gyro_event(void)
Definition: main.c:321
Attitude and Heading Reference System interface.
static void radio_control_periodic_task(void)
Definition: radio_control.h:75
bool_t bat_critical
battery critical status
Definition: electrical.h:53
Common barometric sensor implementation.
STATIC_INLINE void telemetry_periodic(void)
Definition: main.c:236
void autopilot_init(void)
Autopilot inititalization.
Definition: autopilot.c:142
void ins_update_gps(void)
Update INS state with GPS measurements.
uint8_t autopilot_mode
Definition: autopilot.c:47
void imu_float_init(void)
Definition: imu.c:158
Periodic telemetry system header (includes downlink utility and generated code).
void vi_notify_mag_available(void)
bool_t autopilot_motors_on
Definition: autopilot.c:54
uint16_t autopilot_flight_time
flight time in seconds.
Definition: autopilot.c:47
void ahrs_propagate(void)
Propagation.
static void on_accel_event(void)
Definition: main.c:313
void vi_notify_gps_available(void)
#define GpsEvent(_sol_available_callback)
Definition: gps_ardrone2.h:39
struct Ahrs ahrs
global AHRS state
Definition: ahrs.c:30
Main include for ABI (AirBorneInterface).
uint8_t tid_t
sys_time timer id type
Definition: sys_time.h:57
#define ImuScaleMag(_imu)
Definition: ahrs_gx3.h:59
void ahrs_update_gps(void)
Update AHRS state with GPS measurements.
STATIC_INLINE void main_init(void)
Definition: main.c:140
void imu_periodic(void)
Definition: imu_apogee.c:87
Integrated Navigation System interface.
void baro_init(void)
Definition: baro_board.c:68
Autopilot modes.
void ins_init(void)
INS initialization.
Definition: ins_alt_float.c:76
uint8_t status
Definition: ahrs_aligner.h:45
uint8_t fix
status of fix
Definition: gps.h:78
static void on_mag_event(void)
Definition: main.c:351
#define GPS_FIX_3D
Definition: gps.h:43
STATIC_INLINE void handle_periodic_tasks(void)
Definition: main.c:200
tid_t modules_tid
id for modules_periodic_task() timer
Definition: main.c:119
#define BARO_BOARD
Definition: baro_board.h:33
void ins_propagate()
Propagation.
Definition: ins_ardrone2.c:93
void baro_periodic(void)
Definition: baro_board.c:76
Rotorcraft main loop.
void settings_init(void)
Definition: settings.c:41
#define mcu_int_enable()
Definition: mcu_arch.h:37
void autopilot_check_in_flight(bool_t motors_on)
Definition: autopilot.c:436
void stateInit(void)
Definition: state.c:43
#define AHRS_PROPAGATE_FREQUENCY
Definition: main_ap.c:134
void ahrs_update_mag(void)
Update AHRS state with magnetometer measurements.
#define RadioControlEvent(_received_frame_handler)
Definition: dummy.h:28
tid_t radio_control_tid
id for radio_control_periodic_task() timer
Definition: main.c:121
struct AhrsAligner ahrs_aligner
Definition: ahrs_aligner.c:35
tid_t telemetry_tid
id for telemetry_periodic() timer
Definition: main.c:123
Interface for electrical status: supply voltage, current, battery status, etc.
Architecture independent timing functions.
void motor_mixing_init(void)
Definition: motor_mixing.c:96
static void on_gps_event(void)
Definition: main.c:342
Device independent GPS code (interface)
bool_t autopilot_in_flight
Definition: autopilot.c:50
struct Imu imu
global IMU state
Definition: imu_aspirin2.c:47
uint8_t status
status of the AHRS, AHRS_UNINIT or AHRS_RUNNING
Definition: ahrs.h:45
#define ImuScaleAccel(_imu)
Definition: ahrs_gx3.h:54
STATIC_INLINE void main_event(void)
Definition: main.c:285
#define BaroEvent
Definition: baro_board.h:36
int main(void)
Definition: main.c:42
static void radio_control_init(void)
Definition: radio_control.h:62
#define AHRS_ALIGNER_LOCKED
Definition: ahrs_aligner.h:37
#define SetActuatorsFromCommands(commands, AP_MODE)
Definition: actuators_at.h:34
bool_t autopilot_rc
Definition: autopilot.c:57
Air Data interface.
void autopilot_on_rc_frame(void)
Definition: autopilot.c:484
Hardware independent code for commands handling.
struct RadioControl radio_control
Definition: radio_control.c:25
tid_t failsafe_tid
id for failsafe_check() timer
Definition: main.c:120
Hardware independent API for actuators (servos, motor controllers).
#define AP_MODE_NAV
Definition: autopilot.h:52
#define ImuEvent(_gyro_handler, _accel_handler, _mag_handler)
Definition: ins_xsens.h:63
Inertial Measurement Unit interface.
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
void autopilot_periodic(void)
Definition: autopilot.c:279
void air_data_init(void)
AirData initialization.
Definition: air_data.c:61
#define RC_REALLY_LOST
Definition: radio_control.h:47
#define AHRS_UNINIT
Definition: ahrs.h:35
#define RC_OK
Definition: radio_control.h:45
uint8_t status
Definition: radio_control.h:50
uint16_t datalink_time
Definition: sim_ap.c:44
tid_t main_periodic_tid
id for main_periodic() timer
Definition: main.c:118
#define LED_PERIODIC()
Definition: led_hw.h:8
Arch independent mcu ( Micro Controller Unit ) utilities.
pprz_t commands[COMMANDS_NB]
Storage of intermediate command values.
Definition: commands.c:30
API to get/set the generic vehicle states.
Persistent settings interface.
void gps_periodic_check(void)
Periodic GPS check.
Definition: gps.c:136
General stabilization interface for rotorcrafts.
void ahrs_update_accel(void)
Update AHRS state with accerleration measurements.
static bool_t GpsIsLost(void)
Definition: gps.h:114
void vi_notify_imu_available(void)
#define STATIC_INLINE
Definition: main.h:34
#define PERIODIC_FREQUENCY
Definition: imu_aspirin2.c:51
arch independent LED (Light Emitting Diodes) API
#define AP_MODE_FAILSAFE
Definition: autopilot.h:40
#define AP_MODE_HOME
Definition: autopilot.h:41
struct Electrical electrical
STATIC_INLINE void main_periodic(void)
Definition: main.c:219
void udp_init(void)
Definition: udp.c:48
void actuators_init(void)
Definition: actuators_at.c:32
void i2c_event(void)
Definition: i2c_arch.c:368
STATIC_INLINE void failsafe_check(void)
Definition: main.c:245
Motor Mixing.
void imu_init(void)
Definition: imu.c:113
void electrical_periodic(void)
#define BARO_PERIODIC_FREQUENCY
Definition: main.c:101
void autopilot_set_mode(uint8_t new_autopilot_mode)
Definition: autopilot.c:338
void ahrs_aligner_init(void)
Definition: ahrs_aligner.c:62
void ahrs_align(void)
Aligns the AHRS.
void electrical_init(void)
struct GpsState gps
global GPS state
Definition: gps.c:41
void ahrs_aligner_run(void)
Definition: ahrs_aligner.c:84
void xbee_init(void)
Definition: xbee.c:87
static void DetectGroundEvent(void)
Ground detection based on vertical acceleration.
Definition: autopilot.h:152
void gps_init(void)
initialize the global GPS state
Definition: gps.c:107
#define ImuScaleGyro(_imu)
Definition: ahrs_gx3.h:49
#define AHRS_RUNNING
Definition: ahrs.h:36
tid_t electrical_tid
id for electrical_periodic() timer
Definition: main.c:122
#define RC_LOST_MODE
mode to enter when RC is lost while using a mode with RC input (not AP_MODE_NAV)
Definition: main.c:242
void ahrs_init(void)
AHRS initialization.
Architecture independent I2C (Inter-Integrated Circuit Bus) API.