Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Airspeed: support dual airspeed sensors

Browse Source 
allow for a primary and secondary airspeed sensor
This commit is contained in:
Andrew Tridgell 2017-11-03 17:17:34 +11:00
parent eaa4861693
commit 8de2cebd78
13 changed files with 451 additions and 269 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

366
libraries/AP_Airspeed/AP_Airspeed.cpp
View File

@ -52,80 +52,154 @@ extern const AP_HAL::HAL &hal;
// table of user settable parameters // table of user settable parameters
const AP_Param::GroupInfo AP_Airspeed::var_info[] = { const AP_Param::GroupInfo AP_Airspeed::var_info[] = {
// @Param: TYPE // @Param: _TYPE
// @DisplayName: Airspeed type // @DisplayName: Airspeed type
// @Description: Type of airspeed sensor // @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 // @Values: 0:None,1:I2C-MS4525D0,2:Analog,3:I2C-MS5525,4:I2C-MS5525 (0x76),5:I2C-MS5525 (0x77),6:I2C-SDP3X
// @User: Standard // @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 // @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 // @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 // @Values: 0:Don't Use,1:use,2:UseWhenZeroThrottle
// @User: Standard // @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 // @DisplayName: Airspeed offset
// @Description: Airspeed calibration offset // @Description: Airspeed calibration offset
// @Increment: 0.1 // @Increment: 0.1
// @User: Advanced // @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 // @DisplayName: Airspeed ratio
// @Description: Airspeed calibration ratio // @Description: Airspeed calibration ratio
// @Increment: 0.1 // @Increment: 0.1
// @User: Advanced // @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 // @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. // @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 // @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 // @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. // @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 // @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 // @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. // @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 // @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 // @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. // @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 // @Values: 0:Disable,1:Enable
// @User: Advanced // @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 // @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 // @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 // @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 // @DisplayName: Airspeed I2C bus
// @Description: The bus number of the I2C bus to look for the sensor on // @Description: The bus number of the I2C bus to look for the sensor on
// @Values: 0:Bus0(internal),1:Bus1(external),2:Bus2(auxillary) // @Values: 0:Bus0(internal),1:Bus1(external),2:Bus2(auxillary)
// @User: Advanced // @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_GROUPEND
}; };
AP_Airspeed::AP_Airspeed() AP_Airspeed::AP_Airspeed()
: _EAS2TAS(1.0f)
, _calibration()
{ {
for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
state[i].EAS2TAS = 1;
}
AP_Param::setup_object_defaults(this, var_info); AP_Param::setup_object_defaults(this, var_info);
} }
@ -140,184 +214,221 @@ AP_Airspeed::AP_Airspeed()
void AP_Airspeed::init() void AP_Airspeed::init()
{ {
// cope with upgrade from old system // cope with upgrade from old system
if (_pin.load() && _pin.get() != 65) { if (param[0].pin.load() && param[0].pin.get() != 65) {
_type.set_default(TYPE_ANALOG); param[0].type.set_default(TYPE_ANALOG);
} }
_last_pressure = 0; for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
_calibration.init(_ratio); state[i].calibration.init(param[i].ratio);
_last_saved_ratio = _ratio; state[i].last_saved_ratio = param[i].ratio;
_counter = 0;
switch ((enum airspeed_type)_type.get()) { switch ((enum airspeed_type)param[i].type.get()) {
case TYPE_NONE: case TYPE_NONE:
// nothing to do // nothing to do
break; break;
case TYPE_I2C_MS4525: case TYPE_I2C_MS4525:
sensor = new AP_Airspeed_MS4525(*this); sensor[i] = new AP_Airspeed_MS4525(*this, i);
break; break;
case TYPE_ANALOG: case TYPE_ANALOG:
sensor = new AP_Airspeed_Analog(*this); sensor[i] = new AP_Airspeed_Analog(*this, i);
break; break;
case TYPE_I2C_MS5525: case TYPE_I2C_MS5525:
sensor = new AP_Airspeed_MS5525(*this, AP_Airspeed_MS5525::MS5525_ADDR_AUTO); sensor[i] = new AP_Airspeed_MS5525(*this, i, AP_Airspeed_MS5525::MS5525_ADDR_AUTO);
break; break;
case TYPE_I2C_MS5525_ADDRESS_1: case TYPE_I2C_MS5525_ADDRESS_1:
sensor = new AP_Airspeed_MS5525(*this, AP_Airspeed_MS5525::MS5525_ADDR_1); sensor[i] = new AP_Airspeed_MS5525(*this, i, AP_Airspeed_MS5525::MS5525_ADDR_1);
break; break;
case TYPE_I2C_MS5525_ADDRESS_2: case TYPE_I2C_MS5525_ADDRESS_2:
sensor = new AP_Airspeed_MS5525(*this, AP_Airspeed_MS5525::MS5525_ADDR_2); sensor[i] = new AP_Airspeed_MS5525(*this, i, AP_Airspeed_MS5525::MS5525_ADDR_2);
break; break;
case TYPE_I2C_SDP3X: case TYPE_I2C_SDP3X:
sensor = new AP_Airspeed_SDP3X(*this); sensor[i] = new AP_Airspeed_SDP3X(*this, i);
break; break;
} }
if (sensor && !sensor->init()) { if (sensor[i] && !sensor[i]->init()) {
gcs().send_text(MAV_SEVERITY_INFO, "Airspeed init failed"); gcs().send_text(MAV_SEVERITY_INFO, "Airspeed[%u] init failed", i);
delete sensor; delete sensor[i];
sensor = nullptr; sensor[i] = nullptr;
}
} }
} }
// read the airspeed sensor // 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; return 0;
} }
if (_hil_set) { if (state[i].hil_set) {
_healthy = true; state[i].healthy = true;
return _hil_pressure; return state[i].hil_pressure;
} }
float pressure = 0; float pressure = 0;
if (sensor) { if (sensor[i]) {
_healthy = sensor->get_differential_pressure(pressure); state[i].healthy = sensor[i]->get_differential_pressure(pressure);
} }
return pressure; return pressure;
} }
// get a temperature reading if possible // 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; return false;
} }
if (sensor) { if (sensor[i]) {
return sensor->get_temperature(temperature); return sensor[i]->get_temperature(temperature);
} }
return false; return false;
} }
// calibrate the airspeed. This must be called at least once before // calibrate the zero offset for the airspeed. This must be called at
// the get_airspeed() interface can be used // least once before the get_airspeed() interface can be used
void AP_Airspeed::calibrate(bool in_startup) void AP_Airspeed::calibrate(bool in_startup)
{ {
if (!enabled()) { for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
return; if (!enabled(i)) {
continue;
}
if (state[i].use_zero_offset) {
param[i].offset.set(0);
continue;
}
if (in_startup && param[i].skip_cal) {
continue;
}
state[i].cal.start_ms = AP_HAL::millis();
state[i].cal.count = 0;
state[i].cal.sum = 0;
state[i].cal.read_count = 0;
} }
if (in_startup && _skip_cal) {
return;
}
_cal.start_ms = AP_HAL::millis();
_cal.count = 0;
_cal.sum = 0;
_cal.read_count = 0;
} }
/* /*
update async airspeed calibration update async airspeed zero offset calibration
*/ */
void AP_Airspeed::update_calibration(float raw_pressure) void AP_Airspeed::update_calibration(uint8_t i, float raw_pressure)
{ {
if (!enabled(i) || state[i].cal.start_ms == 0) {
return;
}
// consider calibration complete when we have at least 15 samples // consider calibration complete when we have at least 15 samples
// over at least 1 second // over at least 1 second
if (AP_HAL::millis() - _cal.start_ms >= 1000 && if (AP_HAL::millis() - state[i].cal.start_ms >= 1000 &&
_cal.read_count > 15) { state[i].cal.read_count > 15) {
if (_cal.count == 0) { if (state[i].cal.count == 0) {
gcs().send_text(MAV_SEVERITY_INFO, "Airspeed sensor unhealthy"); gcs().send_text(MAV_SEVERITY_INFO, "Airspeed[%u] sensor unhealthy", i);
} else { } else {
gcs().send_text(MAV_SEVERITY_INFO, "Airspeed sensor calibrated"); gcs().send_text(MAV_SEVERITY_INFO, "Airspeed[%u] sensor calibrated", i);
_offset.set_and_save(_cal.sum / _cal.count); 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; return;
} }
// we discard the first 5 samples // we discard the first 5 samples
if (_healthy && _cal.read_count > 5) { if (state[i].healthy && state[i].cal.read_count > 5) {
_cal.sum += raw_pressure; state[i].cal.sum += raw_pressure;
_cal.count++; state[i].cal.count++;
} }
_cal.read_count++; state[i].cal.read_count++;
} }
// read the airspeed sensor // read one airspeed sensor
void AP_Airspeed::read(void) void AP_Airspeed::read(uint8_t i)
{ {
float airspeed_pressure; float airspeed_pressure;
if (!enabled()) { if (!enabled(i) || !sensor[i]) {
return; return;
} }
float raw_pressure = get_pressure(); float raw_pressure = get_pressure(i);
if (_cal.start_ms != 0) { if (state[i].cal.start_ms != 0) {
update_calibration(raw_pressure); update_calibration(i, raw_pressure);
} }
airspeed_pressure = raw_pressure - _offset; airspeed_pressure = raw_pressure - param[i].offset;
// remember raw pressure for logging // remember raw pressure for logging
_corrected_pressure = airspeed_pressure; state[i].corrected_pressure = airspeed_pressure;
// filter before clamping positive // 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 we support different pitot tube setups so user can choose if
they want to be able to detect pressure on the static port 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: case PITOT_TUBE_ORDER_NEGATIVE:
_last_pressure = -airspeed_pressure; state[i].last_pressure = -airspeed_pressure;
_raw_airspeed = sqrtf(MAX(-airspeed_pressure, 0) * _ratio); state[i].raw_airspeed = sqrtf(MAX(-airspeed_pressure, 0) * param[i].ratio);
_airspeed = sqrtf(MAX(-_filtered_pressure, 0) * _ratio); state[i].airspeed = sqrtf(MAX(-state[i].filtered_pressure, 0) * param[i].ratio);
break; break;
case PITOT_TUBE_ORDER_POSITIVE: case PITOT_TUBE_ORDER_POSITIVE:
_last_pressure = airspeed_pressure; state[i].last_pressure = airspeed_pressure;
_raw_airspeed = sqrtf(MAX(airspeed_pressure, 0) * _ratio); state[i].raw_airspeed = sqrtf(MAX(airspeed_pressure, 0) * param[i].ratio);
_airspeed = sqrtf(MAX(_filtered_pressure, 0) * _ratio); state[i].airspeed = sqrtf(MAX(state[i].filtered_pressure, 0) * param[i].ratio);
if (airspeed_pressure < -32) { if (airspeed_pressure < -32) {
// we're reading more than about -8m/s. The user probably has // we're reading more than about -8m/s. The user probably has
// the ports the wrong way around // the ports the wrong way around
_healthy = false; state[i].healthy = false;
} }
break; break;
case PITOT_TUBE_ORDER_AUTO: case PITOT_TUBE_ORDER_AUTO:
default: default:
_last_pressure = fabsf(airspeed_pressure); state[i].last_pressure = fabsf(airspeed_pressure);
_raw_airspeed = sqrtf(fabsf(airspeed_pressure) * _ratio); state[i].raw_airspeed = sqrtf(fabsf(airspeed_pressure) * param[i].ratio);
_airspeed = sqrtf(fabsf(_filtered_pressure) * _ratio); state[i].airspeed = sqrtf(fabsf(state[i].filtered_pressure) * param[i].ratio);
break; 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<AIRSPEED_MAX_SENSORS; i++) {
read(i);
}
#if 1
// debugging until we get MAVLink support for 2nd airspeed sensor
if (enabled(1)) {
mavlink_msg_named_value_float_send(MAVLINK_COMM_0, AP_HAL::millis(), "AS2", get_airspeed(1));
}
#endif
// setup primary
if (healthy(primary_sensor.get())) {
primary = primary_sensor.get();
return;
}
for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
if (healthy(i)) {
primary = i;
break;
}
}
} }
void AP_Airspeed::setHIL(float airspeed, float diff_pressure, float temperature) void AP_Airspeed::setHIL(float airspeed, float diff_pressure, float temperature)
{ {
_raw_airspeed = airspeed; state[0].raw_airspeed = airspeed;
_airspeed = airspeed; state[0].airspeed = airspeed;
_last_pressure = diff_pressure; state[0].last_pressure = diff_pressure;
_last_update_ms = AP_HAL::millis(); state[0].last_update_ms = AP_HAL::millis();
_hil_pressure = diff_pressure; state[0].hil_pressure = diff_pressure;
_hil_set = true; state[0].hil_set = true;
_healthy = true; state[0].healthy = true;
} }
bool AP_Airspeed::use(void) const bool AP_Airspeed::use(uint8_t i) const
{ {
if (!enabled() || !_use) { if (!enabled(i) || !param[i].use) {
return false; return false;
} }
if (_use == 2 && SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) != 0) { if (param[i].use == 2 && SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) != 0) {
// special case for gliders with airspeed sensors behind the // special case for gliders with airspeed sensors behind the
// propeller. Allow airspeed to be disabled when throttle is // propeller. Allow airspeed to be disabled when throttle is
// running // running
@ -325,3 +436,16 @@ bool AP_Airspeed::use(void) const
} }
return true; return true;
} }
/*
return true if all enabled sensors are healthy
*/
bool AP_Airspeed::all_healthy(void) const
{
for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
if (enabled(i) && !healthy(i)) {
return false;
}
}
return true;
}

175
libraries/AP_Airspeed/AP_Airspeed.h
View File

@ -8,6 +8,8 @@
class AP_Airspeed_Backend; class AP_Airspeed_Backend;
#define AIRSPEED_MAX_SENSORS 2
class Airspeed_Calibration { class Airspeed_Calibration {
public: public:
friend class AP_Airspeed; friend class AP_Airspeed;
@ -40,84 +42,86 @@ public:
void init(void); void init(void);
// read the analog source and update _airspeed // read the analog source and update airspeed
void read(void); void read(void);
// calibrate the airspeed. This must be called on startup if the // calibrate the airspeed. This must be called on startup if the
// altitude/climb_rate/acceleration interfaces are ever used // altitude/climb_rate/acceleration interfaces are ever used
void calibrate(bool in_startup); void calibrate(bool in_startup);
// return the current airspeed in m/s // return the current airspeed in m/s
float get_airspeed(void) const { float get_airspeed(uint8_t i) const {
return _airspeed; return state[i].airspeed;
} }
float get_airspeed(void) const { return get_airspeed(primary); }
// return the unfiltered airspeed in m/s // return the unfiltered airspeed in m/s
float get_raw_airspeed(void) const { float get_raw_airspeed(uint8_t i) const {
return _raw_airspeed; return state[i].raw_airspeed;
}
// return the current airspeed in cm/s
float get_airspeed_cm(void) const {
return _airspeed*100;
} }
float get_raw_airspeed(void) const { return get_raw_airspeed(primary); }
// return the current airspeed ratio (dimensionless) // return the current airspeed ratio (dimensionless)
float get_airspeed_ratio(void) const { float get_airspeed_ratio(uint8_t i) const {
return _ratio; return param[i].ratio;
} }
float get_airspeed_ratio(void) const { return get_airspeed_ratio(primary); }
// get temperature if available // get temperature if available
bool get_temperature(float &temperature); bool get_temperature(uint8_t i, float &temperature);
bool get_temperature(float &temperature) { return get_temperature(primary, temperature); }
// set the airspeed ratio (dimensionless) // set the airspeed ratio (dimensionless)
void set_airspeed_ratio(float ratio) { void set_airspeed_ratio(uint8_t i, float ratio) {
_ratio.set(ratio); param[i].ratio.set(ratio);
} }
void set_airspeed_ratio(float ratio) { set_airspeed_ratio(primary, ratio); }
// return true if airspeed is enabled, and airspeed use is set // return true if airspeed is enabled, and airspeed use is set
bool use(void) const; bool use(uint8_t i) const;
bool use(void) const { return use(primary); }
// return true if airspeed is enabled // return true if airspeed is enabled
bool enabled(void) const { bool enabled(uint8_t i) const {
return _type.get() != TYPE_NONE; return param[i].type.get() != TYPE_NONE;
}
// force disable the airspeed sensor
void disable(void) {
_type.set(TYPE_NONE);
} }
bool enabled(void) const { return enabled(primary); }
// used by HIL to set the airspeed // used by HIL to set the airspeed
void set_HIL(float airspeed) { void set_HIL(float airspeed) {
_airspeed = airspeed; state[primary].airspeed = airspeed;
} }
// return the differential pressure in Pascal for the last // return the differential pressure in Pascal for the last
// airspeed reading. Used by the calibration code // airspeed reading. Used by the calibration code
float get_differential_pressure(void) const { float get_differential_pressure(uint8_t i) const {
return _last_pressure; return state[i].last_pressure;
} }
float get_differential_pressure(void) const { return get_differential_pressure(primary); }
// return the current offset // return the current calibration offset
float get_offset(void) const { float get_offset(uint8_t i) const {
return _offset; return param[i].offset;
} }
float get_offset(void) const { return get_offset(primary); }
// return the current corrected pressure // return the current corrected pressure
float get_corrected_pressure(void) const { float get_corrected_pressure(uint8_t i) const {
return _corrected_pressure; return state[i].corrected_pressure;
} }
float get_corrected_pressure(void) const { return get_corrected_pressure(primary); }
// set the apparent to true airspeed ratio // set the apparent to true airspeed ratio
void set_EAS2TAS(float v) { void set_EAS2TAS(uint8_t i, float v) {
_EAS2TAS = v; state[i].EAS2TAS = v;
} }
void set_EAS2TAS(float v) { set_EAS2TAS(primary, v); }
// get the apparent to true airspeed ratio // get the apparent to true airspeed ratio
float get_EAS2TAS(void) const { float get_EAS2TAS(uint8_t i) const {
return _EAS2TAS; return state[i].EAS2TAS;
} }
float get_EAS2TAS(void) const { return get_EAS2TAS(primary); }
// update airspeed ratio calibration // update airspeed ratio calibration
void update_calibration(const Vector3f &vground, int16_t max_airspeed_allowed_during_cal); void update_calibration(const Vector3f &vground, int16_t max_airspeed_allowed_during_cal);
@ -126,12 +130,19 @@ public:
void log_mavlink_send(mavlink_channel_t chan, const Vector3f &vground); void log_mavlink_send(mavlink_channel_t chan, const Vector3f &vground);
// return health status of sensor // return health status of sensor
bool healthy(void) const { return _healthy && (fabsf(_offset) > 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 // 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); void setHIL(float airspeed, float diff_pressure, float temperature);
@ -150,46 +161,60 @@ public:
TYPE_I2C_MS5525_ADDRESS_2=5, TYPE_I2C_MS5525_ADDRESS_2=5,
TYPE_I2C_SDP3X=6, TYPE_I2C_SDP3X=6,
}; };
// get current primary sensor
uint8_t get_primary(void) const { return primary; }
private: 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 { struct {
uint32_t start_ms; AP_Float offset;
uint16_t count; AP_Float ratio;
float sum; AP_Float psi_range;
uint16_t read_count; AP_Int8 use;
} _cal; 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; struct airspeed_state {
float _last_saved_ratio; float raw_airspeed;
uint8_t _counter; 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); Airspeed_Calibration calibration;
void update_calibration(float raw_pressure); 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 AP_Airspeed_Backend *sensor[AIRSPEED_MAX_SENSORS];
bool _allow_zero_offset;
}; };

11
libraries/AP_Airspeed/AP_Airspeed_Backend.cpp
View File

@ -24,8 +24,9 @@
extern const AP_HAL::HAL &hal; extern const AP_HAL::HAL &hal;
AP_Airspeed_Backend::AP_Airspeed_Backend(AP_Airspeed &_frontend) : AP_Airspeed_Backend::AP_Airspeed_Backend(AP_Airspeed &_frontend, uint8_t _instance) :
frontend(_frontend) frontend(_frontend),
instance(_instance)
{ {
sem = hal.util->new_semaphore(); 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 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 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 uint8_t AP_Airspeed_Backend::get_bus(void) const
{ {
return frontend._bus; return frontend.param[instance].bus;
} }

17
libraries/AP_Airspeed/AP_Airspeed_Backend.h
View File

@ -24,7 +24,7 @@
class AP_Airspeed_Backend { class AP_Airspeed_Backend {
public: public:
AP_Airspeed_Backend(AP_Airspeed &frontend); AP_Airspeed_Backend(AP_Airspeed &frontend, uint8_t instance);
virtual ~AP_Airspeed_Backend(); virtual ~AP_Airspeed_Backend();
// probe and initialise the sensor // probe and initialise the sensor
@ -42,31 +42,32 @@ protected:
uint8_t get_bus(void) const; uint8_t get_bus(void) const;
AP_Airspeed::pitot_tube_order get_tube_order(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 // semaphore for access to shared frontend data
AP_HAL::Semaphore *sem; AP_HAL::Semaphore *sem;
float get_airspeed_ratio(void) const { float get_airspeed_ratio(void) const {
return frontend.get_airspeed_ratio(); return frontend.get_airspeed_ratio(instance);
} }
// some sensors allow zero offsets while healthy // some sensors use zero offsets
void set_allow_zero_offset(void) { void set_use_zero_offset(void) {
frontend._allow_zero_offset = true; frontend.state[instance].use_zero_offset = true;
} }
// set to no zero cal, which makes sense for some sensors // set to no zero cal, which makes sense for some sensors
void set_skip_cal(void) { void set_skip_cal(void) {
frontend._skip_cal.set(1); frontend.param[instance].skip_cal.set(1);
} }
// set zero offset // set zero offset
void set_offset(float ofs) { void set_offset(float ofs) {
frontend._offset.set(ofs); frontend.param[instance].offset.set(ofs);
} }
private: private:
AP_Airspeed &frontend; AP_Airspeed &frontend;
uint8_t instance;
}; };

4
libraries/AP_Airspeed/AP_Airspeed_MS4525.cpp
View File

@ -35,8 +35,8 @@ extern const AP_HAL::HAL &hal;
#define MS4525D0_I2C_BUS 1 #define MS4525D0_I2C_BUS 1
#endif #endif
AP_Airspeed_MS4525::AP_Airspeed_MS4525(AP_Airspeed &_frontend) : AP_Airspeed_MS4525::AP_Airspeed_MS4525(AP_Airspeed &_frontend, uint8_t _instance) :
AP_Airspeed_Backend(_frontend) AP_Airspeed_Backend(_frontend, _instance)
{ {
} }

2
libraries/AP_Airspeed/AP_Airspeed_MS4525.h
View File

@ -30,7 +30,7 @@
class AP_Airspeed_MS4525 : public AP_Airspeed_Backend class AP_Airspeed_MS4525 : public AP_Airspeed_Backend
{ {
public: public:
AP_Airspeed_MS4525(AP_Airspeed &frontend); AP_Airspeed_MS4525(AP_Airspeed &frontend, uint8_t _instance);
~AP_Airspeed_MS4525(void) {} ~AP_Airspeed_MS4525(void) {}
// probe and initialise the sensor // probe and initialise the sensor

4
libraries/AP_Airspeed/AP_Airspeed_MS5525.cpp
View File

@ -51,8 +51,8 @@ extern const AP_HAL::HAL &hal;
#define REG_CONVERT_PRESSURE REG_CONVERT_D1_OSR_1024 #define REG_CONVERT_PRESSURE REG_CONVERT_D1_OSR_1024
#define REG_CONVERT_TEMPERATURE REG_CONVERT_D2_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_MS5525::AP_Airspeed_MS5525(AP_Airspeed &_frontend, uint8_t _instance, MS5525_ADDR address) :
AP_Airspeed_Backend(_frontend) AP_Airspeed_Backend(_frontend, _instance)
{ {
_address = address; _address = address;
} }

2
libraries/AP_Airspeed/AP_Airspeed_MS5525.h
View File

@ -36,7 +36,7 @@ public:
MS5525_ADDR_AUTO = 255, // does not need to be 255, just needs to be out of the address array 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) {} ~AP_Airspeed_MS5525(void) {}
// probe and initialise the sensor // probe and initialise the sensor

72
libraries/AP_Airspeed/AP_Airspeed_SDP3X.cpp
View File

@ -19,6 +19,7 @@
with thanks to https://github.com/PX4/Firmware/blob/master/src/drivers/sdp3x_airspeed with thanks to https://github.com/PX4/Firmware/blob/master/src/drivers/sdp3x_airspeed
*/ */
#include "AP_Airspeed_SDP3X.h" #include "AP_Airspeed_SDP3X.h"
#include <GCS_MAVLink/GCS.h>
#include <stdio.h> #include <stdio.h>
@ -35,14 +36,10 @@
#define SDP3X_SCALE_PRESSURE_SDP32 240 #define SDP3X_SCALE_PRESSURE_SDP32 240
#define SDP3X_SCALE_PRESSURE_SDP33 20 #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; extern const AP_HAL::HAL &hal;
AP_Airspeed_SDP3X::AP_Airspeed_SDP3X(AP_Airspeed &_frontend) : AP_Airspeed_SDP3X::AP_Airspeed_SDP3X(AP_Airspeed &_frontend, uint8_t _instance) :
AP_Airspeed_Backend(_frontend) AP_Airspeed_Backend(_frontend, _instance)
{ {
} }
@ -77,19 +74,31 @@ bool AP_Airspeed_SDP3X::init()
// lots of retries during probe // lots of retries during probe
_dev->set_retries(10); _dev->set_retries(10);
// stop any precending measurements // stop continuous average mode
if (!_send_command(SDP3X_CONT_MEAS_STOP)) { if (!_send_command(SDP3X_CONT_MEAS_STOP)) {
_dev->get_semaphore()->give(); _dev->get_semaphore()->give();
continue; 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 // start continuous average mode
if (!_send_command(SDP3X_CONT_MEAS_AVG_MODE)) { if (!_send_command(SDP3X_CONT_MEAS_AVG_MODE)) {
_dev->get_semaphore()->give(); _dev->get_semaphore()->give();
continue; continue;
} }
// these delays are needed for reliable operation
_dev->get_semaphore()->give();
hal.scheduler->delay_microseconds(20000); hal.scheduler->delay_microseconds(20000);
if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
continue;
}
// step 3 - get scale // step 3 - get scale
uint8_t val[9]; uint8_t val[9];
@ -134,7 +143,7 @@ bool AP_Airspeed_SDP3X::init()
/* /*
this sensor uses zero offset and skips cal this sensor uses zero offset and skips cal
*/ */
set_allow_zero_offset(); set_use_zero_offset();
set_skip_cal(); set_skip_cal();
set_offset(0); set_offset(0);
@ -168,13 +177,8 @@ void AP_Airspeed_SDP3X::_timer()
int16_t P = (((int16_t)val[0]) << 8) | val[1]; int16_t P = (((int16_t)val[0]) << 8) | val[1];
int16_t temp = (((int16_t)val[3]) << 8) | val[4]; int16_t temp = (((int16_t)val[3]) << 8) | val[4];
float P_float = (float)P; float diff_press_pa = float(P) / float(_scale);
float temp_float = (float)temp; float temperature = float(temp) / SDP3X_SCALE_TEMPERATURE;
float scale_float = (float)_scale;
float diff_press_pa = P_float / scale_float;
float temperature = temp_float / (float)SDP3X_SCALE_TEMPERATURE;
if (sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { if (sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
_press_sum += diff_press_pa; _press_sum += diff_press_pa;
@ -193,7 +197,6 @@ void AP_Airspeed_SDP3X::_timer()
*/ */
float AP_Airspeed_SDP3X::_correct_pressure(float press) float AP_Airspeed_SDP3X::_correct_pressure(float press)
{ {
const float tube_len = 0.2;
float temperature; float temperature;
AP_Baro *baro = AP_Baro::get_instance(); AP_Baro *baro = AP_Baro::get_instance();
@ -201,26 +204,47 @@ float AP_Airspeed_SDP3X::_correct_pressure(float press)
return press; return press;
} }
float sign = 1;
// fix for tube order // fix for tube order
AP_Airspeed::pitot_tube_order tube_order = get_tube_order(); AP_Airspeed::pitot_tube_order tube_order = get_tube_order();
switch (tube_order) { switch (tube_order) {
case AP_Airspeed::PITOT_TUBE_ORDER_NEGATIVE: case AP_Airspeed::PITOT_TUBE_ORDER_NEGATIVE:
press = -press; press = -press;
sign = -1;
//FALLTHROUGH; //FALLTHROUGH;
case AP_Airspeed::PITOT_TUBE_ORDER_POSITIVE: case AP_Airspeed::PITOT_TUBE_ORDER_POSITIVE:
break; break;
case AP_Airspeed::PITOT_TUBE_ORDER_AUTO: case AP_Airspeed::PITOT_TUBE_ORDER_AUTO:
default: default:
press = fabsf(press); if (press < 0) {
sign = -1;
press = -press;
}
break; break;
} }
if (press <= 0) { if (press <= 0) {
return 0; return 0;
} }
get_temperature(temperature); 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 // flow through sensor
float flow_SDP3X = (300.805f - 300.878f / (0.00344205f * (float)powf(press, 0.68698f) + 1)) * 1.29f / rho_air; float flow_SDP3X = (300.805f - 300.878f / (0.00344205f * (float)powf(press, 0.68698f) + 1)) * 1.29f / rho_air;
if (flow_SDP3X < 0.0f) { if (flow_SDP3X < 0.0f) {
@ -230,11 +254,8 @@ float AP_Airspeed_SDP3X::_correct_pressure(float press)
// diffential pressure through pitot tube // diffential pressure through pitot tube
float dp_pitot = 28557670.0f - 28557670.0f / (1 + (float)powf((flow_SDP3X / 5027611.0f), 1.227924f)); 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 // 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 // correction for speed at pitot-tube tip due to flow through sensor
float dv = 0.0331582 * flow_SDP3X; float dv = 0.0331582 * flow_SDP3X;
@ -242,10 +263,13 @@ float AP_Airspeed_SDP3X::_correct_pressure(float press)
// airspeed ratio // airspeed ratio
float ratio = get_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; 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 // return the current differential_pressure in Pascal

5
libraries/AP_Airspeed/AP_Airspeed_SDP3X.h
View File

@ -34,7 +34,7 @@
class AP_Airspeed_SDP3X : public AP_Airspeed_Backend class AP_Airspeed_SDP3X : public AP_Airspeed_Backend
{ {
public: public:
AP_Airspeed_SDP3X(AP_Airspeed &frontend); AP_Airspeed_SDP3X(AP_Airspeed &frontend, uint8_t _instance);
~AP_Airspeed_SDP3X(void) {} ~AP_Airspeed_SDP3X(void) {}
// probe and initialise the sensor // probe and initialise the sensor
@ -47,11 +47,8 @@ public:
bool get_temperature(float &temperature) override; bool get_temperature(float &temperature) override;
private: private:
void _collect();
void _timer(); void _timer();
bool _send_command(uint16_t cmd); 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); bool _crc(const uint8_t data[], unsigned size, uint8_t checksum);
float _correct_pressure(float press); float _correct_pressure(float press);

4
libraries/AP_Airspeed/AP_Airspeed_analog.cpp
View File

@ -27,8 +27,8 @@ extern const AP_HAL::HAL &hal;
// scaling for 3DR analog airspeed sensor // scaling for 3DR analog airspeed sensor
#define VOLTS_TO_PASCAL 819 #define VOLTS_TO_PASCAL 819
AP_Airspeed_Analog::AP_Airspeed_Analog(AP_Airspeed &_frontend) : AP_Airspeed_Analog::AP_Airspeed_Analog(AP_Airspeed &_frontend, uint8_t _instance) :
AP_Airspeed_Backend(_frontend) AP_Airspeed_Backend(_frontend, _instance)
{ {
_source = hal.analogin->channel(get_pin()); _source = hal.analogin->channel(get_pin());
} }

2
libraries/AP_Airspeed/AP_Airspeed_analog.h
View File

@ -8,7 +8,7 @@
class AP_Airspeed_Analog : public AP_Airspeed_Backend class AP_Airspeed_Analog : public AP_Airspeed_Backend
{ {
public: public:
AP_Airspeed_Analog(AP_Airspeed &frontend); AP_Airspeed_Analog(AP_Airspeed &frontend, uint8_t _instance);
// probe and initialise the sensor // probe and initialise the sensor
bool init(void) override; bool init(void) override;

56
libraries/AP_Airspeed/Airspeed_Calibration.cpp
View File

@ -110,9 +110,9 @@ float Airspeed_Calibration::update(float airspeed, const Vector3f &vg, int16_t m
/* /*
called once a second to do calibration update 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 // auto-calibration not enabled
return; 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 // set state.z based on current ratio, this allows the operator to
// override the current ratio in flight with autocal, which is // override the current ratio in flight with autocal, which is
// very useful both for testing and to force a reasonable value. // 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 // calculate true airspeed, assuming a airspeed ratio of 1.0
float dpress = MAX(get_differential_pressure(), 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)) { if (isnan(zratio) || isinf(zratio)) {
return; 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 // this constrains the resulting ratio to between 1.0 and 4.0
zratio = constrain_float(zratio, 0.5f, 1.0f); zratio = constrain_float(zratio, 0.5f, 1.0f);
_ratio.set(1/sq(zratio)); param[i].ratio.set(1/sq(zratio));
if (_counter > 60) { if (state[i].counter > 60) {
if (_last_saved_ratio > 1.05f*_ratio || if (state[i].last_saved_ratio > 1.05f*param[i].ratio ||
_last_saved_ratio < 0.95f*_ratio) { state[i].last_saved_ratio < 0.95f*param[i].ratio) {
_ratio.save(); param[i].ratio.save();
_last_saved_ratio = _ratio; state[i].last_saved_ratio = param[i].ratio;
_counter = 0; state[i].counter = 0;
} }
} else { } 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<AIRSPEED_MAX_SENSORS; i++) {
update_calibration(i, vground, max_airspeed_allowed_during_cal);
} }
} }
@ -156,13 +166,13 @@ void AP_Airspeed::log_mavlink_send(mavlink_channel_t chan, const Vector3f &vgrou
vground.x, vground.x,
vground.y, vground.y,
vground.z, vground.z,
get_differential_pressure(), get_differential_pressure(primary),
_EAS2TAS, state[primary].EAS2TAS,
_ratio.get(), param[primary].ratio.get(),
_calibration.state.x, state[primary].calibration.state.x,
_calibration.state.y, state[primary].calibration.state.y,
_calibration.state.z, state[primary].calibration.state.z,
_calibration.P.a.x, state[primary].calibration.P.a.x,
_calibration.P.b.y, state[primary].calibration.P.b.y,
_calibration.P.c.z); state[primary].calibration.P.c.z);
} }