Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS: fixed switching airspeed sensor based on EKF3 affinity 

we need to use the EKF selected airspeed sensor when the EKF is setup
to run a different sensor on each lane
This commit is contained in:
Andrew Tridgell 2021-11-24 10:18:11 +11:00 committed by Peter Barker
parent f848fa3a86
commit 0141ec5be7
1 changed files with 12 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AP_AHRS/AP_AHRS.cpp
View File

@ -821,7 +821,8 @@ bool AP_AHRS::airspeed_estimate(float &airspeed_ret) const
{
bool ret = false;
if (airspeed_sensor_enabled()) {
airspeed_ret = AP::airspeed()->get_airspeed();
uint8_t idx = get_active_airspeed_index();
airspeed_ret = AP::airspeed()->get_airspeed(idx);
if (_wind_max > 0 && AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D) {
// constrain the airspeed by the ground speed
@ -2885,18 +2886,22 @@ 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
{
const auto *airspeed = AP::airspeed();
if (airspeed == nullptr) {
return 0;
}
// we only have affinity for EKF3 as of now
#if HAL_NAVEKF3_AVAILABLE
if (active_EKF_type() == EKFType::THREE) {
return EKF3.getActiveAirspeed(get_primary_core_index());
uint8_t ret = EKF3.getActiveAirspeed(get_primary_core_index());
if (ret != 255 && airspeed->healthy(ret)) {
return ret;
}
}
#endif
// for the rest, let the primary airspeed sensor be used
const AP_Airspeed * _airspeed = AP::airspeed();
if (_airspeed != nullptr) {
return _airspeed->get_primary();
}
return 0;
return airspeed->get_primary();
}
// get the index of the current primary IMU