mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-30 20:48:33 -04:00
AP_AHRS: move groundspeed vector up to AP_AHRS_DCM
The other estimators all have their own implementation of this
This commit is contained in:
parent
8169910866
commit
74709c6292
@ -77,7 +77,7 @@ void AP_AHRS::update_orientation()
|
||||
}
|
||||
|
||||
// return a ground speed estimate in m/s
|
||||
Vector2f AP_AHRS_Backend::groundspeed_vector(void)
|
||||
Vector2f AP_AHRS_DCM::groundspeed_vector(void)
|
||||
{
|
||||
// Generate estimate of ground speed vector using air data system
|
||||
Vector2f gndVelADS;
|
||||
|
@ -207,7 +207,7 @@ public:
|
||||
}
|
||||
|
||||
// return a ground vector estimate in meters/second, in North/East order
|
||||
virtual Vector2f groundspeed_vector(void);
|
||||
virtual Vector2f groundspeed_vector(void) = 0;
|
||||
|
||||
// return a ground velocity in meters/second, North/East/Down
|
||||
// order. This will only be accurate if have_inertial_nav() is
|
||||
@ -465,12 +465,6 @@ protected:
|
||||
Vector3f _accel_ef[INS_MAX_INSTANCES];
|
||||
Vector3f _accel_ef_blended;
|
||||
|
||||
// Declare filter states for HPF and LPF used by complementary
|
||||
// filter in AP_AHRS::groundspeed_vector
|
||||
Vector2f _lp; // ground vector low-pass filter
|
||||
Vector2f _hp; // ground vector high-pass filter
|
||||
Vector2f _lastGndVelADS; // previous HPF input
|
||||
|
||||
// helper trig variables
|
||||
float _cos_roll{1.0f};
|
||||
float _cos_pitch{1.0f};
|
||||
|
@ -101,6 +101,9 @@ public:
|
||||
return true;
|
||||
}
|
||||
|
||||
// return a ground vector estimate in meters/second, in North/East order
|
||||
Vector2f groundspeed_vector() override;
|
||||
|
||||
bool use_compass() override;
|
||||
|
||||
// return the quaternion defining the rotation from NED to XYZ (body) axes
|
||||
@ -232,4 +235,10 @@ private:
|
||||
|
||||
// last origin we returned, for DCM fallback from EKF
|
||||
Location last_origin;
|
||||
|
||||
// Declare filter states for HPF and LPF used by complementary
|
||||
// filter in AP_AHRS::groundspeed_vector
|
||||
Vector2f _lp; // ground vector low-pass filter
|
||||
Vector2f _hp; // ground vector high-pass filter
|
||||
Vector2f _lastGndVelADS; // previous HPF input
|
||||
};
|
||||
|
Loading…
Reference in New Issue
Block a user