From 8e57d66a0d9d6059be434bb111f066c70de9f551 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 20 Sep 2023 15:25:08 +1000 Subject: [PATCH] AP_AHRS: move AP_AHRS_DCM::groundspeed_vector into correct file --- libraries/AP_AHRS/AP_AHRS_Backend.cpp | 68 --------------------------- libraries/AP_AHRS/AP_AHRS_DCM.cpp | 68 +++++++++++++++++++++++++++ 2 files changed, 68 insertions(+), 68 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS_Backend.cpp b/libraries/AP_AHRS/AP_AHRS_Backend.cpp index c33c1705d2..763716e148 100644 --- a/libraries/AP_AHRS/AP_AHRS_Backend.cpp +++ b/libraries/AP_AHRS/AP_AHRS_Backend.cpp @@ -90,74 +90,6 @@ void AP_AHRS::update_orientation() AP::compass().set_board_orientation(orientation); } -// return a ground speed estimate in m/s -Vector2f AP_AHRS_DCM::groundspeed_vector(void) -{ - // Generate estimate of ground speed vector using air data system - Vector2f gndVelADS; - Vector2f gndVelGPS; - float airspeed = 0; - const bool gotAirspeed = airspeed_estimate_true(airspeed); - const bool gotGPS = (AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D); - if (gotAirspeed) { - const Vector2f airspeed_vector{_cos_yaw * airspeed, _sin_yaw * airspeed}; - Vector3f wind; - UNUSED_RESULT(wind_estimate(wind)); - gndVelADS = airspeed_vector + wind.xy(); - } - - // Generate estimate of ground speed vector using GPS - if (gotGPS) { - const float cog = radians(AP::gps().ground_course()); - gndVelGPS = Vector2f(cosf(cog), sinf(cog)) * AP::gps().ground_speed(); - } - // If both ADS and GPS data is available, apply a complementary filter - if (gotAirspeed && gotGPS) { - // The LPF is applied to the GPS and the HPF is applied to the air data estimate - // before the two are summed - //Define filter coefficients - // alpha and beta must sum to one - // beta = dt/Tau, where - // dt = filter time step (0.1 sec if called by nav loop) - // Tau = cross-over time constant (nominal 2 seconds) - // More lag on GPS requires Tau to be bigger, less lag allows it to be smaller - // To-Do - set Tau as a function of GPS lag. - const float alpha = 1.0f - beta; - // Run LP filters - _lp = gndVelGPS * beta + _lp * alpha; - // Run HP filters - _hp = (gndVelADS - _lastGndVelADS) + _hp * alpha; - // Save the current ADS ground vector for the next time step - _lastGndVelADS = gndVelADS; - // Sum the HP and LP filter outputs - return _hp + _lp; - } - // Only ADS data is available return ADS estimate - if (gotAirspeed && !gotGPS) { - return gndVelADS; - } - // Only GPS data is available so return GPS estimate - if (!gotAirspeed && gotGPS) { - return gndVelGPS; - } - - if (airspeed > 0) { - // we have a rough airspeed, and we have a yaw. For - // dead-reckoning purposes we can create a estimated - // groundspeed vector - Vector2f ret{_cos_yaw, _sin_yaw}; - ret *= airspeed; - // adjust for estimated wind - Vector3f wind; - UNUSED_RESULT(wind_estimate(wind)); - ret.x += wind.x; - ret.y += wind.y; - return ret; - } - - return Vector2f(0.0f, 0.0f); -} - /* calculate sin and cos of roll/pitch/yaw from a body_to_ned rotation matrix */ diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 386083b31d..ac7a3ed77d 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -1127,6 +1127,74 @@ bool AP_AHRS_DCM::get_velocity_NED(Vector3f &vec) const return true; } +// return a ground speed estimate in m/s +Vector2f AP_AHRS_DCM::groundspeed_vector(void) +{ + // Generate estimate of ground speed vector using air data system + Vector2f gndVelADS; + Vector2f gndVelGPS; + float airspeed = 0; + const bool gotAirspeed = airspeed_estimate_true(airspeed); + const bool gotGPS = (AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D); + if (gotAirspeed) { + const Vector2f airspeed_vector{_cos_yaw * airspeed, _sin_yaw * airspeed}; + Vector3f wind; + UNUSED_RESULT(wind_estimate(wind)); + gndVelADS = airspeed_vector + wind.xy(); + } + + // Generate estimate of ground speed vector using GPS + if (gotGPS) { + const float cog = radians(AP::gps().ground_course()); + gndVelGPS = Vector2f(cosf(cog), sinf(cog)) * AP::gps().ground_speed(); + } + // If both ADS and GPS data is available, apply a complementary filter + if (gotAirspeed && gotGPS) { + // The LPF is applied to the GPS and the HPF is applied to the air data estimate + // before the two are summed + //Define filter coefficients + // alpha and beta must sum to one + // beta = dt/Tau, where + // dt = filter time step (0.1 sec if called by nav loop) + // Tau = cross-over time constant (nominal 2 seconds) + // More lag on GPS requires Tau to be bigger, less lag allows it to be smaller + // To-Do - set Tau as a function of GPS lag. + const float alpha = 1.0f - beta; + // Run LP filters + _lp = gndVelGPS * beta + _lp * alpha; + // Run HP filters + _hp = (gndVelADS - _lastGndVelADS) + _hp * alpha; + // Save the current ADS ground vector for the next time step + _lastGndVelADS = gndVelADS; + // Sum the HP and LP filter outputs + return _hp + _lp; + } + // Only ADS data is available return ADS estimate + if (gotAirspeed && !gotGPS) { + return gndVelADS; + } + // Only GPS data is available so return GPS estimate + if (!gotAirspeed && gotGPS) { + return gndVelGPS; + } + + if (airspeed > 0) { + // we have a rough airspeed, and we have a yaw. For + // dead-reckoning purposes we can create a estimated + // groundspeed vector + Vector2f ret{_cos_yaw, _sin_yaw}; + ret *= airspeed; + // adjust for estimated wind + Vector3f wind; + UNUSED_RESULT(wind_estimate(wind)); + ret.x += wind.x; + ret.y += wind.y; + return ret; + } + + return Vector2f(0.0f, 0.0f); +} + // Get a derivative of the vertical position in m/s which is kinematically consistent with the vertical position is required by some control loops. // This is different to the vertical velocity from the EKF which is not always consistent with the vertical position due to the various errors that are being corrected for. bool AP_AHRS_DCM::get_vert_pos_rate_D(float &velocity) const