Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
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 #include <inttypes.h>
32 #include "mcu.h"
33 #include "mcu_periph/sys_time.h"
34 #include "led.h"
35 
39 #include "subsystems/settings.h"
41 #if DATALINK == UDP
43 #endif
44 
45 #include "subsystems/commands.h"
46 #include "subsystems/actuators.h"
47 #if USE_MOTOR_MIXING
49 #endif
50 
51 #include "subsystems/imu.h"
52 #include "subsystems/gps.h"
53 
55 
56 #include "subsystems/electrical.h"
57 
59 
62 
63 #include "subsystems/ahrs.h"
64 #include "subsystems/ins.h"
65 
66 #include "state.h"
67 
69 
70 #ifdef SITL
71 #include "nps_autopilot_rotorcraft.h"
72 #endif
73 
74 #include "generated/modules.h"
75 
76 
77 /* if PRINT_CONFIG is defined, print some config options */
78 PRINT_CONFIG_VAR(PERIODIC_FREQUENCY)
79 
80 #ifndef TELEMETRY_FREQUENCY
81 #define TELEMETRY_FREQUENCY 60
82 #endif
83 PRINT_CONFIG_VAR(TELEMETRY_FREQUENCY)
84 
85 #ifndef MODULES_FREQUENCY
86 #define MODULES_FREQUENCY 512
87 #endif
88 PRINT_CONFIG_VAR(MODULES_FREQUENCY)
89 
90 #ifndef BARO_PERIODIC_FREQUENCY
91 #define BARO_PERIODIC_FREQUENCY 50
92 #endif
93 PRINT_CONFIG_VAR(BARO_PERIODIC_FREQUENCY)
94 
95 
96 static inline void on_gyro_event( void );
97 static inline void on_accel_event( void );
98 static inline void on_baro_abs_event( void );
99 static inline void on_baro_dif_event( void );
100 static inline void on_gps_event( void );
101 static inline void on_mag_event( void );
102 
103 
111 
112 #ifndef SITL
113 int main( void ) {
114  main_init();
115 
116  while(1) {
118  main_event();
119  }
120  return 0;
121 }
122 #endif /* SITL */
123 
124 STATIC_INLINE void main_init( void ) {
125 
126  mcu_init();
127 
128  electrical_init();
129 
130  stateInit();
131 
132  actuators_init();
133 #if USE_MOTOR_MIXING
135 #endif
136 
138 
139  baro_init();
140  imu_init();
141 
143  ahrs_init();
144 
145  ins_init();
146 
147 #if USE_GPS
148  gps_init();
149 #endif
150 
151  autopilot_init();
152 
153  modules_init();
154 
155  settings_init();
156 
157  mcu_int_enable();
158 
159 #if DATALINK == XBEE
160  xbee_init();
161 #endif
162 
163 #if DATALINK == UDP
164  udp_init();
165 #endif
166 
167  // register the timers for the periodic functions
168  main_periodic_tid = sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL);
169  modules_tid = sys_time_register_timer(1./MODULES_FREQUENCY, NULL);
170  radio_control_tid = sys_time_register_timer((1./60.), NULL);
171  failsafe_tid = sys_time_register_timer(0.05, NULL);
172  electrical_tid = sys_time_register_timer(0.1, NULL);
174  telemetry_tid = sys_time_register_timer((1./TELEMETRY_FREQUENCY), NULL);
175 }
176 
178  if (sys_time_check_and_ack_timer(main_periodic_tid))
179  main_periodic();
180  if (sys_time_check_and_ack_timer(modules_tid))
181  modules_periodic_task();
182  if (sys_time_check_and_ack_timer(radio_control_tid))
184  if (sys_time_check_and_ack_timer(failsafe_tid))
185  failsafe_check();
186  if (sys_time_check_and_ack_timer(electrical_tid))
188  if (sys_time_check_and_ack_timer(baro_tid))
189  baro_periodic();
190  if (sys_time_check_and_ack_timer(telemetry_tid))
192 }
193 
195 
196  imu_periodic();
197 
198  /* run control loops */
200  /* set actuators */
201  //actuators_set(autopilot_motors_on);
203 
204  if (autopilot_in_flight) {
205  RunOnceEvery(PERIODIC_FREQUENCY, { autopilot_flight_time++; datalink_time++; });
206  }
207 
208  RunOnceEvery(10, LED_PERIODIC());
209 }
210 
212  PeriodicSendMain(DefaultChannel,DefaultDevice);
213 }
214 
216  if (radio_control.status != RC_OK &&
219  {
221  }
222 
223 #if USE_GPS
224  if (autopilot_mode == AP_MODE_NAV &&
226 #if NO_GPS_LOST_WITH_RC_VALID
228 #endif
229  GpsIsLost())
230  {
232  }
233 #endif
234 }
235 
236 STATIC_INLINE void main_event( void ) {
237 
238  i2c_event();
239 
240  DatalinkEvent();
241 
242  if (autopilot_rc) {
244  }
245 
247 
249 
250 #if USE_GPS
252 #endif
253 
254 #if FAILSAFE_GROUND_DETECT || KILL_ON_GROUND_DETECT
256 #endif
257 
258  modules_event_task();
259 
260 }
261 
262 static inline void on_accel_event( void ) {
264 
265  if (ahrs.status != AHRS_UNINIT) {
267  }
268 }
269 
270 static inline void on_gyro_event( void ) {
271 
272  ImuScaleGyro(imu);
273 
274  if (ahrs.status == AHRS_UNINIT) {
277  ahrs_align();
278  }
279  else {
280  ahrs_propagate();
281 #ifdef SITL
282  if (nps_bypass_ahrs) sim_overwrite_ahrs();
283 #endif
284  ins_propagate();
285  }
286 #ifdef USE_VEHICLE_INTERFACE
288 #endif
289 }
290 
291 static inline void on_baro_abs_event( void ) {
292  ins_update_baro();
293 #ifdef USE_VEHICLE_INTERFACE
295 #endif
296 }
297 
298 static inline void on_baro_dif_event( void ) {
299 
300 }
301 
302 static inline void on_gps_event(void) {
303  ins_update_gps();
304 #ifdef USE_VEHICLE_INTERFACE
305  if (gps.fix == GPS_FIX_3D)
307 #endif
308 }
309 
310 static inline void on_mag_event(void) {
311  ImuScaleMag(imu);
312 
313 #if USE_MAGNETOMETER
314  if (ahrs.status == AHRS_RUNNING) {
315  ahrs_update_mag();
316  }
317 #endif
318 
319 #ifdef USE_VEHICLE_INTERFACE
321 #endif
322 }
STATIC_INLINE void main_init(void)
Definition: main.c:124
#define AP_MODE_KILL
Definition: autopilot.h:40
void mcu_init(void)
Microcontroller peripherals initialization.
Definition: mcu.c:61
void electrical_init(void)
Attitude and Heading Reference System interface.
STATIC_INLINE void main_periodic(void)
Definition: main.c:194
uint16_t datalink_time
Definition: main.c:24
static void radio_control_periodic_task(void)
Definition: radio_control.h:75
static void on_accel_event(void)
Definition: main.c:262
Common barometric sensor implementation.
void autopilot_init(void)
Autopilot inititalization.
Definition: autopilot.c:48
#define TELEMETRY_FREQUENCY
Definition: main.c:81
void ins_update_gps(void)
Update INS state with GPS measurements.
uint8_t autopilot_mode
Definition: autopilot.c:39
#define BaroEvent(_b_abs_handler, _b_diff_handler)
Definition: baro_board.h:55
void vi_notify_mag_available(void)
Definition: vi_overo_link.c:91
bool_t autopilot_motors_on
Definition: autopilot.c:46
uint16_t autopilot_flight_time
flight time in seconds.
Definition: autopilot.c:37
void ahrs_propagate(void)
Propagation.
static void on_baro_abs_event(void)
Definition: main.c:291
void vi_notify_gps_available(void)
Definition: vi_overo_link.c:87
#define GpsEvent(_sol_available_callback)
Definition: gps_ardrone2.h:39
struct Ahrs ahrs
global AHRS state
Definition: ahrs.c:30
tid_t failsafe_tid
id for failsafe_check() timer
Definition: main.c:106
uint8_t tid_t
sys_time timer id type
Definition: sys_time.h:57
#define ImuScaleMag(_imu)
Definition: ahrs_gx3.h:64
void imu_periodic(void)
Definition: imu_apogee.c:79
Integrated Navigation System interface.
static void on_gyro_event(void)
Definition: main.c:270
void baro_init(void)
Definition: baro_board.c:42
Autopilot modes.
uint8_t status
Definition: ahrs_aligner.h:45
uint8_t fix
status of fix
Definition: gps.h:78
static void on_gps_event(void)
Definition: main.c:302
#define GPS_FIX_3D
Definition: gps.h:43
void imu_init(void)
Definition: ins_xsens700.c:86
tid_t telemetry_tid
id for telemetry_periodic() timer
Definition: main.c:110
void baro_periodic(void)
Definition: baro_board.c:50
Rotorcraft main loop.
STATIC_INLINE void telemetry_periodic(void)
Definition: main.c:211
void settings_init(void)
Definition: settings.c:8
bool_t GpsIsLost(void)
Definition: gps.h:114
#define mcu_int_enable()
Definition: mcu_arch.h:37
void stateInit(void)
Definition: state.c:43
#define RadioControlEvent(_received_frame_handler)
Definition: dummy.h:28
struct AhrsAligner ahrs_aligner
Definition: ahrs_aligner.c:35
Architecture independent timing functions.
void motor_mixing_init(void)
Definition: motor_mixing.c:78
Device independent GPS code (interface)
bool_t autopilot_in_flight
Definition: autopilot.c:42
struct Imu imu
global IMU state
Definition: imu_aspirin2.c:50
uint8_t status
status of the AHRS, AHRS_UNINIT or AHRS_RUNNING
Definition: ahrs.h:45
#define ImuScaleAccel(_imu)
Definition: ahrs_gx3.h:59
static void on_baro_dif_event(void)
Definition: main.c:298
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:49
void autopilot_on_rc_frame(void)
Definition: autopilot.c:309
Hardware independent code for commands handling.
struct RadioControl radio_control
Definition: radio_control.c:25
Hardware independent API for actuators (servos, motor controllers).
#define AP_MODE_NAV
Definition: autopilot.h:51
#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
#define BARO_PERIODIC_FREQUENCY
Definition: main.c:91
static void on_mag_event(void)
Definition: main.c:310
void autopilot_periodic(void)
Definition: autopilot.c:141
#define AHRS_UNINIT
Definition: ahrs.h:35
#define RC_OK
Definition: radio_control.h:45
uint8_t status
Definition: radio_control.h:50
tid_t main_periodic_tid
id for main_periodic() timer
Definition: main.c:104
#define LED_PERIODIC()
Definition: led_hw.h:8
Arch independent mcu ( Micro Controller Unit ) utilities.
void ins_propagate()
Propagation.
Definition: ins_alt_float.c:84
pprz_t commands[COMMANDS_NB]
Storage of intermediate command values.
Definition: commands.c:30
API to get/set the generic vehicle states.
void vi_notify_baro_abs_available(void)
Definition: vi_overo_link.c:95
void ins_update_baro()
Update INS state with barometer measurements.
Definition: ins_alt_float.c:87
#define MODULES_FREQUENCY
Definition: main.c:86
STATIC_INLINE void handle_periodic_tasks(void)
Definition: main.c:177
void ahrs_update_mag(void)
Update AHRS state with magnetometer measurements.
STATIC_INLINE void main_event(void)
Definition: main.c:236
General stabilization interface for rotorcrafts.
void electrical_periodic(void)
void ahrs_update_accel(void)
Update AHRS state with accerleration measurements.
void ins_init(void)
INS initialization.
Definition: ins_xsens700.c:178
void vi_notify_imu_available(void)
Definition: vi_overo_link.c:83
#define STATIC_INLINE
Definition: main.h:34
#define PERIODIC_FREQUENCY
Definition: imu_aspirin2.c:54
arch independent LED (Light Emitting Diodes) API
#define AP_MODE_FAILSAFE
Definition: autopilot.h:39
tid_t modules_tid
id for modules_periodic_task() timer
Definition: main.c:105
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:364
int main(void)
Definition: main.c:42
Motor Mixing.
void autopilot_set_mode(uint8_t new_autopilot_mode)
Definition: autopilot.c:174
void ahrs_aligner_init(void)
Definition: ahrs_aligner.c:45
void ahrs_align(void)
Aligns the AHRS.
STATIC_INLINE void failsafe_check(void)
Definition: main.c:215
struct GpsState gps
global GPS state
Definition: gps.c:33
void ahrs_aligner_run(void)
Definition: ahrs_aligner.c:63
tid_t electrical_tid
id for electrical_periodic() timer
Definition: main.c:108
void xbee_init(void)
Definition: xbee.c:87
static void DetectGroundEvent(void)
Ground detection based on vertical acceleration.
Definition: autopilot.h:149
void gps_init(void)
initialize the global GPS state
Definition: gps.c:37
#define ImuScaleGyro(_imu)
Definition: ahrs_gx3.h:54
#define AHRS_RUNNING
Definition: ahrs.h:36
tid_t baro_tid
id for baro_periodic() timer
Definition: main.c:109
void ahrs_init(void)
AHRS initialization.
tid_t radio_control_tid
id for radio_control_periodic_task() timer
Definition: main.c:107