From 1880abdac50ceb0b02379948a76886fa6557a524 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Aug 2017 23:21:10 +0200 Subject: [PATCH] Airspeed: Base sensor model on device ID from sensor --- src/modules/sensors/parameters.cpp | 2 -- src/modules/sensors/parameters.h | 2 -- src/modules/sensors/sensor_params.c | 10 ---------- src/modules/sensors/sensors.cpp | 21 ++++++++++++++++++++- 4 files changed, 20 insertions(+), 15 deletions(-) diff --git a/src/modules/sensors/parameters.cpp b/src/modules/sensors/parameters.cpp index 6f8a8b2ed4..dc6483e8a7 100644 --- a/src/modules/sensors/parameters.cpp +++ b/src/modules/sensors/parameters.cpp @@ -158,7 +158,6 @@ int initialize_parameter_handles(ParameterHandles ¶meter_handles) parameter_handles.vibe_thresh = param_find("ATT_VIBE_THRESH"); parameter_handles.air_pmodel = param_find("CAL_AIR_PMODEL"); - parameter_handles.air_smodel = param_find("CAL_AIR_SMODEL"); parameter_handles.air_tube_length = param_find("CAL_AIR_TUBELEN"); // These are parameters for which QGroundControl always expects to be returned in a list request. @@ -493,7 +492,6 @@ int update_parameters(const ParameterHandles ¶meter_handles, Parameters &par param_get(parameter_handles.vibe_thresh, ¶meters.vibration_warning_threshold); param_get(parameter_handles.air_pmodel, ¶meters.air_pmodel); - param_get(parameter_handles.air_smodel, ¶meters.air_smodel); param_get(parameter_handles.air_tube_length, ¶meters.air_tube_length); return ret; diff --git a/src/modules/sensors/parameters.h b/src/modules/sensors/parameters.h index 73dd03b746..9eb4116345 100644 --- a/src/modules/sensors/parameters.h +++ b/src/modules/sensors/parameters.h @@ -147,7 +147,6 @@ struct Parameters { float vibration_warning_threshold; int32_t air_pmodel; - int32_t air_smodel; float air_tube_length; }; @@ -232,7 +231,6 @@ struct ParameterHandles { param_t vibe_thresh; /**< vibration threshold */ param_t air_pmodel; - param_t air_smodel; param_t air_tube_length; }; diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index b7f2ea8ba9..1a0d9e3b12 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -823,16 +823,6 @@ PARAM_DEFINE_INT32(CAL_BARO_PRIME, 0); */ PARAM_DEFINE_INT32(CAL_AIR_PMODEL, 0); -/** - * Airspeed sensor model - * - * @value 0 Membrane sensor - * @value 1 Sensirion SDP3x - * - * @group Sensor Calibration - */ -PARAM_DEFINE_INT32(CAL_AIR_SMODEL, 0); - /** * Airspeed sensor tube length * @min 0.01 diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 9d3087d557..ca452c46bf 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -324,10 +324,29 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) _airspeed.confidence = _airspeed_validator.confidence(hrt_absolute_time()); + enum AIRSPEED_SENSOR_MODEL smodel; + + switch ((_diff_pres.device_id >> 16) & 0xFF) { + case DRV_DIFF_PRESS_DEVTYPE_SDP31: + + /* fallthrough */ + case DRV_DIFF_PRESS_DEVTYPE_SDP32: + + /* fallthrough */ + case DRV_DIFF_PRESS_DEVTYPE_SDP33: + /* fallthrough */ + smodel = AIRSPEED_SENSOR_MODEL_SDP3X; + break; + + default: + smodel = AIRSPEED_SENSOR_MODEL_MEMBRANE; + break; + } + /* don't risk to feed negative airspeed into the system */ _airspeed.indicated_airspeed_m_s = math::max(0.0f, calc_indicated_airspeed_corrected((enum AIRSPEED_PITOT_MODEL)_parameters.air_pmodel, - (enum AIRSPEED_SENSOR_MODEL)_parameters.air_smodel, _parameters.air_tube_length, + smodel, _parameters.air_tube_length, _diff_pres.differential_pressure_filtered_pa, _voted_sensors_update.baro_pressure(), air_temperature_celsius));