Paparazzi UAS  v5.14.0_stable-0-g3f680d1
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) 2003-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 
31 #define MODULES_C
32 
33 #define ABI_C
34 
35 #include <math.h>
36 
38 #include "mcu.h"
39 #include "mcu_periph/sys_time.h"
40 #include "inter_mcu.h"
41 #include "link_mcu.h"
42 
43 // Sensors
44 #if USE_GPS
45 #include "subsystems/gps.h"
46 #endif
47 #if USE_IMU
48 #include "subsystems/imu.h"
49 #endif
50 #if USE_AHRS
51 #include "subsystems/ahrs.h"
52 #endif
53 #if USE_BARO_BOARD
55 PRINT_CONFIG_MSG_VALUE("USE_BARO_BOARD is TRUE, reading onboard baro: ", BARO_BOARD)
56 #endif
57 
58 
59 // autopilot
60 #include "state.h"
61 #include "autopilot.h"
63 #include "generated/flight_plan.h"
64 
65 // datalink & telemetry
66 #if PERIODIC_TELEMETRY
68 #endif
69 
70 // modules & settings
71 #include "subsystems/settings.h"
72 #include "generated/modules.h"
73 #include "generated/settings.h"
74 #if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
76 #endif
77 #include "subsystems/abi.h"
78 
79 #include "led.h"
80 
81 #ifdef USE_NPS
82 #include "nps_autopilot.h"
83 #endif
84 
85 /* Default trim commands for roll, pitch and yaw */
86 #ifndef COMMAND_ROLL_TRIM
87 #define COMMAND_ROLL_TRIM 0
88 #endif
89 
90 #ifndef COMMAND_PITCH_TRIM
91 #define COMMAND_PITCH_TRIM 0
92 #endif
93 
94 #ifndef COMMAND_YAW_TRIM
95 #define COMMAND_YAW_TRIM 0
96 #endif
97 
98 /* if PRINT_CONFIG is defined, print some config options */
99 PRINT_CONFIG_VAR(PERIODIC_FREQUENCY)
100 #if !USE_GENERATED_AUTOPILOT
101 PRINT_CONFIG_VAR(NAVIGATION_FREQUENCY)
102 #endif
103 PRINT_CONFIG_VAR(CONTROL_FREQUENCY)
104 
105 /* TELEMETRY_FREQUENCY is defined in generated/periodic_telemetry.h
106  * defaults to 60Hz or set by TELEMETRY_FREQUENCY configure option in airframe file
107  */
108 #ifndef TELEMETRY_FREQUENCY
109 #define TELEMETRY_FREQUENCY 60
110 #endif
111 PRINT_CONFIG_VAR(TELEMETRY_FREQUENCY)
112 
113 /* MODULES_FREQUENCY is defined in generated/modules.h
114  * according to main_freq parameter set for modules in airframe file
115  */
116 PRINT_CONFIG_VAR(MODULES_FREQUENCY)
117 
118 /* BARO_PERIODIC_FREQUENCY is defined in baro_board.makefile
119  * defaults to 50Hz or set by BARO_PERIODIC_FREQUENCY configure option in airframe file
120  */
121 #if USE_BARO_BOARD
122 PRINT_CONFIG_VAR(BARO_PERIODIC_FREQUENCY)
123 #endif
124 
125 
126 #if USE_IMU
127 #ifdef AHRS_PROPAGATE_FREQUENCY
128 #if (AHRS_PROPAGATE_FREQUENCY > PERIODIC_FREQUENCY)
129 #warning "PERIODIC_FREQUENCY should be least equal or greater than AHRS_PROPAGATE_FREQUENCY"
130 INFO_VALUE("it is recommended to configure in your airframe PERIODIC_FREQUENCY to at least ", AHRS_PROPAGATE_FREQUENCY)
131 #endif
132 #endif
133 #endif // USE_IMU
134 
135 
140 #if !USE_GENERATED_AUTOPILOT
142 #endif
144 #if USE_BARO_BOARD
145 tid_t baro_tid;
146 #endif
147 
148 
149 void init_ap(void)
150 {
151 #ifndef SINGLE_MCU
152  mcu_init();
153 #endif /* SINGLE_MCU */
154 
156  mcu_int_enable();
157 
158 #if defined(PPRZ_TRIG_INT_COMPR_FLASH)
159  pprz_trig_int_init();
160 #endif
161 
162  /****** initialize and reset state interface ********/
163 
164  stateInit();
165 
166  /************* Sensors initialization ***************/
167 
168 #if USE_AHRS
169  ahrs_init();
170 #endif
171 
172 #if USE_BARO_BOARD
173  baro_init();
174 #endif
175 
176  /************* Links initialization ***************/
177 #if defined MCU_SPI_LINK || defined MCU_UART_LINK || defined MCU_CAN_LINK
178  link_mcu_init();
179 #endif
180 
181  /************ Internal status ***************/
182  autopilot_init();
183 
184  modules_init();
185 
186  // call autopilot implementation init after guidance modules init
187  // it will set startup mode
188 #if USE_GENERATED_AUTOPILOT
190 #else
192 #endif
193 
194  settings_init();
195 
196  /**** start timers for periodic functions *****/
197  sensors_tid = sys_time_register_timer(1. / PERIODIC_FREQUENCY, NULL);
198 #if !USE_GENERATED_AUTOPILOT
199  navigation_tid = sys_time_register_timer(1. / NAVIGATION_FREQUENCY, NULL);
200 #endif
201  attitude_tid = sys_time_register_timer(1. / CONTROL_FREQUENCY, NULL);
202  modules_tid = sys_time_register_timer(1. / MODULES_FREQUENCY, NULL);
203  telemetry_tid = sys_time_register_timer(1. / TELEMETRY_FREQUENCY, NULL);
204  monitor_tid = sys_time_register_timer(1.0, NULL);
205 #if USE_BARO_BOARD
206  baro_tid = sys_time_register_timer(1. / BARO_PERIODIC_FREQUENCY, NULL);
207 #endif
208 
209 #if DOWNLINK
210  downlink_init();
211 #endif
212 
213  /* set initial trim values.
214  * these are passed to fbw via inter_mcu.
215  */
216  PPRZ_MUTEX_LOCK(ap_state_mtx);
217  ap_state->command_roll_trim = COMMAND_ROLL_TRIM;
218  ap_state->command_pitch_trim = COMMAND_PITCH_TRIM;
219  ap_state->command_yaw_trim = COMMAND_YAW_TRIM;
220  PPRZ_MUTEX_UNLOCK(ap_state_mtx);
221 
222 #if USE_IMU
223  // send body_to_imu from here for now
224  AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
225 #endif
226 }
227 
228 
230 {
231 
232  if (sys_time_check_and_ack_timer(sensors_tid)) {
233  sensors_task();
234  }
235 
236 #if USE_BARO_BOARD
237  if (sys_time_check_and_ack_timer(baro_tid)) {
238  baro_periodic();
239  }
240 #endif
241 
242 #if USE_GENERATED_AUTOPILOT
243  if (sys_time_check_and_ack_timer(attitude_tid)) {
245  }
246 #else
247  // static autopilot
248  if (sys_time_check_and_ack_timer(navigation_tid)) {
249  navigation_task();
250  }
251 
252 #ifndef AHRS_TRIGGERED_ATTITUDE_LOOP
253  if (sys_time_check_and_ack_timer(attitude_tid)) {
254  attitude_loop();
255  }
256 #endif
257 
258 #endif
259 
260  if (sys_time_check_and_ack_timer(modules_tid)) {
261  modules_periodic_task();
262  }
263 
264  if (sys_time_check_and_ack_timer(monitor_tid)) {
265  monitor_task();
266  }
267 
268  if (sys_time_check_and_ack_timer(telemetry_tid)) {
269  reporting_task();
270  LED_PERIODIC();
271  }
272 
273 }
274 
275 
276 
277 /**************************** Periodic tasks ***********************************/
278 
283 void reporting_task(void)
284 {
285  static uint8_t boot = true;
286 
287  /* initialisation phase during boot */
288  if (boot) {
289 #if DOWNLINK
291 #endif
292  boot = false;
293  }
294  /* then report periodicly */
295  else {
296  //PeriodicSendAp(DefaultChannel, DefaultDevice);
297 #if PERIODIC_TELEMETRY
298  periodic_telemetry_send_Ap(DefaultPeriodic, &(DefaultChannel).trans_tx, &(DefaultDevice).device);
299 #endif
300  }
301 }
302 
303 
304 
306 void sensors_task(void)
307 {
308  //FIXME: this is just a kludge
309 #if USE_AHRS && defined SITL && !USE_NPS
311 #endif
312 }
313 
314 #ifdef LOW_BATTERY_KILL_DELAY
315 #warning LOW_BATTERY_KILL_DELAY has been renamed to CATASTROPHIC_BAT_KILL_DELAY, please update your airframe file!
316 #endif
317 
319 #ifndef CATASTROPHIC_BAT_KILL_DELAY
320 #define CATASTROPHIC_BAT_KILL_DELAY 5
321 #endif
322 
324 #ifndef KILL_MODE_DISTANCE
325 #define KILL_MODE_DISTANCE (1.5*MAX_DIST_FROM_HOME)
326 #endif
327 
329 #ifndef MIN_SPEED_FOR_TAKEOFF
330 #define MIN_SPEED_FOR_TAKEOFF 5.
331 #endif
332 
334 void monitor_task(void)
335 {
336  if (autopilot.flight_time) {
338  }
339 #if defined DATALINK || defined SITL
340  datalink_time++;
341 #endif
342 
343  static uint8_t t = 0;
344  if (vsupply < CATASTROPHIC_BAT_LEVEL * 10) {
345  t++;
346  } else {
347  t = 0;
348  }
349 #if !USE_GENERATED_AUTOPILOT
350  // only check for static autopilot
353 #endif
354 
355  if (!autopilot.flight_time &&
358  autopilot.launch = true; /* Not set in non auto launch */
359 #if DOWNLINK
360  uint16_t time_sec = sys_time.nb_sec;
361  DOWNLINK_SEND_TAKEOFF(DefaultChannel, DefaultDevice, &time_sec);
362 #endif
363  }
364 
365 }
366 
367 
368 /*********** EVENT ***********************************************************/
369 void event_task_ap(void)
370 {
371 
372 #ifndef SINGLE_MCU
373  /* for SINGLE_MCU done in main_fbw */
374  /* event functions for mcu peripherals: i2c, usb_serial.. */
375  mcu_event();
376 #endif /* SINGLE_MCU */
377 
378 #if USE_BARO_BOARD
379  BaroEvent();
380 #endif
381 
382  modules_event_task();
383 
384 #if defined MCU_SPI_LINK || defined MCU_UART_LINK
386 #endif
387 
389  /* receive radio control task from fbw */
390  inter_mcu_received_fbw = false;
392  }
393 
394  autopilot_event();
395 
396 } /* event_task_ap() */
397 
unsigned short uint16_t
Definition: types.h:16
void attitude_loop(void)
bool launch
request launch
Definition: autopilot.h:66
Communication between fbw and ap processes.
Dispatcher to register actual AHRS implementations.
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:935
#define COMMAND_YAW_TRIM
Definition: main_ap.c:95
Common barometric sensor implementation.
tid_t attitude_tid
id for attitude_loop() timer
Definition: main_ap.c:139
#define AHRS_PROPAGATE_FREQUENCY
Definition: hf_float.c:55
Variable setting though the radio control.
Periodic telemetry system header (includes downlink utility and generated code).
void init_ap(void)
Definition: main_ap.c:149
tid_t sensors_tid
id for sensors_task() timer
Definition: main_ap.c:138
#define MIN_SPEED_FOR_TAKEOFF
Default minimal speed for takeoff in m/s.
Definition: main_ap.c:330
void autopilot_generated_init(void)
Main include for ABI (AirBorneInterface).
#define PPRZ_MUTEX_LOCK(_mtx)
Definition: pprz_mutex.h:46
tid_t monitor_tid
id for monitor_task() timer
Definition: main_ap.c:143
uint16_t flight_time
flight time in seconds
Definition: autopilot.h:61
void navigation_task(void)
Compute desired_course.
#define CONTROL_FREQUENCY
void baro_init(void)
Definition: baro_board.c:76
void autopilot_init(void)
Autopilot initialization function.
Definition: autopilot.c:91
struct Imu imu
global IMU state
Definition: imu.c:108
#define CATASTROPHIC_BAT_KILL_DELAY
Maximum time allowed for catastrophic battery level before going into kill mode.
Definition: main_ap.c:320
#define mcu_int_enable()
Definition: mcu_arch.h:36
float dist2_to_home
squared distance to home waypoint
Definition: navigation.c:82
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
struct ap_state * ap_state
Definition: inter_mcu.c:37
void baro_periodic(void)
Definition: baro_board.c:90
tid_t navigation_tid
id for navigation_task() timer
Definition: main_ap.c:141
void settings_init(void)
Definition: settings.c:43
void stateInit(void)
Definition: state.c:43
uint16_t vsupply
Supply voltage in deciVolt.
Architecture independent timing functions.
Device independent GPS code (interface)
#define COMMAND_ROLL_TRIM
Definition: main_ap.c:87
#define BaroEvent
Definition: baro_board.h:36
#define NAVIGATION_FREQUENCY
void update_ahrs_from_sim(void)
Definition: ahrs_sim.c:52
int8_t tid_t
sys_time timer id type
Definition: sys_time.h:60
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
#define TELEMETRY_FREQUENCY
Definition: main_ap.c:109
void WEAK autopilot_event(void)
AP event call.
Definition: autopilot.c:145
Inertial Measurement Unit interface.
struct OrientationReps body_to_imu
rotation from body to imu frame
Definition: imu.h:49
#define KILL_MODE_DISTANCE
Maximum distance from HOME waypoint before going into kill mode.
Definition: main_ap.c:325
void autopilot_static_init(void)
Static autopilot API.
uint16_t datalink_time
Definition: sim_ap.c:41
Core autopilot interface common to all firmwares.
volatile bool inter_mcu_received_fbw
Definition: inter_mcu.c:40
tid_t telemetry_tid
id for telemetry_periodic() timer
Definition: main_ap.c:137
#define COMMAND_PITCH_TRIM
Definition: main_ap.c:91
Arch independent mcu ( Micro Controller Unit ) utilities.
unsigned char uint8_t
Definition: types.h:14
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
volatile uint32_t nb_sec
full seconds since startup
Definition: sys_time.h:72
#define BARO_PERIODIC_FREQUENCY
Definition: main_ap.c:94
#define LED_PERIODIC()
Definition: led_hw.h:54
void autopilot_periodic(void)
AP periodic call.
Definition: autopilot.c:134
void reporting_task(void)
Send a series of initialisation messages followed by a stream of periodic ones.
Definition: main_ap.c:283
void handle_periodic_tasks_ap(void)
Definition: main_ap.c:229
void autopilot_on_rc_frame(void)
RC frame handler.
Definition: autopilot.c:149
arch independent LED (Light Emitting Diodes) API
void monitor_task(void)
monitor stuff run at 1Hz
Definition: main_ap.c:334
void sensors_task(void)
Run at PERIODIC_FREQUENCY (60Hz if not defined)
Definition: main_ap.c:306
void mcu_event(void)
MCU event functions.
Definition: mcu.c:241
bool kill_throttle
allow autopilot to use throttle
Definition: autopilot.h:64
void event_task_ap(void)
Definition: main_ap.c:369
void autopilot_send_version(void)
send autopilot version
Definition: autopilot.c:297
tid_t modules_tid
id for modules_periodic_task() timer
Definition: main_ap.c:136
#define PPRZ_MUTEX_UNLOCK(_mtx)
Definition: pprz_mutex.h:47
AP ( AutoPilot ) process API.
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).