Paparazzi UAS  v6.3_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  // Do a failsafe check first
113 
114 }
115 
117 {
119  modules_sensors_periodic_task();
120  }
121 
123  modules_radio_control_periodic_task();
124  }
125 
127  modules_estimation_periodic_task();
128  modules_control_periodic_task();
129  modules_default_periodic_task();
130  modules_actuators_periodic_task();
131  }
132 
134  modules_mcu_periodic_task();
135  modules_core_periodic_task();
136  }
137 
139  modules_datalink_periodic_task();
140  }
141 }
142 
143 void main_ap_event(void)
144 {
145  modules_mcu_event_task();
146  modules_core_event_task();
147  modules_sensors_event_task();
148  modules_estimation_event_task();
149  modules_radio_control_event_task();
150  modules_control_event_task();
151  modules_actuators_event_task();
152  modules_datalink_event_task();
153  modules_default_event_task();
154 }
155 
Main include for ABI (AirBorneInterface).
void WEAK autopilot_failsafe_checks(void)
Failsafe checks.
Definition: autopilot.c:188
#define AHRS_PROPAGATE_FREQUENCY
Definition: hf_float.c:55
void main_ap_periodic(void)
Definition: main_ap.c:116
void main_ap_event(void)
Definition: main_ap.c:143
tid_t modules_mcu_core_tid
IDs for timers.
Definition: main_ap.c:66
#define SENSORS_PERIOD
Definition: main_ap.c:73
tid_t modules_datalink_tid
Definition: main_ap.c:70
#define DATALINK_PERIOD
Definition: main_ap.c:74
tid_t modules_gnc_tid
Definition: main_ap.c:69
void main_ap_init(void)
Definition: main_ap.c:76
tid_t modules_radio_control_tid
Definition: main_ap.c:68
tid_t modules_sensors_tid
Definition: main_ap.c:67
#define SYS_PERIOD
Definition: main_ap.c:72
Autopilot main loop.
tid_t sys_time_register_timer(float duration, sys_time_cb cb)
Register a new system timer.
Definition: sys_time.c:43
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
static bool sys_time_check_and_ack_timer(tid_t id)
Check if timer has elapsed.
Definition: sys_time.h:123
int8_t tid_t
sys_time timer id type
Definition: sys_time.h:60