Paparazzi UAS  v5.15_devel-88-gb3ad7fe
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) 2018 Gautier Hattenberger <gautier.hattenberger@enac.fr>
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, see
18  * <http://www.gnu.org/licenses/>.
19  *
20  */
21 
28 #define MODULES_C
29 
30 #define ABI_C
31 
32 #include <inttypes.h>
33 #include "mcu.h"
34 #include "mcu_periph/sys_time.h"
35 #include "led.h"
36 
40 #include "subsystems/settings.h"
41 
42 #include "subsystems/commands.h"
43 #include "subsystems/actuators.h"
44 
45 #if USE_IMU
46 #include "subsystems/imu.h"
47 #endif
48 #if USE_GPS
49 #include "subsystems/gps.h"
50 #endif
51 
52 #if USE_BARO_BOARD
54 PRINT_CONFIG_MSG_VALUE("USE_BARO_BOARD is TRUE, reading onboard baro: ", BARO_BOARD)
55 #endif
56 
57 #include "subsystems/electrical.h"
58 
59 #include "autopilot.h"
60 
62 
63 #include "subsystems/ahrs.h"
64 
65 #include "state.h"
66 
68 
69 #include "generated/modules.h"
70 #include "subsystems/abi.h"
71 
72 // needed for stop-gap measure waypoints_localize_all()
74 
75 
76 /* if PRINT_CONFIG is defined, print some config options */
77 PRINT_CONFIG_VAR(PERIODIC_FREQUENCY)
78 /* SYS_TIME_FREQUENCY/PERIODIC_FREQUENCY should be an integer, otherwise the timer will not be correct */
79 #if !(SYS_TIME_FREQUENCY/PERIODIC_FREQUENCY*PERIODIC_FREQUENCY == SYS_TIME_FREQUENCY)
80 #warning "The SYS_TIME_FREQUENCY can not be divided by PERIODIC_FREQUENCY. Make sure this is the case for correct timing."
81 #endif
82 
83 /* TELEMETRY_FREQUENCY is defined in generated/periodic_telemetry.h
84  * defaults to 60Hz or set by TELEMETRY_FREQUENCY configure option in airframe file
85  */
86 PRINT_CONFIG_VAR(TELEMETRY_FREQUENCY)
87 
88 /* MODULES_FREQUENCY is defined in generated/modules.h
89  * according to main_freq parameter set for modules in airframe file
90  */
91 PRINT_CONFIG_VAR(MODULES_FREQUENCY)
92 
93 #ifndef BARO_PERIODIC_FREQUENCY
94 #define BARO_PERIODIC_FREQUENCY 50
95 #endif
96 PRINT_CONFIG_VAR(BARO_PERIODIC_FREQUENCY)
97 
98 #if USE_AHRS && USE_IMU && (defined AHRS_PROPAGATE_FREQUENCY)
99 #if (AHRS_PROPAGATE_FREQUENCY > PERIODIC_FREQUENCY)
100 #warning "PERIODIC_FREQUENCY should be least equal or greater than AHRS_PROPAGATE_FREQUENCY"
101 INFO_VALUE("it is recommended to configure in your airframe PERIODIC_FREQUENCY to at least ", AHRS_PROPAGATE_FREQUENCY)
102 #endif
103 #endif
104 
111 #if USE_BARO_BOARD
112 tid_t baro_tid;
113 #endif
114 
115 void main_init(void)
116 {
117  mcu_init();
118 
119 #if defined(PPRZ_TRIG_INT_COMPR_FLASH)
120  pprz_trig_int_init();
121 #endif
122 
123  electrical_init();
124 
125  stateInit();
126 
127  actuators_init();
128 
129 #if USE_MOTOR_MIXING
131 #endif
132 
134 
135 #if USE_BARO_BOARD
136  baro_init();
137 #endif
138 
139 #if USE_AHRS
140  ahrs_init();
141 #endif
142 
143  autopilot_init();
144 
145  modules_init();
146 
147  // call autopilot implementation init after guidance modules init
148  // it will set startup mode
150 
151  /* temporary hack:
152  * Since INS is now a module, LTP_DEF is not yet initialized when autopilot_init is called
153  * This led to the problem that global waypoints were not "localized",
154  * so as a stop-gap measure we localize them all (again) here..
155  */
156  //waypoints_localize_all();
157 
158  settings_init();
159 
160  mcu_int_enable();
161 
162 #if DOWNLINK
163  downlink_init();
164 #endif
165 
166  // register the timers for the periodic functions
167  main_periodic_tid = sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL);
168 #if PERIODIC_FREQUENCY != MODULES_FREQUENCY
169  modules_tid = sys_time_register_timer(1. / MODULES_FREQUENCY, NULL);
170 #endif
171  radio_control_tid = sys_time_register_timer((1. / 60.), NULL);
172  failsafe_tid = sys_time_register_timer(0.05, NULL);
173  electrical_tid = sys_time_register_timer(0.1, NULL);
174  telemetry_tid = sys_time_register_timer((1. / TELEMETRY_FREQUENCY), NULL);
175 #if USE_BARO_BOARD
176  baro_tid = sys_time_register_timer(1. / BARO_PERIODIC_FREQUENCY, NULL);
177 #endif
178 
179 #if USE_IMU
180  // send body_to_imu from here for now
181  AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
182 #endif
183 
184  // Do a failsafe check first
185  failsafe_check();
186 
187 }
188 
190 {
191  if (sys_time_check_and_ack_timer(main_periodic_tid)) {
192  main_periodic();
193 #if PERIODIC_FREQUENCY == MODULES_FREQUENCY
194  /* Use the main periodc freq timer for modules if the freqs are the same
195  * This is mainly useful for logging each step.
196  */
197  modules_periodic_task();
198 #else
199  }
200  /* separate timer for modules, since it has a different freq than main */
201  if (sys_time_check_and_ack_timer(modules_tid)) {
202  modules_periodic_task();
203 #endif
204  }
205  if (sys_time_check_and_ack_timer(radio_control_tid)) {
207  }
208  if (sys_time_check_and_ack_timer(failsafe_tid)) {
209  failsafe_check();
210  }
211  if (sys_time_check_and_ack_timer(electrical_tid)) {
213  }
214  if (sys_time_check_and_ack_timer(telemetry_tid)) {
216  }
217 #if USE_BARO_BOARD
218  if (sys_time_check_and_ack_timer(baro_tid)) {
219  baro_periodic();
220  }
221 #endif
222 }
223 
224 void main_periodic(void)
225 {
226  /* run control loops */
228  /* set actuators */
229  //actuators_set(autopilot_get_motors_on());
230  SetActuatorsFromCommands(commands, autopilot_get_mode());
231 
232  if (autopilot_in_flight()) {
233  RunOnceEvery(PERIODIC_FREQUENCY, autopilot.flight_time++);
234  }
235 
236 #if defined DATALINK || defined SITL
237  RunOnceEvery(PERIODIC_FREQUENCY, datalink_time++);
238 #endif
239 
240  RunOnceEvery(10, LED_PERIODIC());
241 }
242 
244 {
245  static uint8_t boot = true;
246 
247  /* initialisation phase during boot */
248  if (boot) {
249 #if DOWNLINK
251 #endif
252  boot = false;
253  }
254  /* then report periodicly */
255  else {
256 #if PERIODIC_TELEMETRY
257  periodic_telemetry_send_Main(DefaultPeriodic, &(DefaultChannel).trans_tx, &(DefaultDevice).device);
258 #endif
259  }
260 }
261 
263 #ifndef RC_LOST_MODE
264 #define RC_LOST_MODE AP_MODE_FAILSAFE
265 #endif
266 
267 void failsafe_check(void)
268 {
270 }
271 
272 void main_event(void)
273 {
274  /* event functions for mcu peripherals: i2c, usb_serial.. */
275  mcu_event();
276 
277  if (autopilot.use_rc) {
279  }
280 
281 #if USE_BARO_BOARD
282  BaroEvent();
283 #endif
284 
285  autopilot_event();
286 
287  modules_event_task();
288 }
289 
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 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
void main_event(void)
Definition: main_ap.c:336
void autopilot_generated_init(void)
Main include for ABI (AirBorneInterface).
uint16_t flight_time
flight time in seconds
Definition: autopilot.h:65
void baro_init(void)
Definition: baro_board.c:76
tid_t failsafe_tid
id for failsafe_check() timer
Definition: main_ap.c:110
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
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
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 motor_mixing_init(void)
Definition: motor_mixing.c:109
void radio_control_periodic_task(void)
Definition: radio_control.c:46
Device independent GPS code (interface)
#define BaroEvent
Definition: baro_board.h:36
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 DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
Hardware independent code for commands handling.
#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
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
Arch independent mcu ( Micro Controller Unit ) utilities.
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.
Rover main loop.
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
#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
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