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 (for 4.1) 

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:24:29 +11:00
parent 0f9844094a
commit 6cf92b4726
1 changed files with 14 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -622,7 +622,8 @@ bool AP_AHRS_NavEKF::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
@ -2639,18 +2640,23 @@ bool AP_AHRS_NavEKF::getGpsGlitchStatus() const
//get the index of the active airspeed sensor, wrt the primary core
uint8_t AP_AHRS_NavEKF::get_active_airspeed_index() const
{
// we only have affinity for EKF3 as of now
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