Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
ekf_aw_wrapper.c
Go to the documentation of this file.
3#include <stdio.h>
4
5#include "state.h"
7#include "math/pprz_algebra.h"
8
9#include "modules/core/abi.h"
10
11#include "autopilot.h"
13
14#include "mcu_periph/sys_time.h" // FOR DEBUG
15
16
17
18#ifndef EKF_AW_WRAPPER_ROTWING
19#define EKF_AW_WRAPPER_ROTWING false
20#endif
21#ifndef EKF_AW_WRAPPER_RANDOM_INPUTS
22#define EKF_AW_WRAPPER_RANDOM_INPUTS false
23#endif
24#ifndef EKF_AW_QUICK_CONVERGENCE
25#define EKF_AW_QUICK_CONVERGENCE false
26#endif
27#ifndef EKF_AW_QUICK_CONVERGENCE_TIME
28#define EKF_AW_QUICK_CONVERGENCE_TIME 10.0f
29#endif
30#ifndef EKF_AW_DEBUG
31#define EKF_AW_DEBUG false
32#endif
33
34#if EKF_AW_WRAPPER_ROTWING
35#include "modules/rotwing_drone/wing_rotation_controller_servo.h"
36#endif
37
38#if PERIODIC_TELEMETRY
40
41// TO DO: implement circular wrap filter for psi angle
42// TO DO: modify force functions to simpler model (wing)
43
44// Telemetry Message functions
45static void send_airspeed_wind_ekf(struct transport_tx *trans, struct link_device *dev)
46{
47 uint8_t healthy = (uint8_t)ekf_aw.health.healthy;
48
49 pprz_msg_send_AIRSPEED_WIND_ESTIMATOR_EKF(trans, dev, AC_ID,
50 &ekf_aw.V_body.x, &ekf_aw.V_body.y, &ekf_aw.V_body.z,
51 &ekf_aw.wind.x, &ekf_aw.wind.y, &ekf_aw.wind.z,
52 &ekf_aw.offset.x, &ekf_aw.offset.y, &ekf_aw.offset.z,
53 &healthy,
54 &ekf_aw.health.crashes_n,
55 &ekf_aw.innov_V_gnd.x, &ekf_aw.innov_V_gnd.y, &ekf_aw.innov_V_gnd.z,
56 &ekf_aw.innov_acc_filt.x, &ekf_aw.innov_acc_filt.y, &ekf_aw.innov_acc_filt.z,
57 &ekf_aw.innov_V_pitot,
58 &ekf_aw.acc.x,
59 &ekf_aw.acc.y,
60 &ekf_aw.acc.z);
61}
62
63static void debug_vect(struct transport_tx *trans, struct link_device *dev, char *name, float *data, int datasize)
64{
65 pprz_msg_send_DEBUG_VECT(trans, dev, AC_ID,
66 strlen(name), name,
67 datasize, data);
68}
69
70
71
72static void send_airspeed_wind_ekf_debug(struct transport_tx *trans, struct link_device *dev)
73{
74 debug_vect(trans, dev, "process_cov", ekf_aw.process_cov, 12);
75 debug_vect(trans, dev, "meas_cov", ekf_aw.meas_cov, 7);
76 debug_vect(trans, dev, "state_cov", ekf_aw.state_cov, 9);
77
78 debug_vect(trans, dev, "fuselage_force", ekf_aw.fuselage_force, 3);
79 debug_vect(trans, dev, "wing_force", ekf_aw.wing_force, 3);
80 debug_vect(trans, dev, "elevator_force", ekf_aw.elevator_force, 3);
81 debug_vect(trans, dev, "hover_force", ekf_aw.hover_force, 3);
82 debug_vect(trans, dev, "pusher_force", ekf_aw.pusher_force, 3);
83
84 float rw_state[4];
85 rw_state[0] = ekf_aw.skew;
86 rw_state[1] = ekf_aw.elevator_angle;
87 rw_state[2] = ekf_aw.RPM_pusher;
88 rw_state[3] = ekf_aw.RPM_hover[0];
89
90 debug_vect(trans, dev, "rw_state", rw_state, 4);
91}
92#endif
93
94// RPM ABI Event
96float time_of_rpm = 0.0f;
97static void rpm_cb(uint8_t sender_id __attribute__((unused)), struct act_feedback_t *rpm_message, uint8_t num_act);
98
99// Filter struct
100struct ekfAw ekf_aw; // Local wrapper
102
103// Define settings to change filter tau value
104float tau_filter_high = 25.0f;
105float tau_filter_low = 0.2f;
106
107// Bool Reset EKF Filter
108bool reset_filter = false;
109
111 .x = 0.0f,
112 .y = 0.0f,
113 .z = 0.0f
114};
115
116// Define filter arrays
127
128#ifndef PERIODIC_FREQUENCY_AIRSPEED_EKF_FETCH
129#define PERIODIC_FREQUENCY_AIRSPEED_EKF_FETCH 100
130#endif
131
132#ifndef PERIODIC_FREQUENCY_AIRSPEED_EKF
133#define PERIODIC_FREQUENCY_AIRSPEED_EKF 25
134#endif
135
137{
138
139 // Define filter frequencies
140 float sample_time = 1.0f / PERIODIC_FREQUENCY_AIRSPEED_EKF_FETCH;
141 float tau_low = 1.0f / (2.0f * M_PI * tau_filter_low);
142 float tau_high = 1.0f / (2.0f * M_PI * tau_filter_high);
143
144 // Init filters
145 for (int8_t i = 0; i < 3; i++) {
146 init_butterworth_2_low_pass(&filt_groundspeed[i], tau_high, sample_time, 0.0); // Init filters groundspeed
147 init_butterworth_2_low_pass(&filt_acc[i], tau_high, sample_time, 0.0); // Init filters Accelerations
148 init_butterworth_2_low_pass(&filt_acc_low[i], tau_low, sample_time, 0.0); // Init filters Accelerations Low
149 init_butterworth_2_low_pass(&filt_rate[i], tau_high, sample_time, 0.0); // Init filters Rate
150 init_butterworth_2_low_pass(&filt_euler[i], tau_high, sample_time, 0.0); // Init filters Euler
151 }
152
153 for (int8_t i = 0; i < EKF_AW_RPM_HOVER_NUM; i++) {
154 init_butterworth_2_low_pass(&filt_hover_prop_rpm[i], tau_low, sample_time, 0.0);
155 ekf_aw.last_RPM_hover[i] = 0;
156 }
157
158 init_butterworth_2_low_pass(&filt_pusher_prop_rpm, tau_low, sample_time, 0.0); // Init filters Pusher Prop
159 init_butterworth_2_low_pass(&filt_skew, tau_low, sample_time, 0.0); // Init filters Skew
160 init_butterworth_2_low_pass(&filt_elevator_pprz, tau_low, sample_time, 0.0); // Init filters Pusher Prop
161 init_butterworth_2_low_pass(&filt_airspeed_pitot, tau_low, sample_time, 0.0); // Init filters Pusher Prop
162
163#if PERIODIC_TELEMETRY
164 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AIRSPEED_WIND_ESTIMATOR_EKF, send_airspeed_wind_ekf);
166#endif
167
168 // Init EKF Filter
169 ekf_aw_init();
170
171 // Register ABI message
172 AbiBindMsgACT_FEEDBACK(ACT_FEEDBACK_RPM_SENSOR_ID, &RPM_ev, rpm_cb);
173
174
175 //Get EKF param handle
177
178 // Related to in air / on ground logic
179 ekf_aw.in_air = false;
180 ekf_aw.internal_clock = 0;
181 ekf_aw.time_last_on_gnd = 0;
182
183 // For override of filter using dlsettings
184 ekf_aw.override_start = false;
185 ekf_aw.override_quick_convergence = false;
186
187 // Init of public filter states //TO DO: Is it necessary or just safer?
188 for (int i = 0; i < EKF_AW_RPM_HOVER_NUM; i++) {
189 ekf_aw.last_RPM_hover[i] = 0;
190 }
191 ekf_aw.last_RPM_pusher = 0;
192
193 ekf_aw.V_body.x = 0.0f; ekf_aw.V_body.y = 0.0f; ekf_aw.V_body.z = 0.0f;
194 ekf_aw.wind.x = 0.0f; ekf_aw.wind.y = 0.0f; ekf_aw.wind.z = 0.0f;
195 ekf_aw.offset.x = 0.0f; ekf_aw.offset.y = 0.0f; ekf_aw.offset.z = 0.0f;
196 ekf_aw.health.healthy = true; ekf_aw.health.crashes_n = 0.0f;
197 ekf_aw.innov_V_gnd.x = 0.0f; ekf_aw.innov_V_gnd.y = 0.0f; ekf_aw.innov_V_gnd.z = 0.0f;
198 ekf_aw.innov_acc_filt.y = 0.0f; ekf_aw.innov_acc_filt.y = 0.0f; ekf_aw.innov_acc_filt.z = 0.0f;
199 ekf_aw.innov_V_pitot = 0.0f;
200
201 ekf_aw.fuselage_force[0] = 0.0f; ekf_aw.fuselage_force[1] = 0.0f; ekf_aw.fuselage_force[2] = 0.0f;
202 ekf_aw.wing_force[0] = 0.0f; ekf_aw.wing_force[1] = 0.0f; ekf_aw.wing_force[2] = 0.0f;
203 ekf_aw.elevator_force[0] = 0.0f; ekf_aw.elevator_force[1] = 0.0f; ekf_aw.elevator_force[2] = 0.0f;
204 ekf_aw.pusher_force[0] = 0.0f; ekf_aw.pusher_force[1] = 0.0f; ekf_aw.pusher_force[2] = 0.0f;
205 ekf_aw.hover_force[0] = 0.0f; ekf_aw.hover_force[1] = 0.0f; ekf_aw.hover_force[2] = 0.0f;
206
207 ekf_aw.skew = 0.0f;
208 ekf_aw.elevator_angle = 0.0f;
209 ekf_aw.RPM_pusher = 0.0f;
210 for (int i = 0; i < EKF_AW_RPM_HOVER_NUM; i++) {
211 ekf_aw.RPM_hover[i] = 0.0f;
212 }
213};
214
216{
217
218 // FOR DEBUG
219 // uint32_t tic = get_sys_time_usec();
220
221 //printf("Running periodic Airspeed EKF Module\n");
222 //printf("Airspeed is: %2.2f\n",filt_groundspeed[0].o[0]);
223
224 // FOR DEBUG Random acc inputs to filter
226
227 ekf_aw.acc.x = 1.0f * rand() / RAND_MAX; // TO BE REMOVED
228 ekf_aw.acc.y = 1.0f * rand() / RAND_MAX; // TO BE REMOVED
229 ekf_aw.acc.z = 1.0f * rand() / RAND_MAX; // TO BE REMOVED
230 ekf_aw.gyro.p = 0.1f * rand() / RAND_MAX; // TO BE REMOVED
231 ekf_aw.gyro.q = 0.1f * rand() / RAND_MAX; // TO BE REMOVED
232 ekf_aw.gyro.r = 0.1f * rand() / RAND_MAX; // TO BE REMOVED
233 ekf_aw.euler.phi = 1.0f * rand() / RAND_MAX; // TO BE REMOVED
234 ekf_aw.euler.theta = 1.0f * rand() / RAND_MAX; // TO BE REMOVED
235 ekf_aw.euler.psi = 1.0f * rand() / RAND_MAX; // TO BE REMOVED
236 for (int i = 0; i < EKF_AW_RPM_HOVER_NUM; i++) {
237 ekf_aw.RPM_hover[i] = 1000.0f * rand() / RAND_MAX; // TO BE REMOVED
238 }
239 ekf_aw.RPM_pusher = 1000.0f * rand() / RAND_MAX; // TO BE REMOVED
240 ekf_aw.skew = 10.0f * rand() / RAND_MAX; // TO BE REMOVED
241 ekf_aw.elevator_angle = 10.0f * rand() / RAND_MAX; // TO BE REMOVED
242
243 ekf_aw.Vg_NED.x = 10.0f * rand() / RAND_MAX; // TO BE REMOVED
244 ekf_aw.Vg_NED.y = 10.0f * rand() / RAND_MAX; // TO BE REMOVED
245 ekf_aw.Vg_NED.z = 10.0f * rand() / RAND_MAX; // TO BE REMOVED
246
247 ekf_aw.acc_filt.x = 1.0f * rand() / RAND_MAX; // TO BE REMOVED
248 ekf_aw.acc_filt.y = 1.0f * rand() / RAND_MAX; // TO BE REMOVED
249 ekf_aw.acc_filt.z = 1.0f * rand() / RAND_MAX; // TO BE REMOVED
250
251 ekf_aw.V_pitot = 10.0f * rand() / RAND_MAX; // TO BE REMOVED
252 } else {
253 // Get latest filtered values to ekf struct
254 ekf_aw.acc.x = filt_acc[0].o[0];
255 ekf_aw.acc.y = filt_acc[1].o[0];
256 ekf_aw.acc.z = filt_acc[2].o[0];
257
258 ekf_aw.gyro.p = filt_rate[0].o[0];
259 ekf_aw.gyro.q = filt_rate[1].o[0];
260 ekf_aw.gyro.r = filt_rate[2].o[0];
261
262 ekf_aw.euler.phi = filt_euler[0].o[0];
263 ekf_aw.euler.theta = filt_euler[1].o[0];
264 //ekf_aw.euler.psi = filt_euler[2].o[0];
265 ekf_aw.euler.psi = stateGetNedToBodyEulers_f()->psi; // TO DO: implement circular wrap filter for psi angle
266
267 for (int8_t i = 0; i < EKF_AW_RPM_HOVER_NUM; i++) {
268 ekf_aw.RPM_hover[i] = filt_hover_prop_rpm[i].o[0];
269 }
270
271 ekf_aw.RPM_pusher = filt_pusher_prop_rpm.o[0];
272 ekf_aw.skew = filt_skew.o[0];
273 ekf_aw.elevator_angle = filt_elevator_pprz.o[0];
274
275 ekf_aw.Vg_NED.x = filt_groundspeed[0].o[0];
276 ekf_aw.Vg_NED.y = filt_groundspeed[1].o[0];
277 ekf_aw.Vg_NED.z = filt_groundspeed[2].o[0];
278
279 ekf_aw.acc_filt.x = filt_acc_low[0].o[0];
280 ekf_aw.acc_filt.y = filt_acc_low[1].o[0];
281 ekf_aw.acc_filt.z = filt_acc_low[2].o[0];
282
283 ekf_aw.V_pitot = filt_airspeed_pitot.o[0];
284
285 }
286
287 // Sample time of EKF filter
288 float sample_time = 1.0f / PERIODIC_FREQUENCY_AIRSPEED_EKF_FETCH;
289
290 // Check if in flight and altitude higher than 1m
292
293 // Propagate
294 if (ekf_aw.in_air | ekf_aw.override_start) {
295 // Quick convergence for the first 10 s
296 if (((ekf_aw.internal_clock - ekf_aw.time_last_on_gnd < PERIODIC_FREQUENCY_AIRSPEED_EKF * EKF_AW_QUICK_CONVERGENCE_TIME)
297 & EKF_AW_QUICK_CONVERGENCE) | ekf_aw.override_quick_convergence) {
298 ekf_params->quick_convergence = true;
299 } else {
300 ekf_params->quick_convergence = false;
301 }
302
303 ekf_aw_propagate(&ekf_aw.acc, &ekf_aw.gyro, &ekf_aw.euler, &ekf_aw.RPM_pusher, ekf_aw.RPM_hover, &ekf_aw.skew,
304 &ekf_aw.elevator_angle, &ekf_aw.Vg_NED, &ekf_aw.acc_filt, &ekf_aw.V_pitot, sample_time);
305 } else {
306 // Set body velocity to 0 when landed
308 };
309
310 // Get states, health and innovation from EKF filter
311 ekf_aw.V_body = ekf_aw_get_speed_body();
313 ekf_aw.offset = ekf_aw_get_offset();
314 ekf_aw.health = ekf_aw_get_health();
315 ekf_aw.innov_V_gnd = ekf_aw_get_innov_V_gnd();
316 ekf_aw.innov_acc_filt = ekf_aw_get_innov_accel_filt();
317 ekf_aw.innov_V_pitot = ekf_aw_get_innov_V_pitot();
318
319 // Get covariance
320 ekf_aw_get_meas_cov(ekf_aw.meas_cov);
321 ekf_aw_get_state_cov(ekf_aw.state_cov);
322 ekf_aw_get_process_cov(ekf_aw.process_cov);
323
324 // Get forces
325 ekf_aw_get_fuselage_force(ekf_aw.fuselage_force);
326 ekf_aw_get_wing_force(ekf_aw.wing_force);
327 ekf_aw_get_elevator_force(ekf_aw.elevator_force);
328 ekf_aw_get_hover_force(ekf_aw.hover_force);
329 ekf_aw_get_pusher_force(ekf_aw.pusher_force);
330
331 ekf_aw.internal_clock++;
332
333 // FOR DEBUG
334 //ekf_aw.offset.x = get_sys_time_usec()-tic;
335};
336
337// Function to get information from different modules and set it in the different filters
339{
340
341 // For debug
342 // uint32_t tic_2 = get_sys_time_usec();
343
344 // NED Speed
348
349 // Getting body accel
350 struct FloatVect3 body_accel_f = {0.0f, 0.0f, 0.0f};
351#if EKF_AW_WRAPPER_ROTWING
352 // If body accel available, can use this
353 struct Int32Vect3 *body_accel_i;
354 body_accel_i = stateGetAccelBody_i();
355 ACCELS_FLOAT_OF_BFP(body_accel_f, *body_accel_i);
356#else
357 // Transferring from NED to Body as body is not available right now
358 struct NedCoor_i *accel_tmp = stateGetAccelNed_i();
359 struct Int32Vect3 ned_accel_i, body_accel_i;
360 struct Int32RMat *ned_to_body_rmat = stateGetNedToBodyRMat_i();
361 VECT3_COPY(ned_accel_i, (*accel_tmp));
362 ned_accel_i.z += ACCEL_BFP_OF_REAL(-9.81); // Add gravity
363 int32_rmat_vmult(&body_accel_i, ned_to_body_rmat, &ned_accel_i);
364 ACCELS_FLOAT_OF_BFP(body_accel_f, body_accel_i);
365#endif
366
367 // Body accel
374
375 // Body rates
379
380 // Euler angles
383 //update_butterworth_2_low_pass(&filt_euler[2], stateGetNedToBodyEulers_f()->psi); // TO DO: implement circular wrap filter for psi angle
384
385 for (int8_t i = 0; i < EKF_AW_RPM_HOVER_NUM; i++) {
386 update_butterworth_2_low_pass(&filt_hover_prop_rpm[i], ekf_aw.last_RPM_hover[i] * 1.0f);
387 }
389
390#if EKF_AW_WRAPPER_ROTWING
391 update_butterworth_2_low_pass(&filt_skew, RadOfDeg(wing_rotation.wing_angle_deg));
392
393 // Get elevator pprz signal
394 int16_t *elev_pprz = &actuators_pprz[5];
395 float de = 0.0f;
396 // Calculate deflection angle in [deg]
397 de = (-0.004885417f * *elev_pprz + 36.6f) * 3.14f / 180.0f;
398
400#else
403#endif
404
406
407 // FOR DEBUG
408 //ekf_aw.offset.y = get_sys_time_usec()-tic_2;
409
410};
411
412// ABI callback that obtains the RPM from a module
413static void rpm_cb(uint8_t sender_id __attribute__((unused)), struct act_feedback_t *rpm_message,
414 uint8_t num_act_message)
415{
416 for (int i = 0; i < num_act_message; i++) {
417 // Sanity check that index is valid
418 if (rpm_message[i].idx < EKF_AW_RPM_HOVER_NUM) {
419 // Assign rpm to right actuator
420 switch (rpm_message[i].idx) {
421 case 0:
422 ekf_aw.last_RPM_hover[0] = rpm_message[i].rpm;
423 break;
424 case 1:
425 ekf_aw.last_RPM_hover[1] = rpm_message[i].rpm;
426 break;
427 case 2:
428 ekf_aw.last_RPM_hover[2] = rpm_message[i].rpm;
429 break;
430 case 3:
431 ekf_aw.last_RPM_hover[3] = rpm_message[i].rpm;
432 break;
433 case 4:
434 ekf_aw.last_RPM_pusher = rpm_message[i].rpm;
435 break;
436 default:
437 break;
438 }
439 }
441 }
442};
443
444// Set vehicle landed status data
445void set_in_air_status(bool in_air)
446{
447 if (!in_air) {
448 ekf_aw.time_last_on_gnd = ekf_aw.internal_clock;
449
450 } else {
451 ekf_aw.time_last_in_air = ekf_aw.internal_clock;
452 }
453 ekf_aw.in_air = in_air;
454}
455
456/*
457For this debug config:
458
459To start the filter manually
460-"Start" dlsetting can be used to put filter on
461
462To send random values in the filter:
463- Set define EKF_AW_WRAPPER_RANDOM_INPUTS in ekf_aw_wrapper.c to true
464
465To check filter timing:
466- time required to run different parts of filter sent on telementry AIRSPEED_WIND_ESTIMATOR_EKF_FORCES, on different fields, if EKF_AW_DEBUG set to TRUE
467
468*/
Main include for ABI (AirBorneInterface).
struct abi_struct abi_event
Definition abi_common.h:101
#define ACT_FEEDBACK_RPM_SENSOR_ID
bool autopilot_in_flight(void)
get in_flight flag
Definition autopilot.c:351
Core autopilot interface common to all firmwares.
static struct uart_periph * dev
struct ekfAwParameters * ekf_aw_get_param_handle(void)
Definition ekf_aw.cpp:1419
void ekf_aw_get_hover_force(float force[3])
Definition ekf_aw.cpp:1401
float ekf_aw_get_innov_V_pitot(void)
Definition ekf_aw.cpp:1323
struct FloatVect3 ekf_aw_get_innov_accel_filt(void)
Definition ekf_aw.cpp:1313
void ekf_aw_get_meas_cov(float meas_cov[7])
Definition ekf_aw.cpp:1329
void ekf_aw_set_speed_body(struct NedCoor_f *s)
Definition ekf_aw.cpp:1426
void ekf_aw_get_state_cov(float state_cov[9])
Definition ekf_aw.cpp:1344
struct NedCoor_f ekf_aw_get_speed_body(void)
Definition ekf_aw.cpp:1265
struct ekfHealth ekf_aw_get_health(void)
Definition ekf_aw.cpp:1295
void ekf_aw_get_fuselage_force(float force[3])
Definition ekf_aw.cpp:1374
void ekf_aw_propagate(struct FloatVect3 *acc, struct FloatRates *gyro, struct FloatEulers *euler, float *pusher_RPM, float *hover_RPM_array, float *skew, float *elevator_angle, FloatVect3 *V_gnd, FloatVect3 *acc_filt, float *V_pitot, float dt)
Definition ekf_aw.cpp:591
struct NedCoor_f ekf_aw_get_wind_ned(void)
Definition ekf_aw.cpp:1275
void ekf_aw_get_elevator_force(float force[3])
Definition ekf_aw.cpp:1392
struct FloatVect3 ekf_aw_get_innov_V_gnd(void)
Definition ekf_aw.cpp:1304
void ekf_aw_get_pusher_force(float force[3])
Definition ekf_aw.cpp:1410
void ekf_aw_get_process_cov(float process_cov[12])
Definition ekf_aw.cpp:1359
void ekf_aw_get_wing_force(float force[3])
Definition ekf_aw.cpp:1383
void ekf_aw_init(void)
Definition ekf_aw.cpp:457
struct NedCoor_f ekf_aw_get_offset(void)
Definition ekf_aw.cpp:1285
abi_event RPM_ev
#define EKF_AW_QUICK_CONVERGENCE
static void send_airspeed_wind_ekf_debug(struct transport_tx *trans, struct link_device *dev)
Butterworth2LowPass filt_hover_prop_rpm[EKF_AW_RPM_HOVER_NUM]
static void rpm_cb(uint8_t sender_id, struct act_feedback_t *rpm_message, uint8_t num_act)
Butterworth2LowPass filt_groundspeed[3]
Butterworth2LowPass filt_acc_low[3]
void ekf_aw_wrapper_fetch(void)
#define EKF_AW_WRAPPER_RANDOM_INPUTS
struct NedCoor_f zero_speed
void set_in_air_status(bool in_air)
Butterworth2LowPass filt_airspeed_pitot
float time_of_rpm
static void debug_vect(struct transport_tx *trans, struct link_device *dev, char *name, float *data, int datasize)
float tau_filter_high
static void send_airspeed_wind_ekf(struct transport_tx *trans, struct link_device *dev)
struct ekfAw ekf_aw
Butterworth2LowPass filt_skew
Butterworth2LowPass filt_pusher_prop_rpm
#define EKF_AW_QUICK_CONVERGENCE_TIME
Butterworth2LowPass filt_euler[3]
Butterworth2LowPass filt_acc[3]
#define PERIODIC_FREQUENCY_AIRSPEED_EKF_FETCH
void ekf_aw_wrapper_periodic(void)
float tau_filter_low
#define PERIODIC_FREQUENCY_AIRSPEED_EKF
Butterworth2LowPass filt_rate[3]
void ekf_aw_wrapper_init(void)
Butterworth2LowPass filt_elevator_pprz
#define EKF_AW_RPM_HOVER_NUM
Euler angles.
float psi
in radians
#define VECT3_COPY(_a, _b)
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
#define ACCEL_BFP_OF_REAL(_af)
void int32_rmat_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_a2b, struct Int32Vect3 *va)
rotate 3D vector by rotation matrix.
rotation matrix
vector in North East Down coordinates
static struct NedCoor_i * stateGetAccelNed_i(void)
Get acceleration in NED coordinates (int).
Definition state.h:1196
static struct Int32RMat * stateGetNedToBodyRMat_i(void)
Get vehicle body attitude rotation matrix (int).
Definition state.h:1301
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition state.h:1325
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
Definition state.h:850
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
Definition state.h:1392
static struct NedCoor_f * stateGetSpeedNed_f(void)
Get ground speed in local NED coordinates (float).
Definition state.h:1060
static struct Int32Vect3 * stateGetAccelBody_i(void)
Get acceleration in Body coordinates (int).
Definition state.h:1105
static float stateGetAirspeed_f(void)
Get airspeed (float).
Definition state.h:1615
static float p[2][2]
static parameters * ekf_params
The EKF parameters.
Definition ins_ekf2.cpp:377
bool reset_filter
Definition ins_flow.c:184
Simple first order low pass filter with bilinear transform.
struct SecondOrderLowPass Butterworth2LowPass
Second order Butterworth low pass filter.
static void init_butterworth_2_low_pass(Butterworth2LowPass *filter, const float tau, const float sample_time, const float value)
Init a second order Butterworth filter.
static float update_butterworth_2_low_pass(Butterworth2LowPass *filter, const float value)
Update second order Butterworth low pass filter state with a new value.
Hardware independent API for actuators (servos, motor controllers).
int32_t rpm
RPM.
Definition actuators.h:54
static uint32_t idx
Paparazzi generic algebra macros.
float z
in meters
float x
in meters
float y
in meters
vector in North East Down coordinates Units: meters
struct FloatVect3 body_accel_f
int16_t actuators_pprz[INDI_NUM_ACT+1]
PPRZ command to each actuator Can be used to directly control actuators from the control algorithm if...
API to get/set the generic vehicle states.
Architecture independent timing functions.
static float get_sys_time_float(void)
Get the time in seconds since startup.
Definition sys_time.h:241
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition telemetry.h:66
short int16_t
Typedef defining 16 bit short type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
signed char int8_t
Typedef defining 8 bit char type.