diff --git a/libraries/AP_Airspeed/AP_Airspeed.h b/libraries/AP_Airspeed/AP_Airspeed.h index 1af6c88282..23ded9da80 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.h +++ b/libraries/AP_Airspeed/AP_Airspeed.h @@ -7,13 +7,13 @@ #include #include #include -#include +#include class Airspeed_Calibration { public: friend class AP_Airspeed; // constructor - Airspeed_Calibration(const AP_SpdHgtControl::AircraftParameters &parms); + Airspeed_Calibration(const AP_Vehicle::FixedWing &parms); // initialise the calibration void init(float initial_ratio); @@ -29,14 +29,14 @@ private: const float Q1; // process noise matrix bottom right element Vector3f state; // state vector const float DT; // time delta - const AP_SpdHgtControl::AircraftParameters &aparm; + const AP_Vehicle::FixedWing &aparm; }; class AP_Airspeed { public: // constructor - AP_Airspeed(const AP_SpdHgtControl::AircraftParameters &parms) : + AP_Airspeed(const AP_Vehicle::FixedWing &parms) : _ets_fd(-1), _EAS2TAS(1.0f), _calibration(parms) diff --git a/libraries/AP_Airspeed/Airspeed_Calibration.cpp b/libraries/AP_Airspeed/Airspeed_Calibration.cpp index 13c97e39e3..c456c0c92a 100644 --- a/libraries/AP_Airspeed/Airspeed_Calibration.cpp +++ b/libraries/AP_Airspeed/Airspeed_Calibration.cpp @@ -14,7 +14,7 @@ extern const AP_HAL::HAL& hal; // constructor - fill in all the initial values -Airspeed_Calibration::Airspeed_Calibration(const AP_SpdHgtControl::AircraftParameters &parms) : +Airspeed_Calibration::Airspeed_Calibration(const AP_Vehicle::FixedWing &parms) : P(100, 0, 0, 0, 100, 0, 0, 0, 0.000001f), @@ -119,18 +119,27 @@ void AP_Airspeed::update_calibration(const Vector3f &vground) // auto-calibration not enabled return; } + + // set state.z based on current ratio, this allows the operator to + // override the current ratio in flight with autocal, which is + // very useful both for testing and to force a reasonable value. + float ratio = constrain_float(_ratio, 1.0f, 4.0f); + + _calibration.state.z = 1.0 / sqrtf(ratio); + // calculate true airspeed, assuming a airspeed ratio of 1.0 float dpress = get_differential_pressure(); float true_airspeed = sqrtf(dpress) * _EAS2TAS; - float ratio = _calibration.update(true_airspeed, vground); - if (isnan(ratio) || isinf(ratio)) { + float zratio = _calibration.update(true_airspeed, vground); + + if (isnan(zratio) || isinf(zratio)) { return; } // this constrains the resulting ratio to between 1.0 and 4.0 - ratio = constrain_float(ratio, 0.5f, 1.0f); - _ratio.set(1/sq(ratio)); + zratio = constrain_float(zratio, 0.5f, 1.0f); + _ratio.set(1/sq(zratio)); if (_counter > 60) { if (_last_saved_ratio > 1.05f*_ratio || _last_saved_ratio < 0.95f*_ratio) { diff --git a/libraries/AP_Airspeed/examples/Airspeed/Airspeed.pde b/libraries/AP_Airspeed/examples/Airspeed/Airspeed.pde index c69a01fda7..79bd5d4e35 100644 --- a/libraries/AP_Airspeed/examples/Airspeed/Airspeed.pde +++ b/libraries/AP_Airspeed/examples/Airspeed/Airspeed.pde @@ -30,7 +30,7 @@ #include #include #include -#include +#include #include #include #include @@ -43,7 +43,7 @@ AP_ADC_ADS7844 apm1_adc; const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER; -static AP_SpdHgtControl::AircraftParameters aparm; +static AP_Vehicle::FixedWing aparm; AP_Airspeed airspeed(aparm);