Paparazzi UAS  v5.12_stable-4-g9b43e9b
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
main_ap.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 "led.h"
37 
41 #include "subsystems/settings.h"
42 
43 #include "subsystems/commands.h"
44 #include "subsystems/actuators.h"
45 
46 #if USE_IMU
47 #include "subsystems/imu.h"
48 #endif
49 #if USE_GPS
50 #include "subsystems/gps.h"
51 #endif
52 
53 #if USE_BARO_BOARD
55 PRINT_CONFIG_MSG_VALUE("USE_BARO_BOARD is TRUE, reading onboard baro: ", BARO_BOARD)
56 #endif
57 
58 #include "subsystems/electrical.h"
59 
60 #include "autopilot.h"
61 
63 
64 #include "subsystems/ahrs.h"
65 
66 #include "state.h"
67 
69 
70 #ifdef SITL
71 #include "nps_autopilot.h"
72 #endif
73 
74 #include "generated/modules.h"
75 #include "subsystems/abi.h"
76 
77 // needed for stop-gap measure waypoints_localize_all()
79 
80 
81 /* if PRINT_CONFIG is defined, print some config options */
82 PRINT_CONFIG_VAR(PERIODIC_FREQUENCY)
83 
84 /* TELEMETRY_FREQUENCY is defined in generated/periodic_telemetry.h
85  * defaults to 60Hz or set by TELEMETRY_FREQUENCY configure option in airframe file
86  */
87 PRINT_CONFIG_VAR(TELEMETRY_FREQUENCY)
88 
89 /* MODULES_FREQUENCY is defined in generated/modules.h
90  * according to main_freq parameter set for modules in airframe file
91  */
92 PRINT_CONFIG_VAR(MODULES_FREQUENCY)
93 
94 #ifndef BARO_PERIODIC_FREQUENCY
95 #define BARO_PERIODIC_FREQUENCY 50
96 #endif
97 PRINT_CONFIG_VAR(BARO_PERIODIC_FREQUENCY)
98 
99 #if USE_AHRS && USE_IMU && (defined AHRS_PROPAGATE_FREQUENCY)
100 #if (AHRS_PROPAGATE_FREQUENCY > PERIODIC_FREQUENCY)
101 #warning "PERIODIC_FREQUENCY should be least equal or greater than AHRS_PROPAGATE_FREQUENCY"
102 INFO_VALUE("it is recommended to configure in your airframe PERIODIC_FREQUENCY to at least ", AHRS_PROPAGATE_FREQUENCY)
103 #endif
104 #endif
105 
112 #if USE_BARO_BOARD
113 tid_t baro_tid;
114 #endif
115 
116 void main_init(void)
117 {
118  mcu_init();
119 
120 #if defined(PPRZ_TRIG_INT_COMPR_FLASH)
121  pprz_trig_int_init();
122 #endif
123 
124  electrical_init();
125 
126  stateInit();
127 
128 #ifndef INTER_MCU_AP
129  actuators_init();
130 #else
131  intermcu_init();
132 #endif
133 
134 #ifndef INTER_MCU_AP
136 #endif
137 
138 #if USE_BARO_BOARD
139  baro_init();
140 #endif
141 
142 #if USE_AHRS
143  ahrs_init();
144 #endif
145 
146  autopilot_init();
147 
148  modules_init();
149 
150  // call autopilot implementation init after guidance modules init
151  // it will set startup mode
152 #if USE_GENERATED_AUTOPILOT
154 #else
156 #endif
157 
158  /* temporary hack:
159  * Since INS is now a module, LTP_DEF is not yet initialized when autopilot_init is called
160  * This led to the problem that global waypoints were not "localized",
161  * so as a stop-gap measure we localize them all (again) here..
162  */
164 
165  settings_init();
166 
167  mcu_int_enable();
168 
169 #if DOWNLINK
170  downlink_init();
171 #endif
172 
173 #ifdef INTER_MCU_AP
174  intermcu_init();
175 #endif
176 
177  // register the timers for the periodic functions
178  main_periodic_tid = sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL);
179 #if PERIODIC_FREQUENCY != MODULES_FREQUENCY
180  modules_tid = sys_time_register_timer(1. / MODULES_FREQUENCY, NULL);
181 #endif
182  radio_control_tid = sys_time_register_timer((1. / 60.), NULL);
183  failsafe_tid = sys_time_register_timer(0.05, NULL);
184  electrical_tid = sys_time_register_timer(0.1, NULL);
185  telemetry_tid = sys_time_register_timer((1. / TELEMETRY_FREQUENCY), NULL);
186 #if USE_BARO_BOARD
187  baro_tid = sys_time_register_timer(1. / BARO_PERIODIC_FREQUENCY, NULL);
188 #endif
189 
190 #if USE_IMU
191  // send body_to_imu from here for now
192  AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
193 #endif
194 
195  // Do a failsafe check first
196  failsafe_check();
197 
198 }
199 
201 {
202  if (sys_time_check_and_ack_timer(main_periodic_tid)) {
203  main_periodic();
204 #if PERIODIC_FREQUENCY == MODULES_FREQUENCY
205  /* Use the main periodc freq timer for modules if the freqs are the same
206  * This is mainly useful for logging each step.
207  */
208  modules_periodic_task();
209 #else
210  }
211  /* separate timer for modules, since it has a different freq than main */
212  if (sys_time_check_and_ack_timer(modules_tid)) {
213  modules_periodic_task();
214 #endif
215  }
216  if (sys_time_check_and_ack_timer(radio_control_tid)) {
218  }
219  if (sys_time_check_and_ack_timer(failsafe_tid)) {
220  failsafe_check();
221  }
222  if (sys_time_check_and_ack_timer(electrical_tid)) {
224  }
225  if (sys_time_check_and_ack_timer(telemetry_tid)) {
227  }
228 #if USE_BARO_BOARD
229  if (sys_time_check_and_ack_timer(baro_tid)) {
230  baro_periodic();
231  }
232 #endif
233 }
234 
235 void main_periodic(void)
236 {
237 #if INTER_MCU_AP
238  /* Inter-MCU watchdog */
240 #endif
241 
242  /* run control loops */
244  /* set actuators */
245  //actuators_set(autopilot_get_motors_on());
246 #ifndef INTER_MCU_AP
247  SetActuatorsFromCommands(commands, autopilot_get_mode());
248 #else
250 #endif
251 
252  if (autopilot_in_flight()) {
253  RunOnceEvery(PERIODIC_FREQUENCY, autopilot.flight_time++);
254  }
255 
256 #if defined DATALINK || defined SITL
257  RunOnceEvery(PERIODIC_FREQUENCY, datalink_time++);
258 #endif
259 
260  RunOnceEvery(10, LED_PERIODIC());
261 }
262 
264 {
265  static uint8_t boot = true;
266 
267  /* initialisation phase during boot */
268  if (boot) {
269 #if DOWNLINK
271 #endif
272  boot = false;
273  }
274  /* then report periodicly */
275  else {
276 #if PERIODIC_TELEMETRY
277  periodic_telemetry_send_Main(DefaultPeriodic, &(DefaultChannel).trans_tx, &(DefaultDevice).device);
278 #endif
279  }
280 }
281 
283 #ifndef RC_LOST_MODE
284 #define RC_LOST_MODE AP_MODE_FAILSAFE
285 #endif
286 
287 void failsafe_check(void)
288 {
289 #if !USE_GENERATED_AUTOPILOT
299  }
300 
301 #if FAILSAFE_ON_BAT_CRITICAL
302  if (autopilot_get_mode() != AP_MODE_KILL &&
305  }
306 #endif
307 
308 #if USE_GPS
309  if (autopilot_get_mode() == AP_MODE_NAV &&
311 #if NO_GPS_LOST_WITH_RC_VALID
313 #endif
314  GpsIsLost()) {
316  }
317 
318  if (autopilot_get_mode() == AP_MODE_HOME &&
321  }
322 #endif
323 
324 #endif // !USE_GENERATED_AUTOPILOT
325 
327 }
328 
329 void main_event(void)
330 {
331  /* event functions for mcu peripherals: i2c, usb_serial.. */
332  mcu_event();
333 
334  if (autopilot.use_rc) {
336  }
337 
338 #if USE_BARO_BOARD
339  BaroEvent();
340 #endif
341 
342  autopilot_event();
343 
344  modules_event_task();
345 }
void main_periodic(void)
Definition: main_ap.c:235
void mcu_init(void)
Microcontroller peripherals initialization.
Definition: mcu.c:76
Dispatcher to register actual AHRS implementations.
void failsafe_check(void)
Definition: main_ap.c:287
bool use_rc
enable/disable RC input
Definition: autopilot.h:66
Common barometric sensor implementation.
#define AP_MODE_KILL
Static autopilot modes.
#define AHRS_PROPAGATE_FREQUENCY
Definition: hf_float.c:51
Periodic telemetry system header (includes downlink utility and generated code).
bool autopilot_get_motors_on(void)
get motors status
Definition: autopilot.c:207
uint8_t status
Definition: radio_control.h:53
void main_event(void)
Definition: main_ap.c:329
void autopilot_generated_init(void)
Main include for ABI (AirBorneInterface).
void intermcu_periodic(void)
Definition: intermcu_ap.c:90
uint16_t flight_time
flight time in seconds
Definition: autopilot.h:61
void baro_init(void)
Definition: baro_board.c:76
#define AP_MODE_HOME
tid_t failsafe_tid
id for failsafe_check() timer
Definition: main_ap.c:108
bool autopilot_set_mode(uint8_t new_autopilot_mode)
set autopilot mode
Definition: autopilot.c:156
void autopilot_init(void)
Autopilot initialization function.
Definition: autopilot.c:91
struct Imu imu
global IMU state
Definition: imu.c:108
void WEAK autopilot_check_in_flight(bool motors_on)
in flight check utility function actual implementation is firmware dependent
Definition: autopilot.c:233
#define mcu_int_enable()
Definition: mcu_arch.h:36
void ahrs_init(void)
AHRS initialization.
Definition: ahrs.c:73
struct pprz_autopilot autopilot
Global autopilot structure.
Definition: autopilot.c:50
#define BARO_BOARD
Definition: baro_board.h:33
tid_t electrical_tid
id for electrical_periodic() timer
Definition: main_ap.c:110
#define AP_MODE_MODULE
void radio_control_init(void)
Definition: radio_control.c:32
void telemetry_periodic(void)
Definition: main_ap.c:263
void baro_periodic(void)
Definition: baro_board.c:90
void settings_init(void)
Definition: settings.c:43
void stateInit(void)
Definition: state.c:43
#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
bool bat_critical
battery critical status
Definition: electrical.h:53
Interface for electrical status: supply voltage, current, battery status, etc.
Architecture independent timing functions.
bool autopilot_in_flight(void)
get in_flight flag
Definition: autopilot.c:252
void radio_control_periodic_task(void)
Definition: radio_control.c:46
#define RC_LOST_MODE
mode to enter when RC is lost while using a mode with RC input (not AP_MODE_NAV)
Definition: main_ap.c:284
Device independent GPS code (interface)
#define BaroEvent
Definition: baro_board.h:36
#define AP_MODE_GUIDED
int8_t tid_t
sys_time timer id type
Definition: sys_time.h:60
tid_t radio_control_tid
id for radio_control_periodic_task() timer
Definition: main_ap.c:109
#define AP_MODE_FLIP
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
Hardware independent code for commands handling.
struct RadioControl radio_control
Definition: radio_control.c:30
Rotorcraft main loop.
#define TELEMETRY_FREQUENCY
Definition: main_ap.c:109
void WEAK autopilot_event(void)
AP event call.
Definition: autopilot.c:141
#define BARO_PERIODIC_FREQUENCY
Definition: main_ap.c:95
Inertial Measurement Unit interface.
struct OrientationReps body_to_imu
rotation from body to imu frame
Definition: imu.h:49
#define RC_REALLY_LOST
Definition: radio_control.h:50
#define RC_OK
Definition: radio_control.h:48
void autopilot_static_init(void)
Static autopilot API.
void electrical_periodic(void)
Definition: electrical.c:121
uint16_t datalink_time
Definition: sim_ap.c:41
Core autopilot interface common to all firmwares.
tid_t telemetry_tid
id for telemetry_periodic() timer
Definition: main_ap.c:137
void intermcu_init(void)
Definition: intermcu_ap.c:71
Arch independent mcu ( Micro Controller Unit ) utilities.
#define AP_MODE_NAV
unsigned char uint8_t
Definition: types.h:14
pprz_t commands[COMMANDS_NB]
Storage of intermediate command values.
Definition: commands.c:30
tid_t main_periodic_tid
id for main_periodic() timer
Definition: main_ap.c:106
API to get/set the generic vehicle states.
Persistent settings interface.
static bool sys_time_check_and_ack_timer(tid_t id)
Check if timer has elapsed.
Definition: sys_time.h:114
#define LED_PERIODIC()
Definition: led_hw.h:54
void autopilot_periodic(void)
AP periodic call.
Definition: autopilot.c:130
void autopilot_on_rc_frame(void)
RC frame handler.
Definition: autopilot.c:145
arch independent LED (Light Emitting Diodes) API
void main_init(void)
Definition: main_ap.c:116
void handle_periodic_tasks(void)
Definition: main_ap.c:200
void mcu_event(void)
MCU event functions.
Definition: mcu.c:231
struct Electrical electrical
Definition: electrical.c:65
#define GpsIsLost()
Definition: gps.h:45
void intermcu_set_actuators(pprz_t *command_values, uint8_t ap_mode)
Definition: intermcu_ap.c:111
void waypoints_localize_all(void)
update local ENU coordinates of global waypoints
Definition: waypoints.c:265
#define AP_MODE_FAILSAFE
void autopilot_send_version(void)
send autopilot version
Definition: autopilot.c:293
tid_t modules_tid
id for modules_periodic_task() timer
Definition: main_ap.c:136
tid_t sys_time_register_timer(float duration, sys_time_cb cb)
Register a new system timer.
Definition: sys_time.c:43
static struct FloatQuat * orientationGetQuat_f(struct OrientationReps *orientation)
Get vehicle body attitude quaternion (float).
uint8_t autopilot_get_mode(void)
get autopilot mode
Definition: autopilot.c:179