Paparazzi UAS  v4.2.2_stable-4-gcc32f65
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
main.c
Go to the documentation of this file.
1 /*
2  * $Id$
3  *
4  * Copyright (C) 2008-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 
25 #define MODULES_C
26 
27 #include <inttypes.h>
28 #include "mcu.h"
29 #include "mcu_periph/sys_time.h"
30 #include "led.h"
31 
35 #include "subsystems/settings.h"
37 
40 
41 #if defined RADIO_CONTROL
43 #pragma message "CAUTION! RadioControl roll and yaw channel inputs have been reversed to follow aerospace sign conventions.\n You will have to change your radio control xml file to get a positive value when pushing roll stick right and a positive value when pushing yaw stick right!"
44 #endif
45 
46 #include "subsystems/imu.h"
47 #include "subsystems/gps.h"
48 
50 #include "baro_board.h"
51 
52 #include "subsystems/electrical.h"
53 
55 
58 
59 #include "subsystems/ahrs.h"
60 #include "subsystems/ins.h"
61 
63 
64 #ifdef SITL
65 #include "nps_autopilot_rotorcraft.h"
66 #endif
67 
68 #include "generated/modules.h"
69 
70 static inline void on_gyro_event( void );
71 static inline void on_accel_event( void );
72 static inline void on_baro_abs_event( void );
73 static inline void on_baro_dif_event( void );
74 static inline void on_gps_event( void );
75 static inline void on_mag_event( void );
76 
77 
84 
85 #ifndef SITL
86 int main( void ) {
87  main_init();
88 
89  while(1) {
91  main_event();
92  }
93  return 0;
94 }
95 #endif /* SITL */
96 
97 STATIC_INLINE void main_init( void ) {
98 
99  mcu_init();
100 
101  electrical_init();
102 
103  actuators_init();
104  radio_control_init();
105 
106  baro_init();
107  imu_init();
108  autopilot_init();
109  nav_init();
110  guidance_h_init();
111  guidance_v_init();
113 
115  ahrs_init();
116 
117  ins_init();
118 
119 #if USE_GPS
120  gps_init();
121 #endif
122 
123  modules_init();
124 
125  settings_init();
126 
127  mcu_int_enable();
128 
129 #if DATALINK == XBEE
130  xbee_init();
131 #endif
132 
133  // register the timers for the periodic functions
135  radio_control_tid = sys_time_register_timer((1./60.), NULL);
138  baro_tid = sys_time_register_timer(0.02, NULL);
139  telemetry_tid = sys_time_register_timer((1./60.), NULL);
140 }
141 
144  main_periodic();
146  radio_control_periodic_task();
148  failsafe_check();
152  baro_periodic();
155 }
156 
158 
159  imu_periodic();
160 
161  /* run control loops */
163  /* set actuators */
165 
166  modules_periodic_task();
167 
168  if (autopilot_in_flight) {
169  RunOnceEvery(PERIODIC_FREQUENCY, { autopilot_flight_time++; datalink_time++; });
170  }
171 
172  RunOnceEvery(10, LED_PERIODIC());
173 }
174 
176  PeriodicSendMain(DefaultChannel,DefaultDevice);
177 }
178 
180  if (radio_control.status != RC_OK &&
183  {
185  }
186 
187 #if USE_GPS
188  if (radio_control.status != RC_OK &&
190  GpsIsLost())
191  {
193  }
194 #endif
195 }
196 
197 STATIC_INLINE void main_event( void ) {
198 
199  i2c_event();
200 
201  DatalinkEvent();
202 
203  if (autopilot_rc) {
205  }
206 
208 
210 
211 #if USE_GPS
213 #endif
214 
215 #ifdef FAILSAFE_GROUND_DETECT
217 #endif
218 
219  modules_event_task();
220 
221 }
222 
223 static inline void on_accel_event( void ) {
225 
226  if (ahrs.status != AHRS_UNINIT) {
228  }
229 }
230 
231 static inline void on_gyro_event( void ) {
232 
233  ImuScaleGyro(imu);
234 
235  if (ahrs.status == AHRS_UNINIT) {
238  ahrs_align();
239  }
240  else {
241  ahrs_propagate();
242 #ifdef SITL
243  if (nps_bypass_ahrs) sim_overwrite_ahrs();
244 #endif
245  ins_propagate();
246  }
247 #ifdef USE_VEHICLE_INTERFACE
249 #endif
250 }
251 
252 static inline void on_baro_abs_event( void ) {
253  ins_update_baro();
254 #ifdef USE_VEHICLE_INTERFACE
256 #endif
257 }
258 
259 static inline void on_baro_dif_event( void ) {
260 
261 }
262 
263 static inline void on_gps_event(void) {
264  ins_update_gps();
265 #ifdef USE_VEHICLE_INTERFACE
266  if (gps.fix == GPS_FIX_3D)
268 #endif
269 }
270 
271 static inline void on_mag_event(void) {
272  ImuScaleMag(imu);
273 
274 #if USE_MAGNETOMETER
275  if (ahrs.status == AHRS_RUNNING) {
276  ahrs_update_mag();
277  }
278 #endif
279 
280 #ifdef USE_VEHICLE_INTERFACE
282 #endif
283 }
STATIC_INLINE void main_init(void)
Definition: main.c:97
uint8_t autopilot_mode
Definition: autopilot.c:31
#define AP_MODE_KILL
Definition: autopilot.h:33
void actuators_set(bool_t motors_on)
Attitude and Heading Reference System interface.
STATIC_INLINE void main_periodic(void)
Definition: main.c:157
uint16_t datalink_time
Definition: main.c:24
void imu_periodic(void)
Definition: imu_b2_arch.c:83
static void on_accel_event(void)
Definition: main.c:223
void ahrs_update_accel(void)
Update AHRS state with accerleration measurements.
void i2c_event(void)
Definition: i2c_arch.c:344
void vi_notify_mag_available(void)
Definition: vi_overo_link.c:93
void baro_periodic(void)
Definition: baro_board.c:56
bool_t autopilot_in_flight
Definition: autopilot.c:34
static void on_baro_abs_event(void)
Definition: main.c:252
void vi_notify_gps_available(void)
Definition: vi_overo_link.c:89
uint16_t autopilot_flight_time
Definition: autopilot.c:36
#define BaroEvent(_b_abs_handler, _b_diff_handler)
Definition: baro_board.h:22
struct Ahrs ahrs
global AHRS state (fixed point version)
Definition: ahrs.c:24
tid_t failsafe_tid
id for failsafe_check() timer
Definition: main.c:79
uint8_t tid_t
sys_time timer id type
Definition: sys_time.h:43
Device independent INS code.
static void on_gyro_event(void)
Definition: main.c:231
#define ImuScaleGyro(_imu)
Definition: imu_nps.h:89
void stabilization_init(void)
Definition: stabilization.c:30
uint8_t status
Definition: ahrs_aligner.h:40
void autopilot_init(void)
Definition: autopilot.c:68
uint8_t fix
status of fix
Definition: gps.h:77
static void on_gps_event(void)
Definition: main.c:263
#define ImuEvent(_gyro_handler, _accel_handler, _mag_handler)
Definition: imu_navgo.h:58
#define GPS_FIX_3D
Definition: gps.h:42
tid_t telemetry_tid
id for telemetry_periodic() timer
Definition: main.c:83
#define PERIODIC_FREQUENCY
Definition: imu_aspirin2.c:54
STATIC_INLINE void telemetry_periodic(void)
Definition: main.c:175
bool_t autopilot_motors_on
Definition: autopilot.c:38
void settings_init(void)
Definition: settings.c:8
#define mcu_int_enable()
Definition: mcu_arch.h:32
void ins_update_baro()
Definition: ins.c:178
void ahrs_update_mag(void)
Update AHRS state with magnetometer measurements.
void guidance_v_init(void)
#define RadioControlEvent(_received_frame_handler)
Definition: dummy.h:30
void electrical_init(void)
Definition: electrical.c:39
void ahrs_init(void)
AHRS initialization.
Definition: ins_xsens.c:75
struct AhrsAligner ahrs_aligner
Definition: ahrs_aligner.c:30
#define GpsEvent(_sol_available_callback)
Definition: gps_mtk.h:73
Architecture independent timing functions.
Device independent GPS code (interface)
uint8_t status
status of the AHRS, AHRS_UNINIT or AHRS_RUNNING
Definition: ahrs.h:54
static void on_baro_dif_event(void)
Definition: main.c:259
void actuators_init(void)
void ins_propagate()
Definition: ins.c:142
void autopilot_set_mode(uint8_t new_autopilot_mode)
Definition: autopilot.c:117
#define AHRS_ALIGNER_LOCKED
Definition: ahrs_aligner.h:32
#define ImuScaleMag(_imu)
Definition: imu_b2.h:199
void ahrs_align(void)
Aligns the AHRS.
struct RadioControl radio_control
Definition: radio_control.c:27
#define AP_MODE_NAV
Definition: autopilot.h:44
Inertial Measurement Unit interface.
int sys_time_register_timer(float duration, sys_time_cb cb)
Register a new system timer.
Definition: sys_time.c:35
void baro_init(void)
Definition: baro_board.c:38
static bool_t sys_time_check_and_ack_timer(tid_t id)
Definition: sys_time.h:90
void ins_init(void)
Definition: fw_ins_vn100.c:38
static void on_mag_event(void)
Definition: main.c:271
#define AHRS_UNINIT
Definition: ahrs.h:33
tid_t main_periodic_tid
id for main_periodic() timer
Definition: main.c:78
void electrical_periodic(void)
Definition: electrical.c:53
#define LED_PERIODIC()
Definition: led_hw.h:8
arch independent mcu ( Micro Controller Unit ) utilities
void vi_notify_baro_abs_available(void)
Definition: vi_overo_link.c:97
void ins_update_gps(void)
Definition: ins.c:209
STATIC_INLINE void handle_periodic_tasks(void)
Definition: main.c:142
STATIC_INLINE void main_event(void)
Definition: main.c:197
General stabilization interface for rotorcrafts.
bool_t autopilot_rc
Definition: autopilot.c:41
void autopilot_periodic(void)
Definition: autopilot.c:87
void vi_notify_imu_available(void)
Definition: vi_overo_link.c:85
#define STATIC_INLINE
Definition: main.h:30
void autopilot_on_rc_frame(void)
Definition: autopilot.c:244
arch independent LED (Light Emitting Diodes) API
#define AP_MODE_FAILSAFE
Definition: autopilot.h:32
#define ImuScaleAccel(_imu)
Definition: imu_analog.h:49
struct Imu imu
global IMU state
Definition: imu_aspirin2.c:50
#define GpsIsLost()
Definition: gps.h:110
int main(void)
Definition: main.c:45
void imu_init(void)
Definition: imu.c:32
void ahrs_aligner_init(void)
Definition: ahrs_aligner.c:40
STATIC_INLINE void failsafe_check(void)
Definition: main.c:179
struct GpsState gps
global GPS state
Definition: gps.c:31
void ahrs_aligner_run(void)
Definition: ahrs_aligner.c:58
tid_t electrical_tid
id for electrical_periodic() timer
Definition: main.c:81
void guidance_h_init(void)
Definition: guidance_h.c:94
void xbee_init(void)
Definition: xbee.c:47
static void DetectGroundEvent(void)
Definition: autopilot.h:115
void gps_init(void)
Definition: gps.c:37
void ahrs_propagate(void)
Propagation.
#define AHRS_RUNNING
Definition: ahrs.h:34
void mcu_init(void)
Definition: mcu.c:57
tid_t baro_tid
id for baro_periodic() timer
Definition: main.c:82
tid_t radio_control_tid
id for radio_control_periodic_task() timer
Definition: main.c:80