Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-02-05 10:41:00 +08:00
Code Issues Actions 2 Packages Projects Releases Wiki Activity

[modules] EKF Airspeed and Wind (#3078)

Browse Source 
* [modules] EKF Airspeed and Wind

* Test EKF_AW

* Also compile for non-rot-wing

* fix RPM in EKF_AW

* rpm fix

* Remove double debug messages

* follow master spacing

* Fix code style

* rotwing_controller renamed
This commit is contained in:
Christophe De Wagter
2023-10-19 16:34:48 +02:00
committed by GitHub
parent 2db514a43e
commit 1229e0489a
6 changed files with 2548 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

60
conf/airframes/tudelft/nederdrone7.xml
View File

@@ -68,7 +68,7 @@
<module name="fdm" type="jsbsim"/>
<module name="logger_file">
<define name="FILE_LOGGER_PATH" value="/home/ewoud/Documents"/>
<define name="FILE_LOGGER_PATH" value="~/"/>
</module>
<!--Not dealing with these in the simulation-->
@@ -163,6 +163,64 @@
</module>
<module name="motor_mixing"/>
<!--Airspeed estimation using EKF-->
<module name="ekf_aw">
<define name="EKF_AW_P0_V_BODY" value="1.E-2f" description="Initial covariance body velocity"/>
<define name="EKF_AW_P0_MU" value="1.E-5f" description="Initial covariance wind"/>
<define name="EKF_AW_P0_OFFSET" value="1.E-10f" description="Initial covariance offset"/>
<define name="EKF_AW_Q_ACCEL" value="1.0E-4f" description="Accel process noise"/>
<define name="EKF_AW_Q_GYRO" value="1.0E-9f" description="Gyro process noise"/>
<define name="EKF_AW_Q_MU" value="1.0E-6f" description="Wind process noise"/>
<define name="EKF_AW_Q_OFFSET" value="1.0E-8f" description="Offset process noise"/>
<define name="EKF_AW_R_V_GND" value="1.E-5f" description="GPS Velocity measurement noise"/>
<define name="EKF_AW_R_ACCEL_FILT_X" value="1.E-5f" description="Filtered x accel measurement noise"/>
<define name="EKF_AW_R_ACCEL_FILT_Y" value="1.E-5f" description="Filtered y accel measurement noise"/>
<define name="EKF_AW_R_ACCEL_FILT_Z" value="1.E-5f" description="Filtered z accel measurement noise"/>
<define name="EKF_AW_R_V_PITOT" value="1.E-7f" description="Pitot Tube Velocity measurement noise"/>
<define name="EKF_AW_AZ_SCHED_GAIN" value="2" />
<define name="EKF_AW_AZ_SCHED_START_DEG" value="60" />
<define name="EKF_AW_AZ_SCHED_END_DEG" value="70" />
<define name="EKF_AW_AX_SCHED_GAIN" value="0" />
<define name="EKF_AW_AX_SCHED_START_DEG" value="45" />
<define name="EKF_AW_AX_SCHED_END_DEG" value="60" />
<define name="EKF_AW_QUICK_CONVERGENCE" value="true" />
<define name="EKF_AW_QUICK_CONVERGENCE_TIME" value="15" />
<define name="EKF_AW_WING_INSTALLED" value="true" description="Use wing contribution"/>
<define name="EKF_AW_USE_MODEL_BASED_X" value="true" description="Use model based to augment filter"/>
<define name="EKF_AW_USE_MODEL_BASED_Y" value="true" description="Use model based to augment filter"/>
<define name="EKF_AW_USE_MODEL_BASED_Z" value="true" description="Use model based to augment filter"/>
<define name="EKF_AW_VEHICLE_MASS" value="6.5" description="Mass of the vehicle"/>
<define name="EKF_AW_K2_FX_FUSELAGE" value="-4E-2f" description="K*u*u"/>
<define name="EKF_AW_K3_FX_HOVER" value="-3E-1f" description="K*u"/>
<define name="EKF_AW_K1_FX_WING" value="-3.21432e-02f" description="Fx2 = (k1*(1+k5*skew)+(k2*alpha+k3*alpha^2))*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K2_FX_WING" value="1.67195e-01f" description="Fx2 = (k1*(1+k5*skew)+(k2*alpha+k3*alpha^2))*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K3_FX_WING" value="5.9441e-01f" description="Fx2 = (k1*(1+k5*skew)+(k2*alpha+k3*alpha^2))*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K4_FX_WING" value="3.9839e-03f" description="Fx2 = (k1*(1+k5*skew)+(k2*alpha+k3*alpha^2))*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K5_FX_WING" value="3.5321e-03f" description="Fx2 = (k1*(1+k5*skew)+(k2*alpha+k3*alpha^2))*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K_FY_V" value="-3.2E-1f" description="Fy beta coefficient"/>
<define name="EKF_AW_K1_FY_WING" value="0.0f" description="Fy = K1*cos(skew)*sin(skew)*u^2"/>
<define name="EKF_AW_K1_FZ_WING" value="-1.0008e-01f" description="Fz1 = ((k1+k2*alpha+k3*alpha^2)*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K2_FZ_WING" value="-8.6965e-01f" description="Fz1 = ((k1+k2*alpha+k3*alpha^2)*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K3_FZ_WING" value="1.45783e-01f" description="Fz1 = ((k1+k2*alpha+k3*alpha^2)*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_K4_FZ_WING" value="2.185395e-01f" description="Fz1 = ((k1+k2*alpha+k3*alpha^2)*(sin(skew)^2+k4)*V^2"/>
<define name="EKF_AW_TELEMETRY_DEBUG" value="false" description="debug mode to send timing info through telemetry"/>
</module>
</firmware>
<!-- CAN BUS 1 (Front Wing) -->

151
conf/modules/ekf_aw.xml Normal file
View File

@@ -0,0 +1,151 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="ekf_aw" dir="meteo">
<doc>
<description>
Airspeed and Wind Estimator with EKF
Frédéric Larocque
</description>
<define name="PERIODIC_FREQUENCY_AIRSPEED_EKF" value="25" description="Airspeed periodic frequency"/>
<define name="PERIODIC_FREQUENCY_AIRSPEED_EKF_FETCH" value="100" description="Airspeed periodic fetch frequency"/>
<define name="EKF_AW_P0_V_BODY" value="1e-00f" description="Initial covariance body velocity"/>
<define name="EKF_AW_P0_MU" value="1e-00f" description="Initial covariance wind"/>
<define name="EKF_AW_P0_OFFSET" value="1e-00f" description="Initial covariance offset"/>
<define name="EKF_AW_Q_ACCEL" value="1e-00f" description="Accel process noise"/>
<define name="EKF_AW_Q_GYRO" value="1e-00f" description="Gyro process noise"/>
<define name="EKF_AW_Q_MU" value="1e-00f" description="Wind process noise"/>
<define name="EKF_AW_Q_OFFSET" value="1e-00f" description="Offset process noise"/>
<define name="EKF_AW_R_V_GND" value="1e-00f" description="GPS Velocity measurement noise"/>
<define name="EKF_AW_R_ACCEL_FILT_X" value="1e-00f" description="Filtered x accel measurement noise"/>
<define name="EKF_AW_R_ACCEL_FILT_Y" value="1e-00f" description="Filtered y accel measurement noise"/>
<define name="EKF_AW_R_ACCEL_FILT_Z" value="1e-00f" description="Filtered z accel measurement noise"/>
<define name="EKF_AW_R_V_PITOT" value="1e-00f" description="Pitot Tube Velocity measurement noise"/>
<define name="EKF_AW_WING_INSTALLED" value="false" description="Use wing contribution"/>
<define name="EKF_AW_USE_MODEL_BASED" value="false" description="Use model based to augment filter"/>
<define name="EKF_AW_USE_BETA" value="false" description="Use beta to estimate sideforce"/>
<define name="EKF_AW_PROPAGATE_OFFSET" value="false" description="Propagate the offset state"/>
<define name="EKF_AW_VEHICLE_MASS" value="0.0" description="Mass of the vehicle"/>
<define name="EKF_AW_K1_FX_DRAG" value="0.f" description="Drag coefficient linear to u used if use model based is set to false"/>
<define name="EKF_AW_K2_FX_DRAG" value="0.f" description="Drag coefficient linear to u^2 used if use model based is set to false"/>
<define name="EKF_AW_K_FY_BETA" value="1e-00f" description="Fy v (side velocity) coefficient"/>
<define name="EKF_AW_K_FY_V" value="1e-00f" description="Fy beta coefficient"/>
<define name="EKF_AW_K1_FX_FUSELAGE" value="1e-00f" description="Fx Fuselage Coeff"/>
<define name="EKF_AW_K2_FX_FUSELAGE" value="1e-00f" description="Fx Fuselage Coeff"/>
<define name="EKF_AW_K3_FX_FUSELAGE" value="1e-00f" description="Fx Fuselage Coeff"/>
<define name="EKF_AW_K4_FX_FUSELAGE" value="1e-00f" description="Fx Fuselage Coeff"/>
<define name="EKF_AW_K1_FX_HOVER" value="1e-00f" description="Fx Hover Prop Coeff"/>
<define name="EKF_AW_K2_FX_HOVER" value="1e-00f" description="Fx Hover Prop Coeff"/>
<define name="EKF_AW_K1_FX_WING" value="1e-00f" description="Fx Wing Coeff"/>
<define name="EKF_AW_K2_FX_WING" value="1e-00f" description="Fx Wing Coeff"/>
<define name="EKF_AW_K3_FX_WING" value="1e-00f" description="Fx Wing Coeff"/>
<define name="EKF_AW_K4_FX_WING" value="1e-00f" description="Fx Wing Coeff"/>
<define name="EKF_AW_K5_FX_WING" value="1e-00f" description="Fx Wing Coeff"/>
<define name="EKF_AW_K1_FX_PUSH" value="1e-00f" description="Fx Pusher Coeff"/>
<define name="EKF_AW_K2_FX_PUSH" value="1e-00f" description="Fx Pusher Coeff"/>
<define name="EKF_AW_K3_FX_PUSH" value="1e-00f" description="Fx Pusher Coeff"/>
<define name="EKF_AW_K1_FX_ELEV" value="1e-00f" description="Fx Elevator Coeff"/>
<define name="EKF_AW_K2_FX_ELEV" value="1e-00f" description="Fx Elevator Coeff"/>
<define name="EKF_AW_K3_FX_ELEV" value="1e-00f" description="Fx Elevator Coeff"/>
<define name="EKF_AW_K1_FZ_FUSELAGE" value="1e-00f" description="Fz Fuselage Coeff"/>
<define name="EKF_AW_K2_FZ_FUSELAGE" value="1e-00f" description="Fz Fuselage Coeff"/>
<define name="EKF_AW_K3_FZ_FUSELAGE" value="1e-00f" description="Fz Fuselage Coeff"/>
<define name="EKF_AW_K4_FZ_FUSELAGE" value="1e-00f" description="Fz Fuselage Coeff"/>
<define name="EKF_AW_K1_FZ_HOVER" value="1e-00f" description="Fz Hover Prop Coeff"/>
<define name="EKF_AW_K2_FZ_HOVER" value="1e-00f" description="Fz Hover Prop Coeff"/>
<define name="EKF_AW_K3_FZ_HOVER" value="1e-00f" description="Fz Hover Prop Coeff"/>
<define name="EKF_AW_K4_FZ_HOVER" value="1e-00f" description="Fz Hover Prop Coeff"/>
<define name="EKF_AW_K1_Fz_WING" value="1e-00f" description="Fz Wing Coeff"/>
<define name="EKF_AW_K2_Fz_WING" value="1e-00f" description="Fz Wing Coeff"/>
<define name="EKF_AW_K3_Fz_WING" value="1e-00f" description="Fz Wing Coeff"/>
<define name="EKF_AW_K4_Fz_WING" value="1e-00f" description="Fz Wing Coeff"/>
<define name="EKF_AW_K5_Fz_WING" value="1e-00f" description="Fz Wing Coeff"/>
<define name="EKF_AW_K1_FZ_ELEV" value="1e-00f" description="Fz Elevator Coeff"/>
<define name="EKF_AW_K2_FZ_ELEV" value="1e-00f" description="Fz Elevator Coeff"/>
<define name="EKF_AW_ELEV_MAX_ANGLE" value="1e-00f" description="Maximum elevator angle"/>
<define name="EKF_AW_ELEV_MIN_ANGLE" value="1e-00f" description="Minimum elevator angle"/>
<define name="EKF_AW_AOA_MAX_ANGLE" value="1e-00f" description="Maximum angle of attack"/>
<define name="EKF_AW_AOA_MIN_ANGLE" value="1e-00f" description="Minimum angle of attack"/>
</doc>
<settings>
<dl_settings>
<dl_settings name="EKF_Airspeed_Wind">
<dl_setting min="0" max="1" step="1" var="ekf_aw.override_start" module="meteo/ekf_aw_wrapper" shortname="Override Start"/>
<dl_setting min="0" max="1" step="1" var="ekf_aw.override_quick_convergence" module="meteo/ekf_aw_wrapper" shortname="Override Quick Convergence"/>
<dl_setting min="0" max="1" step="1" var="ekf_aw.reset" module="meteo/ekf_aw_wrapper" shortname="Reset States and Health" handler="reset"/>
<dl_setting min="0" max="1" step="1" var="ekf_aw_params.propagate_offset" module="meteo/ekf_aw" shortname="propagate offset"/>
<dl_setting min="1E-1" max="8" step="1E-1" var="ekf_aw_params.vehicle_mass" module="meteo/ekf_aw" shortname="Vehicle Mass"/>
<dl_setting min="-1" max="0" step="1E-3" var="ekf_aw_params.k_fx_drag[0]" module="meteo/ekf_aw" shortname="k_drag_lin"/>
<dl_setting min="-1" max="0" step="1E-3" var="ekf_aw_params.k_fx_drag[1]" module="meteo/ekf_aw" shortname="k_drag_quad"/>
<dl_setting min="-5E-2" max="0" step="1E-4" var="ekf_aw_params.k_fy_beta" module="meteo/ekf_aw" shortname="k_beta"/>
<dl_setting min="-5E-2" max="0" step="1E-4" var="ekf_aw_params.k_fy_v" module="meteo/ekf_aw" shortname="k_v"/>
<dl_setting min="1E-9" max="1E-2" step="1E-8" var="ekf_aw_params.Q_accel" module="meteo/ekf_aw" shortname="Q accel" handler="update_Q_accel"/>
<dl_setting min="1E-9" max="1E-2" step="1E-8" var="ekf_aw_params.Q_gyro" module="meteo/ekf_aw" shortname="Q gyro" handler="update_Q_gyro"/>
<dl_setting min="1E-9" max="1E-2" step="1E-8" var="ekf_aw_params.Q_mu" module="meteo/ekf_aw" shortname="Q wind" handler="update_Q_mu"/>
<dl_setting min="1E-9" max="1E-2" step="1E-8" var="ekf_aw_params.Q_k" module="meteo/ekf_aw" shortname="Q offset" handler="update_Q_k"/>
<dl_setting min="1E-9" max="1E-2" step="1E-8" var="ekf_aw_params.R_V_gnd" module="meteo/ekf_aw" shortname="R V_gnd" handler="update_R_V_gnd"/>
<dl_setting min="1E-9" max="1E-2" step="1E-8" var="ekf_aw_params.R_accel_filt[0]" module="meteo/ekf_aw" shortname="R Accel Filt x" handler="update_R_accel_filt_x"/>
<dl_setting min="1E-9" max="1E-2" step="1E-8" var="ekf_aw_params.R_accel_filt[1]" module="meteo/ekf_aw" shortname="R Accel Filt y" handler="update_R_accel_filt_y"/>
<dl_setting min="1E-9" max="1E-2" step="1E-8" var="ekf_aw_params.R_accel_filt[2]" module="meteo/ekf_aw" shortname="R Accel Filt z" handler="update_R_accel_filt_z"/>
<dl_setting min="1E-9" max="1E-2" step="1E-8" var="ekf_aw_params.R_V_pitot" module="meteo/ekf_aw" shortname="R Pitot" handler="update_R_V_pitot"/>
<dl_setting min="-10" max="10" step="0.1" var="ekf_aw.wind_guess.x" module="meteo/ekf_aw_wrapper" shortname="Wind Guess N" handler="set_wind_N"/>
<dl_setting min="-10" max="10" step="0.1" var="ekf_aw.wind_guess.y" module="meteo/ekf_aw_wrapper" shortname="Wind Guess E" handler="set_wind_E"/>
<dl_setting min="-10" max="10" step="0.1" var="ekf_aw.wind_guess.z" module="meteo/ekf_aw_wrapper" shortname="Wind Guess D" handler="set_wind_D"/>
<dl_setting min="-1" max="1" step="1E-3" var="ekf_aw.offset_guess.x" module="meteo/ekf_aw_wrapper" shortname="Offset Guess x" handler="set_offset_x"/>
<dl_setting min="-1" max="1" step="1E-3" var="ekf_aw.offset_guess.y" module="meteo/ekf_aw_wrapper" shortname="Offset Guess y" handler="set_offset_y"/>
<dl_setting min="-1" max="1" step="1E-3" var="ekf_aw.offset_guess.z" module="meteo/ekf_aw_wrapper" shortname="Offset Guess z" handler="set_offset_z"/>
</dl_settings>
</dl_settings>
</settings>
<header>
<file name="ekf_aw_wrapper.h"/>
</header>
<init fun="ekf_aw_wrapper_init()"/>
<periodic fun="ekf_aw_wrapper_periodic()" freq="25"/>
<periodic fun="ekf_aw_wrapper_fetch()" freq="100"/>
<makefile target="ap|nps">
<file name="ekf_aw.cpp"/>
<file name="ekf_aw_wrapper.c"/>
<include name="$(PAPARAZZI_SRC)/sw/ext/matrix/"/>
</makefile>
</module>
<!--Your comment
Target NPS or AP
<configure name="CXXSTANDARD" value="-std=c++14"/>
<flag name="LDFLAGS" value="lstdc++" />
Target AP
<define name="EIGEN_NO_DEBUG"/> Removes debug checks and should lead to faster code execution
<flag name="CXXFLAGS" value="Wno-bool-compare"/>
<flag name="CXXFLAGS" value="Wno-logical-not-parentheses"/>
<datalink message="PING" fun="datalink_parse_PING(dev, trans, buf)"/>
-->

1605
sw/airborne/modules/meteo/ekf_aw.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

147
sw/airborne/modules/meteo/ekf_aw.h Normal file
View File

@@ -0,0 +1,147 @@
#ifndef EKF_AW_H
#define EKF_AW_H
#ifdef __cplusplus
extern "C" {
#endif
#include "std.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_geodetic_float.h"
// Settings
struct ekfAwParameters {
// Q
float Q_accel; ///< accel process noise
float Q_gyro; ///< gyro process noise
float Q_mu; ///< wind process noise
float Q_k; ///< offset process noise
// R
float R_V_gnd; ///< speed measurement noise
float R_accel_filt[3]; ///< filtered accel measurement noise
float R_V_pitot; ///< airspeed measurement noise
// Model Based Parameters
float vehicle_mass;
// X Axis
float k_fx_drag[2]; // Temporary setting for fuselage + hover prop
float k_fx_fuselage[4];
float k_fx_hover[3];
float k_fx_wing[5];
float k_fx_push[3];
float k_fx_elev[3];
// Y Axis
float k_fy_beta;
float k_fy_v;
float k_fy_wing[5];
// Z Axis
float k_fz_fuselage[4];
float k_fz_wing[4];
float k_fz_hover[5];
float k_fz_elev[2];
// Other options
bool use_model[3];
bool use_pitot;
bool propagate_offset;
bool quick_convergence;
};
struct ekfHealth {
bool healthy;
uint16_t crashes_n;
};
extern struct ekfAwParameters ekf_aw_params;
// Init functions
extern void ekf_aw_init(void);
extern void ekf_aw_reset(void);
// Filtering functions
extern 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, struct FloatVect3 *V_gnd,
struct FloatVect3 *acc_filt, float *V_pitot, float dt);
// Getter/Setter functions
extern struct NedCoor_f ekf_aw_get_speed_body(void);
extern struct NedCoor_f ekf_aw_get_wind_ned(void);
extern struct NedCoor_f ekf_aw_get_offset(void);
extern struct FloatVect3 ekf_aw_get_innov_V_gnd(void);
extern struct FloatVect3 ekf_aw_get_innov_accel_filt(void);
extern float ekf_aw_get_innov_V_pitot(void);
extern void ekf_aw_get_meas_cov(float meas_cov[7]);
extern void ekf_aw_get_state_cov(float state_cov[9]);
extern void ekf_aw_get_process_cov(float process_cov[9]);
extern void ekf_aw_get_fuselage_force(float force[3]);
extern void ekf_aw_get_wing_force(float force[3]);
extern void ekf_aw_get_elevator_force(float force[3]);
extern void ekf_aw_get_hover_force(float force[3]);
extern void ekf_aw_get_pusher_force(float force[3]);
extern struct ekfAwParameters *ekf_aw_get_param_handle(void);
extern void ekf_aw_set_speed_body(struct NedCoor_f *s);
extern void ekf_aw_set_wind(struct NedCoor_f *s);
extern void ekf_aw_set_offset(struct NedCoor_f *s);
extern struct ekfHealth ekf_aw_get_health(void);
// Settings handlers
extern void ekf_aw_update_params(void);
extern void ekf_aw_reset_health(void);
// Handlers
#define ekf_aw_update_Q_accel(_v) { \
ekf_aw_params.Q_accel = _v; \
ekf_aw_update_params(); \
}
#define ekf_aw_update_Q_gyro(_v) { \
ekf_aw_params.Q_gyro = _v; \
ekf_aw_update_params(); \
}
#define ekf_aw_update_Q_mu(_v) { \
ekf_aw_params.Q_mu = _v; \
ekf_aw_update_params(); \
}
#define ekf_aw_update_Q_k(_v) { \
ekf_aw_params.Q_k = _v; \
ekf_aw_update_params(); \
}
#define ekf_aw_update_R_V_gnd(_v) { \
ekf_aw_params.R_V_gnd = _v; \
ekf_aw_update_params(); \
}
#define ekf_aw_update_R_accel_filt_x(_v) { \
ekf_aw_params.R_accel_filt[0] = _v; \
ekf_aw_update_params(); \
}
#define ekf_aw_update_R_accel_filt_y(_v) { \
ekf_aw_params.R_accel_filt[1] = _v; \
ekf_aw_update_params(); \
}
#define ekf_aw_update_R_accel_filt_z(_v) { \
ekf_aw_params.R_accel_filt[2] = _v; \
ekf_aw_update_params(); \
}
#define ekf_aw_update_R_V_pitot(_v) { \
ekf_aw_params.R_V_pitot = _v; \
ekf_aw_update_params(); \
}
#ifdef __cplusplus
}
#endif
#endif /* EKF_AW_H */

468
sw/airborne/modules/meteo/ekf_aw_wrapper.c Normal file
View File

@@ -0,0 +1,468 @@
#include "modules/meteo/ekf_aw_wrapper.h"
#include "modules/meteo/ekf_aw.h"
#include <stdio.h>
#include "state.h"
#include "filters/low_pass_filter.h"
#include "math/pprz_algebra.h"
#include "modules/core/abi.h"
#include "autopilot.h"
#include "modules/actuators/actuators.h"
#include "mcu_periph/sys_time.h" // FOR DEBUG
#ifndef EKF_AW_WRAPPER_ROT_WING
#define EKF_AW_WRAPPER_ROT_WING false
#endif
#ifndef EKF_AW_WRAPPER_RANDOM_INPUTS
#define EKF_AW_WRAPPER_RANDOM_INPUTS false
#endif
#ifndef EKF_AW_QUICK_CONVERGENCE
#define EKF_AW_QUICK_CONVERGENCE false
#endif
#ifndef EKF_AW_QUICK_CONVERGENCE_TIME
#define EKF_AW_QUICK_CONVERGENCE_TIME 10.0f
#endif
#ifndef EKF_AW_DEBUG
#define EKF_AW_DEBUG false
#endif
#if EKF_AW_WRAPPER_ROT_WING
#include "modules/rot_wing_drone/wing_rotation_controller_servo.h"
#endif
#if PERIODIC_TELEMETRY
#include "modules/datalink/telemetry.h"
// TO DO: implement circular wrap filter for psi angle
// TO DO: modify force functions to simpler model (wing)
// Telemetry Message functions
static void send_airspeed_wind_ekf(struct transport_tx *trans, struct link_device *dev)
{
uint8_t healthy = (uint8_t)ekf_aw.health.healthy;
pprz_msg_send_AIRSPEED_WIND_ESTIMATOR_EKF(trans, dev, AC_ID,
&ekf_aw.V_body.x, &ekf_aw.V_body.y, &ekf_aw.V_body.z,
&ekf_aw.wind.x, &ekf_aw.wind.y, &ekf_aw.wind.z,
&ekf_aw.offset.x, &ekf_aw.offset.y, &ekf_aw.offset.z,
&healthy,
&ekf_aw.health.crashes_n,
&ekf_aw.innov_V_gnd.x, &ekf_aw.innov_V_gnd.y, &ekf_aw.innov_V_gnd.z,
&ekf_aw.innov_acc_filt.x, &ekf_aw.innov_acc_filt.y, &ekf_aw.innov_acc_filt.z,
&ekf_aw.innov_V_pitot,
&ekf_aw.acc.x,
&ekf_aw.acc.y,
&ekf_aw.acc.z);
}
static void debug_vect(struct transport_tx *trans, struct link_device *dev, char *name, float *data, int datasize)
{
pprz_msg_send_DEBUG_VECT(trans, dev, AC_ID,
strlen(name), name,
datasize, data);
}
static void send_airspeed_wind_ekf_debug(struct transport_tx *trans, struct link_device *dev)
{
debug_vect(trans, dev, "process_cov", ekf_aw.process_cov, 12);
debug_vect(trans, dev, "meas_cov", ekf_aw.meas_cov, 7);
debug_vect(trans, dev, "state_cov", ekf_aw.state_cov, 9);
debug_vect(trans, dev, "fuselage_force", ekf_aw.fuselage_force, 3);
debug_vect(trans, dev, "wing_force", ekf_aw.wing_force, 3);
debug_vect(trans, dev, "elevator_force", ekf_aw.elevator_force, 3);
debug_vect(trans, dev, "hover_force", ekf_aw.hover_force, 3);
debug_vect(trans, dev, "pusher_force", ekf_aw.pusher_force, 3);
float rw_state[4];
rw_state[0] = ekf_aw.skew;
rw_state[1] = ekf_aw.elevator_angle;
rw_state[2] = ekf_aw.RPM_pusher;
rw_state[3] = ekf_aw.RPM_hover[0];
debug_vect(trans, dev, "rw_state", rw_state, 4);
}
#endif
// RPM ABI Event
abi_event RPM_ev;
float time_of_rpm = 0.0f;
static void rpm_cb(uint8_t sender_id __attribute__((unused)), struct act_feedback_t *rpm_message, uint8_t num_act);
// Filter struct
struct ekfAw ekf_aw; // Local wrapper
static struct ekfAwParameters *ekf_params; ///< The EKF parameters
// Define settings to change filter tau value
float tau_filter_high = 25.0f;
float tau_filter_low = 0.2f;
// Bool Reset EKF Filter
bool reset_filter = false;
struct NedCoor_f zero_speed = {
.x = 0.0f,
.y = 0.0f,
.z = 0.0f
};
// Define filter arrays
Butterworth2LowPass filt_groundspeed[3];
Butterworth2LowPass filt_acc[3];
Butterworth2LowPass filt_acc_low[3];
Butterworth2LowPass filt_rate[3];
Butterworth2LowPass filt_euler[3];
Butterworth2LowPass filt_hover_prop_rpm[EKF_AW_RPM_HOVER_NUM];
Butterworth2LowPass filt_pusher_prop_rpm;
Butterworth2LowPass filt_skew;
Butterworth2LowPass filt_elevator_pprz;
Butterworth2LowPass filt_airspeed_pitot;
#ifndef PERIODIC_FREQUENCY_AIRSPEED_EKF_FETCH
#define PERIODIC_FREQUENCY_AIRSPEED_EKF_FETCH 100
#endif
#ifndef PERIODIC_FREQUENCY_AIRSPEED_EKF
#define PERIODIC_FREQUENCY_AIRSPEED_EKF 25
#endif
void ekf_aw_wrapper_init(void)
{
// Define filter frequencies
float sample_time = 1.0f / PERIODIC_FREQUENCY_AIRSPEED_EKF_FETCH;
float tau_low = 1.0f / (2.0f * M_PI * tau_filter_low);
float tau_high = 1.0f / (2.0f * M_PI * tau_filter_high);
// Init filters
for (int8_t i = 0; i < 3; i++) {
init_butterworth_2_low_pass(&filt_groundspeed[i], tau_high, sample_time, 0.0); // Init filters groundspeed
init_butterworth_2_low_pass(&filt_acc[i], tau_high, sample_time, 0.0); // Init filters Accelerations
init_butterworth_2_low_pass(&filt_acc_low[i], tau_low, sample_time, 0.0); // Init filters Accelerations Low
init_butterworth_2_low_pass(&filt_rate[i], tau_high, sample_time, 0.0); // Init filters Rate
init_butterworth_2_low_pass(&filt_euler[i], tau_high, sample_time, 0.0); // Init filters Euler
}
for (int8_t i = 0; i < EKF_AW_RPM_HOVER_NUM; i++) {
init_butterworth_2_low_pass(&filt_hover_prop_rpm[i], tau_low, sample_time, 0.0);
ekf_aw.last_RPM_hover[i] = 0;
}
init_butterworth_2_low_pass(&filt_pusher_prop_rpm, tau_low, sample_time, 0.0); // Init filters Pusher Prop
init_butterworth_2_low_pass(&filt_skew, tau_low, sample_time, 0.0); // Init filters Skew
init_butterworth_2_low_pass(&filt_elevator_pprz, tau_low, sample_time, 0.0); // Init filters Pusher Prop
init_butterworth_2_low_pass(&filt_airspeed_pitot, tau_low, sample_time, 0.0); // Init filters Pusher Prop
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AIRSPEED_WIND_ESTIMATOR_EKF, send_airspeed_wind_ekf);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_DEBUG_VECT, send_airspeed_wind_ekf_debug);
#endif
// Init EKF Filter
ekf_aw_init();
// Register ABI message
AbiBindMsgACT_FEEDBACK(ACT_FEEDBACK_RPM_SENSOR_ID, &RPM_ev, rpm_cb);
//Get EKF param handle
ekf_params = ekf_aw_get_param_handle();
// Related to in air / on ground logic
ekf_aw.in_air = false;
ekf_aw.internal_clock = 0;
ekf_aw.time_last_on_gnd = 0;
// For override of filter using dlsettings
ekf_aw.override_start = false;
ekf_aw.override_quick_convergence = false;
// Init of public filter states //TO DO: Is it necessary or just safer?
for (int i = 0; i < EKF_AW_RPM_HOVER_NUM; i++) {
ekf_aw.last_RPM_hover[i] = 0;
}
ekf_aw.last_RPM_pusher = 0;
ekf_aw.V_body.x = 0.0f; ekf_aw.V_body.y = 0.0f; ekf_aw.V_body.z = 0.0f;
ekf_aw.wind.x = 0.0f; ekf_aw.wind.y = 0.0f; ekf_aw.wind.z = 0.0f;
ekf_aw.offset.x = 0.0f; ekf_aw.offset.y = 0.0f; ekf_aw.offset.z = 0.0f;
ekf_aw.health.healthy = true; ekf_aw.health.crashes_n = 0.0f;
ekf_aw.innov_V_gnd.x = 0.0f; ekf_aw.innov_V_gnd.y = 0.0f; ekf_aw.innov_V_gnd.z = 0.0f;
ekf_aw.innov_acc_filt.y = 0.0f; ekf_aw.innov_acc_filt.y = 0.0f; ekf_aw.innov_acc_filt.z = 0.0f;
ekf_aw.innov_V_pitot = 0.0f;
ekf_aw.fuselage_force[0] = 0.0f; ekf_aw.fuselage_force[1] = 0.0f; ekf_aw.fuselage_force[2] = 0.0f;
ekf_aw.wing_force[0] = 0.0f; ekf_aw.wing_force[1] = 0.0f; ekf_aw.wing_force[2] = 0.0f;
ekf_aw.elevator_force[0] = 0.0f; ekf_aw.elevator_force[1] = 0.0f; ekf_aw.elevator_force[2] = 0.0f;
ekf_aw.pusher_force[0] = 0.0f; ekf_aw.pusher_force[1] = 0.0f; ekf_aw.pusher_force[2] = 0.0f;
ekf_aw.hover_force[0] = 0.0f; ekf_aw.hover_force[1] = 0.0f; ekf_aw.hover_force[2] = 0.0f;
ekf_aw.skew = 0.0f;
ekf_aw.elevator_angle = 0.0f;
ekf_aw.RPM_pusher = 0.0f;
for (int i = 0; i < EKF_AW_RPM_HOVER_NUM; i++) {
ekf_aw.RPM_hover[i] = 0.0f;
}
};
void ekf_aw_wrapper_periodic(void)
{
// FOR DEBUG
// uint32_t tic = get_sys_time_usec();
//printf("Running periodic Airspeed EKF Module\n");
//printf("Airspeed is: %2.2f\n",filt_groundspeed[0].o[0]);
// FOR DEBUG Random acc inputs to filter
if (EKF_AW_WRAPPER_RANDOM_INPUTS) {
ekf_aw.acc.x = 1.0f * rand() / RAND_MAX; // TO BE REMOVED
ekf_aw.acc.y = 1.0f * rand() / RAND_MAX; // TO BE REMOVED
ekf_aw.acc.z = 1.0f * rand() / RAND_MAX; // TO BE REMOVED
ekf_aw.gyro.p = 0.1f * rand() / RAND_MAX; // TO BE REMOVED
ekf_aw.gyro.q = 0.1f * rand() / RAND_MAX; // TO BE REMOVED
ekf_aw.gyro.r = 0.1f * rand() / RAND_MAX; // TO BE REMOVED
ekf_aw.euler.phi = 1.0f * rand() / RAND_MAX; // TO BE REMOVED
ekf_aw.euler.theta = 1.0f * rand() / RAND_MAX; // TO BE REMOVED
ekf_aw.euler.psi = 1.0f * rand() / RAND_MAX; // TO BE REMOVED
for (int i = 0; i < EKF_AW_RPM_HOVER_NUM; i++) {
ekf_aw.RPM_hover[i] = 1000.0f * rand() / RAND_MAX; // TO BE REMOVED
}
ekf_aw.RPM_pusher = 1000.0f * rand() / RAND_MAX; // TO BE REMOVED
ekf_aw.skew = 10.0f * rand() / RAND_MAX; // TO BE REMOVED
ekf_aw.elevator_angle = 10.0f * rand() / RAND_MAX; // TO BE REMOVED
ekf_aw.Vg_NED.x = 10.0f * rand() / RAND_MAX; // TO BE REMOVED
ekf_aw.Vg_NED.y = 10.0f * rand() / RAND_MAX; // TO BE REMOVED
ekf_aw.Vg_NED.z = 10.0f * rand() / RAND_MAX; // TO BE REMOVED
ekf_aw.acc_filt.x = 1.0f * rand() / RAND_MAX; // TO BE REMOVED
ekf_aw.acc_filt.y = 1.0f * rand() / RAND_MAX; // TO BE REMOVED
ekf_aw.acc_filt.z = 1.0f * rand() / RAND_MAX; // TO BE REMOVED
ekf_aw.V_pitot = 10.0f * rand() / RAND_MAX; // TO BE REMOVED
} else {
// Get latest filtered values to ekf struct
ekf_aw.acc.x = filt_acc[0].o[0];
ekf_aw.acc.y = filt_acc[1].o[0];
ekf_aw.acc.z = filt_acc[2].o[0];
ekf_aw.gyro.p = filt_rate[0].o[0];
ekf_aw.gyro.q = filt_rate[1].o[0];
ekf_aw.gyro.r = filt_rate[2].o[0];
ekf_aw.euler.phi = filt_euler[0].o[0];
ekf_aw.euler.theta = filt_euler[1].o[0];
//ekf_aw.euler.psi = filt_euler[2].o[0];
ekf_aw.euler.psi = stateGetNedToBodyEulers_f()->psi; // TO DO: implement circular wrap filter for psi angle
for (int8_t i = 0; i < EKF_AW_RPM_HOVER_NUM; i++) {
ekf_aw.RPM_hover[i] = filt_hover_prop_rpm[i].o[0];
}
ekf_aw.RPM_pusher = filt_pusher_prop_rpm.o[0];
ekf_aw.skew = filt_skew.o[0];
ekf_aw.elevator_angle = filt_elevator_pprz.o[0];
ekf_aw.Vg_NED.x = filt_groundspeed[0].o[0];
ekf_aw.Vg_NED.y = filt_groundspeed[1].o[0];
ekf_aw.Vg_NED.z = filt_groundspeed[2].o[0];
ekf_aw.acc_filt.x = filt_acc_low[0].o[0];
ekf_aw.acc_filt.y = filt_acc_low[1].o[0];
ekf_aw.acc_filt.z = filt_acc_low[2].o[0];
ekf_aw.V_pitot = filt_airspeed_pitot.o[0];
}
// Sample time of EKF filter
float sample_time = 1.0f / PERIODIC_FREQUENCY_AIRSPEED_EKF_FETCH;
// Check if in flight and altitude higher than 1m
set_in_air_status(autopilot_in_flight() & (-stateGetPositionNed_f()->z > 1.0f));
// Propagate
if (ekf_aw.in_air | ekf_aw.override_start) {
// Quick convergence for the first 10 s
if (((ekf_aw.internal_clock - ekf_aw.time_last_on_gnd < PERIODIC_FREQUENCY_AIRSPEED_EKF * EKF_AW_QUICK_CONVERGENCE_TIME)
& EKF_AW_QUICK_CONVERGENCE) | ekf_aw.override_quick_convergence) {
ekf_params->quick_convergence = true;
} else {
ekf_params->quick_convergence = false;
}
ekf_aw_propagate(&ekf_aw.acc, &ekf_aw.gyro, &ekf_aw.euler, &ekf_aw.RPM_pusher, ekf_aw.RPM_hover, &ekf_aw.skew,
&ekf_aw.elevator_angle, &ekf_aw.Vg_NED, &ekf_aw.acc_filt, &ekf_aw.V_pitot, sample_time);
} else {
// Set body velocity to 0 when landed
ekf_aw_set_speed_body(&zero_speed);
};
// Get states, health and innovation from EKF filter
ekf_aw.V_body = ekf_aw_get_speed_body();
ekf_aw.wind = ekf_aw_get_wind_ned();
ekf_aw.offset = ekf_aw_get_offset();
ekf_aw.health = ekf_aw_get_health();
ekf_aw.innov_V_gnd = ekf_aw_get_innov_V_gnd();
ekf_aw.innov_acc_filt = ekf_aw_get_innov_accel_filt();
ekf_aw.innov_V_pitot = ekf_aw_get_innov_V_pitot();
// Get covariance
ekf_aw_get_meas_cov(ekf_aw.meas_cov);
ekf_aw_get_state_cov(ekf_aw.state_cov);
ekf_aw_get_process_cov(ekf_aw.process_cov);
// Get forces
ekf_aw_get_fuselage_force(ekf_aw.fuselage_force);
ekf_aw_get_wing_force(ekf_aw.wing_force);
ekf_aw_get_elevator_force(ekf_aw.elevator_force);
ekf_aw_get_hover_force(ekf_aw.hover_force);
ekf_aw_get_pusher_force(ekf_aw.pusher_force);
ekf_aw.internal_clock++;
// FOR DEBUG
//ekf_aw.offset.x = get_sys_time_usec()-tic;
};
// Function to get information from different modules and set it in the different filters
void ekf_aw_wrapper_fetch(void)
{
// For debug
// uint32_t tic_2 = get_sys_time_usec();
// NED Speed
update_butterworth_2_low_pass(&filt_groundspeed[0], stateGetSpeedNed_f()->x);
update_butterworth_2_low_pass(&filt_groundspeed[1], stateGetSpeedNed_f()->y);
update_butterworth_2_low_pass(&filt_groundspeed[2], stateGetSpeedNed_f()->z);
// Getting body accel
struct FloatVect3 body_accel_f = {0.0f, 0.0f, 0.0f};
#if EKF_AW_WRAPPER_ROT_WING
// If body accel available, can use this
struct Int32Vect3 *body_accel_i;
body_accel_i = stateGetAccelBody_i();
ACCELS_FLOAT_OF_BFP(body_accel_f, *body_accel_i);
#else
// Transferring from NED to Body as body is not available right now
struct NedCoor_i *accel_tmp = stateGetAccelNed_i();
struct Int32Vect3 ned_accel_i, body_accel_i;
struct Int32RMat *ned_to_body_rmat = stateGetNedToBodyRMat_i();
VECT3_COPY(ned_accel_i, (*accel_tmp));
ned_accel_i.z += ACCEL_BFP_OF_REAL(-9.81); // Add gravity
int32_rmat_vmult(&body_accel_i, ned_to_body_rmat, &ned_accel_i);
ACCELS_FLOAT_OF_BFP(body_accel_f, body_accel_i);
#endif
// Body accel
update_butterworth_2_low_pass(&filt_acc[0], body_accel_f.x);
update_butterworth_2_low_pass(&filt_acc[1], body_accel_f.y);
update_butterworth_2_low_pass(&filt_acc[2], body_accel_f.z);
update_butterworth_2_low_pass(&filt_acc_low[0], body_accel_f.x);
update_butterworth_2_low_pass(&filt_acc_low[1], body_accel_f.y);
update_butterworth_2_low_pass(&filt_acc_low[2], body_accel_f.z);
// Body rates
update_butterworth_2_low_pass(&filt_rate[0], stateGetBodyRates_f()->p);
update_butterworth_2_low_pass(&filt_rate[1], stateGetBodyRates_f()->q);
update_butterworth_2_low_pass(&filt_rate[2], stateGetBodyRates_f()->r);
// Euler angles
update_butterworth_2_low_pass(&filt_euler[0], stateGetNedToBodyEulers_f()->phi);
update_butterworth_2_low_pass(&filt_euler[1], stateGetNedToBodyEulers_f()->theta);
//update_butterworth_2_low_pass(&filt_euler[2], stateGetNedToBodyEulers_f()->psi); // TO DO: implement circular wrap filter for psi angle
for (int8_t i = 0; i < EKF_AW_RPM_HOVER_NUM; i++) {
update_butterworth_2_low_pass(&filt_hover_prop_rpm[i], ekf_aw.last_RPM_hover[i] * 1.0f);
}
update_butterworth_2_low_pass(&filt_pusher_prop_rpm, ekf_aw.last_RPM_pusher * 1.0f);
#if EKF_AW_WRAPPER_ROT_WING
update_butterworth_2_low_pass(&filt_skew, wing_rotation.wing_angle_rad);
// Get elevator pprz signal
int16_t *elev_pprz = &actuators_pprz[5];
float de = 0.0f;
// Calculate deflection angle in [deg]
de = (-0.004885417f * *elev_pprz + 36.6f) * 3.14f / 180.0f;
update_butterworth_2_low_pass(&filt_elevator_pprz, de);
#else
update_butterworth_2_low_pass(&filt_skew, 0.0f);
update_butterworth_2_low_pass(&filt_elevator_pprz, 0.0f);
#endif
update_butterworth_2_low_pass(&filt_airspeed_pitot, stateGetAirspeed_f());
// FOR DEBUG
//ekf_aw.offset.y = get_sys_time_usec()-tic_2;
};
// ABI callback that obtains the RPM from a module
static void rpm_cb(uint8_t sender_id __attribute__((unused)), struct act_feedback_t *rpm_message,
uint8_t num_act_message)
{
for (int i = 0; i < num_act_message; i++) {
// Sanity check that index is valid
if (rpm_message[i].idx < EKF_AW_RPM_HOVER_NUM) {
// Assign rpm to right actuator
switch (rpm_message[i].idx) {
case 0:
ekf_aw.last_RPM_hover[0] = rpm_message[i].rpm;
break;
case 1:
ekf_aw.last_RPM_hover[1] = rpm_message[i].rpm;
break;
case 2:
ekf_aw.last_RPM_hover[2] = rpm_message[i].rpm;
break;
case 3:
ekf_aw.last_RPM_hover[3] = rpm_message[i].rpm;
break;
case 4:
ekf_aw.last_RPM_pusher = rpm_message[i].rpm;
break;
default:
break;
}
}
time_of_rpm = get_sys_time_float();
}
};
// Set vehicle landed status data
void set_in_air_status(bool in_air)
{
if (!in_air) {
ekf_aw.time_last_on_gnd = ekf_aw.internal_clock;
} else {
ekf_aw.time_last_in_air = ekf_aw.internal_clock;
}
ekf_aw.in_air = in_air;
}
/*
For this debug config:
To start the filter manually
-"Start" dlsetting can be used to put filter on
To send random values in the filter:
- Set define EKF_AW_WRAPPER_RANDOM_INPUTS in ekf_aw_wrapper.c to true
To check filter timing:
- 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
*/

118
sw/airborne/modules/meteo/ekf_aw_wrapper.h Normal file
View File

@@ -0,0 +1,118 @@
#ifndef EKF_AW_WRAPPER_H
#define EKF_AW_WRAPPER_H
#include "std.h"
#include "math/pprz_algebra_int.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_geodetic_float.h"
#include "modules/meteo/ekf_aw.h"
// EKF structure
struct ekfAw {
// States
struct NedCoor_f wind;
struct NedCoor_f V_body;
struct NedCoor_f offset;
// Inputs
struct FloatVect3 acc; ///< Last accelerometer measurements
struct FloatRates gyro; ///< Last gyroscope measurements
struct FloatEulers euler; /// Euler angles
#define EKF_AW_RPM_HOVER_NUM 4
int32_t last_RPM_hover[EKF_AW_RPM_HOVER_NUM]; // Value obtained from ABI Callback
int32_t last_RPM_pusher; // Value obtained from ABI Callback
float RPM_hover[EKF_AW_RPM_HOVER_NUM]; /// Hover motor RPM
float RPM_pusher; /// Pusher motor RPM
float skew; /// Skew
float elevator_angle;
// Measurements
struct FloatVect3 Vg_NED; /// Ground Speed
struct FloatVect3 acc_filt; ///< Last accelerometer measurements
float V_pitot; /// Pitot tube airspeed
// Innovation
struct FloatVect3 innov_V_gnd;
struct FloatVect3 innov_acc_filt;
float innov_V_pitot;
// Covariance
float meas_cov[7];
float state_cov[9];
float process_cov[12];
// Forces
float fuselage_force[3];
float wing_force[3];
float elevator_force[3];
float hover_force[3];
float pusher_force[3];
// Other
bool reset;
bool in_air;
struct NedCoor_f wind_guess;
struct NedCoor_f offset_guess;
struct ekfHealth health;
uint64_t internal_clock;
uint64_t time_last_on_gnd;
uint64_t time_last_in_air;
bool override_start;
bool override_quick_convergence;
};
extern void ekf_aw_wrapper_init(void);
extern void ekf_aw_wrapper_periodic(void);
extern void ekf_aw_wrapper_fetch(void);
extern void set_in_air_status(bool);
extern float tau_filter_high;
extern float tau_filter_low;
extern struct ekfAw ekf_aw;
// Handlers
#define ekf_aw_wrapper_reset(_v) { \
ekf_aw.reset = false; \
ekf_aw_reset(); \
ekf_aw_reset_health(); \
}
#define ekf_aw_wrapper_set_wind_N(_v) { \
ekf_aw.wind_guess.x = _v; \
ekf_aw_set_wind(&ekf_aw.wind_guess); \
}
#define ekf_aw_wrapper_set_wind_E(_v) { \
ekf_aw.wind_guess.y = _v; \
ekf_aw_set_wind(&ekf_aw.wind_guess); \
}
#define ekf_aw_wrapper_set_wind_D(_v) { \
ekf_aw.wind_guess.z = _v; \
ekf_aw_set_wind(&ekf_aw.wind_guess); \
}
#define ekf_aw_wrapper_set_offset_x(_v) { \
ekf_aw.offset_guess.x = _v; \
ekf_aw_set_offset(&ekf_aw.offset_guess); \
}
#define ekf_aw_wrapper_set_offset_y(_v) { \
ekf_aw.offset_guess.y = _v; \
ekf_aw_set_offset(&ekf_aw.offset_guess); \
}
#define ekf_aw_wrapper_set_offset_z(_v) { \
ekf_aw.offset_guess.z = _v; \
ekf_aw_set_offset(&ekf_aw.offset_guess); \
}
#endif /* EKF_AW_WRAPPER_H */