mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-02 13:27:32 +08:00
[modules] EKF Airspeed and Wind (#3078)
* [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
committed by
GitHub
GitHub
parent
2db514a43e
commit
1229e0489a
@@ -68,7 +68,7 @@
|
|||||||
<module name="fdm" type="jsbsim"/>
|
<module name="fdm" type="jsbsim"/>
|
||||||
|
|
||||||
<module name="logger_file">
|
<module name="logger_file">
|
||||||
<define name="FILE_LOGGER_PATH" value="/home/ewoud/Documents"/>
|
<define name="FILE_LOGGER_PATH" value="~/"/>
|
||||||
</module>
|
</module>
|
||||||
|
|
||||||
<!--Not dealing with these in the simulation-->
|
<!--Not dealing with these in the simulation-->
|
||||||
@@ -163,6 +163,64 @@
|
|||||||
</module>
|
</module>
|
||||||
|
|
||||||
<module name="motor_mixing"/>
|
<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>
|
</firmware>
|
||||||
|
|
||||||
<!-- CAN BUS 1 (Front Wing) -->
|
<!-- CAN BUS 1 (Front Wing) -->
|
||||||
|
|||||||
@@ -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)"/>
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
-->
|
||||||
|
|
||||||
File diff suppressed because it is too large
Load Diff
@@ -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 */
|
||||||
@@ -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
|
||||||
|
|
||||||
|
*/
|
||||||
@@ -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 */
|
||||||
Reference in New Issue
Block a user