From 456dfcb4b9d9680a8575a201c612406655690bb8 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 25 Oct 2021 10:29:59 +0200 Subject: [PATCH] ekf2: update getter for true airspeed --- src/modules/ekf2/EKF/airspeed_fusion.cpp | 6 ++---- src/modules/ekf2/EKF/ekf.h | 2 +- 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/src/modules/ekf2/EKF/airspeed_fusion.cpp b/src/modules/ekf2/EKF/airspeed_fusion.cpp index 750847c707..77d3c277af 100644 --- a/src/modules/ekf2/EKF/airspeed_fusion.cpp +++ b/src/modules/ekf2/EKF/airspeed_fusion.cpp @@ -168,11 +168,9 @@ void Ekf::fuseAirspeed() } } -void Ekf::get_true_airspeed(float *tas) const +float Ekf::getTrueAirspeed() const { - const float tempvar = sqrtf(sq(_state.vel(0) - _state.wind_vel(0)) + sq(_state.vel(1) - _state.wind_vel(1)) + sq( - _state.vel(2))); - memcpy(tas, &tempvar, sizeof(float)); + return (_state.vel - Vector3f(_state.wind_vel(0), _state.wind_vel(1), 0.f)).norm(); } /* diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 7f0f0aca63..9e2b655994 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -134,7 +134,7 @@ public: Vector2f getWindVelocityVariance() const { return P.slice<2, 2>(22, 22).diag(); } // get the true airspeed in m/s - void get_true_airspeed(float *tas) const; + float getTrueAirspeed() const; // get the full covariance matrix const matrix::SquareMatrix &covariances() const { return P; }