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

AP_AHRS: move to airspeed_estimate with pointer

This commit is contained in:
Peter Hall 2020-01-07 00:45:58 +00:00 committed by WickedShell
parent a2e72de5ae
commit f6897632f8
7 changed files with 19 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_AHRS/AP_AHRS.cpp
View File

@ -180,19 +180,19 @@ Vector3f AP_AHRS::get_gyro_latest(void) const
} }
// return airspeed estimate if available // return airspeed estimate if available
bool AP_AHRS::airspeed_estimate(float *airspeed_ret) const bool AP_AHRS::airspeed_estimate(float &airspeed_ret) const
{ {
if (airspeed_sensor_enabled()) { if (airspeed_sensor_enabled()) {
*airspeed_ret = _airspeed->get_airspeed(); airspeed_ret = _airspeed->get_airspeed();
if (_wind_max > 0 && AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D) { if (_wind_max > 0 && AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D) {
// constrain the airspeed by the ground speed // constrain the airspeed by the ground speed
// and AHRS_WIND_MAX // and AHRS_WIND_MAX
const float gnd_speed = AP::gps().ground_speed(); const float gnd_speed = AP::gps().ground_speed();
float true_airspeed = *airspeed_ret * get_EAS2TAS(); float true_airspeed = airspeed_ret * get_EAS2TAS();
true_airspeed = constrain_float(true_airspeed, true_airspeed = constrain_float(true_airspeed,
gnd_speed - _wind_max, gnd_speed - _wind_max,
gnd_speed + _wind_max); gnd_speed + _wind_max);
*airspeed_ret = true_airspeed / get_EAS2TAS(); airspeed_ret = true_airspeed / get_EAS2TAS();
} }
return true; return true;
} }
@ -251,7 +251,7 @@ Vector2f AP_AHRS::groundspeed_vector(void)
Vector2f gndVelADS; Vector2f gndVelADS;
Vector2f gndVelGPS; Vector2f gndVelGPS;
float airspeed = 0; float airspeed = 0;
const bool gotAirspeed = airspeed_estimate_true(&airspeed); const bool gotAirspeed = airspeed_estimate_true(airspeed);
const bool gotGPS = (AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D); const bool gotGPS = (AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D);
if (gotAirspeed) { if (gotAirspeed) {
const Vector3f wind = wind_estimate(); const Vector3f wind = wind_estimate();

6
libraries/AP_AHRS/AP_AHRS.h
View File

@ -251,15 +251,15 @@ public:
// return an airspeed estimate if available. return true // return an airspeed estimate if available. return true
// if we have an estimate // if we have an estimate
virtual bool airspeed_estimate(float *airspeed_ret) const WARN_IF_UNUSED; virtual bool airspeed_estimate(float &airspeed_ret) const WARN_IF_UNUSED;
// return a true airspeed estimate (navigation airspeed) if // return a true airspeed estimate (navigation airspeed) if
// available. return true if we have an estimate // available. return true if we have an estimate
bool airspeed_estimate_true(float *airspeed_ret) const WARN_IF_UNUSED { bool airspeed_estimate_true(float &airspeed_ret) const WARN_IF_UNUSED {
if (!airspeed_estimate(airspeed_ret)) { if (!airspeed_estimate(airspeed_ret)) {
return false; return false;
} }
*airspeed_ret *= get_EAS2TAS(); airspeed_ret *= get_EAS2TAS();
return true; return true;
} }

12
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -1039,11 +1039,11 @@ bool AP_AHRS_DCM::get_position(struct Location &loc) const
} }
// return an airspeed estimate if available // return an airspeed estimate if available
bool AP_AHRS_DCM::airspeed_estimate(float *airspeed_ret) const bool AP_AHRS_DCM::airspeed_estimate(float &airspeed_ret) const
{ {
bool ret = false; bool ret = false;
if (airspeed_sensor_enabled()) { if (airspeed_sensor_enabled()) {
*airspeed_ret = _airspeed->get_airspeed(); airspeed_ret = _airspeed->get_airspeed();
return true; return true;
} }
@ -1053,7 +1053,7 @@ bool AP_AHRS_DCM::airspeed_estimate(float *airspeed_ret) const
// estimate it via GPS speed and wind // estimate it via GPS speed and wind
if (have_gps()) { if (have_gps()) {
*airspeed_ret = _last_airspeed; airspeed_ret = _last_airspeed;
ret = true; ret = true;
} }
@ -1061,16 +1061,16 @@ bool AP_AHRS_DCM::airspeed_estimate(float *airspeed_ret) const
// constrain the airspeed by the ground speed // constrain the airspeed by the ground speed
// and AHRS_WIND_MAX // and AHRS_WIND_MAX
const float gnd_speed = AP::gps().ground_speed(); const float gnd_speed = AP::gps().ground_speed();
float true_airspeed = *airspeed_ret * get_EAS2TAS(); float true_airspeed = airspeed_ret * get_EAS2TAS();
true_airspeed = constrain_float(true_airspeed, true_airspeed = constrain_float(true_airspeed,
gnd_speed - _wind_max, gnd_speed - _wind_max,
gnd_speed + _wind_max); gnd_speed + _wind_max);
*airspeed_ret = true_airspeed / get_EAS2TAS(); airspeed_ret = true_airspeed / get_EAS2TAS();
} }
if (!ret) { if (!ret) {
// give the last estimate, but return false. This is used by // give the last estimate, but return false. This is used by
// dead-reckoning code // dead-reckoning code
*airspeed_ret = _last_airspeed; airspeed_ret = _last_airspeed;
} }
return ret; return ret;
} }

2
libraries/AP_AHRS/AP_AHRS_DCM.h
View File

@ -92,7 +92,7 @@ public:
// return an airspeed estimate if available. return true // return an airspeed estimate if available. return true
// if we have an estimate // if we have an estimate
bool airspeed_estimate(float *airspeed_ret) const override; bool airspeed_estimate(float &airspeed_ret) const override;
bool use_compass() override; bool use_compass() override;

2
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -489,7 +489,7 @@ Vector3f AP_AHRS_NavEKF::wind_estimate(void) const
// return an airspeed estimate if available. return true // return an airspeed estimate if available. return true
// if we have an estimate // if we have an estimate
bool AP_AHRS_NavEKF::airspeed_estimate(float *airspeed_ret) const bool AP_AHRS_NavEKF::airspeed_estimate(float &airspeed_ret) const
{ {
return AP_AHRS_DCM::airspeed_estimate(airspeed_ret); return AP_AHRS_DCM::airspeed_estimate(airspeed_ret);
} }

2
libraries/AP_AHRS/AP_AHRS_NavEKF.h
View File

@ -83,7 +83,7 @@ public:
// return an airspeed estimate if available. return true // return an airspeed estimate if available. return true
// if we have an estimate // if we have an estimate
bool airspeed_estimate(float *airspeed_ret) const override; bool airspeed_estimate(float &airspeed_ret) const override;
// true if compass is being used // true if compass is being used
bool use_compass() override; bool use_compass() override;

4
libraries/AP_AHRS/AP_AHRS_View.h
View File

@ -88,11 +88,11 @@ public:
return ahrs.wind_estimate(); return ahrs.wind_estimate();
} }
bool airspeed_estimate(float *airspeed_ret) const WARN_IF_UNUSED { bool airspeed_estimate(float &airspeed_ret) const WARN_IF_UNUSED {
return ahrs.airspeed_estimate(airspeed_ret); return ahrs.airspeed_estimate(airspeed_ret);
} }
bool airspeed_estimate_true(float *airspeed_ret) const WARN_IF_UNUSED { bool airspeed_estimate_true(float &airspeed_ret) const WARN_IF_UNUSED {
return ahrs.airspeed_estimate_true(airspeed_ret); return ahrs.airspeed_estimate_true(airspeed_ret);
} }