AP_AHRS: add define AP_AIRSPEED_ENABLED
This commit is contained in:
parent
fa9a852ef7
commit
8a6942a9df
@ -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
|
||||
|
@ -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
|
||||
|
@ -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);
|
||||
|
@ -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;
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user