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

AP_AHRS: Improve use of estimated airspeed 

Enable default airspeed variance to be specified externally to the EKF
Enable use of EKF airspeed estimates
This commit is contained in:
Paul Riseborough 2020-06-07 09:24:22 +10:00 committed by Andrew Tridgell
parent 9a5a264aa0
commit d1d790019c
2 changed files with 60 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -640,7 +640,61 @@ Vector3f AP_AHRS_NavEKF::wind_estimate(void) const
// if we have an estimate
bool AP_AHRS_NavEKF::airspeed_estimate(float &airspeed_ret) const
{
return AP_AHRS_DCM::airspeed_estimate(get_active_airspeed_index(), airspeed_ret);
bool ret = false;
if (airspeed_sensor_enabled()) {
airspeed_ret = AP::airspeed()->get_airspeed();
return true;
}
if (!_flags.wind_estimation) {
return false;
}
// estimate it via nav velocity and wind estimates
// get wind estimates
Vector3f wind_vel;
switch (active_EKF_type()) {
case EKFType::NONE:
return AP_AHRS_DCM::airspeed_estimate(get_active_airspeed_index(), airspeed_ret);
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKFType::SITL:
if (EKF3.getWind(-1,wind_vel)) {
ret = true;
} else {
ret = false;
}
break;
#endif
case EKFType::TWO:
return AP_AHRS_DCM::airspeed_estimate(get_active_airspeed_index(), airspeed_ret);
case EKFType::THREE:
ret = EKF3.getWind(-1,wind_vel);
break;
}
// estimate it via nav velocity and wind estimates
Vector3f nav_vel;
float true_airspeed;
if (ret && have_inertial_nav() && get_velocity_NED(nav_vel)) {
Vector3f true_airspeed_vec = nav_vel - wind_vel;
true_airspeed = true_airspeed_vec.length();
float gnd_speed = nav_vel.length();
if (_wind_max > 0) {
float tas_lim_lower = MAX(0.0f, (gnd_speed - _wind_max));
float tas_lim_upper = MAX(tas_lim_lower, (gnd_speed + _wind_max));
true_airspeed = constrain_float(true_airspeed, tas_lim_lower, tas_lim_upper);
} else {
true_airspeed = MAX(0.0f, true_airspeed);
}
airspeed_ret = true_airspeed / get_EAS2TAS();
}
return ret;
}
// return estimate of true airspeed vector in body frame in m/s
@ -1824,14 +1878,14 @@ void AP_AHRS_NavEKF::writeExtNavData(const Vector3f &pos, const Quaternion &quat
#endif
}
// Writes the default equivalent airspeed in m/s to be used in forward flight if a measured airspeed is required and not available.
void AP_AHRS_NavEKF::writeDefaultAirSpeed(float airspeed)
// Writes the default equivalent airspeed and 1-sigma uncertainty in m/s to be used in forward flight if a measured airspeed is required and not available.
void AP_AHRS_NavEKF::writeDefaultAirSpeed(float airspeed, float uncertainty)
{
#if HAL_NAVEKF2_AVAILABLE
EKF2.writeDefaultAirSpeed(airspeed);
#endif
#if HAL_NAVEKF3_AVAILABLE
EKF3.writeDefaultAirSpeed(airspeed);
EKF3.writeDefaultAirSpeed(airspeed, uncertainty);
#endif
}

4
libraries/AP_AHRS/AP_AHRS_NavEKF.h
View File

@ -185,8 +185,8 @@ public:
// write body odometry measurements to the EKF
void writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, uint16_t delay_ms, const Vector3f &posOffset);
// Writes the default equivalent airspeed in m/s to be used in forward flight if a measured airspeed is required and not available.
void writeDefaultAirSpeed(float airspeed);
// Writes the default equivalent airspeed and its 1-sigma uncertainty in m/s to be used in forward flight if a measured airspeed is required and not available.
void writeDefaultAirSpeed(float airspeed, float uncertainty);
// Write position and quaternion data from an external navigation system
void writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms) override;