Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
30
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 */
45PRINT_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 PERIODIC_FREQUENCY or set by TELEMETRY_FREQUENCY configure option in airframe file
53 */
54#if (TELEMETRY_FREQUENCY > SYS_TIME_FREQUENCY) || !(SYS_TIME_FREQUENCY/TELEMETRY_FREQUENCY*TELEMETRY_FREQUENCY == SYS_TIME_FREQUENCY)
55#warning "The TELEMETRY_FREQUENCY can not be faster than the SYS_TIME_FREQUENCY and needs to be dividable by the SYS_TIME_FREQUENCY."
56#endif
57PRINT_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"
62INFO_VALUE("it is recommended to configure in your airframe PERIODIC_FREQUENCY to at least ", AHRS_PROPAGATE_FREQUENCY)
63#endif
64#endif
65
72tid_t modules_gnc_tid; // estimation, control, actuators, default in a single step
74
75#define SYS_PERIOD (1.f / PERIODIC_FREQUENCY)
76#define SENSORS_PERIOD (1.f / PERIODIC_FREQUENCY)
77#define DATALINK_PERIOD (1.f / TELEMETRY_FREQUENCY)
78
79void main_ap_init(void)
80{
81 // mcu init done in main
82
83 modules_core_init();
84 modules_sensors_init();
85 modules_estimation_init();
86 modules_radio_control_init();
87 modules_control_init();
88 modules_actuators_init();
89 modules_datalink_init();
90 modules_default_init();
91
92 // register timers with temporal dependencies
94
95 // common GNC group (estimation, control, actuators, default)
96 // is called with an offset of half the main period (1/PERIODIC_FREQUENCY)
97 // which is the default resolution of SYS_TIME_FREQUENCY,
98 // hence the resolution of the virtual timers.
99 // In practice, this is the best compromised between having enough time between
100 // the sensor readings (triggerd in sensors task group) and the lag between
101 // the state update and control/actuators update
102 //
103 // | PERIODIC_FREQ |
104 // | | |
105 // read gnc
106 //
107 modules_gnc_tid = sys_time_register_timer_offset(modules_sensors_tid, 1.f / (2.f * PERIODIC_FREQUENCY), NULL);
108
109 // register the timers for the periodic functions
111 modules_radio_control_tid = sys_time_register_timer((1. / 60.), NULL); // FIXME
113
114 // Do a failsafe check first
116
117}
118
120{
122 modules_sensors_periodic_task();
123 }
124
126 modules_radio_control_periodic_task();
127 }
128
130 modules_estimation_periodic_task();
131 modules_control_periodic_task();
132 modules_default_periodic_task();
133 modules_actuators_periodic_task();
134 }
135
137 modules_mcu_periodic_task();
138 modules_core_periodic_task();
139 }
140
142 modules_datalink_periodic_task();
143 }
144}
145
147{
148 modules_mcu_event_task();
149 modules_core_event_task();
150 modules_sensors_event_task();
151 modules_estimation_event_task();
152 modules_radio_control_event_task();
153 modules_control_event_task();
154 modules_actuators_event_task();
155 modules_datalink_event_task();
156 modules_default_event_task();
157}
158
Main include for ABI (AirBorneInterface).
void WEAK autopilot_failsafe_checks(void)
Failsafe checks.
Definition autopilot.c:196
#define AHRS_PROPAGATE_FREQUENCY
Definition hf_float.c:55
void main_ap_periodic(void)
Definition main_ap.c:119
void main_ap_event(void)
Definition main_ap.c:146
tid_t modules_mcu_core_tid
IDs for timers.
Definition main_ap.c:69
#define SENSORS_PERIOD
Definition main_ap.c:76
tid_t modules_datalink_tid
Definition main_ap.c:73
#define DATALINK_PERIOD
Definition main_ap.c:77
tid_t modules_gnc_tid
Definition main_ap.c:72
void main_ap_init(void)
Definition main_ap.c:79
tid_t modules_radio_control_tid
Definition main_ap.c:71
tid_t modules_sensors_tid
Definition main_ap.c:70
#define SYS_PERIOD
Definition main_ap.c:75
Autopilot main loop.
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
tid_t sys_time_register_timer(float duration, sys_time_cb cb)
Register a new system timer.
Definition sys_time.c:172
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:188
static bool sys_time_check_and_ack_timer(tid_t id)
Check if timer has elapsed.
Definition sys_time.h:226
int8_t tid_t
sys_time timer id type
Definition sys_time.h:68