Paparazzi UAS  v6.1.0_stable
Paparazzi is a free software Unmanned Aircraft System.
main_ap.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2018-2021 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 "led.h"
34 
36 
38 
39 #include "generated/modules.h"
40 #include "modules/core/abi.h"
41 
42 /* if PRINT_CONFIG is defined, print some config options */
43 PRINT_CONFIG_VAR(PERIODIC_FREQUENCY)
44 /* SYS_TIME_FREQUENCY/PERIODIC_FREQUENCY should be an integer, otherwise the timer will not be correct */
45 #if !(SYS_TIME_FREQUENCY/PERIODIC_FREQUENCY*PERIODIC_FREQUENCY == SYS_TIME_FREQUENCY)
46 #warning "The SYS_TIME_FREQUENCY can not be divided by PERIODIC_FREQUENCY. Make sure this is the case for correct timing."
47 #endif
48 
49 /* TELEMETRY_FREQUENCY is defined in generated/periodic_telemetry.h
50  * defaults to 60Hz or set by TELEMETRY_FREQUENCY configure option in airframe file
51  */
52 PRINT_CONFIG_VAR(TELEMETRY_FREQUENCY)
53 
54 #if USE_AHRS && USE_IMU && (defined AHRS_PROPAGATE_FREQUENCY)
55 #if (AHRS_PROPAGATE_FREQUENCY > PERIODIC_FREQUENCY)
56 #warning "PERIODIC_FREQUENCY should be least equal or greater than AHRS_PROPAGATE_FREQUENCY"
57 INFO_VALUE("it is recommended to configure in your airframe PERIODIC_FREQUENCY to at least ", AHRS_PROPAGATE_FREQUENCY)
58 #endif
59 #endif
60 
64 tid_t modules_mcu_core_tid; // single step
67 tid_t modules_gnc_tid; // estimation, control, actuators, default in a single step
71 
72 #define SYS_PERIOD (1.f / PERIODIC_FREQUENCY)
73 #define SENSORS_PERIOD (1.f / PERIODIC_FREQUENCY)
74 #define DATALINK_PERIOD (1.f / TELEMETRY_FREQUENCY)
75 
76 void main_init(void)
77 {
78  modules_mcu_init();
79  modules_core_init();
80  modules_sensors_init();
81  modules_estimation_init();
83  // modules_radio_control_init(); FIXME
84  modules_control_init();
85  modules_actuators_init();
86  modules_datalink_init();
87  modules_default_init();
88 
89  // register timers with temporal dependencies
91 
92  // common GNC group (estimation, control, actuators, default)
93  // is called with an offset of half the main period (1/PERIODIC_FREQUENCY)
94  // which is the default resolution of SYS_TIME_FREQUENCY,
95  // hence the resolution of the virtual timers.
96  // In practice, this is the best compromised between having enough time between
97  // the sensor readings (triggerd in sensors task group) and the lag between
98  // the state update and control/actuators update
99  //
100  // | PERIODIC_FREQ |
101  // | | |
102  // read gnc
103  //
104  modules_gnc_tid = sys_time_register_timer_offset(modules_sensors_tid, 1.f / (2.f * PERIODIC_FREQUENCY), NULL);
105 
106  // register the timers for the periodic functions
108  modules_radio_control_tid = sys_time_register_timer((1. / 60.), NULL); // FIXME
110  failsafe_tid = sys_time_register_timer(0.05, NULL); // FIXME
111 
112 #if USE_IMU
113  // send body_to_imu from here for now
114  AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
115 #endif
116 
117  // Do a failsafe check first
118  failsafe_check();
119 
120 }
121 
123 {
125  modules_sensors_periodic_task();
126  }
127 
130  modules_radio_control_periodic_task(); // FIXME integrate above
131  }
132 
134  modules_estimation_periodic_task();
135  modules_control_periodic_task();
136  modules_default_periodic_task();
137  SetActuatorsFromCommands(commands, autopilot_get_mode());
138  modules_actuators_periodic_task(); // FIXME integrate above in actuators periodic
139  if (autopilot_in_flight()) {
140  RunOnceEvery(PERIODIC_FREQUENCY, autopilot.flight_time++); // TODO make it 1Hz periodic ?
141  }
142  }
143 
145  modules_mcu_periodic_task();
146  modules_core_periodic_task();
147  RunOnceEvery(10, LED_PERIODIC()); // FIXME periodic in led module
148  }
149 
152  modules_datalink_periodic_task(); // FIXME integrate above
153 #if defined DATALINK || defined SITL
154  RunOnceEvery(TELEMETRY_FREQUENCY, datalink_time++);
155 #endif
156  }
157 
159  failsafe_check(); // FIXME integrate somewhere else
160  }
161 
162 }
163 
165 {
166  static uint8_t boot = true;
167 
168  /* initialisation phase during boot */
169  if (boot) {
170 #if DOWNLINK
172 #endif
173  boot = false;
174  }
175  /* then report periodicly */
176  else {
177 #if PERIODIC_TELEMETRY
178  periodic_telemetry_send_Main(DefaultPeriodic, &(DefaultChannel).trans_tx, &(DefaultDevice).device);
179 #endif
180  }
181 }
182 
184 #ifndef RC_LOST_MODE
185 #define RC_LOST_MODE AP_MODE_FAILSAFE
186 #endif
187 
188 void failsafe_check(void)
189 {
191 }
192 
193 void main_event(void)
194 {
195  modules_mcu_event_task();
196  modules_core_event_task();
197  modules_sensors_event_task();
198  modules_estimation_event_task();
199  modules_radio_control_event_task(); // FIXME
200  if (autopilot.use_rc) {
202  }
203  modules_control_event_task();
204  modules_actuators_event_task();
205  modules_datalink_event_task();
206  modules_default_event_task();
207 }
208 
main_init
void main_init(void)
Definition: main_ap.c:80
main_ap.h
autopilot_get_motors_on
bool autopilot_get_motors_on(void)
get motors status
Definition: autopilot.c:239
radio_control.h
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
radio_control_periodic_task
void radio_control_periodic_task(void)
Definition: radio_control.c:46
modules_gnc_tid
tid_t modules_gnc_tid
Definition: main_ap.c:97
sys_time_register_timer_offset
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
abi.h
modules_default_tid
tid_t modules_default_tid
Definition: main_ap.c:69
main_event
void main_event(void)
Definition: main_ap.c:246
pprz_autopilot::flight_time
uint16_t flight_time
flight time in seconds
Definition: autopilot.h:65
Imu::body_to_imu
struct OrientationReps body_to_imu
rotation from body to imu frame
Definition: imu.h:49
pprz_autopilot::use_rc
bool use_rc
enable/disable RC input
Definition: autopilot.h:72
modules_radio_control_tid
tid_t modules_radio_control_tid
Definition: main_ap.c:71
DATALINK_PERIOD
#define DATALINK_PERIOD
Definition: main_ap.c:74
autopilot_in_flight
bool autopilot_in_flight(void)
get in_flight flag
Definition: autopilot.c:284
autopilot_send_version
void autopilot_send_version(void)
send autopilot version
Definition: autopilot.c:325
autopilot
struct pprz_autopilot autopilot
Global autopilot structure.
Definition: autopilot.c:50
orientationGetQuat_f
static struct FloatQuat * orientationGetQuat_f(struct OrientationReps *orientation)
Get vehicle body attitude quaternion (float).
Definition: pprz_orientation_conversion.h:225
LED_PERIODIC
#define LED_PERIODIC()
Definition: led_hw.h:55
telemetry_periodic
void telemetry_periodic(void)
Definition: main_ap.c:177
tid_t
int8_t tid_t
sys_time timer id type
Definition: sys_time.h:56
handle_periodic_tasks
void handle_periodic_tasks(void)
Definition: main_ap.c:128
led.h
arch independent LED (Light Emitting Diodes) API
imu
struct Imu imu
global IMU state
Definition: imu.c:108
radio_control_init
void radio_control_init(void)
Definition: radio_control.c:32
f
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup)
Definition: wedgebug.c:204
TELEMETRY_FREQUENCY
#define TELEMETRY_FREQUENCY
Definition: main_ap.c:77
commands
static const ShellCommand commands[]
Definition: shell_arch.c:71
RadioControlEvent
#define RadioControlEvent(_received_frame_handler)
Definition: cc2500_paparazzi.h:37
datalink_time
uint16_t datalink_time
Definition: sim_ap.c:41
SENSORS_PERIOD
#define SENSORS_PERIOD
Definition: main_ap.c:73
modules_mcu_core_tid
tid_t modules_mcu_core_tid
IDs for timers.
Definition: main_ap.c:94
sys_time_register_timer
tid_t sys_time_register_timer(float duration, sys_time_cb cb)
Register a new system timer.
Definition: sys_time.c:43
autopilot_check_in_flight
void WEAK autopilot_check_in_flight(bool motors_on)
in flight check utility function actual implementation is firmware dependent
Definition: autopilot.c:265
AHRS_PROPAGATE_FREQUENCY
#define AHRS_PROPAGATE_FREQUENCY
Definition: hf_float.c:55
autopilot_get_mode
uint8_t autopilot_get_mode(void)
get autopilot mode
Definition: autopilot.c:211
autopilot_on_rc_frame
void autopilot_on_rc_frame(void)
RC frame handler.
Definition: autopilot.c:176
failsafe_check
void failsafe_check(void)
Definition: main_ap.c:201
modules_sensors_tid
tid_t modules_sensors_tid
Definition: main_ap.c:95
modules_datalink_tid
tid_t modules_datalink_tid
Definition: main_ap.c:98
failsafe_tid
tid_t failsafe_tid
id for failsafe_check() timer FIXME
Definition: main_ap.c:74
SYS_PERIOD
#define SYS_PERIOD
Definition: main_ap.c:72
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
sys_time_check_and_ack_timer
static bool sys_time_check_and_ack_timer(tid_t id)
Check if timer has elapsed.
Definition: sys_time.h:119