diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 68b3d27b42..89e6abb4ac 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -48,7 +48,6 @@ void LoggedTopics::add_default_topics() add_topic("action_request"); add_topic("actuator_armed"); add_optional_topic("actuator_controls_status_0", 300); - add_topic("airspeed", 1000); add_optional_topic("airspeed_validated", 200); add_optional_topic("autotune_attitude_control_status", 100); add_optional_topic("camera_capture"); @@ -139,6 +138,7 @@ void LoggedTopics::add_default_topics() // multi topics add_optional_topic_multi("actuator_outputs", 100, 3); + add_optional_topic_multi("airspeed", 1000, 3); add_optional_topic_multi("airspeed_wind", 1000, 4); add_optional_topic_multi("control_allocator_status", 200, 2); add_optional_topic_multi("rate_ctrl_status", 200, 2); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 26cc3329da..264a6217ca 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -59,6 +59,8 @@ Sensors::Sensors(bool hil_enabled) : #if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) /* Differential pressure offset */ _parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF"); + + InitializeVehicleDifferentialPressure(); #ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL _parameter_handles.diff_pres_analog_scale = param_find("SENS_DPRES_ANSC"); #endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ @@ -67,8 +69,11 @@ Sensors::Sensors(bool hil_enabled) : _parameter_handles.air_tube_length = param_find("CAL_AIR_TUBELEN"); _parameter_handles.air_tube_diameter_mm = param_find("CAL_AIR_TUBED_MM"); - _airspeed_validator.set_timeout(300000); - _airspeed_validator.set_equal_value_threshold(100); + for (int i = 0; i < _number_of_differential_pressure_sensors; i++) { + _diff_pres_data[i].airspeed_validator.set_timeout(300000); + _diff_pres_data[i].airspeed_validator.set_equal_value_threshold(100); + } + #endif // CONFIG_SENSORS_VEHICLE_AIRSPEED #if defined(CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY) @@ -260,106 +265,113 @@ int Sensors::parameters_update() #if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) void Sensors::diff_pres_poll() { - differential_pressure_s diff_pres{}; + vehicle_air_data_s air_data{}; + _vehicle_air_data_sub.copy(&air_data); - if (_diff_pres_sub.update(&diff_pres)) { + for (int i = 0; i < _number_of_differential_pressure_sensors; i++) { - if (!PX4_ISFINITE(diff_pres.differential_pressure_pa)) { - // ignore invalid data and reset accumulated + // poll raw differential sensor topic of the i-th sensor + differential_pressure_s diff_pres{}; - // reset - _diff_pres_timestamp_sum = 0; - _diff_pres_pressure_sum = 0; - _diff_pres_temperature_sum = 0; - _baro_pressure_sum = 0; - _diff_pres_count = 0; - return; - } + if (_differential_pressure_subs[i].update(&diff_pres)) { - vehicle_air_data_s air_data{}; - _vehicle_air_data_sub.copy(&air_data); + if (!PX4_ISFINITE(diff_pres.differential_pressure_pa)) { + // ignore invalid data and reset accumulated - float air_temperature_celsius = NAN; - - // assume anything outside of a (generous) operating range of -40C to 125C is invalid - if (PX4_ISFINITE(diff_pres.temperature) && (diff_pres.temperature >= -40.f) && (diff_pres.temperature <= 125.f)) { - - air_temperature_celsius = diff_pres.temperature; - - } else { - // differential pressure temperature invalid, check barometer - if ((air_data.timestamp != 0) && PX4_ISFINITE(air_data.baro_temp_celcius) - && (air_data.baro_temp_celcius >= -40.f) && (air_data.baro_temp_celcius <= 125.f)) { - - // TODO: review PCB_TEMP_ESTIMATE_DEG, ignore for external baro - air_temperature_celsius = air_data.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG; - } - } - - // push raw data into validator - float airspeed_input[3] { diff_pres.differential_pressure_pa, air_temperature_celsius, 0.0f }; - _airspeed_validator.put(diff_pres.timestamp_sample, airspeed_input, diff_pres.error_count, 100); // TODO: real priority? - - // accumulate average for publication - _diff_pres_timestamp_sum += diff_pres.timestamp_sample; - _diff_pres_pressure_sum += diff_pres.differential_pressure_pa; - _diff_pres_temperature_sum += air_temperature_celsius; - _baro_pressure_sum += air_data.baro_pressure_pa; - _diff_pres_count++; - - if ((_diff_pres_count > 0) && hrt_elapsed_time(&_airspeed_last_publish) >= 50_ms) { - - // average data and apply calibration offset (SENS_DPRES_OFF) - const uint64_t timestamp_sample = _diff_pres_timestamp_sum / _diff_pres_count; - const float differential_pressure_pa = _diff_pres_pressure_sum / _diff_pres_count - _parameters.diff_pres_offset_pa; - const float baro_pressure_pa = _baro_pressure_sum / _diff_pres_count; - const float temperature = _diff_pres_temperature_sum / _diff_pres_count; - - // reset - _diff_pres_timestamp_sum = 0; - _diff_pres_pressure_sum = 0; - _diff_pres_temperature_sum = 0; - _baro_pressure_sum = 0; - _diff_pres_count = 0; - - - 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: - smodel = AIRSPEED_SENSOR_MODEL_SDP3X; - break; - - default: - smodel = AIRSPEED_SENSOR_MODEL_MEMBRANE; - break; + // reset + _diff_pres_data[i].diff_pres_timestamp_sum = 0; + _diff_pres_data[i].diff_pres_pressure_sum = 0; + _diff_pres_data[i].diff_pres_temperature_sum = 0; + _diff_pres_data[i].baro_pressure_sum = 0; + _diff_pres_data[i].diff_pres_count = 0; + return; } - float indicated_airspeed_m_s = calc_IAS_corrected((enum AIRSPEED_COMPENSATION_MODEL)_parameters.air_cmodel, - smodel, _parameters.air_tube_length, _parameters.air_tube_diameter_mm, - differential_pressure_pa, baro_pressure_pa, temperature); + float air_temperature_celsius = NAN; - // assume that CAS = IAS as we don't have an CAS-scale here - float true_airspeed_m_s = calc_TAS_from_CAS(indicated_airspeed_m_s, baro_pressure_pa, temperature); + // assume anything outside of a (generous) operating range of -40C to 125C is invalid + if (PX4_ISFINITE(diff_pres.temperature) && (diff_pres.temperature >= -40.f) && (diff_pres.temperature <= 125.f)) { - if (PX4_ISFINITE(indicated_airspeed_m_s) && PX4_ISFINITE(true_airspeed_m_s)) { + air_temperature_celsius = diff_pres.temperature; - airspeed_s airspeed; - airspeed.timestamp_sample = timestamp_sample; - airspeed.indicated_airspeed_m_s = indicated_airspeed_m_s; - airspeed.true_airspeed_m_s = true_airspeed_m_s; - airspeed.air_temperature_celsius = temperature; - airspeed.confidence = _airspeed_validator.confidence(hrt_absolute_time()); - airspeed.timestamp = hrt_absolute_time(); - _airspeed_pub.publish(airspeed); + } else { + // differential pressure temperature invalid, check barometer + if ((air_data.timestamp != 0) && PX4_ISFINITE(air_data.baro_temp_celcius) + && (air_data.baro_temp_celcius >= -40.f) && (air_data.baro_temp_celcius <= 125.f)) { - _airspeed_last_publish = airspeed.timestamp; + // TODO: review PCB_TEMP_ESTIMATE_DEG, ignore for external baro + air_temperature_celsius = air_data.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG; + } + } + + // push raw data into validator + float airspeed_input[3] { diff_pres.differential_pressure_pa, air_temperature_celsius, 0.0f }; + _diff_pres_data[i].airspeed_validator.put(diff_pres.timestamp_sample, airspeed_input, diff_pres.error_count, + 100); // TODO: real priority? + + // accumulate average for publication + _diff_pres_data[i].diff_pres_timestamp_sum += diff_pres.timestamp_sample; + _diff_pres_data[i].diff_pres_pressure_sum += diff_pres.differential_pressure_pa; + _diff_pres_data[i].diff_pres_temperature_sum += air_temperature_celsius; + _diff_pres_data[i].baro_pressure_sum += air_data.baro_pressure_pa; + _diff_pres_data[i].diff_pres_count++; + + if ((_diff_pres_data[i].diff_pres_count > 0) && hrt_elapsed_time(&_diff_pres_data[i].airspeed_last_publish) >= 50_ms) { + + // average data and apply calibration offset (SENS_DPRES_OFF) + const uint64_t timestamp_sample = _diff_pres_data[i].diff_pres_timestamp_sum / _diff_pres_data[i].diff_pres_count; + const float differential_pressure_pa = _diff_pres_data[i].diff_pres_pressure_sum / _diff_pres_data[i].diff_pres_count - + _parameters.diff_pres_offset_pa; + const float baro_pressure_pa = _diff_pres_data[i].baro_pressure_sum / _diff_pres_data[i].diff_pres_count; + const float temperature = _diff_pres_data[i].diff_pres_temperature_sum / _diff_pres_data[i].diff_pres_count; + + // reset + _diff_pres_data[i].diff_pres_timestamp_sum = 0; + _diff_pres_data[i].diff_pres_pressure_sum = 0; + _diff_pres_data[i].diff_pres_temperature_sum = 0; + _diff_pres_data[i].baro_pressure_sum = 0; + _diff_pres_data[i].diff_pres_count = 0; + + + 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: + smodel = AIRSPEED_SENSOR_MODEL_SDP3X; + break; + + default: + smodel = AIRSPEED_SENSOR_MODEL_MEMBRANE; + break; + } + + float indicated_airspeed_m_s = calc_IAS_corrected((enum AIRSPEED_COMPENSATION_MODEL)_parameters.air_cmodel, + smodel, _parameters.air_tube_length, _parameters.air_tube_diameter_mm, + differential_pressure_pa, baro_pressure_pa, temperature); + + // assume that CAS = IAS as we don't have an CAS-scale here + float true_airspeed_m_s = calc_TAS_from_CAS(indicated_airspeed_m_s, baro_pressure_pa, temperature); + + if (PX4_ISFINITE(indicated_airspeed_m_s) && PX4_ISFINITE(true_airspeed_m_s)) { + + airspeed_s airspeed; + airspeed.timestamp_sample = timestamp_sample; + airspeed.indicated_airspeed_m_s = indicated_airspeed_m_s; + airspeed.true_airspeed_m_s = true_airspeed_m_s; + airspeed.air_temperature_celsius = temperature; + airspeed.confidence = _diff_pres_data[i].airspeed_validator.confidence(hrt_absolute_time()); + airspeed.timestamp = hrt_absolute_time(); + + _airspeed_pubs[i].publish(airspeed); + + _diff_pres_data[i].airspeed_last_publish = airspeed.timestamp; + } } } } @@ -432,6 +444,22 @@ void Sensors::InitializeVehicleAirData() } #endif // CONFIG_SENSORS_VEHICLE_AIR_DATA +#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) +void Sensors::InitializeVehicleDifferentialPressure() +{ + // check for connected differential pressure sensors + for (int i = 0; i < _differential_pressure_subs.size(); i++) { + if (!_differential_pressure_subs[i].advertised()) { + break; + } + + _airspeed_pubs[i].advertise(); + + _number_of_differential_pressure_sensors = i + 1; + } +} +#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED + #if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) void Sensors::InitializeVehicleGPSPosition() { @@ -708,9 +736,16 @@ int Sensors::print_status() #endif // CONFIG_SENSORS_VEHICLE_AIR_DATA #if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) + PX4_INFO_RAW("\n"); PX4_INFO_RAW("Airspeed status:\n"); - _airspeed_validator.print(); + + for (int i = 0; i < _number_of_differential_pressure_sensors; i++) { + PX4_INFO_RAW("Instance #%i:\n", i); + _diff_pres_data[i].airspeed_validator.print(); + PX4_INFO_RAW("\n"); + } + #endif // CONFIG_SENSORS_VEHICLE_AIRSPEED #if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW) diff --git a/src/modules/sensors/sensors.hpp b/src/modules/sensors/sensors.hpp index 911005249c..e6936783fd 100644 --- a/src/modules/sensors/sensors.hpp +++ b/src/modules/sensors/sensors.hpp @@ -49,6 +49,7 @@ #include #include #include +#include #include "voted_sensors_update.h" #include "vehicle_imu/VehicleIMU.hpp" @@ -124,6 +125,8 @@ private: void InitializeVehicleAirData(); + void InitializeVehicleDifferentialPressure(); + void InitializeVehicleGPSPosition(); void InitializeVehicleIMU(); @@ -179,21 +182,22 @@ private: */ void adc_poll(); - uORB::Subscription _diff_pres_sub {ORB_ID(differential_pressure)}; + uORB::SubscriptionMultiArray < differential_pressure_s, MAX_SENSOR_COUNT - 1 > _differential_pressure_subs{ORB_ID::differential_pressure}; + int _number_of_differential_pressure_sensors{0}; + uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; - uORB::Publication _airspeed_pub{ORB_ID(airspeed)}; + uORB::PublicationMulti _airspeed_pubs[MAX_SENSOR_COUNT - 1] {ORB_ID(airspeed), ORB_ID(airspeed), ORB_ID(airspeed)}; - DataValidator _airspeed_validator; /**< data validator to monitor airspeed */ - - float _diff_pres_pressure_sum{0.f}; - float _diff_pres_temperature_sum{0.f}; - float _baro_pressure_sum{0.f}; - - int _diff_pres_count{0}; - - uint64_t _airspeed_last_publish{0}; - uint64_t _diff_pres_timestamp_sum{0}; + struct DifferentialPressureData { + DataValidator airspeed_validator; + float diff_pres_pressure_sum; + float diff_pres_temperature_sum; + int diff_pres_count; + uint64_t airspeed_last_publish; + uint64_t diff_pres_timestamp_sum; + float baro_pressure_sum; + } _diff_pres_data[MAX_SENSOR_COUNT - 1] {}; # ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL uORB::Subscription _adc_report_sub {ORB_ID(adc_report)};