From 1abf90c6d48572958e1887d51b49c36edccd915a Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 14 Mar 2018 13:46:19 -0400 Subject: [PATCH] EKF2 only publish wind_estimate if wind velocity is being estimated - refactor body wind velocity calculation --- src/lib/ecl | 2 +- src/modules/ekf2/ekf2_main.cpp | 110 ++++++++++++++++++++------------- 2 files changed, 68 insertions(+), 44 deletions(-) diff --git a/src/lib/ecl b/src/lib/ecl index 041886a28957..341f8962d23a 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit 041886a28957c17505b19367eb8703444123a4c3 +Subproject commit 341f8962d23ac5cd17297dc72952db0d6c284664 diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 487474054d6a..4ba9ad0e3a92 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -107,6 +107,9 @@ class Ekf2 final : public ModuleBase, public ModuleParams template void update_mag_bias(Param &mag_bias_param, int axis_index); + bool publish_wind_estimate(const hrt_abstime ×tamp); + + const Vector3f get_vel_body_wind(); bool _replay_mode = false; ///< true when we use replay data from a log @@ -173,9 +176,6 @@ class Ekf2 final : public ModuleBase, public ModuleParams uORB::Publication _vehicle_local_position_pub; uORB::Publication _vehicle_global_position_pub; - // Used to correct baro data for positional errors - Vector3f _vel_body_wind = {}; // XYZ velocity relative to wind in body frame (m/s) - Ekf _ekf; parameters *_params; ///< pointer to ekf parameter struct (located in _ekf class instance) @@ -814,20 +814,24 @@ void Ekf2::run() // calculate static pressure error = Pmeas - Ptruth // model position error sensitivity as a body fixed ellipse with different scale in the positive and negtive X direction - float max_airspeed_sq = _aspd_max.get(); - max_airspeed_sq *= max_airspeed_sq; + const float max_airspeed_sq = _aspd_max.get() * _aspd_max.get(); float K_pstatic_coef_x; - if (_vel_body_wind(0) >= 0.0f) { + const Vector3f vel_body_wind = get_vel_body_wind(); + + if (vel_body_wind(0) >= 0.0f) { K_pstatic_coef_x = _K_pstatic_coef_xp.get(); } else { K_pstatic_coef_x = _K_pstatic_coef_xn.get(); } - float pstatic_err = 0.5f * rho * (K_pstatic_coef_x * fminf(_vel_body_wind(0) * _vel_body_wind(0), max_airspeed_sq) + - _K_pstatic_coef_y.get() * fminf(_vel_body_wind(1) * _vel_body_wind(1), max_airspeed_sq) + - _K_pstatic_coef_z.get() * fminf(_vel_body_wind(2) * _vel_body_wind(2), max_airspeed_sq)); + const float x_v2 = fminf(vel_body_wind(0) * vel_body_wind(0), max_airspeed_sq); + const float y_v2 = fminf(vel_body_wind(1) * vel_body_wind(1), max_airspeed_sq); + const float z_v2 = fminf(vel_body_wind(2) * vel_body_wind(2), max_airspeed_sq); + + const float pstatic_err = 0.5f * rho * (K_pstatic_coef_x * x_v2) + (_K_pstatic_coef_y.get() * y_v2) + + (_K_pstatic_coef_z.get() * z_v2); // correct baro measurement using pressure error estimate and assuming sea level gravity balt_data_avg += pstatic_err / (rho * CONSTANTS_ONE_G); @@ -1266,40 +1270,7 @@ void Ekf2::run() _total_cal_time_us = 0; } - { - // Velocity of body origin in local NED frame (m/s) - float velocity[3]; - _ekf.get_velocity(velocity); - - matrix::Quatf q; - _ekf.copy_quaternion(q.data()); - - // Calculate wind-compensated velocity in body frame - Vector3f v_wind_comp(velocity); - matrix::Dcmf R_to_body(q.inversed()); - - float velNE_wind[2]; - _ekf.get_wind_velocity(velNE_wind); - - v_wind_comp(0) -= velNE_wind[0]; - v_wind_comp(1) -= velNE_wind[1]; - _vel_body_wind = R_to_body * v_wind_comp; // TODO: move this elsewhere - - // Publish wind estimate - wind_estimate_s wind_estimate; - wind_estimate.timestamp = now; - wind_estimate.windspeed_north = velNE_wind[0]; - wind_estimate.windspeed_east = velNE_wind[1]; - wind_estimate.variance_north = status.covariances[22]; - wind_estimate.variance_east = status.covariances[23]; - - if (_wind_pub == nullptr) { - _wind_pub = orb_advertise(ORB_ID(wind_estimate), &wind_estimate); - - } else { - orb_publish(ORB_ID(wind_estimate), _wind_pub, &wind_estimate); - } - } + publish_wind_estimate(now); } { @@ -1513,6 +1484,59 @@ int Ekf2::getRangeSubIndex(const int *subs) return -1; } +bool Ekf2::publish_wind_estimate(const hrt_abstime ×tamp) +{ + bool published = false; + + if (_ekf.get_wind_status()) { + float velNE_wind[2]; + _ekf.get_wind_velocity(velNE_wind); + + float wind_var[2]; + _ekf.get_wind_velocity_var(wind_var); + + // Publish wind estimate + wind_estimate_s wind_estimate; + wind_estimate.timestamp = timestamp; + wind_estimate.windspeed_north = velNE_wind[0]; + wind_estimate.windspeed_east = velNE_wind[1]; + wind_estimate.variance_north = wind_var[0]; + wind_estimate.variance_east = wind_var[1]; + + if (_wind_pub == nullptr) { + _wind_pub = orb_advertise(ORB_ID(wind_estimate), &wind_estimate); + + } else { + orb_publish(ORB_ID(wind_estimate), _wind_pub, &wind_estimate); + } + + published = true; + } + + return published; +} + +const Vector3f Ekf2::get_vel_body_wind() +{ + // Used to correct baro data for positional errors + + matrix::Quatf q; + _ekf.copy_quaternion(q.data()); + matrix::Dcmf R_to_body(q.inversed()); + + // Calculate wind-compensated velocity in body frame + // Velocity of body origin in local NED frame (m/s) + float velocity[3]; + _ekf.get_velocity(velocity); + + float velNE_wind[2]; + _ekf.get_wind_velocity(velNE_wind); + + Vector3f v_wind_comp = {velocity[0] - velNE_wind[0], velocity[1] - velNE_wind[1], velocity[2]}; + + return R_to_body * v_wind_comp; +} + Ekf2 *Ekf2::instantiate(int argc, char *argv[]) { Ekf2 *instance = new Ekf2();