diff --git a/libraries/AP_Airspeed/AP_Airspeed.cpp b/libraries/AP_Airspeed/AP_Airspeed.cpp index 296e9815ad..ff354d91ba 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed.cpp @@ -52,80 +52,154 @@ extern const AP_HAL::HAL &hal; // table of user settable parameters const AP_Param::GroupInfo AP_Airspeed::var_info[] = { - // @Param: TYPE + // @Param: _TYPE // @DisplayName: Airspeed type // @Description: Type of airspeed sensor // @Values: 0:None,1:I2C-MS4525D0,2:Analog,3:I2C-MS5525,4:I2C-MS5525 (0x76),5:I2C-MS5525 (0x77),6:I2C-SDP3X // @User: Standard - AP_GROUPINFO_FLAGS("TYPE", 0, AP_Airspeed, _type, ARSPD_DEFAULT_TYPE, AP_PARAM_FLAG_ENABLE), + AP_GROUPINFO_FLAGS("_TYPE", 0, AP_Airspeed, param[0].type, ARSPD_DEFAULT_TYPE, AP_PARAM_FLAG_ENABLE), - // @Param: USE + // @Param: _USE // @DisplayName: Airspeed use // @Description: use airspeed for flight control. When set to 0 airspeed sensor can be logged and displayed on a GCS but won't be used for flight. When set to 1 it will be logged and used. When set to 2 it will be only used when the throttle is zero, which can be useful in gliders with airspeed sensors behind a propeller // @Values: 0:Don't Use,1:use,2:UseWhenZeroThrottle // @User: Standard - AP_GROUPINFO("USE", 1, AP_Airspeed, _use, 0), + AP_GROUPINFO("_USE", 1, AP_Airspeed, param[0].use, 0), - // @Param: OFFSET + // @Param: _OFFSET // @DisplayName: Airspeed offset // @Description: Airspeed calibration offset // @Increment: 0.1 // @User: Advanced - AP_GROUPINFO("OFFSET", 2, AP_Airspeed, _offset, 0), + AP_GROUPINFO("_OFFSET", 2, AP_Airspeed, param[0].offset, 0), - // @Param: RATIO + // @Param: _RATIO // @DisplayName: Airspeed ratio // @Description: Airspeed calibration ratio // @Increment: 0.1 // @User: Advanced - AP_GROUPINFO("RATIO", 3, AP_Airspeed, _ratio, 1.9936f), + AP_GROUPINFO("_RATIO", 3, AP_Airspeed, param[0].ratio, 1.9936f), - // @Param: PIN + // @Param: _PIN // @DisplayName: Airspeed pin // @Description: The pin number that the airspeed sensor is connected to for analog sensors. Set to 15 on the Pixhawk for the analog airspeed port. // @User: Advanced - AP_GROUPINFO("PIN", 4, AP_Airspeed, _pin, ARSPD_DEFAULT_PIN), + AP_GROUPINFO("_PIN", 4, AP_Airspeed, param[0].pin, ARSPD_DEFAULT_PIN), - // @Param: AUTOCAL + // @Param: _AUTOCAL // @DisplayName: Automatic airspeed ratio calibration // @Description: If this is enabled then the APM will automatically adjust the ARSPD_RATIO during flight, based upon an estimation filter using ground speed and true airspeed. The automatic calibration will save the new ratio to EEPROM every 2 minutes if it changes by more than 5%. This option should be enabled for a calibration flight then disabled again when calibration is complete. Leaving it enabled all the time is not recommended. // @User: Advanced - AP_GROUPINFO("AUTOCAL", 5, AP_Airspeed, _autocal, 0), + AP_GROUPINFO("_AUTOCAL", 5, AP_Airspeed, param[0].autocal, 0), - // @Param: TUBE_ORDER + // @Param: _TUBE_ORDER // @DisplayName: Control pitot tube order // @Description: This parameter allows you to control whether the order in which the tubes are attached to your pitot tube matters. If you set this to 0 then the top connector on the sensor needs to be the dynamic pressure. If set to 1 then the bottom connector needs to be the dynamic pressure. If set to 2 (the default) then the airspeed driver will accept either order. The reason you may wish to specify the order is it will allow your airspeed sensor to detect if the aircraft it receiving excessive pressure on the static port, which would otherwise be seen as a positive airspeed. // @User: Advanced - AP_GROUPINFO("TUBE_ORDER", 6, AP_Airspeed, _tube_order, 2), + AP_GROUPINFO("_TUBE_ORDER", 6, AP_Airspeed, param[0].tube_order, 2), - // @Param: SKIP_CAL + // @Param: _SKIP_CAL // @DisplayName: Skip airspeed calibration on startup // @Description: This parameter allows you to skip airspeed offset calibration on startup, instead using the offset from the last calibration. This may be desirable if the offset variance between flights for your sensor is low and you want to avoid having to cover the pitot tube on each boot. // @Values: 0:Disable,1:Enable // @User: Advanced - AP_GROUPINFO("SKIP_CAL", 7, AP_Airspeed, _skip_cal, 0), + AP_GROUPINFO("_SKIP_CAL", 7, AP_Airspeed, param[0].skip_cal, 0), - // @Param: PSI_RANGE + // @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_GROUPINFO("_PSI_RANGE", 8, AP_Airspeed, param[0].psi_range, PSI_RANGE_DEFAULT), - // @Param: BUS + // @Param: _BUS // @DisplayName: Airspeed I2C bus // @Description: The bus number of the I2C bus to look for the sensor on // @Values: 0:Bus0(internal),1:Bus1(external),2:Bus2(auxillary) // @User: Advanced - AP_GROUPINFO("BUS", 9, AP_Airspeed, _bus, 1), + AP_GROUPINFO("_BUS", 9, AP_Airspeed, param[0].bus, 1), + + // @Param: _PRIMARY + // @DisplayName: Primary airspeed sensor + // @Description: This selects which airspeed sensor will be the primary if multiple sensors are found + // @Values: 0:FirstSensor,1:2ndSensor + // @User: Advanced + AP_GROUPINFO("_PRIMARY", 10, AP_Airspeed, primary_sensor, 0), + + // @Param: 2_TYPE + // @DisplayName: Second Airspeed type + // @Description: Type of 2nd airspeed sensor + // @Values: 0:None,1:I2C-MS4525D0,2:Analog,3:I2C-MS5525,4:I2C-SDP3X + // @User: Standard + AP_GROUPINFO_FLAGS("2_TYPE", 11, AP_Airspeed, param[1].type, 0, AP_PARAM_FLAG_ENABLE), + + // @Param: 2_USE + // @DisplayName: Enable use of 2nd airspeed sensor + // @Description: use airspeed for flight control. When set to 0 airspeed sensor can be logged and displayed on a GCS but won't be used for flight. When set to 1 it will be logged and used. When set to 2 it will be only used when the throttle is zero, which can be useful in gliders with airspeed sensors behind a propeller + // @Values: 0:Don't Use,1:use,2:UseWhenZeroThrottle + // @User: Standard + AP_GROUPINFO("2_USE", 12, AP_Airspeed, param[1].use, 0), + + // @Param: 2_OFFSET + // @DisplayName: Airspeed offset for 2nd airspeed sensor + // @Description: Airspeed calibration offset + // @Increment: 0.1 + // @User: Advanced + AP_GROUPINFO("2_OFFSET", 13, AP_Airspeed, param[1].offset, 0), + + // @Param: 2_RATIO + // @DisplayName: Airspeed ratio for 2nd airspeed sensor + // @Description: Airspeed calibration ratio + // @Increment: 0.1 + // @User: Advanced + AP_GROUPINFO("2_RATIO", 14, AP_Airspeed, param[1].ratio, 2), + + // @Param: 2_PIN + // @DisplayName: Airspeed pin for 2nd airspeed sensor + // @Description: The pin number that the airspeed sensor is connected to for analog sensors. Set to 15 on the Pixhawk for the analog airspeed port. + // @User: Advanced + AP_GROUPINFO("2_PIN", 15, AP_Airspeed, param[1].pin, 0), + + // @Param: 2_AUTOCAL + // @DisplayName: Automatic airspeed ratio calibration for 2nd airspeed sensor + // @Description: If this is enabled then the APM will automatically adjust the ARSPD_RATIO during flight, based upon an estimation filter using ground speed and true airspeed. The automatic calibration will save the new ratio to EEPROM every 2 minutes if it changes by more than 5%. This option should be enabled for a calibration flight then disabled again when calibration is complete. Leaving it enabled all the time is not recommended. + // @User: Advanced + AP_GROUPINFO("2_AUTOCAL", 16, AP_Airspeed, param[1].autocal, 0), + + // @Param: 2_TUBE_ORDR + // @DisplayName: Control pitot tube order of 2nd airspeed sensor + // @Description: This parameter allows you to control whether the order in which the tubes are attached to your pitot tube matters. If you set this to 0 then the top connector on the sensor needs to be the dynamic pressure. If set to 1 then the bottom connector needs to be the dynamic pressure. If set to 2 (the default) then the airspeed driver will accept either order. The reason you may wish to specify the order is it will allow your airspeed sensor to detect if the aircraft it receiving excessive pressure on the static port, which would otherwise be seen as a positive airspeed. + // @User: Advanced + AP_GROUPINFO("2_TUBE_ORDR", 17, AP_Airspeed, param[1].tube_order, 2), + + // @Param: 2_SKIP_CAL + // @DisplayName: Skip airspeed calibration on startup for 2nd sensor + // @Description: This parameter allows you to skip airspeed offset calibration on startup, instead using the offset from the last calibration. This may be desirable if the offset variance between flights for your sensor is low and you want to avoid having to cover the pitot tube on each boot. + // @Values: 0:Disable,1:Enable + // @User: Advanced + AP_GROUPINFO("2_SKIP_CAL", 18, AP_Airspeed, param[1].skip_cal, 0), + + // @Param: 2_PSI_RANGE + // @DisplayName: The PSI range of the device for 2nd sensor + // @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("2_PSI_RANGE", 19, AP_Airspeed, param[1].psi_range, PSI_RANGE_DEFAULT), + + // @Param: 2_BUS + // @DisplayName: Airspeed I2C bus for 2nd sensor + // @Description: The bus number of the I2C bus to look for the sensor on + // @Values: 0:Bus0(internal),1:Bus1(external),2:Bus2(auxillary) + // @User: Advanced + AP_GROUPINFO("2_BUS", 20, AP_Airspeed, param[1].bus, 1), AP_GROUPEND }; AP_Airspeed::AP_Airspeed() - : _EAS2TAS(1.0f) - , _calibration() { + for (uint8_t i=0; iinit()) { - gcs().send_text(MAV_SEVERITY_INFO, "Airspeed init failed"); - delete sensor; - sensor = nullptr; + switch ((enum airspeed_type)param[i].type.get()) { + case TYPE_NONE: + // nothing to do + break; + case TYPE_I2C_MS4525: + sensor[i] = new AP_Airspeed_MS4525(*this, i); + break; + case TYPE_ANALOG: + sensor[i] = new AP_Airspeed_Analog(*this, i); + break; + case TYPE_I2C_MS5525: + sensor[i] = new AP_Airspeed_MS5525(*this, i, AP_Airspeed_MS5525::MS5525_ADDR_AUTO); + break; + case TYPE_I2C_MS5525_ADDRESS_1: + sensor[i] = new AP_Airspeed_MS5525(*this, i, AP_Airspeed_MS5525::MS5525_ADDR_1); + break; + case TYPE_I2C_MS5525_ADDRESS_2: + sensor[i] = new AP_Airspeed_MS5525(*this, i, AP_Airspeed_MS5525::MS5525_ADDR_2); + break; + case TYPE_I2C_SDP3X: + sensor[i] = new AP_Airspeed_SDP3X(*this, i); + break; + } + if (sensor[i] && !sensor[i]->init()) { + gcs().send_text(MAV_SEVERITY_INFO, "Airspeed[%u] init failed", i); + delete sensor[i]; + sensor[i] = nullptr; + } } } // read the airspeed sensor -float AP_Airspeed::get_pressure(void) +float AP_Airspeed::get_pressure(uint8_t i) { - if (!enabled()) { + if (!enabled(i)) { return 0; } - if (_hil_set) { - _healthy = true; - return _hil_pressure; + if (state[i].hil_set) { + state[i].healthy = true; + return state[i].hil_pressure; } float pressure = 0; - if (sensor) { - _healthy = sensor->get_differential_pressure(pressure); + if (sensor[i]) { + state[i].healthy = sensor[i]->get_differential_pressure(pressure); } return pressure; } // get a temperature reading if possible -bool AP_Airspeed::get_temperature(float &temperature) +bool AP_Airspeed::get_temperature(uint8_t i, float &temperature) { - if (!enabled()) { + if (!enabled(i)) { return false; } - if (sensor) { - return sensor->get_temperature(temperature); + if (sensor[i]) { + return sensor[i]->get_temperature(temperature); } return false; } -// calibrate the airspeed. This must be called at least once before -// the get_airspeed() interface can be used +// calibrate the zero offset for the airspeed. This must be called at +// least once before the get_airspeed() interface can be used void AP_Airspeed::calibrate(bool in_startup) { - if (!enabled()) { - return; + for (uint8_t i=0; i= 1000 && - _cal.read_count > 15) { - if (_cal.count == 0) { - gcs().send_text(MAV_SEVERITY_INFO, "Airspeed sensor unhealthy"); + if (AP_HAL::millis() - state[i].cal.start_ms >= 1000 && + state[i].cal.read_count > 15) { + if (state[i].cal.count == 0) { + gcs().send_text(MAV_SEVERITY_INFO, "Airspeed[%u] sensor unhealthy", i); } else { - gcs().send_text(MAV_SEVERITY_INFO, "Airspeed sensor calibrated"); - _offset.set_and_save(_cal.sum / _cal.count); + gcs().send_text(MAV_SEVERITY_INFO, "Airspeed[%u] sensor calibrated", i); + param[i].offset.set_and_save(state[i].cal.sum / state[i].cal.count); } - _cal.start_ms = 0; + state[i].cal.start_ms = 0; return; } // we discard the first 5 samples - if (_healthy && _cal.read_count > 5) { - _cal.sum += raw_pressure; - _cal.count++; + if (state[i].healthy && state[i].cal.read_count > 5) { + state[i].cal.sum += raw_pressure; + state[i].cal.count++; } - _cal.read_count++; + state[i].cal.read_count++; } -// read the airspeed sensor -void AP_Airspeed::read(void) +// read one airspeed sensor +void AP_Airspeed::read(uint8_t i) { float airspeed_pressure; - if (!enabled()) { + if (!enabled(i) || !sensor[i]) { return; } - float raw_pressure = get_pressure(); - if (_cal.start_ms != 0) { - update_calibration(raw_pressure); + float raw_pressure = get_pressure(i); + if (state[i].cal.start_ms != 0) { + update_calibration(i, raw_pressure); } - airspeed_pressure = raw_pressure - _offset; + airspeed_pressure = raw_pressure - param[i].offset; // remember raw pressure for logging - _corrected_pressure = airspeed_pressure; + state[i].corrected_pressure = airspeed_pressure; // filter before clamping positive - _filtered_pressure = 0.7f * _filtered_pressure + 0.3f * airspeed_pressure; + state[i].filtered_pressure = 0.7f * state[i].filtered_pressure + 0.3f * airspeed_pressure; /* we support different pitot tube setups so user can choose if they want to be able to detect pressure on the static port */ - switch ((enum pitot_tube_order)_tube_order.get()) { + switch ((enum pitot_tube_order)param[i].tube_order.get()) { case PITOT_TUBE_ORDER_NEGATIVE: - _last_pressure = -airspeed_pressure; - _raw_airspeed = sqrtf(MAX(-airspeed_pressure, 0) * _ratio); - _airspeed = sqrtf(MAX(-_filtered_pressure, 0) * _ratio); + state[i].last_pressure = -airspeed_pressure; + state[i].raw_airspeed = sqrtf(MAX(-airspeed_pressure, 0) * param[i].ratio); + state[i].airspeed = sqrtf(MAX(-state[i].filtered_pressure, 0) * param[i].ratio); break; case PITOT_TUBE_ORDER_POSITIVE: - _last_pressure = airspeed_pressure; - _raw_airspeed = sqrtf(MAX(airspeed_pressure, 0) * _ratio); - _airspeed = sqrtf(MAX(_filtered_pressure, 0) * _ratio); + state[i].last_pressure = airspeed_pressure; + state[i].raw_airspeed = sqrtf(MAX(airspeed_pressure, 0) * param[i].ratio); + state[i].airspeed = sqrtf(MAX(state[i].filtered_pressure, 0) * param[i].ratio); if (airspeed_pressure < -32) { // we're reading more than about -8m/s. The user probably has // the ports the wrong way around - _healthy = false; + state[i].healthy = false; } break; case PITOT_TUBE_ORDER_AUTO: default: - _last_pressure = fabsf(airspeed_pressure); - _raw_airspeed = sqrtf(fabsf(airspeed_pressure) * _ratio); - _airspeed = sqrtf(fabsf(_filtered_pressure) * _ratio); + state[i].last_pressure = fabsf(airspeed_pressure); + state[i].raw_airspeed = sqrtf(fabsf(airspeed_pressure) * param[i].ratio); + state[i].airspeed = sqrtf(fabsf(state[i].filtered_pressure) * param[i].ratio); break; } - _last_update_ms = AP_HAL::millis(); + state[i].last_update_ms = AP_HAL::millis(); +} + +// read all airspeed sensors +void AP_Airspeed::read(void) +{ + for (uint8_t i=0; i 0 || _allow_zero_offset) && enabled(); } + bool healthy(uint8_t i) const { + return state[i].healthy && (fabsf(param[i].offset) > 0 || state[i].use_zero_offset) && enabled(i); + } + bool healthy(void) const { return healthy(primary); } - void setHIL(float pressure) { _healthy=_hil_set=true; _hil_pressure=pressure; } + // return true if all enabled sensors are healthy + bool all_healthy(void) const; + + void setHIL(float pressure) { state[0].healthy=state[0].hil_set=true; state[0].hil_pressure=pressure; } // return time in ms of last update - uint32_t last_update_ms(void) const { return _last_update_ms; } + uint32_t last_update_ms(uint8_t i) const { return state[i].last_update_ms; } + uint32_t last_update_ms(void) const { return last_update_ms(primary); } void setHIL(float airspeed, float diff_pressure, float temperature); @@ -150,46 +161,60 @@ public: TYPE_I2C_MS5525_ADDRESS_2=5, TYPE_I2C_SDP3X=6, }; + + // get current primary sensor + uint8_t get_primary(void) const { return primary; } private: - AP_Float _offset; - AP_Float _ratio; - AP_Float _psi_range; - AP_Int8 _use; - AP_Int8 _type; - AP_Int8 _pin; - AP_Int8 _bus; - AP_Int8 _autocal; - AP_Int8 _tube_order; - AP_Int8 _skip_cal; - float _raw_airspeed; - float _airspeed; - float _last_pressure; - float _filtered_pressure; - float _corrected_pressure; - float _EAS2TAS; - bool _healthy:1; - bool _hil_set:1; - float _hil_pressure; - uint32_t _last_update_ms; - // state of runtime calibration + AP_Int8 primary_sensor; + struct { - uint32_t start_ms; - uint16_t count; - float sum; - uint16_t read_count; - } _cal; + AP_Float offset; + AP_Float ratio; + AP_Float psi_range; + AP_Int8 use; + AP_Int8 type; + AP_Int8 pin; + AP_Int8 bus; + AP_Int8 autocal; + AP_Int8 tube_order; + AP_Int8 skip_cal; + } param[AIRSPEED_MAX_SENSORS]; - Airspeed_Calibration _calibration; - float _last_saved_ratio; - uint8_t _counter; + struct airspeed_state { + float raw_airspeed; + float airspeed; + float last_pressure; + float filtered_pressure; + float corrected_pressure; + float EAS2TAS; + bool healthy:1; + bool hil_set:1; + float hil_pressure; + uint32_t last_update_ms; + bool use_zero_offset; + + // state of runtime calibration + struct { + uint32_t start_ms; + uint16_t count; + float sum; + uint16_t read_count; + } cal; - float get_pressure(void); - void update_calibration(float raw_pressure); + Airspeed_Calibration calibration; + float last_saved_ratio; + uint8_t counter; + } state[AIRSPEED_MAX_SENSORS]; - AP_Airspeed_Backend *sensor; + // current primary sensor + uint8_t primary; + + void read(uint8_t i); + float get_pressure(uint8_t i); + void update_calibration(uint8_t i, float raw_pressure); + void update_calibration(uint8_t i, const Vector3f &vground, int16_t max_airspeed_allowed_during_cal); - // some sensors can have zero offset and be healthy - bool _allow_zero_offset; + AP_Airspeed_Backend *sensor[AIRSPEED_MAX_SENSORS]; }; diff --git a/libraries/AP_Airspeed/AP_Airspeed_Backend.cpp b/libraries/AP_Airspeed/AP_Airspeed_Backend.cpp index 44292f633b..9f1f9935eb 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_Backend.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed_Backend.cpp @@ -24,8 +24,9 @@ extern const AP_HAL::HAL &hal; -AP_Airspeed_Backend::AP_Airspeed_Backend(AP_Airspeed &_frontend) : - frontend(_frontend) +AP_Airspeed_Backend::AP_Airspeed_Backend(AP_Airspeed &_frontend, uint8_t _instance) : + frontend(_frontend), + instance(_instance) { sem = hal.util->new_semaphore(); } @@ -38,15 +39,15 @@ AP_Airspeed_Backend::~AP_Airspeed_Backend(void) int8_t AP_Airspeed_Backend::get_pin(void) const { - return frontend._pin; + return frontend.param[instance].pin; } float AP_Airspeed_Backend::get_psi_range(void) const { - return frontend._psi_range; + return frontend.param[instance].psi_range; } uint8_t AP_Airspeed_Backend::get_bus(void) const { - return frontend._bus; + return frontend.param[instance].bus; } diff --git a/libraries/AP_Airspeed/AP_Airspeed_Backend.h b/libraries/AP_Airspeed/AP_Airspeed_Backend.h index e4197a8c87..7551c68a0d 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_Backend.h +++ b/libraries/AP_Airspeed/AP_Airspeed_Backend.h @@ -24,7 +24,7 @@ class AP_Airspeed_Backend { public: - AP_Airspeed_Backend(AP_Airspeed &frontend); + AP_Airspeed_Backend(AP_Airspeed &frontend, uint8_t instance); virtual ~AP_Airspeed_Backend(); // probe and initialise the sensor @@ -42,31 +42,32 @@ protected: uint8_t get_bus(void) const; AP_Airspeed::pitot_tube_order get_tube_order(void) const { - return AP_Airspeed::pitot_tube_order(frontend._tube_order.get()); + return AP_Airspeed::pitot_tube_order(frontend.param[instance].tube_order.get()); } // semaphore for access to shared frontend data AP_HAL::Semaphore *sem; float get_airspeed_ratio(void) const { - return frontend.get_airspeed_ratio(); + return frontend.get_airspeed_ratio(instance); } - // some sensors allow zero offsets while healthy - void set_allow_zero_offset(void) { - frontend._allow_zero_offset = true; + // some sensors use zero offsets + void set_use_zero_offset(void) { + frontend.state[instance].use_zero_offset = true; } // set to no zero cal, which makes sense for some sensors void set_skip_cal(void) { - frontend._skip_cal.set(1); + frontend.param[instance].skip_cal.set(1); } // set zero offset void set_offset(float ofs) { - frontend._offset.set(ofs); + frontend.param[instance].offset.set(ofs); } private: AP_Airspeed &frontend; + uint8_t instance; }; diff --git a/libraries/AP_Airspeed/AP_Airspeed_MS4525.cpp b/libraries/AP_Airspeed/AP_Airspeed_MS4525.cpp index 1c837bde29..b1941b32a8 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_MS4525.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed_MS4525.cpp @@ -29,8 +29,8 @@ extern const AP_HAL::HAL &hal; #define MS4525D0_I2C_ADDR 0x28 -AP_Airspeed_MS4525::AP_Airspeed_MS4525(AP_Airspeed &_frontend) : - AP_Airspeed_Backend(_frontend) +AP_Airspeed_MS4525::AP_Airspeed_MS4525(AP_Airspeed &_frontend, uint8_t _instance) : + AP_Airspeed_Backend(_frontend, _instance) { } diff --git a/libraries/AP_Airspeed/AP_Airspeed_MS4525.h b/libraries/AP_Airspeed/AP_Airspeed_MS4525.h index 9ec66d07b1..53045b3688 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_MS4525.h +++ b/libraries/AP_Airspeed/AP_Airspeed_MS4525.h @@ -30,7 +30,7 @@ class AP_Airspeed_MS4525 : public AP_Airspeed_Backend { public: - AP_Airspeed_MS4525(AP_Airspeed &frontend); + AP_Airspeed_MS4525(AP_Airspeed &frontend, uint8_t _instance); ~AP_Airspeed_MS4525(void) {} // probe and initialise the sensor diff --git a/libraries/AP_Airspeed/AP_Airspeed_MS5525.cpp b/libraries/AP_Airspeed/AP_Airspeed_MS5525.cpp index 9ccc533154..f0181ff76f 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_MS5525.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed_MS5525.cpp @@ -51,8 +51,8 @@ extern const AP_HAL::HAL &hal; #define REG_CONVERT_PRESSURE REG_CONVERT_D1_OSR_1024 #define REG_CONVERT_TEMPERATURE REG_CONVERT_D2_OSR_1024 -AP_Airspeed_MS5525::AP_Airspeed_MS5525(AP_Airspeed &_frontend, MS5525_ADDR address) : - AP_Airspeed_Backend(_frontend) +AP_Airspeed_MS5525::AP_Airspeed_MS5525(AP_Airspeed &_frontend, uint8_t _instance, MS5525_ADDR address) : + AP_Airspeed_Backend(_frontend, _instance) { _address = address; } diff --git a/libraries/AP_Airspeed/AP_Airspeed_MS5525.h b/libraries/AP_Airspeed/AP_Airspeed_MS5525.h index 1370c906ca..a6d9b691fa 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_MS5525.h +++ b/libraries/AP_Airspeed/AP_Airspeed_MS5525.h @@ -36,7 +36,7 @@ public: MS5525_ADDR_AUTO = 255, // does not need to be 255, just needs to be out of the address array }; - AP_Airspeed_MS5525(AP_Airspeed &frontend, MS5525_ADDR address); + AP_Airspeed_MS5525(AP_Airspeed &frontend, uint8_t _instance, MS5525_ADDR address); ~AP_Airspeed_MS5525(void) {} // probe and initialise the sensor diff --git a/libraries/AP_Airspeed/AP_Airspeed_SDP3X.cpp b/libraries/AP_Airspeed/AP_Airspeed_SDP3X.cpp index 535b6836dc..88ffb5aca7 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_SDP3X.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed_SDP3X.cpp @@ -19,6 +19,7 @@ with thanks to https://github.com/PX4/Firmware/blob/master/src/drivers/sdp3x_airspeed */ #include "AP_Airspeed_SDP3X.h" +#include #include @@ -35,14 +36,10 @@ #define SDP3X_SCALE_PRESSURE_SDP32 240 #define SDP3X_SCALE_PRESSURE_SDP33 20 -#define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C 1.225f /* kg/m^3 */ -#define CONSTANTS_AIR_GAS_CONST 287.1f /* J/(kg * K) */ -#define CONSTANTS_ABSOLUTE_NULL_CELSIUS -273.15f /* °C */ - extern const AP_HAL::HAL &hal; -AP_Airspeed_SDP3X::AP_Airspeed_SDP3X(AP_Airspeed &_frontend) : - AP_Airspeed_Backend(_frontend) +AP_Airspeed_SDP3X::AP_Airspeed_SDP3X(AP_Airspeed &_frontend, uint8_t _instance) : + AP_Airspeed_Backend(_frontend, _instance) { } @@ -77,19 +74,31 @@ bool AP_Airspeed_SDP3X::init() // lots of retries during probe _dev->set_retries(10); - // stop any precending measurements + // stop continuous average mode if (!_send_command(SDP3X_CONT_MEAS_STOP)) { _dev->get_semaphore()->give(); continue; } + // these delays are needed for reliable operation + _dev->get_semaphore()->give(); + hal.scheduler->delay_microseconds(20000); + if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { + continue; + } + // start continuous average mode if (!_send_command(SDP3X_CONT_MEAS_AVG_MODE)) { _dev->get_semaphore()->give(); continue; } + // these delays are needed for reliable operation + _dev->get_semaphore()->give(); hal.scheduler->delay_microseconds(20000); + if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { + continue; + } // step 3 - get scale uint8_t val[9]; @@ -134,7 +143,7 @@ bool AP_Airspeed_SDP3X::init() /* this sensor uses zero offset and skips cal */ - set_allow_zero_offset(); + set_use_zero_offset(); set_skip_cal(); set_offset(0); @@ -168,13 +177,8 @@ void AP_Airspeed_SDP3X::_timer() int16_t P = (((int16_t)val[0]) << 8) | val[1]; int16_t temp = (((int16_t)val[3]) << 8) | val[4]; - float P_float = (float)P; - float temp_float = (float)temp; - - float scale_float = (float)_scale; - - float diff_press_pa = P_float / scale_float; - float temperature = temp_float / (float)SDP3X_SCALE_TEMPERATURE; + float diff_press_pa = float(P) / float(_scale); + float temperature = float(temp) / SDP3X_SCALE_TEMPERATURE; if (sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { _press_sum += diff_press_pa; @@ -193,7 +197,6 @@ void AP_Airspeed_SDP3X::_timer() */ float AP_Airspeed_SDP3X::_correct_pressure(float press) { - const float tube_len = 0.2; float temperature; AP_Baro *baro = AP_Baro::get_instance(); @@ -201,26 +204,47 @@ float AP_Airspeed_SDP3X::_correct_pressure(float press) return press; } + float sign = 1; + // fix for tube order AP_Airspeed::pitot_tube_order tube_order = get_tube_order(); switch (tube_order) { case AP_Airspeed::PITOT_TUBE_ORDER_NEGATIVE: press = -press; + sign = -1; //FALLTHROUGH; case AP_Airspeed::PITOT_TUBE_ORDER_POSITIVE: break; case AP_Airspeed::PITOT_TUBE_ORDER_AUTO: default: - press = fabsf(press); + if (press < 0) { + sign = -1; + press = -press; + } break; } + if (press <= 0) { return 0; } get_temperature(temperature); - float rho_air = baro->get_pressure() / (CONSTANTS_AIR_GAS_CONST * (temperature - CONSTANTS_ABSOLUTE_NULL_CELSIUS)); + float rho_air = baro->get_pressure() / (ISA_GAS_CONSTANT * (temperature + C_TO_KELVIN)); + /* + the constants in the code below come from a calibrated test of + the drotek pitot tube by Sensiron. They are specific to the droktek pitot tube + + At 25m/s, the rough proportions of each pressure correction are: + + - dp_pitot: 5% + - press_correction: 14% + - press: 81% + + dp_tube has been removed from the Sensiron model as it is + insignificant (less than 0.02% over the supported speed ranges) + */ + // flow through sensor float flow_SDP3X = (300.805f - 300.878f / (0.00344205f * (float)powf(press, 0.68698f) + 1)) * 1.29f / rho_air; if (flow_SDP3X < 0.0f) { @@ -230,11 +254,8 @@ float AP_Airspeed_SDP3X::_correct_pressure(float press) // diffential pressure through pitot tube float dp_pitot = 28557670.0f - 28557670.0f / (1 + (float)powf((flow_SDP3X / 5027611.0f), 1.227924f)); - // pressure drop through tube - float dp_tube = flow_SDP3X * 0.000746124f * tube_len * rho_air; - // uncorrected pressure - float press_uncorrected = (press + dp_tube + dp_pitot) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C; + float press_uncorrected = (press + dp_pitot) / AIR_DENSITY_SEA_LEVEL; // correction for speed at pitot-tube tip due to flow through sensor float dv = 0.0331582 * flow_SDP3X; @@ -242,10 +263,13 @@ float AP_Airspeed_SDP3X::_correct_pressure(float press) // airspeed ratio float ratio = get_airspeed_ratio(); - // calculate equivalent pressure correction + // calculate equivalent pressure correction. This formula comes + // from turning the dv correction above into an equivalent + // pressure correction. We need to do this so the airspeed ratio + // calibrator can work, as it operates on pressure values float press_correction = sq(sqrtf(press_uncorrected*ratio)+dv)/ratio - press_uncorrected; - return press_uncorrected + press_correction; + return (press_uncorrected + press_correction) * sign; } // return the current differential_pressure in Pascal diff --git a/libraries/AP_Airspeed/AP_Airspeed_SDP3X.h b/libraries/AP_Airspeed/AP_Airspeed_SDP3X.h index cb756b63a3..4873a4bc60 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_SDP3X.h +++ b/libraries/AP_Airspeed/AP_Airspeed_SDP3X.h @@ -34,7 +34,7 @@ class AP_Airspeed_SDP3X : public AP_Airspeed_Backend { public: - AP_Airspeed_SDP3X(AP_Airspeed &frontend); + AP_Airspeed_SDP3X(AP_Airspeed &frontend, uint8_t _instance); ~AP_Airspeed_SDP3X(void) {} // probe and initialise the sensor @@ -47,11 +47,8 @@ public: bool get_temperature(float &temperature) override; private: - void _collect(); void _timer(); bool _send_command(uint16_t cmd); - float _get_pressure(int16_t dp_raw) const; - float _get_temperature(int16_t dT_raw) const; bool _crc(const uint8_t data[], unsigned size, uint8_t checksum); float _correct_pressure(float press); diff --git a/libraries/AP_Airspeed/AP_Airspeed_analog.cpp b/libraries/AP_Airspeed/AP_Airspeed_analog.cpp index 916e8daca6..0bc221781d 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_analog.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed_analog.cpp @@ -27,8 +27,8 @@ extern const AP_HAL::HAL &hal; // scaling for 3DR analog airspeed sensor #define VOLTS_TO_PASCAL 819 -AP_Airspeed_Analog::AP_Airspeed_Analog(AP_Airspeed &_frontend) : - AP_Airspeed_Backend(_frontend) +AP_Airspeed_Analog::AP_Airspeed_Analog(AP_Airspeed &_frontend, uint8_t _instance) : + AP_Airspeed_Backend(_frontend, _instance) { _source = hal.analogin->channel(get_pin()); } diff --git a/libraries/AP_Airspeed/AP_Airspeed_analog.h b/libraries/AP_Airspeed/AP_Airspeed_analog.h index 9613d6603c..1131a12ec0 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_analog.h +++ b/libraries/AP_Airspeed/AP_Airspeed_analog.h @@ -8,7 +8,7 @@ class AP_Airspeed_Analog : public AP_Airspeed_Backend { public: - AP_Airspeed_Analog(AP_Airspeed &frontend); + AP_Airspeed_Analog(AP_Airspeed &frontend, uint8_t _instance); // probe and initialise the sensor bool init(void) override; diff --git a/libraries/AP_Airspeed/Airspeed_Calibration.cpp b/libraries/AP_Airspeed/Airspeed_Calibration.cpp index 652567c6eb..d259e07abd 100644 --- a/libraries/AP_Airspeed/Airspeed_Calibration.cpp +++ b/libraries/AP_Airspeed/Airspeed_Calibration.cpp @@ -110,9 +110,9 @@ float Airspeed_Calibration::update(float airspeed, const Vector3f &vg, int16_t m /* called once a second to do calibration update */ -void AP_Airspeed::update_calibration(const Vector3f &vground, int16_t max_airspeed_allowed_during_cal) +void AP_Airspeed::update_calibration(uint8_t i, const Vector3f &vground, int16_t max_airspeed_allowed_during_cal) { - if (!_autocal) { + if (!param[i].autocal) { // auto-calibration not enabled return; } @@ -120,15 +120,15 @@ void AP_Airspeed::update_calibration(const Vector3f &vground, int16_t max_airspe // 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); + float ratio = constrain_float(param[i].ratio, 1.0f, 4.0f); - _calibration.state.z = 1.0f / sqrtf(ratio); + state[i].calibration.state.z = 1.0f / sqrtf(ratio); // calculate true airspeed, assuming a airspeed ratio of 1.0 float dpress = MAX(get_differential_pressure(), 0); - float true_airspeed = sqrtf(dpress) * _EAS2TAS; + float true_airspeed = sqrtf(dpress) * state[i].EAS2TAS; - float zratio = _calibration.update(true_airspeed, vground, max_airspeed_allowed_during_cal); + float zratio = state[i].calibration.update(true_airspeed, vground, max_airspeed_allowed_during_cal); if (isnan(zratio) || isinf(zratio)) { return; @@ -136,16 +136,26 @@ void AP_Airspeed::update_calibration(const Vector3f &vground, int16_t max_airspe // this constrains the resulting ratio to between 1.0 and 4.0 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) { - _ratio.save(); - _last_saved_ratio = _ratio; - _counter = 0; + param[i].ratio.set(1/sq(zratio)); + if (state[i].counter > 60) { + if (state[i].last_saved_ratio > 1.05f*param[i].ratio || + state[i].last_saved_ratio < 0.95f*param[i].ratio) { + param[i].ratio.save(); + state[i].last_saved_ratio = param[i].ratio; + state[i].counter = 0; } } else { - _counter++; + state[i].counter++; + } +} + +/* + called once a second to do calibration update + */ +void AP_Airspeed::update_calibration(const Vector3f &vground, int16_t max_airspeed_allowed_during_cal) +{ + for (uint8_t i=0; i