936 changes: 491 additions & 445 deletions sw/airborne/modules/meteo/ekf_aw.cpp
View file

Large diffs are not rendered by default.

100 changes: 51 additions & 49 deletions sw/airborne/modules/meteo/ekf_aw.h
View file
Expand Up @@ -23,26 +23,26 @@ struct ekfAwParameters {
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];
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];
Expand All @@ -51,7 +51,7 @@ struct ekfAwParameters {
bool quick_convergence;
};

struct ekfHealth{
struct ekfHealth {
bool healthy;
uint16_t crashes_n;
};
Expand All @@ -63,7 +63,9 @@ 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);
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);
Expand Down Expand Up @@ -94,49 +96,49 @@ 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(); \
}
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(); \
}
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(); \
}
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(); \
}
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(); \
}
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(); \
}
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(); \
}
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(); \
}
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(); \
}
ekf_aw_params.R_V_pitot = _v; \
ekf_aw_update_params(); \
}

#ifdef __cplusplus
}
Expand Down
271 changes: 137 additions & 134 deletions sw/airborne/modules/meteo/ekf_aw_wrapper.c
View file
Expand Up @@ -32,7 +32,7 @@
#endif

#if EKF_AW_WRAPPER_ROT_WING
#include "modules/rot_wing_drone/wing_rotation_controller_v3b.h"
#include "modules/rot_wing_drone/wing_rotation_controller_v3b.h"
#endif

#if PERIODIC_TELEMETRY
Expand All @@ -45,26 +45,26 @@
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);
&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)
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);
pprz_msg_send_DEBUG_VECT(trans, dev, AC_ID,
strlen(name), name,
datasize, data);
}


Expand Down Expand Up @@ -108,10 +108,10 @@ float tau_filter_low = 0.2f;
bool reset_filter = false;

struct NedCoor_f zero_speed = {
.x = 0.0f,
.y = 0.0f,
.z = 0.0f
};
.x = 0.0f,
.y = 0.0f,
.z = 0.0f
};

// Define filter arrays
Butterworth2LowPass filt_groundspeed[3];
Expand All @@ -133,23 +133,24 @@ Butterworth2LowPass filt_airspeed_pitot;
#define PERIODIC_FREQUENCY_AIRSPEED_EKF 25
#endif

void ekf_aw_wrapper_init(void){

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++) {
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++) {
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;
}
Expand All @@ -159,32 +160,32 @@ void ekf_aw_wrapper_init(void){
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
#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;
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++) {
for (int i = 0; i < EKF_AW_RPM_HOVER_NUM; i++) {
ekf_aw.last_RPM_hover[i] = 0;
}
ekf_aw.last_RPM_pusher = 0;
Expand All @@ -194,66 +195,66 @@ void ekf_aw_wrapper_init(void){
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_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.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.elevator_angle = 0.0f;
ekf_aw.RPM_pusher = 0.0f;
for (int i=0;i<EKF_AW_RPM_HOVER_NUM;i++) {
for (int i = 0; i < EKF_AW_RPM_HOVER_NUM; i++) {
ekf_aw.RPM_hover[i] = 0.0f;
}
};

void ekf_aw_wrapper_periodic(void){
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
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.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.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.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{
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];
Expand All @@ -263,7 +264,7 @@ void ekf_aw_wrapper_periodic(void){
//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++) {
for (int8_t i = 0; i < EKF_AW_RPM_HOVER_NUM; i++) {
ekf_aw.RPM_hover[i] = filt_hover_prop_rpm[i].o[0];
}

Expand All @@ -280,28 +281,28 @@ void ekf_aw_wrapper_periodic(void){
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));
set_in_air_status(autopilot_in_flight() & (-stateGetPositionNed_f()->z > 1.0f));

// Propagate
if (ekf_aw.in_air | ekf_aw.override_start){
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){
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{
} 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{

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);
};
Expand Down Expand Up @@ -334,7 +335,8 @@ void ekf_aw_wrapper_periodic(void){
};

// Function to get information from different modules and set it in the different filters
void ekf_aw_wrapper_fetch(void){
void ekf_aw_wrapper_fetch(void)
{

// For debug
// uint32_t tic_2 = get_sys_time_usec();
Expand All @@ -345,7 +347,7 @@ void ekf_aw_wrapper_fetch(void){
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};
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;
Expand All @@ -354,7 +356,7 @@ void ekf_aw_wrapper_fetch(void){
#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 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
Expand All @@ -380,10 +382,10 @@ void ekf_aw_wrapper_fetch(void){
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);
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);
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);
Expand All @@ -392,8 +394,8 @@ void ekf_aw_wrapper_fetch(void){
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;
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);
Expand All @@ -404,62 +406,63 @@ void ekf_aw_wrapper_fetch(void){

// 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)
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++) {
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){
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;
ekf_aw.last_RPM_hover[0] = rpm_message[i].rpm;
break;
case 1:
ekf_aw.last_RPM_hover[1] = rpm_message[i].rpm;
break;
ekf_aw.last_RPM_hover[1] = rpm_message[i].rpm;
break;
case 2:
ekf_aw.last_RPM_hover[2] = rpm_message[i].rpm;
break;
ekf_aw.last_RPM_hover[2] = rpm_message[i].rpm;
break;
case 3:
ekf_aw.last_RPM_hover[3] = rpm_message[i].rpm;
break;
ekf_aw.last_RPM_hover[3] = rpm_message[i].rpm;
break;
case 4:
ekf_aw.last_RPM_pusher = rpm_message[i].rpm;
break;
ekf_aw.last_RPM_pusher = rpm_message[i].rpm;
break;
default:
break;
break;
}
}
}
time_of_rpm = get_sys_time_float();
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
*/
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
*/
54 changes: 27 additions & 27 deletions sw/airborne/modules/meteo/ekf_aw_wrapper.h
View file
Expand Up @@ -20,16 +20,16 @@ struct ekfAw {
struct FloatVect3 acc; ///< Last accelerometer measurements
struct FloatRates gyro; ///< Last gyroscope measurements
struct FloatEulers euler; /// Euler angles
#define EKF_AW_RPM_HOVER_NUM 4

#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
Expand Down Expand Up @@ -57,7 +57,7 @@ struct ekfAw {
bool in_air;
struct NedCoor_f wind_guess;
struct NedCoor_f offset_guess;
struct ekfHealth health;
struct ekfHealth health;
uint64_t internal_clock;
uint64_t time_last_on_gnd;
uint64_t time_last_in_air;
Expand All @@ -80,39 +80,39 @@ extern struct ekfAw ekf_aw;

// Handlers
#define ekf_aw_wrapper_reset(_v) { \
ekf_aw.reset = false; \
ekf_aw_reset(); \
ekf_aw_reset_health(); \
}
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); \
}
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); \
}
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); \
}
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); \
}
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); \
}
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); \
}
ekf_aw.offset_guess.z = _v; \
ekf_aw_set_offset(&ekf_aw.offset_guess); \
}

#endif /* EKF_AW_WRAPPER_H */