Paparazzi UAS  v5.15_devel-81-gd13dafb
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 /* SYS_TIME_FREQUENCY/PERIODIC_FREQUENCY should be an integer, otherwise the timer will not be correct */
84 #if !(SYS_TIME_FREQUENCY/PERIODIC_FREQUENCY*PERIODIC_FREQUENCY == SYS_TIME_FREQUENCY)
85 #warning "The SYS_TIME_FREQUENCY can not be divided by PERIODIC_FREQUENCY. Make sure this is the case for correct timing."
86 #endif
87 
88 /* TELEMETRY_FREQUENCY is defined in generated/periodic_telemetry.h
89  * defaults to 60Hz or set by TELEMETRY_FREQUENCY configure option in airframe file
90  */
91 PRINT_CONFIG_VAR(TELEMETRY_FREQUENCY)
92 
93 /* MODULES_FREQUENCY is defined in generated/modules.h
94  * according to main_freq parameter set for modules in airframe file
95  */
96 PRINT_CONFIG_VAR(MODULES_FREQUENCY)
97 
98 /* BARO_PERIODIC_FREQUENCY is defined in the shared/baro_board.makefile and defaults to 50Hz */
99 PRINT_CONFIG_VAR(BARO_PERIODIC_FREQUENCY)
100 
101 #if USE_AHRS && USE_IMU && (defined AHRS_PROPAGATE_FREQUENCY)
102 #if (AHRS_PROPAGATE_FREQUENCY > PERIODIC_FREQUENCY)
103 #warning "PERIODIC_FREQUENCY should be least equal or greater than AHRS_PROPAGATE_FREQUENCY"
104 INFO_VALUE("it is recommended to configure in your airframe PERIODIC_FREQUENCY to at least ", AHRS_PROPAGATE_FREQUENCY)
105 #endif
106 #endif
107 
114 #if USE_BARO_BOARD
115 tid_t baro_tid;
116 #endif
117 
118 void main_init(void)
119 {
120  mcu_init();
121 
122 #if defined(PPRZ_TRIG_INT_COMPR_FLASH)
123  pprz_trig_int_init();
124 #endif
125 
126  electrical_init();
127 
128  stateInit();
129 
130 #ifndef INTER_MCU_AP
131  actuators_init();
132 #else
133  intermcu_init();
134 #endif
135 
136 #ifndef INTER_MCU_AP
138 #endif
139 
140 #if USE_BARO_BOARD
141  baro_init();
142 #endif
143 
144 #if USE_AHRS
145  ahrs_init();
146 #endif
147 
148  autopilot_init();
149 
150  modules_init();
151 
152  // call autopilot implementation init after guidance modules init
153  // it will set startup mode
154 #if USE_GENERATED_AUTOPILOT
156 #else
158 #endif
159 
160  /* temporary hack:
161  * Since INS is now a module, LTP_DEF is not yet initialized when autopilot_init is called
162  * This led to the problem that global waypoints were not "localized",
163  * so as a stop-gap measure we localize them all (again) here..
164  */
166 
167  settings_init();
168 
169  mcu_int_enable();
170 
171 #if DOWNLINK
172  downlink_init();
173 #endif
174 
175 #ifdef INTER_MCU_AP
176  intermcu_init();
177 #endif
178 
179  // register the timers for the periodic functions
180  main_periodic_tid = sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL);
181 #if PERIODIC_FREQUENCY != MODULES_FREQUENCY
182  modules_tid = sys_time_register_timer(1. / MODULES_FREQUENCY, NULL);
183 #endif
184  radio_control_tid = sys_time_register_timer((1. / 60.), NULL);
185  failsafe_tid = sys_time_register_timer(0.05, NULL);
186  electrical_tid = sys_time_register_timer(0.1, NULL);
187  telemetry_tid = sys_time_register_timer((1. / TELEMETRY_FREQUENCY), NULL);
188 #if USE_BARO_BOARD
189  baro_tid = sys_time_register_timer(1. / BARO_PERIODIC_FREQUENCY, NULL);
190 #endif
191 
192 #if USE_IMU
193  // send body_to_imu from here for now
194  AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
195 #endif
196 
197  // Do a failsafe check first
198  failsafe_check();
199 
200 }
201 
203 {
204  if (sys_time_check_and_ack_timer(main_periodic_tid)) {
205  main_periodic();
206 #if PERIODIC_FREQUENCY == MODULES_FREQUENCY
207  /* Use the main periodc freq timer for modules if the freqs are the same
208  * This is mainly useful for logging each step.
209  */
210  modules_periodic_task();
211 #else
212  }
213  /* separate timer for modules, since it has a different freq than main */
214  if (sys_time_check_and_ack_timer(modules_tid)) {
215  modules_periodic_task();
216 #endif
217  }
218  if (sys_time_check_and_ack_timer(radio_control_tid)) {
220  }
221  if (sys_time_check_and_ack_timer(failsafe_tid)) {
222  failsafe_check();
223  }
224  if (sys_time_check_and_ack_timer(electrical_tid)) {
226  }
227  if (sys_time_check_and_ack_timer(telemetry_tid)) {
229  }
230 #if USE_BARO_BOARD
231  if (sys_time_check_and_ack_timer(baro_tid)) {
232  baro_periodic();
233  }
234 #endif
235 }
236 
237 void main_periodic(void)
238 {
239 #if INTER_MCU_AP
240  /* Inter-MCU watchdog */
242 #endif
243 
244  /* run control loops */
246  /* set actuators */
247  //actuators_set(autopilot_get_motors_on());
248 
249 #if USE_THROTTLE_CURVES
251 #endif
252 
253 #ifndef INTER_MCU_AP
254  SetActuatorsFromCommands(commands, autopilot_get_mode());
255 #else
257 #endif
258 
259  if (autopilot_in_flight()) {
260  RunOnceEvery(PERIODIC_FREQUENCY, autopilot.flight_time++);
261  }
262 
263 #if defined DATALINK || defined SITL
264  RunOnceEvery(PERIODIC_FREQUENCY, datalink_time++);
265 #endif
266 
267  RunOnceEvery(10, LED_PERIODIC());
268 }
269 
271 {
272  static uint8_t boot = true;
273 
274  /* initialisation phase during boot */
275  if (boot) {
276 #if DOWNLINK
278 #endif
279  boot = false;
280  }
281  /* then report periodicly */
282  else {
283 #if PERIODIC_TELEMETRY
284  periodic_telemetry_send_Main(DefaultPeriodic, &(DefaultChannel).trans_tx, &(DefaultDevice).device);
285 #endif
286  }
287 }
288 
290 #ifndef RC_LOST_MODE
291 #define RC_LOST_MODE AP_MODE_FAILSAFE
292 #endif
293 
294 void failsafe_check(void)
295 {
296 #if !USE_GENERATED_AUTOPILOT
306  }
307 
308 #if FAILSAFE_ON_BAT_CRITICAL
309  if (autopilot_get_mode() != AP_MODE_KILL &&
312  }
313 #endif
314 
315 #if USE_GPS
316  if (autopilot_get_mode() == AP_MODE_NAV &&
318 #if NO_GPS_LOST_WITH_RC_VALID
320 #endif
321  GpsIsLost()) {
323  }
324 
325  if (autopilot_get_mode() == AP_MODE_HOME &&
328  }
329 #endif
330 
331 #endif // !USE_GENERATED_AUTOPILOT
332 
334 }
335 
336 void main_event(void)
337 {
338  /* event functions for mcu peripherals: i2c, usb_serial.. */
339  mcu_event();
340 
341  if (autopilot.use_rc) {
343  }
344 
345 #if USE_BARO_BOARD
346  BaroEvent();
347 #endif
348 
349  autopilot_event();
350 
351  modules_event_task();
352 }
void main_periodic(void)
Definition: main_ap.c:237
void mcu_init(void)
Microcontroller peripherals initialization.
Definition: mcu.c:83
Dispatcher to register actual AHRS implementations.
void failsafe_check(void)
Definition: main_ap.c:294
bool use_rc
enable/disable RC input
Definition: autopilot.h:72
Common barometric sensor implementation.
#define AP_MODE_KILL
Static autopilot modes.
#define AHRS_PROPAGATE_FREQUENCY
Definition: hf_float.c:55
Periodic telemetry system header (includes downlink utility and generated code).
bool autopilot_get_motors_on(void)
get motors status
Definition: autopilot.c:212
uint8_t status
Definition: radio_control.h:64
void main_event(void)
Definition: main_ap.c:336
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:65
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:110
bool autopilot_set_mode(uint8_t new_autopilot_mode)
set autopilot mode
Definition: autopilot.c:161
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:238
#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:112
#define AP_MODE_MODULE
void radio_control_init(void)
Definition: radio_control.c:32
void telemetry_periodic(void)
Definition: main_ap.c:270
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:100
bool bat_critical
battery critical status
Definition: electrical.h:50
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:257
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:291
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:111
#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:146
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:58
#define RC_OK
Definition: radio_control.h:56
void autopilot_static_init(void)
Static autopilot API.
void electrical_periodic(void)
Definition: electrical.c:123
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:108
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 BARO_PERIODIC_FREQUENCY
Definition: main_ap.c:94
void throttle_curve_run(pprz_t cmds[], uint8_t ap_mode)
Run the throttle curve and generate the output throttle and pitch This depends on the FMODE(flight mo...
#define LED_PERIODIC()
Definition: led_hw.h:55
void autopilot_periodic(void)
AP periodic call.
Definition: autopilot.c:135
void autopilot_on_rc_frame(void)
RC frame handler.
Definition: autopilot.c:150
arch independent LED (Light Emitting Diodes) API
void main_init(void)
Definition: main_ap.c:118
void handle_periodic_tasks(void)
Definition: main_ap.c:202
void mcu_event(void)
MCU event functions.
Definition: mcu.c:248
struct Electrical electrical
Definition: electrical.c:66
#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:293
#define AP_MODE_FAILSAFE
void autopilot_send_version(void)
send autopilot version
Definition: autopilot.c:298
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:184