diff --git a/msg/airspeed.msg b/msg/airspeed.msg index e9b771e638..897c6441eb 100644 --- a/msg/airspeed.msg +++ b/msg/airspeed.msg @@ -1,9 +1,10 @@ -uint64 timestamp # time since system start (microseconds) +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) float32 indicated_airspeed_m_s # indicated airspeed in m/s float32 true_airspeed_m_s # true filtered airspeed in m/s -float32 air_temperature_celsius # air temperature in degrees celsius, -1000 if unknown +float32 air_temperature_celsius # air temperature in degrees celsius, NAN if unknown float32 confidence # confidence value from 0 to 1 for this sensor diff --git a/src/lib/airspeed/airspeed.cpp b/src/lib/airspeed/airspeed.cpp index 50b8a0614e..658349029d 100644 --- a/src/lib/airspeed/airspeed.cpp +++ b/src/lib/airspeed/airspeed.cpp @@ -183,7 +183,6 @@ float calc_IAS(float differential_pressure) } else { return -sqrtf((2.0f * fabsf(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); } - } float calc_TAS_from_CAS(float speed_calibrated, float pressure_ambient, float temperature_celsius) diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt index e2f605325a..83deb0d873 100644 --- a/src/modules/sensors/CMakeLists.txt +++ b/src/modules/sensors/CMakeLists.txt @@ -34,6 +34,7 @@ add_subdirectory(data_validator) include_directories(${CMAKE_CURRENT_SOURCE_DIR}) +add_subdirectory(airspeed) add_subdirectory(vehicle_acceleration) add_subdirectory(vehicle_angular_velocity) add_subdirectory(vehicle_air_data) @@ -55,6 +56,7 @@ px4_add_module( git_ecl mathlib sensor_calibration + sensors_airspeed vehicle_acceleration vehicle_angular_velocity vehicle_air_data diff --git a/src/modules/sensors/airspeed/Airspeed.cpp b/src/modules/sensors/airspeed/Airspeed.cpp new file mode 100644 index 0000000000..4e4f0814b5 --- /dev/null +++ b/src/modules/sensors/airspeed/Airspeed.cpp @@ -0,0 +1,352 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "Airspeed.hpp" + +#include +#include +#include + +#include + +namespace sensors +{ + +using namespace matrix; +using namespace time_literals; + +static constexpr uint32_t SENSOR_TIMEOUT{300_ms}; + +/** + * HACK - true temperature is much less than indicated temperature in baro, + * subtract 5 degrees in an attempt to account for the electrical upheating of the PCB + */ +#define PCB_TEMP_ESTIMATE_DEG 5.0f + +Airspeed::Airspeed() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers) +{ + _voter.set_timeout(SENSOR_TIMEOUT); + _voter.set_equal_value_threshold(100); + + ParametersUpdate(true); +} + +Airspeed::~Airspeed() +{ + Stop(); + perf_free(_cycle_perf); +} + +bool Airspeed::Start() +{ + ScheduleNow(); + return true; +} + +void Airspeed::Stop() +{ + Deinit(); + + // clear all registered callbacks + for (auto &sub : _sensor_sub) { + sub.unregisterCallback(); + } +} + +void Airspeed::ParametersUpdate(bool force) +{ + // Check if parameters have changed + if (_parameter_update_sub.updated() || force) { + // clear update + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + updateParams(); + + /* update airspeed scale */ + int fd = px4_open(AIRSPEED0_DEVICE_PATH, 0); + + /* this sensor is optional, abort without error */ + if (fd >= 0) { + struct airspeed_scale airscale = { + _param_sens_dpres_off.get(), + 1.0f, + }; + + if (OK != px4_ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { + PX4_ERR("failed to set offset for differential pressure sensor"); + } + + px4_close(fd); + } + } +} + +void Airspeed::Run() +{ + perf_begin(_cycle_perf); + + ParametersUpdate(); + + if (_vehicle_air_data_sub.updated()) { + + vehicle_air_data_s air_data; + + if (_vehicle_air_data_sub.copy(&air_data)) { + 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 + _baro_air_temperature = air_data.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG; + } + } + } + + bool updated[MAX_SENSOR_COUNT] {}; + + for (int uorb_index = 0; uorb_index < MAX_SENSOR_COUNT; uorb_index++) { + + if (!_advertised[uorb_index]) { + // use data's timestamp to throttle advertisement checks + if ((_last_data[uorb_index].timestamp == 0) || (hrt_elapsed_time(&_last_data[uorb_index].timestamp) > 1_s)) { + if (_sensor_sub[uorb_index].advertised()) { + if (uorb_index > 0) { + /* the first always exists, but for each further sensor, add a new validator */ + if (!_voter.add_new_validator()) { + PX4_ERR("failed to add validator for %s %i", "DPRES", uorb_index); + } + } + + _advertised[uorb_index] = true; + + // advertise outputs in order if publishing all + if (!_param_sens_dpres_mode.get()) { + for (int instance = 0; instance < uorb_index; instance++) { + _airspeed_multi_pub[instance].advertise(); + } + } + + if (_selected_sensor_sub_index < 0) { + _sensor_sub[uorb_index].registerCallback(); + } + + } else { + _last_data[uorb_index].timestamp = hrt_absolute_time(); + } + } + } + + if (_advertised[uorb_index]) { + differential_pressure_s diff_pres; + + while (_sensor_sub[uorb_index].update(&diff_pres)) { + updated[uorb_index] = true; + + _device_id[uorb_index] = diff_pres.device_id; + + float vect[3] {diff_pres.differential_pressure_raw_pa, diff_pres.temperature, 0.f}; + _voter.put(uorb_index, diff_pres.timestamp, vect, diff_pres.error_count, _priority[uorb_index]); + + + 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, use barometer temperature if available + if (PX4_ISFINITE(_baro_air_temperature)) { + air_temperature_celsius = _baro_air_temperature; + } + } + + // average raw data for all instances + _timestamp_sample_sum[uorb_index] += diff_pres.timestamp; + _differential_pressure_sum[uorb_index] += diff_pres.differential_pressure_filtered_pa; + _temperature_sum[uorb_index] += air_temperature_celsius; + _sum_count[uorb_index]++; + } + } + } + + // check for the current best sensor + int best_index = 0; + _voter.get_best(hrt_absolute_time(), &best_index); + + if (best_index >= 0) { + if (_selected_sensor_sub_index != best_index) { + // clear all registered callbacks + for (auto &sub : _sensor_sub) { + sub.unregisterCallback(); + } + + if (_param_sens_dpres_mode.get()) { + if (_selected_sensor_sub_index >= 0) { + PX4_INFO("%s switch from #%u -> #%d", "DPRES", _selected_sensor_sub_index, best_index); + } + } + + _selected_sensor_sub_index = best_index; + _sensor_sub[_selected_sensor_sub_index].registerCallback(); + } + } + + // Publish + if (_param_sens_dpres_mode.get()) { + // publish only best sensor + if ((_selected_sensor_sub_index >= 0) + && (_voter.get_sensor_state(_selected_sensor_sub_index) == DataValidator::ERROR_FLAG_NO_ERROR) + && updated[_selected_sensor_sub_index]) { + + Publish(_selected_sensor_sub_index); + } + + } else { + // publish all + for (int uorb_index = 0; uorb_index < MAX_SENSOR_COUNT; uorb_index++) { + // publish all sensors as separate instances + if (updated[uorb_index] && (_device_id[uorb_index] != 0)) { + Publish(uorb_index, true); + } + } + } + + + // check failover and report + if (_param_sens_dpres_mode.get()) { + if (_last_failover_count != _voter.failover_count()) { + uint32_t flags = _voter.failover_state(); + int failover_index = _voter.failover_index(); + + if (flags != DataValidator::ERROR_FLAG_NO_ERROR) { + if (failover_index != -1) { + const hrt_abstime now = hrt_absolute_time(); + + if (now - _last_error_message > 3_s) { + mavlink_log_emergency(&_mavlink_log_pub, "%s #%i failed: %s%s%s%s%s!", + "DPRES", + failover_index, + ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " OFF" : ""), + ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " STALE" : ""), + ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " TIMEOUT" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " ERR CNT" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " ERR DNST" : "")); + _last_error_message = now; + } + + // reduce priority of failed sensor to the minimum + _priority[failover_index] = 1; + } + } + + _last_failover_count = _voter.failover_count(); + } + } + + // reschedule timeout + ScheduleDelayed(100_ms); + + perf_end(_cycle_perf); +} + +void Airspeed::Publish(uint8_t instance, bool multi) +{ + if ((_param_sens_dpres_rate.get() > 0) + && hrt_elapsed_time(&_last_publication_timestamp[instance]) >= (1e6f / _param_sens_dpres_rate.get())) { + + const float differential_pressure = _differential_pressure_sum[instance] / _sum_count[instance]; + const float temperature = _temperature_sum[instance] / _sum_count[instance]; + const hrt_abstime timestamp_sample = _timestamp_sample_sum[instance] / _sum_count[instance]; + + // reset + _timestamp_sample_sum[instance] = 0; + _differential_pressure_sum[instance] = 0; + _temperature_sum[instance] = 0; + _sum_count[instance] = 0; + + airspeed_s out{}; + out.timestamp_sample = timestamp_sample; + out.air_temperature_celsius = temperature; + out.confidence = 1.f; // TODO + + switch ((_device_id[instance] >> 16) & 0xFF) { + case DRV_DIFF_PRESS_DEVTYPE_SDP31: + + // fallthrough + case DRV_DIFF_PRESS_DEVTYPE_SDP32: + + // fallthrough + case DRV_DIFF_PRESS_DEVTYPE_SDP33: + out.indicated_airspeed_m_s = calc_IAS_corrected((enum AIRSPEED_COMPENSATION_MODEL) _param_cal_air_cmodel.get(), + AIRSPEED_SENSOR_MODEL_SDP3X, _param_cal_air_tubelen.get(), _param_cal_air_tubed_mm.get(), + differential_pressure, _baro_pressure_pa, temperature); + break; + + default: + out.indicated_airspeed_m_s = calc_IAS(differential_pressure); + break; + } + + // assume that CAS = IAS as we don't have an CAS-scale here + out.true_airspeed_m_s = calc_TAS_from_CAS(out.indicated_airspeed_m_s, _baro_pressure_pa, temperature); + + if (PX4_ISFINITE(out.indicated_airspeed_m_s) && PX4_ISFINITE(out.true_airspeed_m_s)) { + out.timestamp = hrt_absolute_time(); + + if (multi) { + _airspeed_multi_pub[instance].publish(out); + + } else { + // otherwise only ever publish the first instance + _airspeed_multi_pub[0].publish(out); + } + + _last_publication_timestamp[instance] = out.timestamp; + } + } +} + +void Airspeed::PrintStatus() +{ + if (_selected_sensor_sub_index >= 0) { + PX4_INFO("selected differential pressure: %d (%d)", _device_id[_selected_sensor_sub_index], _selected_sensor_sub_index); + } + + _voter.print(); +} + +}; // namespace sensors diff --git a/src/modules/sensors/airspeed/Airspeed.hpp b/src/modules/sensors/airspeed/Airspeed.hpp new file mode 100644 index 0000000000..28c89df85d --- /dev/null +++ b/src/modules/sensors/airspeed/Airspeed.hpp @@ -0,0 +1,131 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "data_validator/DataValidatorGroup.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +namespace sensors +{ +class Airspeed : public ModuleParams, public px4::ScheduledWorkItem +{ +public: + + Airspeed(); + ~Airspeed() override; + + bool Start(); + void Stop(); + + void PrintStatus(); + +private: + void Run() override; + + void ParametersUpdate(bool force = false); + + void Publish(uint8_t instance, bool multi = false); + + static constexpr int MAX_SENSOR_COUNT = 4; + + uORB::PublicationMulti _airspeed_multi_pub[MAX_SENSOR_COUNT] { + {ORB_ID(airspeed)}, + {ORB_ID(airspeed)}, + {ORB_ID(airspeed)}, + {ORB_ID(airspeed)}, + }; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; + + uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] { + {this, ORB_ID(differential_pressure), 0}, + {this, ORB_ID(differential_pressure), 1}, + {this, ORB_ID(differential_pressure), 2}, + {this, ORB_ID(differential_pressure), 3}, + }; + + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; + + hrt_abstime _last_error_message{0}; + orb_advert_t _mavlink_log_pub{nullptr}; + + DataValidatorGroup _voter{1}; + unsigned _last_failover_count{0}; + + uint32_t _device_id[MAX_SENSOR_COUNT] {}; + uint64_t _timestamp_sample_sum[MAX_SENSOR_COUNT] {0}; + hrt_abstime _last_publication_timestamp[MAX_SENSOR_COUNT] {}; + float _differential_pressure_sum[MAX_SENSOR_COUNT] {}; + float _temperature_sum[MAX_SENSOR_COUNT] {}; + int _sum_count[MAX_SENSOR_COUNT] {}; + + differential_pressure_s _last_data[MAX_SENSOR_COUNT] {}; + bool _advertised[MAX_SENSOR_COUNT] {}; + + uint8_t _priority[MAX_SENSOR_COUNT] {100, 100, 100, 100}; + + int8_t _selected_sensor_sub_index{-1}; + + float _baro_pressure_pa{101325.f}; // Sea level standard atmospheric pressure + float _baro_air_temperature{15.f}; + + DEFINE_PARAMETERS( + (ParamInt) _param_sens_dpres_mode, + (ParamFloat) _param_sens_dpres_off, + (ParamFloat) _param_sens_dpres_rate, + (ParamInt) _param_cal_air_cmodel, + (ParamFloat) _param_cal_air_tubelen, + (ParamFloat) _param_cal_air_tubed_mm + ) +}; +}; // namespace sensors diff --git a/src/modules/sensors/airspeed/CMakeLists.txt b/src/modules/sensors/airspeed/CMakeLists.txt new file mode 100644 index 0000000000..aa610bb364 --- /dev/null +++ b/src/modules/sensors/airspeed/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2020 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(sensors_airspeed + Airspeed.cpp + Airspeed.hpp +) +target_link_libraries(sensors_airspeed + PRIVATE + airspeed + px4_work_queue +) diff --git a/src/modules/sensors/airspeed/params.c b/src/modules/sensors/airspeed/params.c new file mode 100644 index 0000000000..e6b7922e3b --- /dev/null +++ b/src/modules/sensors/airspeed/params.c @@ -0,0 +1,115 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * Airspeed sensor compensation model for the SDP3x + * + * Model with Pitot + * CAL_AIR_TUBED_MM: Not used, 1.5 mm tubes assumed. + * CAL_AIR_TUBELEN: Length of the tubes connecting the pitot to the sensor. + * Model without Pitot (1.5 mm tubes) + * CAL_AIR_TUBED_MM: Not used, 1.5 mm tubes assumed. + * CAL_AIR_TUBELEN: Length of the tubes connecting the pitot to the sensor. + * Tube Pressure Drop + * CAL_AIR_TUBED_MM: Diameter in mm of the pitot and tubes, must have the same diameter. + * CAL_AIR_TUBELEN: Length of the tubes connecting the pitot to the sensor and the static + dynamic port length of the pitot. + * + * @value 0 Model with Pitot + * @value 1 Model without Pitot (1.5 mm tubes) + * @value 2 Tube Pressure Drop + * + * @group Sensors + */ +PARAM_DEFINE_INT32(CAL_AIR_CMODEL, 0); + +/** + * Airspeed sensor tube length. + * + * See the CAL_AIR_CMODEL explanation on how this parameter should be set. + * + * @min 0.01 + * @max 2.00 + * @unit m + * + * @group Sensors + */ +PARAM_DEFINE_FLOAT(CAL_AIR_TUBELEN, 0.2f); + +/** + * Airspeed sensor tube diameter. Only used for the Tube Pressure Drop Compensation. + * + * @min 0.1 + * @max 100 + * @unit mm + * + * @group Sensors + */ +PARAM_DEFINE_FLOAT(CAL_AIR_TUBED_MM, 1.5f); + +/** + * Differential pressure sensor offset + * + * The offset (zero-reading) in Pascal + * + * @category system + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f); + +/** + * Sensors hub differential pressure mode + * + * @value 0 Publish all airspeeds + * @value 1 Publish primary airspeed + * + * @category system + * @reboot_required true + * @group Sensors + */ +PARAM_DEFINE_INT32(SENS_DPRES_MODE, 1); + +/** + * Differential pressure (airspeed) max rate. + * + * Airspeed data maximum publication rate. This is an upper bound, + * actual differential pressure data rate is still dependant on the sensor. + * + * @min 1 + * @max 100 + * @group Sensors + * @unit Hz + * + * @reboot_required true + * + */ +PARAM_DEFINE_FLOAT(SENS_DPRES_RATE, 10.0f); diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 60778f73ff..1ed991e7ba 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2021 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -31,62 +31,6 @@ * ****************************************************************************/ -/** - * Airspeed sensor compensation model for the SDP3x - * - * Model with Pitot - * CAL_AIR_TUBED_MM: Not used, 1.5 mm tubes assumed. - * CAL_AIR_TUBELEN: Length of the tubes connecting the pitot to the sensor. - * Model without Pitot (1.5 mm tubes) - * CAL_AIR_TUBED_MM: Not used, 1.5 mm tubes assumed. - * CAL_AIR_TUBELEN: Length of the tubes connecting the pitot to the sensor. - * Tube Pressure Drop - * CAL_AIR_TUBED_MM: Diameter in mm of the pitot and tubes, must have the same diameter. - * CAL_AIR_TUBELEN: Length of the tubes connecting the pitot to the sensor and the static + dynamic port length of the pitot. - * - * @value 0 Model with Pitot - * @value 1 Model without Pitot (1.5 mm tubes) - * @value 2 Tube Pressure Drop - * - * @group Sensors - */ -PARAM_DEFINE_INT32(CAL_AIR_CMODEL, 0); - -/** - * Airspeed sensor tube length. - * - * See the CAL_AIR_CMODEL explanation on how this parameter should be set. - * - * @min 0.01 - * @max 2.00 - * @unit m - * - * @group Sensors - */ -PARAM_DEFINE_FLOAT(CAL_AIR_TUBELEN, 0.2f); - -/** - * Airspeed sensor tube diameter. Only used for the Tube Pressure Drop Compensation. - * - * @min 0.1 - * @max 100 - * @unit mm - * - * @group Sensors - */ -PARAM_DEFINE_FLOAT(CAL_AIR_TUBED_MM, 1.5f); - -/** - * Differential pressure sensor offset - * - * The offset (zero-reading) in Pascal - * - * @category system - * @group Sensor Calibration - * @volatile - */ -PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f); - /** * Differential pressure sensor analog scaling * diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 8cf3faa1f8..8ddaf12717 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -44,7 +44,6 @@ #include #include #include -#include #include #include #include @@ -61,7 +60,6 @@ #include #include #include -#include #include #include #include @@ -70,6 +68,7 @@ #include #include "voted_sensors_update.h" +#include "airspeed/Airspeed.hpp" #include "vehicle_acceleration/VehicleAcceleration.hpp" #include "vehicle_angular_velocity/VehicleAngularVelocity.hpp" #include "vehicle_air_data/VehicleAirData.hpp" @@ -80,11 +79,6 @@ using namespace sensors; using namespace time_literals; -/** - * HACK - true temperature is much less than indicated temperature in baro, - * subtract 5 degrees in an attempt to account for the electrical upheating of the PCB - */ -#define PCB_TEMP_ESTIMATE_DEG 5.0f class Sensors : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: @@ -126,17 +120,12 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - uORB::Subscription _diff_pres_sub{ORB_ID(differential_pressure)}; uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)}; - uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; - uORB::Publication _airspeed_pub{ORB_ID(airspeed)}; uORB::Publication _sensor_pub{ORB_ID(sensor_combined)}; perf_counter_t _loop_perf; /**< loop performance counter */ - DataValidator _airspeed_validator; /**< data validator to monitor airspeed */ - #ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL hrt_abstime _last_adc{0}; /**< last time we took input from the ADC */ @@ -153,9 +142,6 @@ private: float diff_pres_analog_scale; #endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ - int32_t air_cmodel; - float air_tube_length; - float air_tube_diameter_mm; } _parameters{}; /**< local copies of interesting parameters */ struct ParameterHandles { @@ -164,15 +150,14 @@ private: param_t diff_pres_analog_scale; #endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ - param_t air_cmodel; - param_t air_tube_length; - param_t air_tube_diameter_mm; } _parameter_handles{}; /**< handles for interesting parameters */ VotedSensorsUpdate _voted_sensors_update; VehicleAcceleration _vehicle_acceleration; VehicleAngularVelocity _vehicle_angular_velocity; + + Airspeed *_airspeed{nullptr}; VehicleAirData *_vehicle_air_data{nullptr}; VehicleMagnetometer *_vehicle_magnetometer{nullptr}; VehicleGPSPosition *_vehicle_gps_position{nullptr}; @@ -184,14 +169,6 @@ private: */ int parameters_update(); - /** - * Poll the differential pressure sensor for updated data. - * - * @param raw Combined sensor data structure into which - * data should be returned. - */ - void diff_pres_poll(); - /** * Check for changes in parameters. */ @@ -205,6 +182,7 @@ private: */ void adc_poll(); + void InitializeAirspeed(); void InitializeVehicleAirData(); void InitializeVehicleGPSPosition(); void InitializeVehicleIMU(); @@ -230,10 +208,6 @@ Sensors::Sensors(bool hil_enabled) : _parameter_handles.diff_pres_analog_scale = param_find("SENS_DPRES_ANSC"); #endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ - _parameter_handles.air_cmodel = param_find("CAL_AIR_CMODEL"); - _parameter_handles.air_tube_length = param_find("CAL_AIR_TUBELEN"); - _parameter_handles.air_tube_diameter_mm = param_find("CAL_AIR_TUBED_MM"); - param_find("SYS_FAC_CAL_MODE"); // Parameters controlling the on-board sensor thermal calibrator @@ -241,9 +215,6 @@ Sensors::Sensors(bool hil_enabled) : param_find("SYS_CAL_TMAX"); param_find("SYS_CAL_TMIN"); - _airspeed_validator.set_timeout(300000); - _airspeed_validator.set_equal_value_threshold(100); - _vehicle_acceleration.Start(); _vehicle_angular_velocity.Start(); } @@ -258,6 +229,11 @@ Sensors::~Sensors() _vehicle_acceleration.Stop(); _vehicle_angular_velocity.Stop(); + if (_airspeed) { + _airspeed->Stop(); + delete _airspeed; + } + if (_vehicle_air_data) { _vehicle_air_data->Stop(); delete _vehicle_air_data; @@ -302,88 +278,11 @@ int Sensors::parameters_update() param_get(_parameter_handles.diff_pres_analog_scale, &(_parameters.diff_pres_analog_scale)); #endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ - param_get(_parameter_handles.air_cmodel, &_parameters.air_cmodel); - param_get(_parameter_handles.air_tube_length, &_parameters.air_tube_length); - param_get(_parameter_handles.air_tube_diameter_mm, &_parameters.air_tube_diameter_mm); - _voted_sensors_update.parametersUpdate(); return PX4_OK; } -void Sensors::diff_pres_poll() -{ - differential_pressure_s diff_pres{}; - - if (_diff_pres_sub.update(&diff_pres)) { - - vehicle_air_data_s air_data{}; - _vehicle_air_data_sub.copy(&air_data); - - 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; - } - } - - airspeed_s airspeed{}; - airspeed.timestamp = diff_pres.timestamp; - - /* push data into validator */ - float airspeed_input[3] = { diff_pres.differential_pressure_raw_pa, diff_pres.temperature, 0.0f }; - - _airspeed_validator.put(airspeed.timestamp, airspeed_input, diff_pres.error_count, 100); // TODO: real priority? - - 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 = calc_IAS_corrected((enum AIRSPEED_COMPENSATION_MODEL) - _parameters.air_cmodel, - smodel, _parameters.air_tube_length, _parameters.air_tube_diameter_mm, - diff_pres.differential_pressure_filtered_pa, air_data.baro_pressure_pa, - air_temperature_celsius); - - airspeed.true_airspeed_m_s = calc_TAS_from_CAS(airspeed.indicated_airspeed_m_s, air_data.baro_pressure_pa, - air_temperature_celsius); // assume that CAS = IAS as we don't have an CAS-scale here - - airspeed.air_temperature_celsius = air_temperature_celsius; - - if (PX4_ISFINITE(airspeed.indicated_airspeed_m_s) && PX4_ISFINITE(airspeed.true_airspeed_m_s)) { - _airspeed_pub.publish(airspeed); - } - } -} - void Sensors::parameter_update_poll(bool forced) { @@ -396,23 +295,6 @@ Sensors::parameter_update_poll(bool forced) // update parameters from storage parameters_update(); updateParams(); - - /* update airspeed scale */ - int fd = px4_open(AIRSPEED0_DEVICE_PATH, 0); - - /* this sensor is optional, abort without error */ - if (fd >= 0) { - struct airspeed_scale airscale = { - _parameters.diff_pres_offset_pa, - 1.0f, - }; - - if (OK != px4_ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { - warn("WARNING: failed to set scale / offsets for airspeed sensor"); - } - - px4_close(fd); - } } } @@ -427,7 +309,7 @@ void Sensors::adc_poll() if (_parameters.diff_pres_analog_scale > 0.0f) { - hrt_abstime t = hrt_absolute_time(); + const hrt_abstime t = hrt_absolute_time(); /* rate limit to 100 Hz */ if (t - _last_adc >= 10000) { @@ -456,11 +338,12 @@ void Sensors::adc_poll() if (voltage > 0.4f) { const float diff_pres_pa_raw = voltage * _parameters.diff_pres_analog_scale - _parameters.diff_pres_offset_pa; - _diff_pres.timestamp = t; _diff_pres.differential_pressure_raw_pa = diff_pres_pa_raw; _diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) + (diff_pres_pa_raw * 0.1f); - _diff_pres.temperature = -1000.0f; + _diff_pres.temperature = NAN; + + _diff_pres.timestamp = hrt_absolute_time(); _diff_pres_pub.publish(_diff_pres); } @@ -475,6 +358,19 @@ void Sensors::adc_poll() #endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ } +void Sensors::InitializeAirspeed() +{ + if (_airspeed == nullptr) { + if (orb_exists(ORB_ID(differential_pressure), 0) == PX4_OK) { + _airspeed = new Airspeed(); + + if (_airspeed) { + _airspeed->Start(); + } + } + } +} + void Sensors::InitializeVehicleAirData() { if (_param_sys_has_baro.get()) { @@ -572,6 +468,7 @@ void Sensors::Run() // run once if (_last_config_update == 0) { + InitializeAirspeed(); InitializeVehicleAirData(); InitializeVehicleIMU(); InitializeVehicleGPSPosition(); @@ -599,8 +496,6 @@ void Sensors::Run() // check analog airspeed adc_poll(); - diff_pres_poll(); - if (_sensor_combined.timestamp != _sensor_combined_prev_timestamp) { _voted_sensors_update.setRelativeTimestamps(_sensor_combined); @@ -612,6 +507,7 @@ void Sensors::Run() // when not adding sensors poll for param updates if (!_armed && hrt_elapsed_time(&_last_config_update) > 500_ms) { _voted_sensors_update.initializeSensors(); + InitializeAirspeed(); InitializeVehicleAirData(); InitializeVehicleIMU(); InitializeVehicleGPSPosition(); @@ -691,9 +587,10 @@ int Sensors::print_status() _vehicle_air_data->PrintStatus(); } - PX4_INFO_RAW("\n"); - PX4_INFO("Airspeed status:"); - _airspeed_validator.print(); + if (_airspeed) { + PX4_INFO_RAW("\n"); + _airspeed->PrintStatus(); + } PX4_INFO_RAW("\n"); _vehicle_acceleration.PrintStatus();