diff --git a/libraries/AP_Airspeed/AP_Airspeed.cpp b/libraries/AP_Airspeed/AP_Airspeed.cpp index e20a357940..679f93bf61 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed.cpp @@ -64,10 +64,17 @@ extern const AP_HAL::HAL &hal; #else #define ARSPD_DEFAULT_PIN AP_AIRSPEED_I2C_PIN #endif + #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO + #define PSI_RANGE_DEFAULT 0.05 + #endif #else #define ARSPD_DEFAULT_PIN 0 #endif +#ifndef PSI_RANGE_DEFAULT +#define PSI_RANGE_DEFAULT 1.0f +#endif + // table of user settable parameters const AP_Param::GroupInfo AP_Airspeed::var_info[] = { @@ -120,6 +127,12 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = { // @User: Advanced AP_GROUPINFO("SKIP_CAL", 7, AP_Airspeed, _skip_cal, 0), + // @Param: PSI_RANGE + // @DisplayName: The PSI range of the device + // @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), + AP_GROUPEND }; @@ -127,7 +140,6 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = { AP_Airspeed::AP_Airspeed(const AP_Vehicle::FixedWing &parms) : _EAS2TAS(1.0f) , _calibration(parms) - , analog(_pin) { 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 6991ca43ce..844160f954 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.h +++ b/libraries/AP_Airspeed/AP_Airspeed.h @@ -149,6 +149,7 @@ public: private: AP_Float _offset; AP_Float _ratio; + AP_Float _psi_range; AP_Int8 _use; AP_Int8 _enable; AP_Int8 _pin; @@ -180,10 +181,10 @@ private: float get_pressure(void); void update_calibration(float raw_pressure); - AP_Airspeed_Analog analog; + AP_Airspeed_Analog analog{_pin, _psi_range}; #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN - AP_Airspeed_PX4 digital; + AP_Airspeed_PX4 digital{_psi_range}; #else - AP_Airspeed_I2C digital; + AP_Airspeed_I2C digital{_psi_range}; #endif }; diff --git a/libraries/AP_Airspeed/AP_Airspeed_I2C.cpp b/libraries/AP_Airspeed/AP_Airspeed_I2C.cpp index 379f6d39cf..5ed8926c54 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_I2C.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed_I2C.cpp @@ -37,6 +37,11 @@ extern const AP_HAL::HAL &hal; #define MS4525D0_I2C_BUS 1 #endif +AP_Airspeed_I2C::AP_Airspeed_I2C(const AP_Float &psi_range) : + _psi_range(psi_range) +{ +} + // probe and initialise the sensor bool AP_Airspeed_I2C::init() { @@ -91,8 +96,8 @@ void AP_Airspeed_I2C::_collect() dT_raw = (data[2] << 8) + data[3]; dT_raw = (0xFFE0 & dT_raw) >> 5; - const float P_min = -1.0f; - const float P_max = 1.0f; + const float P_max = _psi_range.get(); + const float P_min = - P_max; const float PSI_to_Pa = 6894.757f; /* this equation is an inversion of the equation in the diff --git a/libraries/AP_Airspeed/AP_Airspeed_I2C.h b/libraries/AP_Airspeed/AP_Airspeed_I2C.h index d8a18aa8fc..5b9f732228 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_I2C.h +++ b/libraries/AP_Airspeed/AP_Airspeed_I2C.h @@ -21,6 +21,10 @@ */ #include +#include +#include +#include +#include #include "AP_Airspeed_Backend.h" #include @@ -28,6 +32,9 @@ class AP_Airspeed_I2C : public AP_Airspeed_Backend { public: + AP_Airspeed_I2C(const AP_Float &psi_range); + ~AP_Airspeed_I2C(void) {} + // probe and initialise the sensor bool init(); @@ -46,4 +53,5 @@ private: uint32_t _last_sample_time_ms; uint32_t _measurement_started_ms; AP_HAL::OwnPtr _dev; + const AP_Float &_psi_range; }; diff --git a/libraries/AP_Airspeed/AP_Airspeed_PX4.cpp b/libraries/AP_Airspeed/AP_Airspeed_PX4.cpp index a263693de4..6b319f5ed0 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_PX4.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed_PX4.cpp @@ -61,7 +61,7 @@ bool AP_Airspeed_PX4::get_differential_pressure(float &pressure) while (::read(_fd, &report, sizeof(report)) == sizeof(report) && report.timestamp != _last_timestamp) { - psum += report.differential_pressure_raw_pa; + psum += report.differential_pressure_raw_pa / _psi_range.get(); tsum += report.temperature; count++; _last_timestamp = report.timestamp; diff --git a/libraries/AP_Airspeed/AP_Airspeed_PX4.h b/libraries/AP_Airspeed/AP_Airspeed_PX4.h index 51bf4eed5a..e7424563af 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_PX4.h +++ b/libraries/AP_Airspeed/AP_Airspeed_PX4.h @@ -21,12 +21,14 @@ */ #include +#include #include "AP_Airspeed_Backend.h" class AP_Airspeed_PX4 : public AP_Airspeed_Backend { public: // constructor - AP_Airspeed_PX4() : _fd(-1) {} + AP_Airspeed_PX4(const AP_Float &psi_range) : + _psi_range(psi_range) {} // probe and initialise the sensor bool init(void); @@ -38,7 +40,8 @@ public: bool get_temperature(float &temperature); private: - int _fd; + int _fd = -1; uint64_t _last_timestamp; float _temperature; + const AP_Float &_psi_range; }; diff --git a/libraries/AP_Airspeed/AP_Airspeed_analog.cpp b/libraries/AP_Airspeed/AP_Airspeed_analog.cpp index 8f1b041e84..cec9e0f639 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_analog.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed_analog.cpp @@ -42,6 +42,6 @@ bool AP_Airspeed_Analog::get_differential_pressure(float &pressure) return false; } _source->set_pin(_pin); - pressure = _source->voltage_average_ratiometric() * VOLTS_TO_PASCAL; + pressure = _source->voltage_average_ratiometric() * VOLTS_TO_PASCAL / _psi_range.get(); return true; } diff --git a/libraries/AP_Airspeed/AP_Airspeed_analog.h b/libraries/AP_Airspeed/AP_Airspeed_analog.h index cfee6d08c8..016461b4eb 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_analog.h +++ b/libraries/AP_Airspeed/AP_Airspeed_analog.h @@ -9,9 +9,10 @@ class AP_Airspeed_Analog : public AP_Airspeed_Backend { public: - AP_Airspeed_Analog(const AP_Int8 &pin) + AP_Airspeed_Analog(const AP_Int8 &pin, const AP_Float &psi_range) : _source(NULL) , _pin(pin) + , _psi_range(psi_range) { } // probe and initialise the sensor @@ -26,4 +27,5 @@ public: private: AP_HAL::AnalogSource *_source; const AP_Int8 &_pin; + const AP_Float &_psi_range; };