aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_AHRS: move AP_AHRS_DCM::groundspeed_vector into correct file

Browse Source
This commit is contained in:
Peter Barker 2023-09-20 15:25:08 +10:00 committed by Peter Barker
parent b44682d1a6
commit 8e57d66a0d
2 changed files with 68 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

68
libraries/AP_AHRS/AP_AHRS_Backend.cpp
View File

@ -90,74 +90,6 @@ void AP_AHRS::update_orientation()
AP::compass().set_board_orientation(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 calculate sin and cos of roll/pitch/yaw from a body_to_ned rotation matrix
*/ */

68
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -1127,6 +1127,74 @@ bool AP_AHRS_DCM::get_velocity_NED(Vector3f &vec) const
return true; 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. // 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. // 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 bool AP_AHRS_DCM::get_vert_pos_rate_D(float &velocity) const