AP_AHRS: add define AP_AIRSPEED_ENABLED

This commit is contained in:
Joshua Henderson 2022-01-09 20:37:03 -05:00 committed by Andrew Tridgell
parent fa9a852ef7
commit 8a6942a9df
4 changed files with 75 additions and 18 deletions

View File

@ -821,6 +821,8 @@ Vector3f AP_AHRS::wind_estimate(void) const
bool AP_AHRS::airspeed_estimate(float &airspeed_ret) const
{
bool ret = false;
#if AP_AIRSPEED_ENABLED
if (airspeed_sensor_enabled()) {
uint8_t idx = get_active_airspeed_index();
airspeed_ret = AP::airspeed()->get_airspeed(idx);
@ -838,6 +840,7 @@ bool AP_AHRS::airspeed_estimate(float &airspeed_ret) const
return true;
}
#endif
if (!get_wind_estimation_enabled()) {
return false;
@ -2887,6 +2890,7 @@ bool AP_AHRS::get_vel_innovations_and_variances_for_source(uint8_t source, Vecto
//get the index of the active airspeed sensor, wrt the primary core
uint8_t AP_AHRS::get_active_airspeed_index() const
{
#if AP_AIRSPEED_ENABLED
const auto *airspeed = AP::airspeed();
if (airspeed == nullptr) {
return 0;
@ -2901,8 +2905,13 @@ uint8_t AP_AHRS::get_active_airspeed_index() const
}
}
#endif
// for the rest, let the primary airspeed sensor be used
return airspeed->get_primary();
#else
return 0;
#endif // AP_AIRSPEED_ENABLED
}
// get the index of the current primary IMU

View File

@ -157,15 +157,23 @@ public:
// return true if airspeed comes from an airspeed sensor, as
// opposed to an IMU estimate
bool airspeed_sensor_enabled(void) const {
#if AP_AIRSPEED_ENABLED
const AP_Airspeed *_airspeed = AP::airspeed();
return _airspeed != nullptr && _airspeed->use() && _airspeed->healthy();
#else
return false;
#endif
}
// return true if airspeed comes from a specific airspeed sensor, as
// opposed to an IMU estimate
bool airspeed_sensor_enabled(uint8_t airspeed_index) const {
#if AP_AIRSPEED_ENABLED
const AP_Airspeed *_airspeed = AP::airspeed();
return _airspeed != nullptr && _airspeed->use(airspeed_index) && _airspeed->healthy(airspeed_index);
#else
return false;
#endif
}
// return a ground vector estimate in meters/second, in North/East order

View File

@ -716,12 +716,14 @@ AP_AHRS_DCM::drift_correction(float deltat)
// not enough time has accumulated
return;
}
float airspeed;
float airspeed = _last_airspeed;
#if AP_AIRSPEED_ENABLED
if (airspeed_sensor_enabled()) {
airspeed = AP::airspeed()->get_airspeed();
} else {
airspeed = _last_airspeed;
}
#endif
// use airspeed to estimate our ground velocity in
// earth frame by subtracting the wind
velocity = _dcm_matrix.colx() * airspeed;
@ -1025,12 +1027,18 @@ void AP_AHRS_DCM::estimate_wind(void)
}
_last_wind_time = now;
} else if (now - _last_wind_time > 2000 && airspeed_sensor_enabled()) {
return;
}
#if AP_AIRSPEED_ENABLED
if (now - _last_wind_time > 2000 && airspeed_sensor_enabled()) {
// when flying straight use airspeed to get wind estimate if available
const Vector3f airspeed = _dcm_matrix.colx() * AP::airspeed()->get_airspeed();
const Vector3f wind = velocity - (airspeed * get_EAS2TAS());
_wind = _wind * 0.92f + wind * 0.08f;
}
#endif
}
@ -1064,13 +1072,15 @@ bool AP_AHRS_DCM::get_position(struct Location &loc) const
bool AP_AHRS_DCM::airspeed_estimate(float &airspeed_ret) const
{
#if AP_AIRSPEED_ENABLED
const auto *airspeed = AP::airspeed();
if (airspeed == nullptr) {
if (airspeed != nullptr) {
return airspeed_estimate(airspeed->get_primary(), airspeed_ret);
}
#endif
// airspeed_estimate will also make this nullptr check and act
// appropriately when we call it with a dummy sensor ID.
return airspeed_estimate(0, airspeed_ret);
}
return airspeed_estimate(airspeed->get_primary(), airspeed_ret);
}
// return an airspeed estimate:
@ -1079,15 +1089,12 @@ bool AP_AHRS_DCM::airspeed_estimate(float &airspeed_ret) const
// - otherwise from a cached wind-triangle estimate value (but returning false)
bool AP_AHRS_DCM::airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret) const
{
if (airspeed_sensor_enabled(airspeed_index)) {
airspeed_ret = AP::airspeed()->get_airspeed(airspeed_index);
} else if (AP::ahrs().get_wind_estimation_enabled() && have_gps()) {
// estimate it via GPS speed and wind
airspeed_ret = _last_airspeed;
} else {
// give the last estimate, but return false. This is used by
// dead-reckoning code
airspeed_ret = _last_airspeed;
// airspeed_ret: will always be filled-in by get_unconstrained_airspeed_estimate which fills in airspeed_ret in this order:
// airspeed as filled-in by an enabled airsped sensor
// if no airspeed sensor: airspeed estimated using the GPS speed & wind_speed_estimation
// Or if none of the above, fills-in using the previous airspeed estimate
// Return false: if we are using the previous airspeed estimate
if (!get_unconstrained_airspeed_estimate(airspeed_index, airspeed_ret)) {
return false;
}
@ -1106,6 +1113,32 @@ bool AP_AHRS_DCM::airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret)
return true;
}
// airspeed_ret: will always be filled-in by get_unconstrained_airspeed_estimate which fills in airspeed_ret in this order:
// airspeed as filled-in by an enabled airsped sensor
// if no airspeed sensor: airspeed estimated using the GPS speed & wind_speed_estimation
// Or if none of the above, fills-in using the previous airspeed estimate
// Return false: if we are using the previous airspeed estimate
bool AP_AHRS_DCM::get_unconstrained_airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret) const
{
#if AP_AIRSPEED_ENABLED
if (airspeed_sensor_enabled(airspeed_index)) {
airspeed_ret = AP::airspeed()->get_airspeed(airspeed_index);
return true;
}
#endif
if (AP::ahrs().get_wind_estimation_enabled() && have_gps()) {
// estimated via GPS speed and wind
airspeed_ret = _last_airspeed;
return true;
}
// Else give the last estimate, but return false.
// This is used by the dead-reckoning code
airspeed_ret = _last_airspeed;
return false;
}
bool AP_AHRS::set_home(const Location &loc)
{
WITH_SEMAPHORE(_rsem);

View File

@ -163,6 +163,13 @@ private:
// DCM matrix from the eulers. Called internally we may.
void reset(bool recover_eulers);
// airspeed_ret: will always be filled-in by get_unconstrained_airspeed_estimate which fills in airspeed_ret in this order:
// airspeed as filled-in by an enabled airsped sensor
// if no airspeed sensor: airspeed estimated using the GPS speed & wind_speed_estimation
// Or if none of the above, fills-in using the previous airspeed estimate
// Return false: if we are using the previous airspeed estimate
bool get_unconstrained_airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret) const;
// primary representation of attitude of board used for all inertial calculations
Matrix3f _dcm_matrix;