diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index a2da67cf8d..660083575d 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -133,6 +133,12 @@ then ms5525 start -X -q ets_airspeed start -X -q + + # analog differential pressure + if param greater -s SENS_DPRES_ANSC 0 + then + analog_diffpres_main start + fi fi fi fi diff --git a/boards/px4/raspberrypi/src/board_config.h b/boards/px4/raspberrypi/src/board_config.h index 8b8921177f..525a40a706 100644 --- a/boards/px4/raspberrypi/src/board_config.h +++ b/boards/px4/raspberrypi/src/board_config.h @@ -51,7 +51,5 @@ #define ADC_BATTERY_CURRENT_CHANNEL -1 #define ADC_AIRSPEED_VOLTAGE_CHANNEL 2 -#define ADC_DP_V_DIV 1.0f - #include #include diff --git a/boards/scumaker/pilotpi/src/board_config.h b/boards/scumaker/pilotpi/src/board_config.h index 4ce9b73e38..1f2fb1a257 100644 --- a/boards/scumaker/pilotpi/src/board_config.h +++ b/boards/scumaker/pilotpi/src/board_config.h @@ -52,7 +52,6 @@ #define ADC_AIRSPEED_VOLTAGE_CHANNEL 2 #define BOARD_BATTERY1_V_DIV 5.7f // 1K + 4.7K -#define ADC_DP_V_DIV 1.0f #define BOARD_ADC_OPEN_CIRCUIT_V 5.3f // Powered from USB diff --git a/src/drivers/differential_pressure/CMakeLists.txt b/src/drivers/differential_pressure/CMakeLists.txt index 4f951d949a..5ea0affaed 100644 --- a/src/drivers/differential_pressure/CMakeLists.txt +++ b/src/drivers/differential_pressure/CMakeLists.txt @@ -31,6 +31,7 @@ # ############################################################################ +add_subdirectory(analog_diffpres) add_subdirectory(ets) add_subdirectory(ms4515) add_subdirectory(ms4525) diff --git a/src/drivers/differential_pressure/analog_diffpres/AnalogDifferentialPressure.cpp b/src/drivers/differential_pressure/analog_diffpres/AnalogDifferentialPressure.cpp new file mode 100644 index 0000000000..ef3421c2a4 --- /dev/null +++ b/src/drivers/differential_pressure/analog_diffpres/AnalogDifferentialPressure.cpp @@ -0,0 +1,205 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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 + * 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 "AnalogDifferentialPressure.hpp" + +#include + +AnalogDifferentialPressure::AnalogDifferentialPressure(int16_t adc_channel) : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default), + _adc_channel(adc_channel) +{ +} + +int AnalogDifferentialPressure::init() +{ + _adc_report_sub.registerCallback(); + ScheduleNow(); + + return PX4_OK; +} + +void AnalogDifferentialPressure::Run() +{ + // check for parameter updates + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + // update parameters from storage + updateParams(); + } + + perf_begin(_sample_perf); + + if (_param_sens_dpres_ansc.get() > 0.0f) { + adc_report_s adc; + + if (_adc_report_sub.update(&adc)) { + /* Read add channels we got */ + for (unsigned i = 0; i < PX4_MAX_ADC_CHANNELS; i++) { + if (_adc_channel == adc.channel_id[i]) { + + // The scale factor defined by HW's resistive divider (Rt+Rb)/ Rb +#if defined(ADC_DP_V_DIV) + static constexpr float scale = ADC_DP_V_DIV; +#else + static constexpr float scale = 1.0f; +#endif + + /* calculate airspeed, raw is the difference from */ + const float voltage = (float)(adc.raw_data[i]) * adc.v_ref / adc.resolution * scale; + + /** + * The voltage divider pulls the signal down, only act on + * a valid voltage from a connected sensor. Also assume a non- + * zero offset from the sensor if its connected. + * + * Notice: This won't work on devices which have PGA controlled + * vref. Those devices require no divider at all. + */ + if (voltage > 0.4f) { + const float differential_pressure_pa = voltage * _param_sens_dpres_ansc.get(); + + if (fabsf(differential_pressure_pa - _differential_pressure_pa_last) > 0.01f) { + // only publish changes + sensor_differential_pressure_s diff_pres{}; + diff_pres.device_id = 0xAA; // TODO DRV_DIFF_PRESS_DEVTYPE_MS4525 + diff_pres.timestamp_sample = adc.timestamp; + diff_pres.differential_pressure_pa = differential_pressure_pa; + diff_pres.temperature = NAN; + diff_pres.timestamp = hrt_absolute_time(); + _diff_pres_pub.publish(diff_pres); + + _differential_pressure_pa_last = differential_pressure_pa; + } + } + + break; + } + } + + PX4_DEBUG("ADC channel %d not found", _adc_channel); + } + } + + perf_end(_sample_perf); +} + +int AnalogDifferentialPressure::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int AnalogDifferentialPressure::task_spawn(int argc, char *argv[]) +{ +#if defined(ADC_AIRSPEED_VOLTAGE_CHANNEL) + int16_t adc_channel = ADC_AIRSPEED_VOLTAGE_CHANNEL; +#else + int16_t adc_channel = -1; +#endif + + bool error_flag = false; + int myoptind = 1; + int ch; + const char *myoptarg = nullptr; + + while ((ch = px4_getopt(argc, argv, "c:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'c': + adc_channel = strtoul(myoptarg, nullptr, 10); + break; + + case '?': + error_flag = true; + break; + + default: + PX4_WARN("unrecognized flag"); + error_flag = true; + break; + } + } + + if (error_flag) { + return PX4_ERROR; + } + + AnalogDifferentialPressure *instance = new AnalogDifferentialPressure(adc_channel); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init() == PX4_OK) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +int AnalogDifferentialPressure::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +ADC driver. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("analog_diffpres", "driver"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAM_INT('c', -1, 0, 32, "ADC channel", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int analog_diffpres_main(int argc, char *argv[]) +{ + return AnalogDifferentialPressure::main(argc, argv); +} diff --git a/src/drivers/differential_pressure/analog_diffpres/AnalogDifferentialPressure.hpp b/src/drivers/differential_pressure/analog_diffpres/AnalogDifferentialPressure.hpp new file mode 100644 index 0000000000..d17d55ed5c --- /dev/null +++ b/src/drivers/differential_pressure/analog_diffpres/AnalogDifferentialPressure.hpp @@ -0,0 +1,93 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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 + * 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. + * + ****************************************************************************/ + +/** + * + * Driver for analog differential pressure sensor + * + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +class AnalogDifferentialPressure : public ModuleBase, public ModuleParams, + public px4::ScheduledWorkItem +{ +public: + AnalogDifferentialPressure(int16_t adc_channel = -1); + ~AnalogDifferentialPressure() override = default; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + int init(); + +private: + + void Run() override; + + uORB::SubscriptionCallbackWorkItem _adc_report_sub{this, ORB_ID(adc_report)}; + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::PublicationMulti _diff_pres_pub{ORB_ID(sensor_differential_pressure)}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_sens_dpres_ansc + ) + + perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")}; + perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com err")}; + + float _differential_pressure_pa_last{0.f}; + + int16_t _adc_channel{-1}; +}; diff --git a/src/drivers/differential_pressure/analog_diffpres/CMakeLists.txt b/src/drivers/differential_pressure/analog_diffpres/CMakeLists.txt new file mode 100644 index 0000000000..b23fd2bf3c --- /dev/null +++ b/src/drivers/differential_pressure/analog_diffpres/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2013-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 +# 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_module( + MODULE drivers__differential_pressure__analog_diffpres + MAIN analog_diffpres + SRCS + AnalogDifferentialPressure.cpp + AnalogDifferentialPressure.hpp + DEPENDS + px4_work_queue + ) diff --git a/src/drivers/differential_pressure/analog_diffpres/params.c b/src/drivers/differential_pressure/analog_diffpres/params.c new file mode 100644 index 0000000000..ddc22170b7 --- /dev/null +++ b/src/drivers/differential_pressure/analog_diffpres/params.c @@ -0,0 +1,50 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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 + * 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. + * + ****************************************************************************/ + +/** + * Differential pressure sensor analog scaling + * + * Pick the appropriate scaling from the datasheet. + * this number defines the (linear) conversion from voltage + * to Pascal (pa). For the MPXV7002DP this is 1000. + * + * NOTE: If the sensor always registers zero, try switching + * the static and dynamic tubes. + * + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(SENS_DPRES_ANSC, 0); + + +// SENS_DPRES_ANLG_EN +// SENS_DPRES_ANLG_CHANNEL? diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 1ed991e7ba..d12f64f6c0 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -31,20 +31,6 @@ * ****************************************************************************/ -/** - * Differential pressure sensor analog scaling - * - * Pick the appropriate scaling from the datasheet. - * this number defines the (linear) conversion from voltage - * to Pascal (pa). For the MPXV7002DP this is 1000. - * - * NOTE: If the sensor always registers zero, try switching - * the static and dynamic tubes. - * - * @group Sensor Calibration - */ -PARAM_DEFINE_FLOAT(SENS_DPRES_ANSC, 0); - /** * Board rotation * diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 00babd9b53..657a1fcbbb 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -41,7 +41,6 @@ * @author Beat Küng */ -#include #include #include #include @@ -59,7 +58,6 @@ #include #include #include -#include #include #include #include @@ -124,11 +122,6 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ -#if defined(ADC_AIRSPEED_VOLTAGE_CHANNEL) - uORB::Subscription _adc_report_sub {ORB_ID(adc_report)}; - uORB::PublicationMulti _diff_pres_pub{ORB_ID(sensor_differential_pressure)}; /**< differential_pressure */ -#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ - VotedSensorsUpdate _voted_sensors_update; VehicleAcceleration _vehicle_acceleration; @@ -146,14 +139,6 @@ private: */ void parameter_update_poll(bool forced = false); - /** - * Poll the ADC and update readings to suit. - * - * @param raw Combined sensor data structure into which - * data should be returned. - */ - void adc_poll(); - void InitializeAirspeed(); void InitializeVehicleAirData(); void InitializeVehicleGPSPosition(); @@ -161,10 +146,6 @@ private: void InitializeVehicleMagnetometer(); DEFINE_PARAMETERS( -#if defined(ADC_AIRSPEED_VOLTAGE_CHANNEL) - (ParamFloat) _param_sens_dpres_ansc, -#endif // ADC_AIRSPEED_VOLTAGE_CHANNEL - (ParamBool) _param_sys_has_baro, (ParamBool) _param_sys_has_mag, (ParamBool) _param_sens_imu_mode @@ -252,49 +233,6 @@ void Sensors::parameter_update_poll(bool forced) } } -void Sensors::adc_poll() -{ - /* only read if not in HIL mode */ - if (_hil_enabled) { - return; - } - -#if defined(ADC_AIRSPEED_VOLTAGE_CHANNEL) - - if (_param_sens_dpres_ansc.get() > 0.0f) { - adc_report_s adc; - - if (_adc_report_sub.update(&adc)) { - /* Read add channels we got */ - for (unsigned i = 0; i < PX4_MAX_ADC_CHANNELS; i++) { - if (ADC_AIRSPEED_VOLTAGE_CHANNEL == adc.channel_id[i]) { - /* calculate airspeed, raw is the difference from */ - const float voltage = (float)(adc.raw_data[i]) * adc.v_ref / adc.resolution * ADC_DP_V_DIV; - - /** - * The voltage divider pulls the signal down, only act on - * a valid voltage from a connected sensor. Also assume a non- - * zero offset from the sensor if its connected. - * - * Notice: This won't work on devices which have PGA controlled - * vref. Those devices require no divider at all. - */ - if (voltage > 0.4f) { - sensor_differential_pressure_s diff_pres{}; - diff_pres.timestamp_sample = adc.timestamp; - diff_pres.differential_pressure_pa = voltage * _param_sens_dpres_ansc.get(); - diff_pres.temperature = NAN; - diff_pres.timestamp = hrt_absolute_time(); - _diff_pres_pub.publish(diff_pres); - } - } - } - } - } - -#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ -} - void Sensors::InitializeAirspeed() { if (_airspeed == nullptr) { @@ -437,9 +375,6 @@ void Sensors::Run() _voted_sensors_update.sensorsPoll(_sensor_combined); - // check analog airspeed - adc_poll(); - if (_sensor_combined.timestamp != _sensor_combined_prev_timestamp) { _voted_sensors_update.setRelativeTimestamps(_sensor_combined);