Paparazzi UAS  v6.0_unstable-92-g17422e4-dirty
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-2021 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 "led.h"
35 
37 
39 
40 #ifdef SITL
41 #include "nps_autopilot.h"
42 #endif
43 
44 #include "generated/modules.h"
45 #include "subsystems/abi.h"
46 
47 /* if PRINT_CONFIG is defined, print some config options */
48 PRINT_CONFIG_VAR(PERIODIC_FREQUENCY)
49 /* SYS_TIME_FREQUENCY/PERIODIC_FREQUENCY should be an integer, otherwise the timer will not be correct */
50 #if !(SYS_TIME_FREQUENCY/PERIODIC_FREQUENCY*PERIODIC_FREQUENCY == SYS_TIME_FREQUENCY)
51 #warning "The SYS_TIME_FREQUENCY can not be divided by PERIODIC_FREQUENCY. Make sure this is the case for correct timing."
52 #endif
53 
54 /* TELEMETRY_FREQUENCY is defined in generated/periodic_telemetry.h
55  * defaults to 60Hz or set by TELEMETRY_FREQUENCY configure option in airframe file
56  */
57 PRINT_CONFIG_VAR(TELEMETRY_FREQUENCY)
58 
59 #if USE_AHRS && USE_IMU && (defined AHRS_PROPAGATE_FREQUENCY)
60 #if (AHRS_PROPAGATE_FREQUENCY > PERIODIC_FREQUENCY)
61 #warning "PERIODIC_FREQUENCY should be least equal or greater than AHRS_PROPAGATE_FREQUENCY"
62 INFO_VALUE("it is recommended to configure in your airframe PERIODIC_FREQUENCY to at least ", AHRS_PROPAGATE_FREQUENCY)
63 #endif
64 #endif
65 
69 tid_t modules_mcu_core_tid; // single step
72 tid_t modules_gnc_tid; // estimation, control, actuators, default in a single step
75 
76 #define SYS_PERIOD (1.f / PERIODIC_FREQUENCY)
77 #define SENSORS_PERIOD (1.f / PERIODIC_FREQUENCY)
78 #define DATALINK_PERIOD (1.f / TELEMETRY_FREQUENCY)
79 
80 void main_init(void)
81 {
82  modules_mcu_init();
83  modules_core_init();
84  modules_sensors_init();
85  modules_estimation_init();
86 #ifndef INTER_MCU_AP
88  // modules_radio_control_init(); FIXME
89 #endif
90  modules_control_init();
91  modules_actuators_init();
92  modules_datalink_init();
93  modules_default_init();
94 
95  // call autopilot implementation init after guidance modules init
96  // it will set startup mode
97 #if USE_GENERATED_AUTOPILOT
99 #else
101 #endif
102 
103  // register timers with temporal dependencies
104  modules_sensors_tid = sys_time_register_timer(SENSORS_PERIOD, NULL);
105 
106  // common GNC group (estimation, control, actuators, default)
107  // is called with an offset of half the main period (1/PERIODIC_FREQUENCY)
108  // which is the default resolution of SYS_TIME_FREQUENCY,
109  // hence the resolution of the virtual timers.
110  // In practice, this is the best compromised between having enough time between
111  // the sensor readings (triggerd in sensors task group) and the lag between
112  // the state update and control/actuators update
113  //
114  // | PERIODIC_FREQ |
115  // | | |
116  // read gnc
117  //
118  modules_gnc_tid = sys_time_register_timer_offset(modules_sensors_tid, 1.f / (2.f * PERIODIC_FREQUENCY), NULL);
119 
120  // register the timers for the periodic functions
121  modules_mcu_core_tid = sys_time_register_timer(SYS_PERIOD, NULL);
122  modules_radio_control_tid = sys_time_register_timer((1. / 60.), NULL); // FIXME
123  modules_datalink_tid = sys_time_register_timer(DATALINK_PERIOD, NULL);
124  failsafe_tid = sys_time_register_timer(0.05, NULL); // FIXME
125 
126 #if USE_IMU
127  // send body_to_imu from here for now
128  AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
129 #endif
130 
131  // Do a failsafe check first
132  failsafe_check();
133 
134 }
135 
137 {
138  if (sys_time_check_and_ack_timer(modules_sensors_tid)) {
139  modules_sensors_periodic_task();
140  }
141 
142  if (sys_time_check_and_ack_timer(modules_radio_control_tid)) {
144  modules_radio_control_periodic_task(); // FIXME integrate above
145  }
146 
147  if (sys_time_check_and_ack_timer(modules_gnc_tid)) {
148  modules_estimation_periodic_task();
149  modules_control_periodic_task();
150  modules_default_periodic_task();
151 #if USE_THROTTLE_CURVES
153 #endif
154 #ifndef INTER_MCU_AP
155  SetActuatorsFromCommands(commands, autopilot_get_mode());
156 #else
158 #endif
159  modules_actuators_periodic_task(); // FIXME integrate above in actuators periodic
160  if (autopilot_in_flight()) {
161  RunOnceEvery(PERIODIC_FREQUENCY, autopilot.flight_time++); // TODO make it 1Hz periodic ?
162  }
163  }
164 
165  if (sys_time_check_and_ack_timer(modules_mcu_core_tid)) {
166  modules_mcu_periodic_task();
167  modules_core_periodic_task();
168  RunOnceEvery(10, LED_PERIODIC()); // FIXME periodic in led module
169  }
170 
171  if (sys_time_check_and_ack_timer(modules_datalink_tid)) {
173  modules_datalink_periodic_task(); // FIXME integrate above
174 #if defined DATALINK || defined SITL
175  RunOnceEvery(TELEMETRY_FREQUENCY, datalink_time++);
176 #endif
177  }
178 
179  if (sys_time_check_and_ack_timer(failsafe_tid)) {
180  failsafe_check(); // FIXME integrate somewhere else
181  }
182 
183 }
184 
186 {
187  static uint8_t boot = true;
188 
189  /* initialisation phase during boot */
190  if (boot) {
191 #if DOWNLINK
193 #endif
194  boot = false;
195  }
196  /* then report periodicly */
197  else {
198 #if PERIODIC_TELEMETRY
199  periodic_telemetry_send_Main(DefaultPeriodic, &(DefaultChannel).trans_tx, &(DefaultDevice).device);
200 #endif
201  }
202 }
203 
205 #ifndef RC_LOST_MODE
206 #define RC_LOST_MODE AP_MODE_FAILSAFE
207 #endif
208 
209 void failsafe_check(void)
210 {
211 #if !USE_GENERATED_AUTOPILOT
221  }
222 
223 #if FAILSAFE_ON_BAT_CRITICAL
224  if (autopilot_get_mode() != AP_MODE_KILL &&
227  }
228 #endif
229 
230 #if USE_GPS
231  if (autopilot_get_mode() == AP_MODE_NAV &&
233 #if NO_GPS_LOST_WITH_RC_VALID
235 #endif
236 #ifdef NO_GPS_LOST_WITH_DATALINK_TIME
237  datalink_time > NO_GPS_LOST_WITH_DATALINK_TIME &&
238 #endif
239  GpsIsLost()) {
241  }
242 
243  if (autopilot_get_mode() == AP_MODE_HOME &&
246  }
247 #endif
248 
249 #endif // !USE_GENERATED_AUTOPILOT
250 
252 }
253 
254 void main_event(void)
255 {
256  modules_mcu_event_task();
257  modules_core_event_task();
258  modules_sensors_event_task();
259  modules_estimation_event_task();
260  modules_radio_control_event_task(); // FIXME
261  if (autopilot.use_rc) {
263  }
264  modules_actuators_event_task();
265  modules_datalink_event_task();
266  modules_default_event_task();
267 }
tid_t modules_gnc_tid
Definition: main_ap.c:97
void failsafe_check(void)
Definition: main_ap.c:209
bool use_rc
enable/disable RC input
Definition: autopilot.h:72
#define AP_MODE_KILL
Static autopilot modes.
#define AHRS_PROPAGATE_FREQUENCY
Definition: hf_float.c:55
#define RadioControlEvent(_received_frame_handler)
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:254
void autopilot_generated_init(void)
Main include for ABI (AirBorneInterface).
uint16_t flight_time
flight time in seconds
Definition: autopilot.h:65
#define AP_MODE_HOME
tid_t failsafe_tid
id for failsafe_check() timer FIXME
Definition: main_ap.c:74
bool autopilot_set_mode(uint8_t new_autopilot_mode)
set autopilot mode
Definition: autopilot.c:161
#define DATALINK_PERIOD
Definition: main_ap.c:78
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
struct pprz_autopilot autopilot
Global autopilot structure.
Definition: autopilot.c:50
#define AP_MODE_MODULE
void radio_control_init(void)
Definition: radio_control.c:32
void telemetry_periodic(void)
Definition: main_ap.c:185
#define SYS_PERIOD
Definition: main_ap.c:76
#define SENSORS_PERIOD
Definition: main_ap.c:77
bool bat_critical
battery critical status
Definition: electrical.h:50
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:206
tid_t modules_radio_control_tid
Definition: main_ap.c:71
#define AP_MODE_GUIDED
int8_t tid_t
sys_time timer id type
Definition: sys_time.h:56
#define AP_MODE_FLIP
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
struct RadioControl radio_control
Definition: radio_control.c:30
Rotorcraft main loop.
#define TELEMETRY_FREQUENCY
Definition: main_ap.c:77
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup) ...
Definition: wedgebug.c:204
tid_t sys_time_register_timer_offset(tid_t timer, float offset, sys_time_cb cb)
Register a new system timer with an fixed offset from another one.
Definition: sys_time.c:59
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.
uint16_t datalink_time
Definition: sim_ap.c:41
#define AP_MODE_NAV
static bool sys_time_check_and_ack_timer(tid_t id)
Check if timer has elapsed.
Definition: sys_time.h:119
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_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:80
void handle_periodic_tasks(void)
Definition: main_ap.c:136
tid_t modules_mcu_core_tid
IDs for timers.
Definition: main_ap.c:94
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
tid_t modules_datalink_tid
Definition: main_ap.c:98
#define AP_MODE_FAILSAFE
static const ShellCommand commands[]
Definition: shell_arch.c:71
void autopilot_send_version(void)
send autopilot version
Definition: autopilot.c:298
tid_t modules_sensors_tid
Definition: main_ap.c:95
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
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98