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) 2003-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 
31 #define MODULES_C
32 
33 #define ABI_C
34 
35 #include <math.h>
36 
38 #include "mcu.h"
39 #include "mcu_periph/sys_time.h"
40 #include "inter_mcu.h"
41 #include "link_mcu.h"
42 
43 #include "generated/airframe.h"
44 #include "generated/modules.h"
45 #include "subsystems/abi.h"
46 
47 #include "led.h"
48 
49 #ifdef USE_NPS
50 #include "nps_autopilot.h"
51 #endif
52 
53 /* Default trim commands for roll, pitch and yaw */
54 #ifndef COMMAND_ROLL_TRIM
55 #define COMMAND_ROLL_TRIM 0
56 #endif
57 
58 #ifndef COMMAND_PITCH_TRIM
59 #define COMMAND_PITCH_TRIM 0
60 #endif
61 
62 #ifndef COMMAND_YAW_TRIM
63 #define COMMAND_YAW_TRIM 0
64 #endif
65 
66 /* if PRINT_CONFIG is defined, print some config options */
67 PRINT_CONFIG_VAR(PERIODIC_FREQUENCY)
68 #if !USE_GENERATED_AUTOPILOT
69 PRINT_CONFIG_VAR(NAVIGATION_FREQUENCY)
70 #endif
71 PRINT_CONFIG_VAR(CONTROL_FREQUENCY)
72 
73 /* TELEMETRY_FREQUENCY is defined in generated/periodic_telemetry.h
74  * defaults to 60Hz or set by TELEMETRY_FREQUENCY configure option in airframe file
75  */
76 #ifndef TELEMETRY_FREQUENCY
77 #define TELEMETRY_FREQUENCY 60
78 #endif
79 PRINT_CONFIG_VAR(TELEMETRY_FREQUENCY)
80 
81 
82 #if USE_IMU
83 #ifdef AHRS_PROPAGATE_FREQUENCY
84 #if (AHRS_PROPAGATE_FREQUENCY > PERIODIC_FREQUENCY)
85 #warning "PERIODIC_FREQUENCY should be least equal or greater than AHRS_PROPAGATE_FREQUENCY"
86 INFO_VALUE("it is recommended to configure in your airframe PERIODIC_FREQUENCY to at least ", AHRS_PROPAGATE_FREQUENCY)
87 #endif
88 #endif
89 #endif // USE_IMU
90 
94 tid_t modules_mcu_core_tid; // single step
96 //tid_t modules_radio_control_tid; // done in FBW
97 tid_t modules_gnc_tid; // estimation, control, actuators, default in a single step
99 
100 #define SYS_PERIOD (1.f / PERIODIC_FREQUENCY)
101 #define SENSORS_PERIOD (1.f / PERIODIC_FREQUENCY)
102 #define DATALINK_PERIOD (1.f / TELEMETRY_FREQUENCY)
103 
104 void init_ap(void)
105 {
106 #ifndef SINGLE_MCU
107  modules_mcu_init();
108 #endif
109  modules_core_init();
110  modules_sensors_init();
111  modules_estimation_init();
112  //radio_control_init(); FIXME done in FBW
113  // modules_radio_control_init(); FIXME
114  modules_control_init();
115  modules_actuators_init();
116  modules_datalink_init();
117  modules_default_init();
118 
119  // call autopilot implementation init after guidance modules init
120  // it will set startup mode
121 #if USE_GENERATED_AUTOPILOT
123 #else
125 #endif
126 
127  // register timers with temporal dependencies
128  modules_sensors_tid = sys_time_register_timer(SENSORS_PERIOD, NULL);
129 
130  // common GNC group (estimation, control, actuators, default)
131  // is called with an offset of half the main period (1/PERIODIC_FREQUENCY)
132  // which is the default resolution of SYS_TIME_FREQUENCY,
133  // hence the resolution of the virtual timers.
134  // In practice, this is the best compromised between having enough time between
135  // the sensor readings (triggerd in sensors task group) and the lag between
136  // the state update and control/actuators update
137  //
138  // | PERIODIC_FREQ |
139  // | | |
140  // read gnc
141  //
142  modules_gnc_tid = sys_time_register_timer_offset(modules_sensors_tid, 1.f / (2.f * PERIODIC_FREQUENCY), NULL);
143 
144  // register the timers for the periodic functions
145  modules_mcu_core_tid = sys_time_register_timer(SYS_PERIOD, NULL);
146  modules_datalink_tid = sys_time_register_timer(DATALINK_PERIOD, NULL);
147 
148  /* set initial trim values.
149  * these are passed to fbw via inter_mcu.
150  */
151  PPRZ_MUTEX_LOCK(ap_state_mtx);
152  ap_state->command_roll_trim = COMMAND_ROLL_TRIM;
153  ap_state->command_pitch_trim = COMMAND_PITCH_TRIM;
154  ap_state->command_yaw_trim = COMMAND_YAW_TRIM;
155  PPRZ_MUTEX_UNLOCK(ap_state_mtx);
156 
157 #if USE_IMU
158  // send body_to_imu from here for now
159  AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
160 #endif
161 
162 }
163 
164 
166 {
167  if (sys_time_check_and_ack_timer(modules_sensors_tid)) {
168  modules_sensors_periodic_task();
169  }
170 
171  if (sys_time_check_and_ack_timer(modules_gnc_tid)) {
172  modules_estimation_periodic_task();
173  modules_control_periodic_task();
174  modules_default_periodic_task();
175  }
176 
177  if (sys_time_check_and_ack_timer(modules_mcu_core_tid)) {
178  modules_mcu_periodic_task();
179  modules_core_periodic_task();
180  LED_PERIODIC(); // FIXME periodic in led module
181  }
182 
183  if (sys_time_check_and_ack_timer(modules_datalink_tid)) {
184  reporting_task();
185  modules_datalink_periodic_task(); // FIXME integrate above
186 #if defined DATALINK || defined SITL
187  RunOnceEvery(TELEMETRY_FREQUENCY, datalink_time++);
188 #endif
189  }
190 
191 }
192 
193 
194 
195 /**************************** Periodic tasks ***********************************/
196 
200 void reporting_task(void)
201 {
202  static uint8_t boot = true;
203 
204  /* initialisation phase during boot */
205  if (boot) {
206 #if DOWNLINK
208 #endif
209  boot = false;
210  }
211  /* then report periodicly */
212  else {
213 #if PERIODIC_TELEMETRY
214  periodic_telemetry_send_Ap(DefaultPeriodic, &(DefaultChannel).trans_tx, &(DefaultDevice).device);
215 #endif
216  }
217 }
218 
219 
220 /*********** EVENT ***********************************************************/
221 void event_task_ap(void)
222 {
223 #ifndef SINGLE_MCU
224  /* for SINGLE_MCU done in main_fbw */
225  /* event functions for mcu peripherals: i2c, usb_serial.. */
226  modules_mcu_event_task();
227 #endif /* SINGLE_MCU */
228  modules_core_event_task();
229  modules_sensors_event_task();
230  modules_estimation_event_task();
231  modules_datalink_event_task();
232  modules_default_event_task();
233 
234 
235  // TODO integrate in modules
236 #if defined MCU_SPI_LINK || defined MCU_UART_LINK
238 #endif
240  /* receive radio control task from fbw */
241  inter_mcu_received_fbw = false;
243  }
244 
245 } /* event_task_ap() */
246 
tid_t modules_gnc_tid
Definition: main_ap.c:97
#define DATALINK_PERIOD
Definition: main_ap.c:102
Communication between fbw and ap processes.
#define COMMAND_YAW_TRIM
Definition: main_ap.c:63
#define AHRS_PROPAGATE_FREQUENCY
Definition: hf_float.c:55
void init_ap(void)
Definition: main_ap.c:104
void autopilot_generated_init(void)
Main include for ABI (AirBorneInterface).
#define PPRZ_MUTEX_LOCK(_mtx)
Definition: pprz_mutex.h:46
#define CONTROL_FREQUENCY
struct Imu imu
global IMU state
Definition: imu.c:108
struct ap_state * ap_state
Definition: inter_mcu.c:37
#define SYS_PERIOD
Definition: main_ap.c:100
Architecture independent timing functions.
#define SENSORS_PERIOD
Definition: main_ap.c:101
#define COMMAND_ROLL_TRIM
Definition: main_ap.c:55
#define NAVIGATION_FREQUENCY
int8_t tid_t
sys_time timer id type
Definition: sys_time.h:56
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
#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
void autopilot_static_init(void)
Static autopilot API.
uint16_t datalink_time
Definition: sim_ap.c:41
volatile bool inter_mcu_received_fbw
Definition: inter_mcu.c:40
#define COMMAND_PITCH_TRIM
Definition: main_ap.c:59
Arch independent mcu ( Micro Controller Unit ) utilities.
static bool sys_time_check_and_ack_timer(tid_t id)
Check if timer has elapsed.
Definition: sys_time.h:119
#define LED_PERIODIC()
Definition: led_hw.h:55
void reporting_task(void)
Send a series of initialisation messages followed by a stream of periodic ones.
Definition: main_ap.c:200
void handle_periodic_tasks_ap(void)
Definition: main_ap.c:165
void autopilot_on_rc_frame(void)
RC frame handler.
Definition: autopilot.c:150
arch independent LED (Light Emitting Diodes) API
tid_t modules_mcu_core_tid
IDs for timers.
Definition: main_ap.c:94
tid_t modules_datalink_tid
Definition: main_ap.c:98
void event_task_ap(void)
Definition: main_ap.c:221
void autopilot_send_version(void)
send autopilot version
Definition: autopilot.c:298
#define PPRZ_MUTEX_UNLOCK(_mtx)
Definition: pprz_mutex.h:47
AP ( AutoPilot ) process API.
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).
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98