From ae46c38ff7e3eec6087efa74fad7eb8c853ad24e Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Sun, 7 Aug 2016 20:28:24 -0700 Subject: [PATCH] AP_Airspeed: pass in max airspeed via function call instead of using aparm --- libraries/AP_Airspeed/AP_Airspeed.h | 4 ++-- libraries/AP_Airspeed/Airspeed_Calibration.cpp | 10 +++++----- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/libraries/AP_Airspeed/AP_Airspeed.h b/libraries/AP_Airspeed/AP_Airspeed.h index 844160f954..8b66945407 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.h +++ b/libraries/AP_Airspeed/AP_Airspeed.h @@ -23,7 +23,7 @@ public: // take current airspeed in m/s and ground speed vector and return // new scaling factor - float update(float airspeed, const Vector3f &vg); + float update(float airspeed, const Vector3f &vg, int16_t max_airspeed_allowed_during_cal); private: // state of kalman filter for airspeed ratio estimation @@ -125,7 +125,7 @@ public: } // update airspeed ratio calibration - void update_calibration(const Vector3f &vground); + void update_calibration(const Vector3f &vground, int16_t max_airspeed_allowed_during_cal); // log data to MAVLink void log_mavlink_send(mavlink_channel_t chan, const Vector3f &vground); diff --git a/libraries/AP_Airspeed/Airspeed_Calibration.cpp b/libraries/AP_Airspeed/Airspeed_Calibration.cpp index 890ce8ae5e..d8b349d7a3 100644 --- a/libraries/AP_Airspeed/Airspeed_Calibration.cpp +++ b/libraries/AP_Airspeed/Airspeed_Calibration.cpp @@ -39,7 +39,7 @@ void Airspeed_Calibration::init(float initial_ratio) update the state of the airspeed calibration - needs to be called once a second */ -float Airspeed_Calibration::update(float airspeed, const Vector3f &vg) +float Airspeed_Calibration::update(float airspeed, const Vector3f &vg, int16_t max_airspeed_allowed_during_cal) { // Perform the covariance prediction // Q is a diagonal matrix so only need to add three terms in @@ -101,8 +101,8 @@ float Airspeed_Calibration::update(float airspeed, const Vector3f &vg) P.b.y = MAX(P.b.y, 0.0f); P.c.z = MAX(P.c.z, 0.0f); - state.x = constrain_float(state.x, -aparm.airspeed_max, aparm.airspeed_max); - state.y = constrain_float(state.y, -aparm.airspeed_max, aparm.airspeed_max); + state.x = constrain_float(state.x, -max_airspeed_allowed_during_cal, max_airspeed_allowed_during_cal); + state.y = constrain_float(state.y, -max_airspeed_allowed_during_cal, max_airspeed_allowed_during_cal); state.z = constrain_float(state.z, 0.5f, 1.0f); return state.z; @@ -112,7 +112,7 @@ float Airspeed_Calibration::update(float airspeed, const Vector3f &vg) /* called once a second to do calibration update */ -void AP_Airspeed::update_calibration(const Vector3f &vground) +void AP_Airspeed::update_calibration(const Vector3f &vground, int16_t max_airspeed_allowed_during_cal) { if (!_autocal) { // auto-calibration not enabled @@ -130,7 +130,7 @@ void AP_Airspeed::update_calibration(const Vector3f &vground) float dpress = get_differential_pressure(); float true_airspeed = sqrtf(dpress) * _EAS2TAS; - float zratio = _calibration.update(true_airspeed, vground); + float zratio = _calibration.update(true_airspeed, vground, max_airspeed_allowed_during_cal); if (isnan(zratio) || isinf(zratio)) { return;