Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
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, see
18  * <http://www.gnu.org/licenses/>.
19  *
20  */
21 
31 #define MODULES_C
32 
33 #define ABI_C
34 
35 #include "main_ap.h"
36 #include "generated/airframe.h"
37 #include "generated/modules.h"
38 #include "modules/core/abi.h"
39 
40 #ifdef USE_NPS
41 #include "nps_autopilot.h"
42 #endif
43 
44 /* if PRINT_CONFIG is defined, print some config options */
45 PRINT_CONFIG_VAR(PERIODIC_FREQUENCY)
46 /* SYS_TIME_FREQUENCY/PERIODIC_FREQUENCY should be an integer, otherwise the timer will not be correct */
47 #if !(SYS_TIME_FREQUENCY/PERIODIC_FREQUENCY*PERIODIC_FREQUENCY == SYS_TIME_FREQUENCY)
48 #warning "The SYS_TIME_FREQUENCY can not be divided by PERIODIC_FREQUENCY. Make sure this is the case for correct timing."
49 #endif
50 
51 /* TELEMETRY_FREQUENCY is defined in generated/periodic_telemetry.h
52  * defaults to 60Hz or set by TELEMETRY_FREQUENCY configure option in airframe file
53  */
54 PRINT_CONFIG_VAR(TELEMETRY_FREQUENCY)
55 
56 #if USE_AHRS && USE_IMU && (defined AHRS_PROPAGATE_FREQUENCY)
57 #if (AHRS_PROPAGATE_FREQUENCY > PERIODIC_FREQUENCY)
58 #warning "PERIODIC_FREQUENCY should be least equal or greater than AHRS_PROPAGATE_FREQUENCY"
59 INFO_VALUE("it is recommended to configure in your airframe PERIODIC_FREQUENCY to at least ", AHRS_PROPAGATE_FREQUENCY)
60 #endif
61 #endif
62 
66 tid_t modules_mcu_core_tid; // single step
69 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_ap_init(void)
77 {
78  // mcu init done in main
79 
80  modules_core_init();
81  modules_sensors_init();
82  modules_estimation_init();
83  modules_radio_control_init();
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 
111 #if USE_IMU
112  // send body_to_imu from here for now
113  AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
114 #endif
115 
116  // Do a failsafe check first
118 
119 }
120 
122 {
124  modules_sensors_periodic_task();
125  }
126 
128  modules_radio_control_periodic_task();
129  }
130 
132  modules_estimation_periodic_task();
133  modules_control_periodic_task();
134  modules_default_periodic_task();
135  modules_actuators_periodic_task();
136  }
137 
139  modules_mcu_periodic_task();
140  modules_core_periodic_task();
141  }
142 
144  modules_datalink_periodic_task();
145  }
146 }
147 
148 void main_ap_event(void)
149 {
150  modules_mcu_event_task();
151  modules_core_event_task();
152  modules_sensors_event_task();
153  modules_estimation_event_task();
154  modules_radio_control_event_task();
155  modules_control_event_task();
156  modules_actuators_event_task();
157  modules_datalink_event_task();
158  modules_default_event_task();
159 }
160 
main_ap_init
void main_ap_init(void)
Definition: main_ap.c:76
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_radio_control_tid
tid_t modules_radio_control_tid
Definition: main_ap.c:68
Imu::body_to_imu
struct OrientationReps body_to_imu
rotation from body to imu frame
Definition: imu.h:49
modules_datalink_tid
tid_t modules_datalink_tid
Definition: main_ap.c:70
modules_sensors_tid
tid_t modules_sensors_tid
Definition: main_ap.c:67
nps_autopilot.h
main_ap_periodic
void main_ap_periodic(void)
Definition: main_ap.c:121
DATALINK_PERIOD
#define DATALINK_PERIOD
Definition: main_ap.c:74
main_ap.h
orientationGetQuat_f
static struct FloatQuat * orientationGetQuat_f(struct OrientationReps *orientation)
Get vehicle body attitude quaternion (float).
Definition: pprz_orientation_conversion.h:225
modules_gnc_tid
tid_t modules_gnc_tid
Definition: main_ap.c:69
SENSORS_PERIOD
#define SENSORS_PERIOD
Definition: main_ap.c:73
tid_t
int8_t tid_t
sys_time timer id type
Definition: sys_time.h:60
modules_mcu_core_tid
tid_t modules_mcu_core_tid
IDs for timers.
Definition: main_ap.c:66
imu
struct Imu imu
global IMU state
Definition: imu.c:108
f
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup)
Definition: wedgebug.c:204
SYS_PERIOD
#define SYS_PERIOD
Definition: main_ap.c:72
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
AHRS_PROPAGATE_FREQUENCY
#define AHRS_PROPAGATE_FREQUENCY
Definition: hf_float.c:55
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:123
main_ap_event
void main_ap_event(void)
Definition: main_ap.c:148
autopilot_failsafe_checks
void WEAK autopilot_failsafe_checks(void)
Failsafe checks.
Definition: autopilot.c:188