From 1586abab8dbc4de03f050c9cf7b19ae0aab7b6de Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Sun, 7 Aug 2016 17:48:54 -0700 Subject: [PATCH] Revert "AP_Airspeed: Converted library to be stand-alone from APM:Plane." This reverts commit 5439257236ae4a2beaafc94976ab076e898faf44. --- libraries/AP_Airspeed/AP_Airspeed.cpp | 28 +++---------------- libraries/AP_Airspeed/AP_Airspeed.h | 17 ++--------- .../AP_Airspeed/Airspeed_Calibration.cpp | 12 ++++---- .../examples/Airspeed/Airspeed.cpp | 4 ++- 4 files changed, 16 insertions(+), 45 deletions(-) diff --git a/libraries/AP_Airspeed/AP_Airspeed.cpp b/libraries/AP_Airspeed/AP_Airspeed.cpp index f01de6affe..01d97ec2cf 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed.cpp @@ -134,37 +134,17 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = { // @Description: This parameter allows you to to set the PSI (pounds per square inch) range for your sensor. You should not change this unless you examine the datasheet for your device // @User: Advanced AP_GROUPINFO("PSI_RANGE", 8, AP_Airspeed, _psi_range, PSI_RANGE_DEFAULT), - - // @Param: ARSPD_FBW_MIN - // @DisplayName: Minimum Airspeed - // @Description: This is the minimum airspeed you want to fly at in modes where the autopilot controls the airspeed. This should be set to a value around 20% higher than the level flight stall speed for the airframe. This value is also used in the STALL_PREVENTION code. - // @Units: m/s - // @Range: 5 100 - // @Increment: 1 - // @User: Standard - AP_GROUPINFO("FBW_MIN", 9, AP_Airspeed, _airspeed_min, 9), - - // @Param: ARSPD_FBW_MAX - // @DisplayName: Maximum Airspeed - // @Description: This is the maximum airspeed that you want to allow for your airframe in auto-throttle modes. You should ensure that this value is sufficiently above the ARSPD_MIN value to allow for a sufficient flight envelope to accurately control altitude using airspeed. A value at least 50% above ARSPD_MIN is recommended. - // @Units: m/s - // @Range: 5 100 - // @Increment: 1 - // @User: Standard - AP_GROUPINFO("FBW_MAX", 10, AP_Airspeed, _airspeed_max, 22), - + AP_GROUPEND }; -AP_Airspeed::AP_Airspeed() +AP_Airspeed::AP_Airspeed(const AP_Vehicle::FixedWing &parms) : _EAS2TAS(1.0f) - , _calibration() - , analog(_pin, _psi_range) - , digital(_psi_range) + , _calibration(parms) { AP_Param::setup_object_defaults(this, var_info); -}; +} /* diff --git a/libraries/AP_Airspeed/AP_Airspeed.h b/libraries/AP_Airspeed/AP_Airspeed.h index 8e0154fe56..844160f954 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.h +++ b/libraries/AP_Airspeed/AP_Airspeed.h @@ -16,7 +16,7 @@ class Airspeed_Calibration { public: friend class AP_Airspeed; // constructor - Airspeed_Calibration(void); + Airspeed_Calibration(const AP_Vehicle::FixedWing &parms); // initialise the calibration void init(float initial_ratio); @@ -32,13 +32,14 @@ private: const float Q1; // process noise matrix bottom right element Vector3f state; // state vector const float DT; // time delta + const AP_Vehicle::FixedWing &aparm; }; class AP_Airspeed { public: // constructor - AP_Airspeed(); + AP_Airspeed(const AP_Vehicle::FixedWing &parms); void init(void); @@ -123,16 +124,6 @@ public: return _EAS2TAS; } - // get the min set airspeed - float get_airspeed_min(void) const { - return (float)(_airspeed_min); - } - - // get the max set airspeed - float get_airspeed_max(void) const { - return (float)(_airspeed_max); - } - // update airspeed ratio calibration void update_calibration(const Vector3f &vground); @@ -165,8 +156,6 @@ private: AP_Int8 _autocal; AP_Int8 _tube_order; AP_Int8 _skip_cal; - AP_Int16 _airspeed_min; - AP_Int16 _airspeed_max; float _raw_airspeed; float _airspeed; float _last_pressure; diff --git a/libraries/AP_Airspeed/Airspeed_Calibration.cpp b/libraries/AP_Airspeed/Airspeed_Calibration.cpp index 5a281c3a67..890ce8ae5e 100644 --- a/libraries/AP_Airspeed/Airspeed_Calibration.cpp +++ b/libraries/AP_Airspeed/Airspeed_Calibration.cpp @@ -13,10 +13,9 @@ #include "AP_Airspeed.h" extern const AP_HAL::HAL& hal; -AP_Airspeed arspd; // constructor - fill in all the initial values -Airspeed_Calibration::Airspeed_Calibration() +Airspeed_Calibration::Airspeed_Calibration(const AP_Vehicle::FixedWing &parms) : P(100, 0, 0, 0, 100, 0, 0, 0, 0.000001f) @@ -24,6 +23,7 @@ Airspeed_Calibration::Airspeed_Calibration() , Q1(0.0000005f) , state(0, 0, 0) , DT(1) + , aparm(parms) { } @@ -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 aspeed, const Vector3f &vg) +float Airspeed_Calibration::update(float airspeed, const Vector3f &vg) { // Perform the covariance prediction // Q is a diagonal matrix so only need to add three terms in @@ -54,7 +54,7 @@ float Airspeed_Calibration::update(float aspeed, const Vector3f &vg) // invariant plus process noise // Ignore vertical wind component float TAS_pred = state.z * norm(vg.x - state.x, vg.y - state.y, vg.z); - float TAS_mea = aspeed; + float TAS_mea = airspeed; // Calculate the observation Jacobian H_TAS float SH1 = sq(vg.y - state.y) + sq(vg.x - state.x); @@ -101,8 +101,8 @@ float Airspeed_Calibration::update(float aspeed, 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, -arspd.get_airspeed_max(), arspd.get_airspeed_max()); - state.y = constrain_float(state.y, -arspd.get_airspeed_max(), arspd.get_airspeed_max()); + 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.z = constrain_float(state.z, 0.5f, 1.0f); return state.z; diff --git a/libraries/AP_Airspeed/examples/Airspeed/Airspeed.cpp b/libraries/AP_Airspeed/examples/Airspeed/Airspeed.cpp index 19b34c106e..2049c79f89 100644 --- a/libraries/AP_Airspeed/examples/Airspeed/Airspeed.cpp +++ b/libraries/AP_Airspeed/examples/Airspeed/Airspeed.cpp @@ -25,9 +25,11 @@ const AP_HAL::HAL& hal = AP_HAL::get_HAL(); +static AP_Vehicle::FixedWing aparm; + float temperature; -AP_Airspeed airspeed; +AP_Airspeed airspeed(aparm); void setup() {