From f390f52058327afe6a2ab636cbfeca023b1ac4b6 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 27 Dec 2021 11:01:25 -0500 Subject: [PATCH] drivers/differential_pressure: remove lib/drivers/airspeed dependency and cleanup - split ms4525_airspeed into separate ms4515 and ms4525 drivers --- ROMFS/px4fmu_common/init.d/rc.sensors | 34 +- ROMFS/px4fmu_test/init.d/rc.sensors | 6 +- boards/ark/cannode/init/rc.board_sensors | 16 +- boards/av/x-v1/init/rc.board_sensors | 2 +- boards/px4/fmu-v2/fixedwing.px4board | 2 +- posix-configs/rpi/pilotpi_fw.config | 2 +- posix-configs/rpi/px4_fw.config | 2 +- src/drivers/differential_pressure/Kconfig | 5 +- .../differential_pressure/ets/CMakeLists.txt | 10 +- .../ets/{ets_airspeed.cpp => ETSAirspeed.cpp} | 168 +++------ .../ets/ETSAirspeed.hpp} | 95 +++-- .../ets/ets_airspeed_main.cpp | 77 +++++ .../{ms4525 => ms4515}/CMakeLists.txt | 14 +- .../differential_pressure/ms4515/Kconfig | 5 + .../differential_pressure/ms4515/MS4515.cpp | 222 ++++++++++++ .../differential_pressure/ms4515/MS4515.hpp | 97 ++++++ .../ms4515/ms4515_main.cpp | 77 +++++ .../{ms4525 => ms4515}/parameters.c | 4 +- .../differential_pressure/ms4525/Kconfig | 5 - .../ms4525/ms4525_airspeed.cpp | 327 ------------------ .../{ms5525 => ms4525do}/CMakeLists.txt | 15 +- .../differential_pressure/ms4525do/Kconfig | 5 + .../ms4525do/MS4525DO.cpp | 221 ++++++++++++ .../ms4525do/MS4525DO.hpp | 97 ++++++ .../ms4525do/ms4525do_main.cpp | 77 +++++ .../{ms5525 => ms4525do}/parameters.c | 6 +- .../differential_pressure/ms5525/Kconfig | 5 - .../{ => ms5525dso}/CMakeLists.txt | 17 +- .../differential_pressure/ms5525dso/Kconfig | 5 + .../MS5525.cpp => ms5525dso/MS5525DSO.cpp} | 110 ++++-- .../MS5525.hpp => ms5525dso/MS5525DSO.hpp} | 56 +-- .../ms5525dso_main.cpp} | 29 +- .../ms5525dso/parameters.c} | 56 +-- .../sdp3x/CMakeLists.txt | 13 +- .../differential_pressure/sdp3x/SDP3X.cpp | 102 +++--- .../differential_pressure/sdp3x/SDP3X.hpp | 51 +-- .../sdp3x/{SDP3X_main.cpp => sdp3x_main.cpp} | 22 +- src/drivers/drv_sensor.h | 21 +- src/lib/drivers/CMakeLists.txt | 1 - src/lib/drivers/airspeed/CMakeLists.txt | 35 -- src/lib/parameters/param_translation.cpp | 15 + .../commander/airspeed_calibration.cpp | 48 ++- src/modules/mavlink/mavlink_receiver.cpp | 7 +- .../mavlink/streams/SCALED_PRESSURE.hpp | 10 +- .../mavlink/streams/SCALED_PRESSURE2.hpp | 10 +- .../mavlink/streams/SCALED_PRESSURE3.hpp | 10 +- src/modules/sensors/sensors.cpp | 2 +- src/modules/sih/sih.cpp | 5 +- src/modules/simulator/simulator.h | 2 - src/modules/simulator/simulator_mavlink.cpp | 6 +- 50 files changed, 1350 insertions(+), 879 deletions(-) rename src/drivers/differential_pressure/ets/{ets_airspeed.cpp => ETSAirspeed.cpp} (51%) rename src/{lib/drivers/airspeed/airspeed.cpp => drivers/differential_pressure/ets/ETSAirspeed.hpp} (52%) create mode 100644 src/drivers/differential_pressure/ets/ets_airspeed_main.cpp rename src/drivers/differential_pressure/{ms4525 => ms4515}/CMakeLists.txt (89%) create mode 100644 src/drivers/differential_pressure/ms4515/Kconfig create mode 100644 src/drivers/differential_pressure/ms4515/MS4515.cpp create mode 100644 src/drivers/differential_pressure/ms4515/MS4515.hpp create mode 100644 src/drivers/differential_pressure/ms4515/ms4515_main.cpp rename src/drivers/differential_pressure/{ms4525 => ms4515}/parameters.c (94%) delete mode 100644 src/drivers/differential_pressure/ms4525/Kconfig delete mode 100644 src/drivers/differential_pressure/ms4525/ms4525_airspeed.cpp rename src/drivers/differential_pressure/{ms5525 => ms4525do}/CMakeLists.txt (89%) create mode 100644 src/drivers/differential_pressure/ms4525do/Kconfig create mode 100644 src/drivers/differential_pressure/ms4525do/MS4525DO.cpp create mode 100644 src/drivers/differential_pressure/ms4525do/MS4525DO.hpp create mode 100644 src/drivers/differential_pressure/ms4525do/ms4525do_main.cpp rename src/drivers/differential_pressure/{ms5525 => ms4525do}/parameters.c (90%) delete mode 100644 src/drivers/differential_pressure/ms5525/Kconfig rename src/drivers/differential_pressure/{ => ms5525dso}/CMakeLists.txt (86%) create mode 100644 src/drivers/differential_pressure/ms5525dso/Kconfig rename src/drivers/differential_pressure/{ms5525/MS5525.cpp => ms5525dso/MS5525DSO.cpp} (80%) rename src/drivers/differential_pressure/{ms5525/MS5525.hpp => ms5525dso/MS5525DSO.hpp} (71%) rename src/drivers/differential_pressure/{ms5525/MS5525_main.cpp => ms5525dso/ms5525dso_main.cpp} (81%) rename src/{lib/drivers/airspeed/airspeed.h => drivers/differential_pressure/ms5525dso/parameters.c} (60%) rename src/drivers/differential_pressure/sdp3x/{SDP3X_main.cpp => sdp3x_main.cpp} (86%) delete mode 100644 src/lib/drivers/airspeed/CMakeLists.txt diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 0345a507bb..2694eb3c64 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -129,13 +129,31 @@ fi # Sensirion SDP3X differential pressure sensor external I2C if param compare -s SENS_EN_SDP3X 1 then - if ! sdp3x_airspeed start -X + if ! sdp3x start -X then # try another common address - sdp3x_airspeed start -X -a 0x22 + sdp3x start -X -a 0x22 fi fi +# TE MS4515 differential pressure sensor external I2C +if param compare -s SENS_EN_MS4515 1 +then + ms4515 start -X +fi + +# TE MS4525DO differential pressure sensor external I2C +if param compare -s SENS_EN_MS4525DO 1 +then + ms4525do start -X +fi + +# TE MS5525DSO differential pressure sensor external I2C +if param compare -s SENS_EN_MS5525DS 1 +then + ms5525dso start -X +fi + # SHT3x temperature and hygrometer sensor, external I2C if param compare -s SENS_EN_SHT3X 1 then @@ -143,18 +161,6 @@ then sht3x start -X -a 0x45 fi -# TE MS4525 differential pressure sensor external I2C -if param compare -s SENS_EN_MS4525 1 -then - ms4525_airspeed start -X -fi - -# TE MS5525 differential pressure sensor external I2C -if param compare -s SENS_EN_MS5525 1 -then - ms5525_airspeed start -X -fi - # IR-LOCK sensor external I2C if param compare -s SENS_EN_IRLOCK 1 then diff --git a/ROMFS/px4fmu_test/init.d/rc.sensors b/ROMFS/px4fmu_test/init.d/rc.sensors index 4291deaf29..ab856ee2a7 100644 --- a/ROMFS/px4fmu_test/init.d/rc.sensors +++ b/ROMFS/px4fmu_test/init.d/rc.sensors @@ -16,15 +16,15 @@ if board_adc start then fi -if sdp3x_airspeed start -X +if sdp3x start -X then fi -if ms5525_airspeed start -X +if ms5525dso start -X then fi -if ms4525_airspeed start -X +if ms4525do start -X then fi diff --git a/boards/ark/cannode/init/rc.board_sensors b/boards/ark/cannode/init/rc.board_sensors index c14e6a4cbc..bbddf5f5d4 100644 --- a/boards/ark/cannode/init/rc.board_sensors +++ b/boards/ark/cannode/init/rc.board_sensors @@ -61,10 +61,10 @@ fi # Sensirion SDP3X differential pressure sensor external I2C if param compare -s SENS_EN_SDP3X 1 then - if ! sdp3x_airspeed start -X + if ! sdp3x start -X then # try another common address - sdp3x_airspeed start -X -a 0x22 + sdp3x start -X -a 0x22 fi fi @@ -75,16 +75,16 @@ then sht3x start -X -a 0x45 fi -# TE MS4525 differential pressure sensor external I2C -if param compare -s SENS_EN_MS4525 1 +# TE MS4525DO differential pressure sensor external I2C +if param compare -s SENS_EN_MS4525DO 1 then - ms4525_airspeed start -X + ms4525do start -X fi -# TE MS5525 differential pressure sensor external I2C -if param compare -s SENS_EN_MS5525 1 +# TE MS5525DSO differential pressure sensor external I2C +if param compare -s SENS_EN_MS5525DS 1 then - ms5525_airspeed start -X + ms5525dso start -X fi # IR-LOCK sensor external I2C diff --git a/boards/av/x-v1/init/rc.board_sensors b/boards/av/x-v1/init/rc.board_sensors index 215d39b042..86caac97af 100644 --- a/boards/av/x-v1/init/rc.board_sensors +++ b/boards/av/x-v1/init/rc.board_sensors @@ -11,7 +11,7 @@ lps22hb -s start lsm303agr -s -R 4 start -ms4525_airspeed -T 4515 -I -b 3 start +ms4515 -I -b 3 start if ! param greater SENS_EN_PMW3901 0 then diff --git a/boards/px4/fmu-v2/fixedwing.px4board b/boards/px4/fmu-v2/fixedwing.px4board index 0f04516d6d..ebfab39394 100644 --- a/boards/px4/fmu-v2/fixedwing.px4board +++ b/boards/px4/fmu-v2/fixedwing.px4board @@ -4,7 +4,7 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n CONFIG_MODULES_MC_POS_CONTROL=n CONFIG_MODULES_MC_RATE_CONTROL=n CONFIG_SYSTEMCMDS_BL_UPDATE=n -CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_MS4525=y +CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_MS4525DO=y CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_I2C=y CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_SERIAL=y CONFIG_DRIVERS_DISTANCE_SENSOR_LL40LS=y diff --git a/posix-configs/rpi/pilotpi_fw.config b/posix-configs/rpi/pilotpi_fw.config index 5c8b5c0190..ac11697725 100644 --- a/posix-configs/rpi/pilotpi_fw.config +++ b/posix-configs/rpi/pilotpi_fw.config @@ -50,7 +50,7 @@ gps start -d /dev/ttySC0 -i uart -p ubx -s #ist8310 start -X # Airspeed -ms4525_airspeed start -X +ms4525do start -X rc_input start -d /dev/ttyAMA0 diff --git a/posix-configs/rpi/px4_fw.config b/posix-configs/rpi/px4_fw.config index dfdc176c04..28f1c4ce7e 100644 --- a/posix-configs/rpi/px4_fw.config +++ b/posix-configs/rpi/px4_fw.config @@ -33,7 +33,7 @@ board_adc start battery_status start gps start -d /dev/spidev0.0 -i spi -p ubx -ms4525_airspeed start -X +ms4525do start -X rc_update start sensors start commander start diff --git a/src/drivers/differential_pressure/Kconfig b/src/drivers/differential_pressure/Kconfig index bd7b3c0e1d..a34dc5a40e 100644 --- a/src/drivers/differential_pressure/Kconfig +++ b/src/drivers/differential_pressure/Kconfig @@ -3,8 +3,9 @@ menu "Differential pressure" bool "Common differential pressure module's" default n select DRIVERS_DIFFERENTIAL_PRESSURE_ETS - select DRIVERS_DIFFERENTIAL_PRESSURE_MS4525 - select DRIVERS_DIFFERENTIAL_PRESSURE_MS5525 + select DRIVERS_DIFFERENTIAL_PRESSURE_MS4515 + select DRIVERS_DIFFERENTIAL_PRESSURE_MS4525DO + select DRIVERS_DIFFERENTIAL_PRESSURE_MS5525DSO select DRIVERS_DIFFERENTIAL_PRESSURE_SDP3X ---help--- Enable default set of differential pressure drivers diff --git a/src/drivers/differential_pressure/ets/CMakeLists.txt b/src/drivers/differential_pressure/ets/CMakeLists.txt index fe77c1172c..23dc3f26aa 100644 --- a/src/drivers/differential_pressure/ets/CMakeLists.txt +++ b/src/drivers/differential_pressure/ets/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# Copyright (c) 2015-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,11 +31,13 @@ # ############################################################################ px4_add_module( - MODULE drivers__ets_airspeed + MODULE drivers__differential_pressure__ets_airspeed MAIN ets_airspeed COMPILE_FLAGS SRCS - ets_airspeed.cpp + ets_airspeed_main.cpp + ETSAirspeed.cpp + ETSAirspeed.hpp DEPENDS - drivers__airspeed + px4_work_queue ) diff --git a/src/drivers/differential_pressure/ets/ets_airspeed.cpp b/src/drivers/differential_pressure/ets/ETSAirspeed.cpp similarity index 51% rename from src/drivers/differential_pressure/ets/ets_airspeed.cpp rename to src/drivers/differential_pressure/ets/ETSAirspeed.cpp index 8f0213ef78..93193a2721 100644 --- a/src/drivers/differential_pressure/ets/ets_airspeed.cpp +++ b/src/drivers/differential_pressure/ets/ETSAirspeed.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * 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 @@ -31,67 +31,36 @@ * ****************************************************************************/ -/** - * @file ets_airspeed.cpp - * @author Simon Wilks - * - * Driver for the Eagle Tree Airspeed V3 connected via I2C. - */ +#include "ETSAirspeed.hpp" -#include - -#include -#include -#include -#include - -/* I2C bus address */ -#define I2C_ADDRESS 0x75 /* 7-bit address. 8-bit address is 0xEA */ - -/* Register address */ -#define READ_CMD 0x07 /* Read the data */ - -/** - * The Eagle Tree Airspeed V3 cannot provide accurate reading below speeds of 15km/h. - * You can set this value to 12 if you want a zero reading below 15km/h. - */ -#define MIN_ACCURATE_DIFF_PRES_PA 0 - -/* Measurement rate is 100Hz */ -#define CONVERSION_INTERVAL (1000000 / 100) /* microseconds */ - -class ETSAirspeed : public Airspeed, public I2CSPIDriver +ETSAirspeed::ETSAirspeed(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config) { -public: - ETSAirspeed(const I2CSPIDriverConfig &config); - - virtual ~ETSAirspeed() = default; - - static void print_usage(); - - void RunImpl(); - - int init() override; -protected: - int measure() override; - int collect() override; -}; - -/* - * Driver 'main' command. - */ -extern "C" __EXPORT int ets_airspeed_main(int argc, char *argv[]); - -ETSAirspeed::ETSAirspeed(const I2CSPIDriverConfig &config) - : Airspeed(config.bus, config.bus_frequency, config.i2c_address, CONVERSION_INTERVAL), - I2CSPIDriver(config) -{ - _device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_MS4525; } -int ETSAirspeed::init() +ETSAirspeed::~ETSAirspeed() { - int ret = Airspeed::init(); + perf_free(_sample_perf); + perf_free(_comms_errors); +} + +int ETSAirspeed::probe() +{ + _retries = 1; + int ret = measure(); + + return ret; +} + +int ETSAirspeed::init() +{ + int ret = I2C::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("I2C::init failed (%i)", ret); + return ret; + } if (ret == PX4_OK) { ScheduleNow(); @@ -100,17 +69,13 @@ int ETSAirspeed::init() return ret; } - -int -ETSAirspeed::measure() +int ETSAirspeed::measure() { - int ret; - /* * Send the command to begin a measurement. */ uint8_t cmd = READ_CMD; - ret = transfer(&cmd, 1, nullptr, 0); + int ret = transfer(&cmd, 1, nullptr, 0); if (OK != ret) { perf_count(_comms_errors); @@ -119,8 +84,7 @@ ETSAirspeed::measure() return ret; } -int -ETSAirspeed::collect() +int ETSAirspeed::collect() { /* read from the sensor */ uint8_t val[2] = {0, 0}; @@ -134,36 +98,32 @@ ETSAirspeed::collect() return ret; } - float diff_pres_pa_raw = (float)(val[1] << 8 | val[0]); + float diff_press_pa = (float)(val[1] << 8 | val[0]); - if (diff_pres_pa_raw < FLT_EPSILON) { + if (diff_press_pa < FLT_EPSILON) { // a zero value indicates no measurement // since the noise floor has been arbitrarily killed // it defeats our stuck sensor detection - the best we // can do is to output some numerical noise to show // that we are still correctly sampling. - diff_pres_pa_raw = 0.001f * (timestamp_sample & 0x01); + diff_press_pa = 0.001f * (timestamp_sample & 0x01); } - differential_pressure_s report{}; - report.timestamp_sample = timestamp_sample; - report.device_id = _device_id.devid; - report.differential_pressure_pa = diff_pres_pa_raw; - report.temperature = NAN; - report.error_count = perf_event_count(_comms_errors); - report.timestamp = hrt_absolute_time(); - - _airspeed_pub.publish(report); - - ret = OK; + differential_pressure_s differential_pressure{}; + differential_pressure.timestamp_sample = timestamp_sample; + differential_pressure.device_id = get_device_id(); + differential_pressure.differential_pressure_pa = diff_press_pa; + differential_pressure.temperature = NAN; + differential_pressure.error_count = perf_event_count(_comms_errors); + differential_pressure.timestamp = hrt_absolute_time(); + _differential_pressure_pub.publish(differential_pressure); perf_end(_sample_perf); - return ret; + return PX4_OK; } -void -ETSAirspeed::RunImpl() +void ETSAirspeed::RunImpl() { int ret; @@ -212,47 +172,3 @@ ETSAirspeed::RunImpl() /* schedule a fresh cycle call when the measurement is done */ ScheduleDelayed(CONVERSION_INTERVAL); } - -void -ETSAirspeed::print_usage() -{ - PRINT_MODULE_USAGE_NAME("ets_airspeed", "driver"); - PRINT_MODULE_USAGE_SUBCATEGORY("airspeed_sensor"); - PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); - PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x75); - PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); -} - -int -ets_airspeed_main(int argc, char *argv[]) -{ - using ThisDriver = ETSAirspeed; - BusCLIArguments cli{true, false}; - cli.default_i2c_frequency = 100000; - cli.i2c_address = I2C_ADDRESS; - - const char *verb = cli.parseDefaultArguments(argc, argv); - - if (!verb) { - ThisDriver::print_usage(); - return -1; - } - - BusInstanceIterator iterator(MODULE_NAME, cli, DRV_DIFF_PRESS_DEVTYPE_ETS3); - - if (!strcmp(verb, "start")) { - return ThisDriver::module_start(cli, iterator); - } - - if (!strcmp(verb, "stop")) { - return ThisDriver::module_stop(iterator); - } - - if (!strcmp(verb, "status")) { - return ThisDriver::module_status(iterator); - } - - ThisDriver::print_usage(); - return -1; -} diff --git a/src/lib/drivers/airspeed/airspeed.cpp b/src/drivers/differential_pressure/ets/ETSAirspeed.hpp similarity index 52% rename from src/lib/drivers/airspeed/airspeed.cpp rename to src/drivers/differential_pressure/ets/ETSAirspeed.hpp index 61bfbc51a7..7c3281abcc 100644 --- a/src/lib/drivers/airspeed/airspeed.cpp +++ b/src/drivers/differential_pressure/ets/ETSAirspeed.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * 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 @@ -32,68 +32,61 @@ ****************************************************************************/ /** - * @file airspeed.cpp - * @author Simon Wilks - * @author Lorenz Meier - * + * Driver for the Eagle Tree Airspeed V3 connected via I2C. */ -#include -#include +#pragma once -#include - -#include -#include -#include +#include #include - +#include +#include +#include +#include #include -#include +static constexpr uint32_t I2C_SPEED = 100 * 1000; // 100 kHz I2C serial interface +static constexpr uint8_t I2C_ADDRESS_DEFAULT = 0x75; /* 7-bit address. 8-bit address is 0xEA */ -Airspeed::Airspeed(int bus, int bus_frequency, int address, unsigned conversion_interval) : - I2C(0, "Airspeed", bus, address, bus_frequency), - _sensor_ok(false), - _measure_interval(conversion_interval), - _collect_phase(false), - _conversion_interval(conversion_interval), - _sample_perf(perf_alloc(PC_ELAPSED, "aspd_read")), - _comms_errors(perf_alloc(PC_COUNT, "aspd_com_err")) +/* Register address */ +#define READ_CMD 0x07 /* Read the data */ + +/** + * The Eagle Tree Airspeed V3 cannot provide accurate reading below speeds of 15km/h. + * You can set this value to 12 if you want a zero reading below 15km/h. + */ +#define MIN_ACCURATE_DIFF_PRES_PA 0 + +/* Measurement rate is 100Hz */ +#define CONVERSION_INTERVAL (1000000 / 100) /* microseconds */ + +class ETSAirspeed : public device::I2C, public I2CSPIDriver { -} +public: + ETSAirspeed(const I2CSPIDriverConfig &config); + ~ETSAirspeed() override; -Airspeed::~Airspeed() -{ - // free perf counters - perf_free(_sample_perf); - perf_free(_comms_errors); -} + static void print_usage(); -int -Airspeed::init() -{ - /* do I2C init (and probe) first */ - if (I2C::init() != PX4_OK) { - return PX4_ERROR; - } + void RunImpl(); - /* advertise sensor topic, measure manually to initialize valid report */ - measure(); + int init() override; - return PX4_OK; -} +private: + int probe() override; -int -Airspeed::probe() -{ - /* on initial power up the device may need more than one retry - for detection. Once it is running the number of retries can - be reduced - */ - _retries = 1; - int ret = measure(); + int measure(); + int collect(); - return ret; -} + uint32_t _measure_interval{CONVERSION_INTERVAL}; + uint32_t _conversion_interval{CONVERSION_INTERVAL}; + + bool _sensor_ok{false}; + bool _collect_phase{false}; + + uORB::PublicationMulti _differential_pressure_pub{ORB_ID(differential_pressure)}; + + perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")}; + perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": communication errors")}; +}; diff --git a/src/drivers/differential_pressure/ets/ets_airspeed_main.cpp b/src/drivers/differential_pressure/ets/ets_airspeed_main.cpp new file mode 100644 index 0000000000..06c5605b1c --- /dev/null +++ b/src/drivers/differential_pressure/ets/ets_airspeed_main.cpp @@ -0,0 +1,77 @@ +/**************************************************************************** + * + * 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 "ETSAirspeed.hpp" + +#include +#include + +void ETSAirspeed::print_usage() +{ + PRINT_MODULE_USAGE_NAME("ets_airspeed", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("airspeed_sensor"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x75); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +extern "C" int ets_airspeed_main(int argc, char *argv[]) +{ + using ThisDriver = ETSAirspeed; + BusCLIArguments cli{true, false}; + cli.i2c_address = I2C_ADDRESS_DEFAULT; + cli.default_i2c_frequency = I2C_SPEED; + + const char *verb = cli.parseDefaultArguments(argc, argv); + + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_DIFF_PRESS_DEVTYPE_ETS3); + + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); + + } else if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); + + } else if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + ThisDriver::print_usage(); + return -1; +} diff --git a/src/drivers/differential_pressure/ms4525/CMakeLists.txt b/src/drivers/differential_pressure/ms4515/CMakeLists.txt similarity index 89% rename from src/drivers/differential_pressure/ms4525/CMakeLists.txt rename to src/drivers/differential_pressure/ms4515/CMakeLists.txt index bac360d01e..e3e1d1ad55 100644 --- a/src/drivers/differential_pressure/ms4525/CMakeLists.txt +++ b/src/drivers/differential_pressure/ms4515/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# Copyright (c) 2015-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 @@ -30,13 +30,15 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ + px4_add_module( - MODULE drivers__ms4525_airspeed - MAIN ms4525_airspeed + MODULE drivers__differential_pressure__ms4515 + MAIN ms4515 COMPILE_FLAGS SRCS - ms4525_airspeed.cpp + ms4515_main.cpp + MS4515.cpp + MS4515.hpp DEPENDS - drivers__airspeed - mathlib + px4_work_queue ) diff --git a/src/drivers/differential_pressure/ms4515/Kconfig b/src/drivers/differential_pressure/ms4515/Kconfig new file mode 100644 index 0000000000..b344e1d786 --- /dev/null +++ b/src/drivers/differential_pressure/ms4515/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_DIFFERENTIAL_PRESSURE_MS4515 + bool "ms4515" + default n + ---help--- + Enable support for ms4515 diff --git a/src/drivers/differential_pressure/ms4515/MS4515.cpp b/src/drivers/differential_pressure/ms4515/MS4515.cpp new file mode 100644 index 0000000000..8abac257f7 --- /dev/null +++ b/src/drivers/differential_pressure/ms4515/MS4515.cpp @@ -0,0 +1,222 @@ +/**************************************************************************** + * + * 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 "MS4515.hpp" + +MS4515::MS4515(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config) +{ +} + +MS4515::~MS4515() +{ + perf_free(_sample_perf); + perf_free(_comms_errors); +} + +int MS4515::probe() +{ + uint8_t cmd = 0; + int ret = transfer(&cmd, 1, nullptr, 0); + + return ret; +} + +int MS4515::init() +{ + int ret = I2C::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("I2C::init failed (%i)", ret); + return ret; + } + + if (ret == PX4_OK) { + ScheduleNow(); + } + + return ret; +} + +int MS4515::measure() +{ + // Send the command to begin a measurement. + uint8_t cmd = 0; + int ret = transfer(&cmd, 1, nullptr, 0); + + if (OK != ret) { + perf_count(_comms_errors); + } + + return ret; +} + +int MS4515::collect() +{ + /* read from the sensor */ + perf_begin(_sample_perf); + + const hrt_abstime timestamp_sample = hrt_absolute_time(); + + uint8_t val[4] {}; + int ret = transfer(nullptr, 0, &val[0], 4); + + if (ret < 0) { + perf_count(_comms_errors); + perf_end(_sample_perf); + return ret; + } + + uint8_t status = (val[0] & 0xC0) >> 6; + + switch (status) { + case 0: + // Normal Operation. Good Data Packet + break; + + case 1: + // Reserved + return -EAGAIN; + + case 2: + // Stale Data. Data has been fetched since last measurement cycle. + return -EAGAIN; + + case 3: + // Fault Detected + perf_count(_comms_errors); + perf_end(_sample_perf); + return -EAGAIN; + } + + /* mask the used bits */ + int16_t dp_raw = (0x3FFF & ((val[0] << 8) + val[1])); + int16_t dT_raw = (0xFFE0 & ((val[2] << 8) + val[3])) >> 5; + + // dT max is almost certainly an invalid reading + if (dT_raw == 2047) { + perf_count(_comms_errors); + return -EAGAIN; + } + + // only publish changes + if ((dp_raw != _dp_raw_prev) || (dT_raw != _dT_raw_prev)) { + + _dp_raw_prev = dp_raw; + _dT_raw_prev = dT_raw; + + float temperature_c = ((200.0f * dT_raw) / 2047) - 50; + + // Calculate differential pressure. As its centered around 8000 + // and can go positive or negative + const float P_min = -1.0f; + const float P_max = 1.0f; + const float IN_H20_to_Pa = 248.84f; + /* + this equation is an inversion of the equation in the + pressure transfer function figure on page 4 of the datasheet + + We negate the result so that positive differential pressures + are generated when the bottom port is used as the static + port on the pitot and top port is used as the dynamic port + */ + float diff_press_PSI = -((dp_raw - 0.1f * 16383) * (P_max - P_min) / (0.8f * 16383) + P_min); + float diff_press_pa = diff_press_PSI * IN_H20_to_Pa; + + /* + With the above calculation the MS4515 sensor will produce a + positive number when the top port is used as a dynamic port + and bottom port is used as the static port + */ + differential_pressure_s differential_pressure{}; + differential_pressure.timestamp_sample = timestamp_sample; + differential_pressure.device_id = get_device_id(); + differential_pressure.differential_pressure_pa = diff_press_pa; + differential_pressure.temperature = temperature_c; + differential_pressure.error_count = perf_event_count(_comms_errors); + differential_pressure.timestamp = hrt_absolute_time(); + _differential_pressure_pub.publish(differential_pressure); + } + + perf_end(_sample_perf); + + return PX4_OK; +} + +void MS4515::RunImpl() +{ + int ret = PX4_ERROR; + + // collection phase + if (_collect_phase) { + // perform collection + ret = collect(); + + if (OK != ret) { + perf_count(_comms_errors); + /* restart the measurement state machine */ + _collect_phase = false; + _sensor_ok = false; + ScheduleNow(); + return; + } + + // next phase is measurement + _collect_phase = false; + + // is there a collect->measure gap? + if (_measure_interval > CONVERSION_INTERVAL) { + + // schedule a fresh cycle call when we are ready to measure again + ScheduleDelayed(_measure_interval - CONVERSION_INTERVAL); + + return; + } + } + + /* measurement phase */ + ret = measure(); + + if (OK != ret) { + DEVICE_DEBUG("measure error"); + } + + _sensor_ok = (ret == OK); + + // next phase is collection + _collect_phase = true; + + // schedule a fresh cycle call when the measurement is done + ScheduleDelayed(CONVERSION_INTERVAL); +} diff --git a/src/drivers/differential_pressure/ms4515/MS4515.hpp b/src/drivers/differential_pressure/ms4515/MS4515.hpp new file mode 100644 index 0000000000..5b9a048114 --- /dev/null +++ b/src/drivers/differential_pressure/ms4515/MS4515.hpp @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * 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 the MEAS Spec series MS4515 connected via I2C. + * + * Supported sensors: + * + * - MS4515DO + * + * Interface application notes: + * + * - Interfacing to MEAS Digital Pressure Modules (http://www.meas-spec.com/downloads/Interfacing_to_MEAS_Digital_Pressure_Modules.pdf) + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +static constexpr uint32_t I2C_SPEED = 100 * 1000; // 100 kHz I2C serial interface +static constexpr uint8_t I2C_ADDRESS_DEFAULT = 0x46; + +/* Register address */ +#define ADDR_READ_MR 0x00 /* write to this address to start conversion */ + +/* Measurement rate is 100Hz */ +#define MEAS_RATE 100 +#define CONVERSION_INTERVAL (1000000 / MEAS_RATE) /* microseconds */ + +class MS4515 : public device::I2C, public I2CSPIDriver +{ +public: + MS4515(const I2CSPIDriverConfig &config); + ~MS4515() override; + + static void print_usage(); + + void RunImpl(); + + int init() override; + +private: + int probe() override; + + int measure(); + int collect(); + + uint32_t _measure_interval{CONVERSION_INTERVAL}; + uint32_t _conversion_interval{CONVERSION_INTERVAL}; + + int16_t _dp_raw_prev{0}; + int16_t _dT_raw_prev{0}; + + bool _sensor_ok{false}; + bool _collect_phase{false}; + + uORB::PublicationMulti _differential_pressure_pub{ORB_ID(differential_pressure)}; + + perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")}; + perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": communication errors")}; +}; diff --git a/src/drivers/differential_pressure/ms4515/ms4515_main.cpp b/src/drivers/differential_pressure/ms4515/ms4515_main.cpp new file mode 100644 index 0000000000..a5f22907cb --- /dev/null +++ b/src/drivers/differential_pressure/ms4515/ms4515_main.cpp @@ -0,0 +1,77 @@ +/**************************************************************************** + * + * Copyright (c) 2020-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 "MS4515.hpp" + +#include +#include + +void MS4515::print_usage() +{ + PRINT_MODULE_USAGE_NAME("ms4515", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("airspeed_sensor"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x46); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +extern "C" int ms4515_main(int argc, char *argv[]) +{ + using ThisDriver = MS4515; + BusCLIArguments cli{true, false}; + cli.i2c_address = I2C_ADDRESS_DEFAULT; + cli.default_i2c_frequency = I2C_SPEED; + + const char *verb = cli.parseDefaultArguments(argc, argv); + + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_DIFF_PRESS_DEVTYPE_MS4515); + + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); + + } else if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); + + } else if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + ThisDriver::print_usage(); + return -1; +} diff --git a/src/drivers/differential_pressure/ms4525/parameters.c b/src/drivers/differential_pressure/ms4515/parameters.c similarity index 94% rename from src/drivers/differential_pressure/ms4525/parameters.c rename to src/drivers/differential_pressure/ms4515/parameters.c index c2ea85334b..25337376a3 100644 --- a/src/drivers/differential_pressure/ms4525/parameters.c +++ b/src/drivers/differential_pressure/ms4515/parameters.c @@ -32,10 +32,10 @@ ****************************************************************************/ /** - * TE MS4525 differential pressure sensor (external I2C) + * TE MS4515 differential pressure sensor (external I2C) * * @reboot_required true * @group Sensors * @boolean */ -PARAM_DEFINE_INT32(SENS_EN_MS4525, 0); +PARAM_DEFINE_INT32(SENS_EN_MS4515, 0); diff --git a/src/drivers/differential_pressure/ms4525/Kconfig b/src/drivers/differential_pressure/ms4525/Kconfig deleted file mode 100644 index 7b836d360e..0000000000 --- a/src/drivers/differential_pressure/ms4525/Kconfig +++ /dev/null @@ -1,5 +0,0 @@ -menuconfig DRIVERS_DIFFERENTIAL_PRESSURE_MS4525 - bool "ms4525" - default n - ---help--- - Enable support for ms4525 \ No newline at end of file diff --git a/src/drivers/differential_pressure/ms4525/ms4525_airspeed.cpp b/src/drivers/differential_pressure/ms4525/ms4525_airspeed.cpp deleted file mode 100644 index c2d15ba27e..0000000000 --- a/src/drivers/differential_pressure/ms4525/ms4525_airspeed.cpp +++ /dev/null @@ -1,327 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013, 2014, 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. - * - ****************************************************************************/ - -/** - * @file meas_airspeed.cpp - * @author Lorenz Meier - * @author Sarthak Kaingade - * @author Simon Wilks - * @author Thomas Gubler - * - * Driver for the MEAS Spec series connected via I2C. - * - * Supported sensors: - * - * - MS4525DO (http://www.meas-spec.com/downloads/MS4525DO.pdf) - * - * Interface application notes: - * - * - Interfacing to MEAS Digital Pressure Modules (http://www.meas-spec.com/downloads/Interfacing_to_MEAS_Digital_Pressure_Modules.pdf) - */ - -#include -#include -#include - -#include - -enum MS_DEVICE_TYPE { - DEVICE_TYPE_MS4515 = 4515, - DEVICE_TYPE_MS4525 = 4525 -}; - -/* I2C bus address is 1010001x */ -#define I2C_ADDRESS_MS4515DO 0x46 -#define I2C_ADDRESS_MS4525DO 0x28 /**< 7-bit address. Depends on the order code (this is for code "I") */ - -/* Register address */ -#define ADDR_READ_MR 0x00 /* write to this address to start conversion */ - -/* Measurement rate is 100Hz */ -#define MEAS_RATE 100 -#define CONVERSION_INTERVAL (1000000 / MEAS_RATE) /* microseconds */ - - -class MEASAirspeed : public Airspeed, public I2CSPIDriver -{ -public: - MEASAirspeed(const I2CSPIDriverConfig &config); - - virtual ~MEASAirspeed() = default; - - static void print_usage(); - - void RunImpl(); - - int init() override; - -protected: - - int measure() override; - int collect() override; -}; - -/* - * Driver 'main' command. - */ -extern "C" __EXPORT int ms4525_airspeed_main(int argc, char *argv[]); - -MEASAirspeed::MEASAirspeed(const I2CSPIDriverConfig &config) - : Airspeed(config.bus, config.bus_frequency, config.i2c_address, CONVERSION_INTERVAL), - I2CSPIDriver(config) -{ - _device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_MS4525; -} - -int MEASAirspeed::init() -{ - int ret = Airspeed::init(); - - if (ret == PX4_OK) { - ScheduleNow(); - } - - return ret; -} - -int -MEASAirspeed::measure() -{ - // Send the command to begin a measurement. - uint8_t cmd = 0; - int ret = transfer(&cmd, 1, nullptr, 0); - - if (OK != ret) { - perf_count(_comms_errors); - } - - return ret; -} - -int -MEASAirspeed::collect() -{ - /* read from the sensor */ - perf_begin(_sample_perf); - - const hrt_abstime timestamp_sample = hrt_absolute_time(); - uint8_t val[4] = {0, 0, 0, 0}; - int ret = transfer(nullptr, 0, &val[0], 4); - - if (ret < 0) { - perf_count(_comms_errors); - perf_end(_sample_perf); - return ret; - } - - uint8_t status = (val[0] & 0xC0) >> 6; - - switch (status) { - case 0: - // Normal Operation. Good Data Packet - break; - - case 1: - // Reserved - return -EAGAIN; - - case 2: - // Stale Data. Data has been fetched since last measurement cycle. - return -EAGAIN; - - case 3: - // Fault Detected - perf_count(_comms_errors); - perf_end(_sample_perf); - return -EAGAIN; - } - - int16_t dp_raw = 0, dT_raw = 0; - dp_raw = (val[0] << 8) + val[1]; - /* mask the used bits */ - dp_raw = 0x3FFF & dp_raw; - dT_raw = (val[2] << 8) + val[3]; - dT_raw = (0xFFE0 & dT_raw) >> 5; - - // dT max is almost certainly an invalid reading - if (dT_raw == 2047) { - perf_count(_comms_errors); - return -EAGAIN; - } - - float temperature = ((200.0f * dT_raw) / 2047) - 50; - - // Calculate differential pressure. As its centered around 8000 - // and can go positive or negative - const float P_min = -1.0f; - const float P_max = 1.0f; - const float PSI_to_Pa = 6894.757f; - /* - this equation is an inversion of the equation in the - pressure transfer function figure on page 4 of the datasheet - - We negate the result so that positive differential pressures - are generated when the bottom port is used as the static - port on the pitot and top port is used as the dynamic port - */ - float diff_press_PSI = -((dp_raw - 0.1f * 16383) * (P_max - P_min) / (0.8f * 16383) + P_min); - float diff_press_pa = diff_press_PSI * PSI_to_Pa; - - /* - With the above calculation the MS4525 sensor will produce a - positive number when the top port is used as a dynamic port - and bottom port is used as the static port - */ - differential_pressure_s report; - report.timestamp_sample = timestamp_sample; - report.device_id = get_device_id(); - report.differential_pressure_pa = diff_press_pa; - report.temperature = temperature; - report.error_count = perf_event_count(_comms_errors); - report.timestamp = hrt_absolute_time(); - _airspeed_pub.publish(report); - - perf_end(_sample_perf); - - return PX4_OK; -} - -void -MEASAirspeed::RunImpl() -{ - int ret; - - /* collection phase? */ - if (_collect_phase) { - - /* perform collection */ - ret = collect(); - - if (OK != ret) { - /* restart the measurement state machine */ - _collect_phase = false; - _sensor_ok = false; - ScheduleNow(); - return; - } - - /* next phase is measurement */ - _collect_phase = false; - - /* - * Is there a collect->measure gap? - */ - if (_measure_interval > CONVERSION_INTERVAL) { - - /* schedule a fresh cycle call when we are ready to measure again */ - ScheduleDelayed(_measure_interval - CONVERSION_INTERVAL); - - return; - } - } - - /* measurement phase */ - ret = measure(); - - if (OK != ret) { - DEVICE_DEBUG("measure error"); - } - - _sensor_ok = (ret == OK); - - /* next phase is collection */ - _collect_phase = true; - - /* schedule a fresh cycle call when the measurement is done */ - ScheduleDelayed(CONVERSION_INTERVAL); -} - -void -MEASAirspeed::print_usage() -{ - PRINT_MODULE_USAGE_NAME("ms4525_airspeed", "driver"); - PRINT_MODULE_USAGE_SUBCATEGORY("airspeed_sensor"); - PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); - PRINT_MODULE_USAGE_PARAM_STRING('T', "4525", "4525|4515", "Device type", true); - PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); -} - -int -ms4525_airspeed_main(int argc, char *argv[]) -{ - int ch; - using ThisDriver = MEASAirspeed; - BusCLIArguments cli{true, false}; - cli.default_i2c_frequency = 100000; - int device_type = DEVICE_TYPE_MS4525; - - while ((ch = cli.getOpt(argc, argv, "T:")) != EOF) { - switch (ch) { - case 'T': - device_type = atoi(cli.optArg()); - break; - } - } - - const char *verb = cli.optArg(); - - if (!verb) { - ThisDriver::print_usage(); - return -1; - } - - if (device_type == DEVICE_TYPE_MS4525) { - cli.i2c_address = I2C_ADDRESS_MS4525DO; - - } else { - cli.i2c_address = I2C_ADDRESS_MS4515DO; - } - - BusInstanceIterator iterator(MODULE_NAME, cli, DRV_DIFF_PRESS_DEVTYPE_MS4525); - - if (!strcmp(verb, "start")) { - return ThisDriver::module_start(cli, iterator); - } - - if (!strcmp(verb, "stop")) { - return ThisDriver::module_stop(iterator); - } - - if (!strcmp(verb, "status")) { - return ThisDriver::module_status(iterator); - } - - ThisDriver::print_usage(); - return -1; -} diff --git a/src/drivers/differential_pressure/ms5525/CMakeLists.txt b/src/drivers/differential_pressure/ms4525do/CMakeLists.txt similarity index 89% rename from src/drivers/differential_pressure/ms5525/CMakeLists.txt rename to src/drivers/differential_pressure/ms4525do/CMakeLists.txt index 21e4195034..49bf83e181 100644 --- a/src/drivers/differential_pressure/ms5525/CMakeLists.txt +++ b/src/drivers/differential_pressure/ms4525do/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# Copyright (c) 2015-2022 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 @@ -30,14 +30,15 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ + px4_add_module( - MODULE drivers__ms5525_airspeed - MAIN ms5525_airspeed + MODULE drivers__differential_pressure__ms4525do + MAIN ms4525do COMPILE_FLAGS SRCS - MS5525.cpp - MS5525_main.cpp + ms4525do_main.cpp + MS4525DO.cpp + MS4525DO.hpp DEPENDS - drivers__airspeed - mathlib + px4_work_queue ) diff --git a/src/drivers/differential_pressure/ms4525do/Kconfig b/src/drivers/differential_pressure/ms4525do/Kconfig new file mode 100644 index 0000000000..1a1635aa03 --- /dev/null +++ b/src/drivers/differential_pressure/ms4525do/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_DIFFERENTIAL_PRESSURE_MS4525DO + bool "ms4525do" + default n + ---help--- + Enable support for ms4525do diff --git a/src/drivers/differential_pressure/ms4525do/MS4525DO.cpp b/src/drivers/differential_pressure/ms4525do/MS4525DO.cpp new file mode 100644 index 0000000000..3e321a65a3 --- /dev/null +++ b/src/drivers/differential_pressure/ms4525do/MS4525DO.cpp @@ -0,0 +1,221 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2022 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 "MS4525DO.hpp" + +using namespace time_literals; + +MS4525DO::MS4525DO(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config) +{ +} + +MS4525DO::~MS4525DO() +{ + perf_free(_sample_perf); + perf_free(_comms_errors); + perf_free(_fault_perf); +} + +int MS4525DO::probe() +{ + _retries = 1; + + for (int i = 0; i < 10; i++) { + // perform 2 x Read_DF3 (Data Fetch 3 Bytes) + // 1st read: require status = Normal Operation. Good Data Packet + // 2nd read: require status = Stale Data, data should match first read + uint8_t data_1[3] {}; + int ret1 = transfer(nullptr, 0, &data_1[0], sizeof(data_1)); + + uint8_t data_2[3] {}; + int ret2 = transfer(nullptr, 0, &data_2[0], sizeof(data_2)); + + if (ret1 == PX4_OK && ret2 == PX4_OK) { + // Status bits + const uint8_t status_1 = (data_1[0] & 0b1100'0000) >> 6; + const uint8_t status_2 = (data_2[0] & 0b1100'0000) >> 6; + + if ((status_1 == (uint8_t)STATUS::Normal_Operation) + && (status_2 == (uint8_t)STATUS::Stale_Data) + && (data_1[2] == data_1[2])) { + + _retries = 1; // enable retries during operation + return PX4_OK; + + } else { + PX4_ERR("status: %X status: %X", status_1, status_2); + } + + } else { + px4_usleep(1000); // TODO + } + } + + return PX4_ERROR; +} + +int MS4525DO::init() +{ + int ret = I2C::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("I2C::init failed (%i)", ret); + return ret; + } + + if (ret == PX4_OK) { + ScheduleNow(); + } + + return ret; +} + +void MS4525DO::print_status() +{ + I2CSPIDriverBase::print_status(); + + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + perf_print_counter(_fault_perf); +} + +void MS4525DO::RunImpl() +{ + switch (_state) { + case STATE::MEASURE: { + // Send the command to begin a measurement (Read_MR) + uint8_t cmd = ADDR_READ_MR; + + if (transfer(&cmd, 1, nullptr, 0) == OK) { + _timestamp_sample = hrt_absolute_time(); + _state = STATE::READ; + ScheduleDelayed(2_ms); + + } else { + perf_count(_comms_errors); + _state = STATE::MEASURE; + ScheduleDelayed(10_ms); // try again in 10 ms + } + } + + break; + + case STATE::READ: + // perform 2 x Read_DF4 (Data Fetch 4 Bytes) + // 1st read: require status = Normal Operation. Good Data Packet + // 2nd read: require status = Stale Data, data should match first read + perf_begin(_sample_perf); + uint8_t data_1[4] {}; + int ret1 = transfer(nullptr, 0, &data_1[0], sizeof(data_1)); + + uint8_t data_2[4] {}; + int ret2 = transfer(nullptr, 0, &data_2[0], sizeof(data_2)); + perf_end(_sample_perf); + + if (ret1 != PX4_OK || ret2 != PX4_OK) { + perf_count(_comms_errors); + + } else { + // Status bits + const uint8_t status_1 = (data_1[0] & 0b1100'0000) >> 6; + const uint8_t status_2 = (data_2[0] & 0b1100'0000) >> 6; + + const uint8_t bridge_data_1_msb = (data_1[0] & 0b0011'1111); + const uint8_t bridge_data_2_msb = (data_2[0] & 0b0011'1111); + + const uint8_t bridge_data_1_lsb = data_1[1]; + const uint8_t bridge_data_2_lsb = data_2[1]; + + // Bridge Data [13:8] + Bridge Data [7:0] + int16_t bridge_data_1 = (bridge_data_1_msb << 8) + bridge_data_1_lsb; + int16_t bridge_data_2 = (bridge_data_2_msb << 8) + bridge_data_2_lsb; + + // 11-bit temperature data + // Temperature Data [10:3] + Temperature Data [2:0] + int16_t temperature_1 = ((data_1[2] << 8) + (0b1110'0000 & data_1[3])) / (1 << 5); + int16_t temperature_2 = ((data_2[2] << 8) + (0b1110'0000 & data_2[3])) / (1 << 5); + + if ((status_1 == (uint8_t)STATUS::Fault_Detected) || (status_2 == (uint8_t)STATUS::Fault_Detected)) { + // Fault Detected + perf_count(_fault_perf); + + } else if ((status_1 == (uint8_t)STATUS::Normal_Operation) && (status_2 == (uint8_t)STATUS::Stale_Data) + && (bridge_data_1_msb == bridge_data_2_msb) && (temperature_1 == temperature_2)) { + + float temperature_c = ((200.f * temperature_1) / 2047) - 50.f; + + // Output is proportional to the difference between Port 1 and Port 2. Output swings + // positive when Port 1> Port 2. Output is 50% of supply voltage when Port 1=Port 2. + + // Calculate differential pressure. As its centered around 8000 + // and can go positive or negative + static constexpr float P_min = -1.f; // -1 PSI + static constexpr float P_max = 1.f; // +1 PSI + + // this equation is an inversion of the equation in the + // pressure transfer function figure on page 4 of the datasheet + + // We negate the result so that positive differential pressures + // are generated when the bottom port is used as the static + // port on the pitot and top port is used as the dynamic port + const float diff_press_PSI = -((bridge_data_1 - 0.1f * 16383.f) * (P_max - P_min) / (0.8f * 16383.f) + P_min); + + static constexpr float PSI_to_Pa = 6894.757f; + float diff_press_pa = diff_press_PSI * PSI_to_Pa; + + if (hrt_elapsed_time(&_timestamp_sample) < 20_ms) { + differential_pressure_s differential_pressure{}; + differential_pressure.timestamp_sample = _timestamp_sample; + differential_pressure.device_id = get_device_id(); + differential_pressure.differential_pressure_pa = diff_press_pa; + differential_pressure.temperature = temperature_c; + differential_pressure.error_count = perf_event_count(_comms_errors); + differential_pressure.timestamp = hrt_absolute_time(); + _differential_pressure_pub.publish(differential_pressure); + + _timestamp_sample = 0; + } + + } else { + PX4_DEBUG("status:%X|%X, B:%X|%X, T:%X|%X", status_1, status_2, bridge_data_1, bridge_data_2, temperature_1, + temperature_2); + } + } + + _state = STATE::MEASURE; + ScheduleDelayed(10_ms); + break; + } +} diff --git a/src/drivers/differential_pressure/ms4525do/MS4525DO.hpp b/src/drivers/differential_pressure/ms4525do/MS4525DO.hpp new file mode 100644 index 0000000000..b98483d2f1 --- /dev/null +++ b/src/drivers/differential_pressure/ms4525do/MS4525DO.hpp @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2022 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 the MEAS Spec series MS4525DO connected via I2C. + * + * Supported sensors: + * + * - MS4525DO + * + * Interface application notes: + * + * - Interfacing to MEAS Digital Pressure Modules (http://www.meas-spec.com/downloads/Interfacing_to_MEAS_Digital_Pressure_Modules.pdf) + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +static constexpr uint32_t I2C_SPEED = 100 * 1000; // 100 kHz I2C serial interface +static constexpr uint8_t I2C_ADDRESS_DEFAULT = 0x28; + +/* Register address */ +#define ADDR_READ_MR 0x00 + +class MS4525DO : public device::I2C, public I2CSPIDriver +{ +public: + MS4525DO(const I2CSPIDriverConfig &config); + ~MS4525DO() override; + + static void print_usage(); + + void RunImpl(); + + int init() override; + void print_status() override; + +private: + int probe() override; + + enum class STATE : uint8_t { + MEASURE, + READ, + } _state{STATE::MEASURE}; + + enum class STATUS : uint8_t { + Normal_Operation = 0b00, // 0: Normal Operation. Good Data Packet + Reserved = 0b01, // 1: Reserved + Stale_Data = 0b10, // 2: Stale Data. Data has been fetched since last measurement cycle. + Fault_Detected = 0b11, // 3: Fault Detected + }; + + hrt_abstime _timestamp_sample{0}; + + uORB::PublicationMulti _differential_pressure_pub{ORB_ID(differential_pressure)}; + + perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")}; + perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": communication errors")}; + perf_counter_t _fault_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": fault detected")}; +}; diff --git a/src/drivers/differential_pressure/ms4525do/ms4525do_main.cpp b/src/drivers/differential_pressure/ms4525do/ms4525do_main.cpp new file mode 100644 index 0000000000..abea2a67f0 --- /dev/null +++ b/src/drivers/differential_pressure/ms4525do/ms4525do_main.cpp @@ -0,0 +1,77 @@ +/**************************************************************************** + * + * Copyright (c) 2020-2022 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 "MS4525DO.hpp" + +#include +#include + +void MS4525DO::print_usage() +{ + PRINT_MODULE_USAGE_NAME("ms4525do", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("airspeed_sensor"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x28); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +extern "C" int ms4525do_main(int argc, char *argv[]) +{ + using ThisDriver = MS4525DO; + BusCLIArguments cli{true, false}; + cli.i2c_address = I2C_ADDRESS_DEFAULT; + cli.default_i2c_frequency = I2C_SPEED; + + const char *verb = cli.parseDefaultArguments(argc, argv); + + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_DIFF_PRESS_DEVTYPE_MS4525DO); + + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); + + } else if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); + + } else if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + ThisDriver::print_usage(); + return -1; +} diff --git a/src/drivers/differential_pressure/ms5525/parameters.c b/src/drivers/differential_pressure/ms4525do/parameters.c similarity index 90% rename from src/drivers/differential_pressure/ms5525/parameters.c rename to src/drivers/differential_pressure/ms4525do/parameters.c index 6b7eae2aca..86f722ab19 100644 --- a/src/drivers/differential_pressure/ms5525/parameters.c +++ b/src/drivers/differential_pressure/ms4525do/parameters.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2021-2022 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 @@ -32,10 +32,10 @@ ****************************************************************************/ /** - * TE MS5525 differential pressure sensor (external I2C) + * TE MS4525DO differential pressure sensor (external I2C) * * @reboot_required true * @group Sensors * @boolean */ -PARAM_DEFINE_INT32(SENS_EN_MS5525, 0); +PARAM_DEFINE_INT32(SENS_EN_MS4525DO, 0); diff --git a/src/drivers/differential_pressure/ms5525/Kconfig b/src/drivers/differential_pressure/ms5525/Kconfig deleted file mode 100644 index 9e2e1057d5..0000000000 --- a/src/drivers/differential_pressure/ms5525/Kconfig +++ /dev/null @@ -1,5 +0,0 @@ -menuconfig DRIVERS_DIFFERENTIAL_PRESSURE_MS5525 - bool "ms5525" - default n - ---help--- - Enable support for ms5525 \ No newline at end of file diff --git a/src/drivers/differential_pressure/CMakeLists.txt b/src/drivers/differential_pressure/ms5525dso/CMakeLists.txt similarity index 86% rename from src/drivers/differential_pressure/CMakeLists.txt rename to src/drivers/differential_pressure/ms5525dso/CMakeLists.txt index ab6865d6b3..1cf253b7aa 100644 --- a/src/drivers/differential_pressure/CMakeLists.txt +++ b/src/drivers/differential_pressure/ms5525dso/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2017 PX4 Development Team. All rights reserved. +# Copyright (c) 2015-2022 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,7 +31,14 @@ # ############################################################################ -add_subdirectory(ets) -add_subdirectory(ms4525) -add_subdirectory(ms5525) -add_subdirectory(sdp3x) \ No newline at end of file +px4_add_module( + MODULE drivers__differential_pressure__ms5525dso + MAIN ms5525dso + COMPILE_FLAGS + SRCS + ms5525dso_main.cpp + MS5525DSO.cpp + MS5525DSO.hpp + DEPENDS + px4_work_queue + ) diff --git a/src/drivers/differential_pressure/ms5525dso/Kconfig b/src/drivers/differential_pressure/ms5525dso/Kconfig new file mode 100644 index 0000000000..d5e6653f7f --- /dev/null +++ b/src/drivers/differential_pressure/ms5525dso/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_DIFFERENTIAL_PRESSURE_MS5525DSO + bool "ms5525dso" + default n + ---help--- + Enable support for ms5525dso diff --git a/src/drivers/differential_pressure/ms5525/MS5525.cpp b/src/drivers/differential_pressure/ms5525dso/MS5525DSO.cpp similarity index 80% rename from src/drivers/differential_pressure/ms5525/MS5525.cpp rename to src/drivers/differential_pressure/ms5525dso/MS5525DSO.cpp index 080bd66af3..4b62025377 100644 --- a/src/drivers/differential_pressure/ms5525/MS5525.cpp +++ b/src/drivers/differential_pressure/ms5525dso/MS5525DSO.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2017-2022 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,11 +31,35 @@ * ****************************************************************************/ -#include "MS5525.hpp" +#include "MS5525DSO.hpp" -int MS5525::init() +MS5525DSO::MS5525DSO(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config) { - int ret = Airspeed::init(); +} + +MS5525DSO::~MS5525DSO() +{ + perf_free(_sample_perf); + perf_free(_comms_errors); +} + +int MS5525DSO::probe() +{ + _retries = 1; + + return init_ms5525dso() ? PX4_OK : PX4_ERROR; +} + +int MS5525DSO::init() +{ + int ret = I2C::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("I2C::init failed (%i)", ret); + return ret; + } if (ret == PX4_OK) { ScheduleNow(); @@ -44,8 +68,15 @@ int MS5525::init() return ret; } -int -MS5525::measure() +void MS5525DSO::print_status() +{ + I2CSPIDriverBase::print_status(); + + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); +} + +int MS5525DSO::measure() { int ret = PX4_ERROR; @@ -59,7 +90,7 @@ MS5525::measure() } } else { - _inited = init_ms5525(); + _inited = init_ms5525dso(); if (_inited) { ret = PX4_OK; @@ -69,8 +100,7 @@ MS5525::measure() return ret; } -bool -MS5525::init_ms5525() +bool MS5525DSO::init_ms5525dso() { // Step 1 - reset uint8_t cmd = CMD_RESET; @@ -89,7 +119,8 @@ MS5525::init_ms5525() // 0 factory data and the setup // 1-6 calibration coefficients // 7 serial code and CRC - uint16_t prom[8]; + uint16_t prom[8] {}; + bool prom_all_zero = true; for (uint8_t i = 0; i < 8; i++) { cmd = CMD_PROM_START + i * 2; @@ -109,12 +140,20 @@ MS5525::init_ms5525() if (ret == PX4_OK) { prom[i] = (val[0] << 8) | val[1]; + if (prom[i] != 0) { + prom_all_zero = false; + } + } else { perf_count(_comms_errors); return false; } } + if (prom_all_zero) { + return false; + } + // Step 3 - check CRC const uint8_t crc = prom_crc4(prom); const uint8_t onboard_crc = prom[7] & 0xF; @@ -129,18 +168,17 @@ MS5525::init_ms5525() C6 = prom[6]; Tref = int64_t(C5) * (1UL << Q5); - _device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_MS5525; return true; } else { PX4_ERR("CRC mismatch"); - return false; } + + return false; } -uint8_t -MS5525::prom_crc4(uint16_t n_prom[]) const +uint8_t MS5525DSO::prom_crc4(uint16_t n_prom[]) const { // see Measurement Specialties AN520 @@ -177,27 +215,29 @@ MS5525::prom_crc4(uint16_t n_prom[]) const return (n_rem ^ 0x00); } -int -MS5525::collect() +int MS5525DSO::collect() { perf_begin(_sample_perf); + const hrt_abstime timestamp_sample = hrt_absolute_time(); + // read ADC uint8_t cmd = CMD_ADC_READ; - const hrt_abstime timestamp_sample = hrt_absolute_time(); int ret = transfer(&cmd, 1, nullptr, 0); if (ret != PX4_OK) { perf_count(_comms_errors); + perf_end(_sample_perf); return ret; } // read 24 bits from the sensor - uint8_t val[3]; + uint8_t val[3] {}; ret = transfer(nullptr, 0, &val[0], 3); if (ret != PX4_OK) { perf_count(_comms_errors); + perf_end(_sample_perf); return ret; } @@ -209,7 +249,8 @@ MS5525::collect() // incorrect result as well. if (adc == 0) { perf_count(_comms_errors); - return EAGAIN; + perf_end(_sample_perf); + return -EAGAIN; } if (_current_cmd == CMD_CONVERT_PRES) { @@ -231,7 +272,8 @@ MS5525::collect() // not ready yet if (D1 == 0 || D2 == 0) { - return EAGAIN; + perf_end(_sample_perf); + return -EAGAIN; } // Difference between actual and reference temperature @@ -262,24 +304,21 @@ MS5525::collect() const float temperature_c = TEMP * 0.01f; - differential_pressure_s diff_pressure; - diff_pressure.timestamp_sample = timestamp_sample; - diff_pressure.device_id = get_device_id(); - diff_pressure.differential_pressure_pa = diff_press_pa; - diff_pressure.temperature = temperature_c; - diff_pressure.error_count = perf_event_count(_comms_errors); - diff_pressure.timestamp = hrt_absolute_time(); - _airspeed_pub.publish(diff_pressure); - - ret = OK; + differential_pressure_s differential_pressure{}; + differential_pressure.timestamp_sample = timestamp_sample; + differential_pressure.device_id = get_device_id(); + differential_pressure.differential_pressure_pa = diff_press_pa; + differential_pressure.temperature = temperature_c; + differential_pressure.error_count = perf_event_count(_comms_errors); + differential_pressure.timestamp = hrt_absolute_time(); + _differential_pressure_pub.publish(differential_pressure); perf_end(_sample_perf); - return ret; + return PX4_OK; } -void -MS5525::RunImpl() +void MS5525DSO::RunImpl() { int ret = PX4_ERROR; @@ -288,7 +327,8 @@ MS5525::RunImpl() // perform collection ret = collect(); - if (OK != ret) { + if (PX4_OK != ret) { + perf_count(_comms_errors); /* restart the measurement state machine */ _collect_phase = false; _sensor_ok = false; @@ -312,7 +352,7 @@ MS5525::RunImpl() /* measurement phase */ ret = measure(); - if (OK != ret) { + if (PX4_OK != ret) { DEVICE_DEBUG("measure error"); } diff --git a/src/drivers/differential_pressure/ms5525/MS5525.hpp b/src/drivers/differential_pressure/ms5525dso/MS5525DSO.hpp similarity index 71% rename from src/drivers/differential_pressure/ms5525/MS5525.hpp rename to src/drivers/differential_pressure/ms5525dso/MS5525DSO.hpp index bd3d59730f..c6408566aa 100644 --- a/src/drivers/differential_pressure/ms5525/MS5525.hpp +++ b/src/drivers/differential_pressure/ms5525dso/MS5525DSO.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2017-2022 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 @@ -33,40 +33,45 @@ #pragma once -#include #include -#include -#include -#include -/* The MS5525DSO address is 111011Cx, where C is the complementary value of the pin CSB */ -static constexpr uint8_t I2C_ADDRESS_1_MS5525DSO = 0x76; +#include +#include +#include +#include +#include +#include + +/* The MS5525DSODSO address is 111011Cx, where C is the complementary value of the pin CSB */ +static constexpr uint32_t I2C_SPEED = 100 * 1000; // 100 kHz I2C serial interface +static constexpr uint8_t I2C_ADDRESS_DEFAULT = 0x76; /* Measurement rate is 100Hz */ static constexpr unsigned MEAS_RATE = 100; static constexpr int64_t CONVERSION_INTERVAL = (1000000 / MEAS_RATE); /* microseconds */ -class MS5525 : public Airspeed, public I2CSPIDriver +class MS5525DSO : public device::I2C, public I2CSPIDriver { public: - MS5525(const I2CSPIDriverConfig &config) : - Airspeed(config.bus, config.bus_frequency, config.i2c_address, CONVERSION_INTERVAL), - I2CSPIDriver(config) - { - } - - virtual ~MS5525() = default; + MS5525DSO(const I2CSPIDriverConfig &config); + ~MS5525DSO() override; static void print_usage(); - void RunImpl(); + void RunImpl(); int init() override; + void print_status() override; private: + int probe() override; - int measure() override; - int collect() override; + bool init_ms5525dso(); + + uint8_t prom_crc4(uint16_t n_prom[]) const; + + int measure(); + int collect(); static constexpr uint8_t CMD_RESET = 0x1E; // ADC reset command static constexpr uint8_t CMD_ADC_READ = 0x00; // ADC read command @@ -79,7 +84,7 @@ private: // Convert D1 (OSR=1024) 0x44 // Convert D1 (OSR=2048) 0x46 // Convert D1 (OSR=4096) 0x48 - static constexpr uint8_t CMD_CONVERT_PRES = 0x44; + static constexpr uint8_t CMD_CONVERT_PRES = 0x48; // D2 - temperature convert commands // Convert D2 (OSR=256) 0x50 @@ -87,7 +92,7 @@ private: // Convert D2 (OSR=1024) 0x54 // Convert D2 (OSR=2048) 0x56 // Convert D2 (OSR=4096) 0x58 - static constexpr uint8_t CMD_CONVERT_TEMP = 0x54; + static constexpr uint8_t CMD_CONVERT_TEMP = 0x58; uint8_t _current_cmd{CMD_CONVERT_PRES}; @@ -116,9 +121,16 @@ private: uint32_t D1{0}; uint32_t D2{0}; - bool init_ms5525(); bool _inited{false}; - uint8_t prom_crc4(uint16_t n_prom[]) const; + uint32_t _measure_interval{CONVERSION_INTERVAL}; + uint32_t _conversion_interval{CONVERSION_INTERVAL}; + bool _sensor_ok{false}; + bool _collect_phase{false}; + + uORB::PublicationMulti _differential_pressure_pub{ORB_ID(differential_pressure)}; + + perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")}; + perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": communication errors")}; }; diff --git a/src/drivers/differential_pressure/ms5525/MS5525_main.cpp b/src/drivers/differential_pressure/ms5525dso/ms5525dso_main.cpp similarity index 81% rename from src/drivers/differential_pressure/ms5525/MS5525_main.cpp rename to src/drivers/differential_pressure/ms5525dso/ms5525dso_main.cpp index e663d3c849..a6704f21e9 100644 --- a/src/drivers/differential_pressure/ms5525/MS5525_main.cpp +++ b/src/drivers/differential_pressure/ms5525dso/ms5525dso_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2017-2022 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,14 +31,14 @@ * ****************************************************************************/ -#include "MS5525.hpp" +#include "MS5525DSO.hpp" -extern "C" __EXPORT int ms5525_airspeed_main(int argc, char *argv[]); +#include +#include -void -MS5525::print_usage() +void MS5525DSO::print_usage() { - PRINT_MODULE_USAGE_NAME("ms5525_airspeed", "driver"); + PRINT_MODULE_USAGE_NAME("ms5525dso", "driver"); PRINT_MODULE_USAGE_SUBCATEGORY("airspeed_sensor"); PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); @@ -46,13 +46,12 @@ MS5525::print_usage() PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -int -ms5525_airspeed_main(int argc, char *argv[]) +extern "C" int ms5525dso_main(int argc, char *argv[]) { - using ThisDriver = MS5525; + using ThisDriver = MS5525DSO; BusCLIArguments cli{true, false}; - cli.default_i2c_frequency = 100000; - cli.i2c_address = I2C_ADDRESS_1_MS5525DSO; + cli.i2c_address = I2C_ADDRESS_DEFAULT; + cli.default_i2c_frequency = I2C_SPEED; const char *verb = cli.parseDefaultArguments(argc, argv); @@ -61,17 +60,15 @@ ms5525_airspeed_main(int argc, char *argv[]) return -1; } - BusInstanceIterator iterator(MODULE_NAME, cli, DRV_DIFF_PRESS_DEVTYPE_MS5525); + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_DIFF_PRESS_DEVTYPE_MS5525DSO); if (!strcmp(verb, "start")) { return ThisDriver::module_start(cli, iterator); - } - if (!strcmp(verb, "stop")) { + } else if (!strcmp(verb, "stop")) { return ThisDriver::module_stop(iterator); - } - if (!strcmp(verb, "status")) { + } else if (!strcmp(verb, "status")) { return ThisDriver::module_status(iterator); } diff --git a/src/lib/drivers/airspeed/airspeed.h b/src/drivers/differential_pressure/ms5525dso/parameters.c similarity index 60% rename from src/lib/drivers/airspeed/airspeed.h rename to src/drivers/differential_pressure/ms5525dso/parameters.c index b5b4d7ff78..a96d82b981 100644 --- a/src/lib/drivers/airspeed/airspeed.h +++ b/src/drivers/differential_pressure/ms5525dso/parameters.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2021-2022 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,49 +31,11 @@ * ****************************************************************************/ -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include - -class __EXPORT Airspeed : public device::I2C -{ -public: - Airspeed(int bus, int bus_frequency, int address, unsigned conversion_interval); - virtual ~Airspeed(); - - int init() override; - -private: - Airspeed(const Airspeed &) = delete; - Airspeed &operator=(const Airspeed &) = delete; - -protected: - int probe() override; - - /** - * Perform a poll cycle; collect from the previous measurement - * and start a new one. - */ - virtual int measure() = 0; - virtual int collect() = 0; - - bool _sensor_ok; - int _measure_interval; - bool _collect_phase; - - uORB::PublicationMulti _airspeed_pub{ORB_ID(differential_pressure)}; - - unsigned _conversion_interval; - - perf_counter_t _sample_perf; - perf_counter_t _comms_errors; -}; - - +/** + * TE MS5525DSO differential pressure sensor (external I2C) + * + * @reboot_required true + * @group Sensors + * @boolean + */ +PARAM_DEFINE_INT32(SENS_EN_MS5525DS, 0); diff --git a/src/drivers/differential_pressure/sdp3x/CMakeLists.txt b/src/drivers/differential_pressure/sdp3x/CMakeLists.txt index 3ba5eba2fe..f0e626485d 100644 --- a/src/drivers/differential_pressure/sdp3x/CMakeLists.txt +++ b/src/drivers/differential_pressure/sdp3x/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2017 PX4 Development Team. All rights reserved. +# Copyright (c) 2017-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 @@ -30,14 +30,15 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ + px4_add_module( - MODULE drivers__sdp3x_airspeed - MAIN sdp3x_airspeed + MODULE drivers__differential_pressure__sdp3x + MAIN sdp3x COMPILE_FLAGS SRCS + sdp3x_main.cpp SDP3X.cpp - SDP3X_main.cpp + SDP3X.hpp DEPENDS - drivers__airspeed - mathlib + px4_work_queue ) diff --git a/src/drivers/differential_pressure/sdp3x/SDP3X.cpp b/src/drivers/differential_pressure/sdp3x/SDP3X.cpp index 6017388636..82a7c4c24d 100644 --- a/src/drivers/differential_pressure/sdp3x/SDP3X.cpp +++ b/src/drivers/differential_pressure/sdp3x/SDP3X.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2017-2022 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,20 +31,50 @@ * ****************************************************************************/ -/** - * @file SDP3X.hpp - * - * Driver for Sensirion SDP3X Differential Pressure Sensor - * - */ - #include "SDP3X.hpp" using namespace time_literals; -int -SDP3X::probe() +SDP3X::SDP3X(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config), + _keep_retrying(config.keep_running) { +} + +SDP3X::~SDP3X() +{ + perf_free(_sample_perf); + perf_free(_comms_errors); +} + +int SDP3X::init() +{ + int ret = I2C::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("I2C::init failed (%i)", ret); + return ret; + } + + if (ret == PX4_OK) { + ScheduleNow(); + } + + return ret; +} + +void SDP3X::print_status() +{ + I2CSPIDriverBase::print_status(); + + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); +} + +int SDP3X::probe() +{ + _retries = 1; bool require_initialization = !init_sdp3x(); if (require_initialization && _keep_retrying) { @@ -63,14 +93,12 @@ int SDP3X::write_command(uint16_t command) return transfer(&cmd[0], 2, nullptr, 0); } -bool -SDP3X::init_sdp3x() +bool SDP3X::init_sdp3x() { return configure() == 0; } -int -SDP3X::configure() +int SDP3X::configure() { int ret = write_command(SDP3X_CONT_MODE_STOP); @@ -91,8 +119,7 @@ SDP3X::configure() return ret; } -int -SDP3X::read_scale() +int SDP3X::read_scale() { // get scale uint8_t val[9]; @@ -114,42 +141,29 @@ SDP3X::read_scale() switch (_scale) { case SDP3X_SCALE_PRESSURE_SDP31: - _device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_SDP31; + set_device_type(DRV_DIFF_PRESS_DEVTYPE_SDP31); break; case SDP3X_SCALE_PRESSURE_SDP32: - _device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_SDP32; + set_device_type(DRV_DIFF_PRESS_DEVTYPE_SDP32); break; case SDP3X_SCALE_PRESSURE_SDP33: - _device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_SDP33; + set_device_type(DRV_DIFF_PRESS_DEVTYPE_SDP33); break; } return PX4_OK; } -int SDP3X::init() -{ - int ret = Airspeed::init(); - - if (ret == PX4_OK) { - // make sure to wait 10ms after configuring the measurement mode - ScheduleDelayed(10_ms); - } - - return ret; -} - -int -SDP3X::collect() +int SDP3X::collect() { perf_begin(_sample_perf); const hrt_abstime timestamp_sample = hrt_absolute_time(); // read 6 bytes from the sensor - uint8_t val[6]; + uint8_t val[6] {}; int ret = transfer(nullptr, 0, &val[0], sizeof(val)); if (ret != PX4_OK) { @@ -169,22 +183,21 @@ SDP3X::collect() float diff_press_pa = static_cast(P) / static_cast(_scale); float temperature_c = temp / static_cast(SDP3X_SCALE_TEMPERATURE); - differential_pressure_s report; - report.device_id = get_device_id(); - report.timestamp_sample = timestamp_sample; - report.differential_pressure_pa = diff_press_pa; - report.temperature = temperature_c; - report.error_count = perf_event_count(_comms_errors); - report.timestamp = hrt_absolute_time(); - _airspeed_pub.publish(report); + differential_pressure_s differential_pressure{}; + differential_pressure.timestamp_sample = timestamp_sample; + differential_pressure.device_id = get_device_id(); + differential_pressure.differential_pressure_pa = diff_press_pa; + differential_pressure.temperature = temperature_c; + differential_pressure.error_count = perf_event_count(_comms_errors); + differential_pressure.timestamp = hrt_absolute_time(); + _differential_pressure_pub.publish(differential_pressure); perf_end(_sample_perf); return ret; } -void -SDP3X::RunImpl() +void SDP3X::RunImpl() { switch (_state) { case State::RequireConfig: @@ -213,7 +226,6 @@ SDP3X::RunImpl() int ret = collect(); if (ret != 0 && ret != -EAGAIN) { - _sensor_ok = false; DEVICE_DEBUG("measure error"); _state = State::RequireConfig; } diff --git a/src/drivers/differential_pressure/sdp3x/SDP3X.hpp b/src/drivers/differential_pressure/sdp3x/SDP3X.hpp index 5791935de0..d9c89c5821 100644 --- a/src/drivers/differential_pressure/sdp3x/SDP3X.hpp +++ b/src/drivers/differential_pressure/sdp3x/SDP3X.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2017-2022 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 @@ -32,7 +32,6 @@ ****************************************************************************/ /** - * @file SDP3X.hpp * * Driver for Sensirion SDP3X Differential Pressure Sensor * @@ -41,15 +40,18 @@ #pragma once -#include -#include -#include -#include +#include +#include +#include #include +#include +#include -#define I2C_ADDRESS_1_SDP3X 0x21 -#define I2C_ADDRESS_2_SDP3X 0x22 -#define I2C_ADDRESS_3_SDP3X 0x23 +#define I2C_ADDRESS_1_SDP3X 0x21 +#define I2C_ADDRESS_2_SDP3X 0x22 +#define I2C_ADDRESS_3_SDP3X 0x23 + +static constexpr uint32_t I2C_SPEED = 100 * 1000; // 100 kHz I2C serial interface #define SDP3X_SCALE_TEMPERATURE 200.0f #define SDP3X_RESET_ADDR 0x00 @@ -65,36 +67,32 @@ #define SPD3X_MEAS_RATE 100 #define CONVERSION_INTERVAL (1000000 / SPD3X_MEAS_RATE) /* microseconds */ -class SDP3X : public Airspeed, public I2CSPIDriver +class SDP3X : public device::I2C, public I2CSPIDriver { public: - SDP3X(const I2CSPIDriverConfig &config) : - Airspeed(config.bus, config.bus_frequency, config.i2c_address, CONVERSION_INTERVAL), - I2CSPIDriver(config), - _keep_retrying{config.keep_running} - { - } - - virtual ~SDP3X() = default; + SDP3X(const I2CSPIDriverConfig &config); + ~SDP3X() override; static void print_usage(); - void RunImpl(); + void RunImpl(); int init() override; + void print_status() override; private: + int probe() override; + enum class State { RequireConfig, Configuring, Running }; - int measure() override { return 0; } - int collect() override; - int probe() override; - int configure(); - int read_scale(); + int collect(); + + int configure(); + int read_scale(); bool init_sdp3x(); @@ -111,4 +109,9 @@ private: uint16_t _scale{0}; const bool _keep_retrying; State _state{State::RequireConfig}; + + uORB::PublicationMulti _differential_pressure_pub{ORB_ID(differential_pressure)}; + + perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")}; + perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": communication errors")}; }; diff --git a/src/drivers/differential_pressure/sdp3x/SDP3X_main.cpp b/src/drivers/differential_pressure/sdp3x/sdp3x_main.cpp similarity index 86% rename from src/drivers/differential_pressure/sdp3x/SDP3X_main.cpp rename to src/drivers/differential_pressure/sdp3x/sdp3x_main.cpp index 9281276280..ddc48d36de 100644 --- a/src/drivers/differential_pressure/sdp3x/SDP3X_main.cpp +++ b/src/drivers/differential_pressure/sdp3x/sdp3x_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2017-2022 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 @@ -33,12 +33,12 @@ #include "SDP3X.hpp" -extern "C" __EXPORT int sdp3x_airspeed_main(int argc, char *argv[]); +#include +#include -void -SDP3X::print_usage() +void SDP3X::print_usage() { - PRINT_MODULE_USAGE_NAME("sdp3x_airspeed", "driver"); + PRINT_MODULE_USAGE_NAME("sdp3x", "driver"); PRINT_MODULE_USAGE_SUBCATEGORY("airspeed_sensor"); PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); @@ -47,14 +47,12 @@ SDP3X::print_usage() PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -int -sdp3x_airspeed_main(int argc, char *argv[]) +extern "C" int sdp3x_main(int argc, char *argv[]) { using ThisDriver = SDP3X; BusCLIArguments cli{true, false}; - cli.default_i2c_frequency = 100000; cli.i2c_address = I2C_ADDRESS_1_SDP3X; - cli.support_keep_running = true; + cli.default_i2c_frequency = I2C_SPEED; const char *verb = cli.parseDefaultArguments(argc, argv); @@ -67,13 +65,11 @@ sdp3x_airspeed_main(int argc, char *argv[]) if (!strcmp(verb, "start")) { return ThisDriver::module_start(cli, iterator); - } - if (!strcmp(verb, "stop")) { + } else if (!strcmp(verb, "stop")) { return ThisDriver::module_stop(iterator); - } - if (!strcmp(verb, "status")) { + } else if (!strcmp(verb, "status")) { return ThisDriver::module_status(iterator); } diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 1dc3149821..360c5b9913 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -67,6 +67,7 @@ #define DRV_IMU_DEVTYPE_LSM303D 0x11 #define DRV_IMU_DEVTYPE_SIM 0x14 +#define DRV_DIFF_PRESS_DEVTYPE_SIM 0x15 #define DRV_IMU_DEVTYPE_MPU6000 0x21 #define DRV_GYR_DEVTYPE_L3GD20 0x22 @@ -99,17 +100,19 @@ #define DRV_MAG_DEVTYPE_BMM150 0x43 #define DRV_IMU_DEVTYPE_ST_LSM9DS1_AG 0x44 #define DRV_MAG_DEVTYPE_ST_LSM9DS1_M 0x45 -#define DRV_DIFF_PRESS_DEVTYPE_ETS3 0x46 -#define DRV_DIFF_PRESS_DEVTYPE_MS4525 0x47 -#define DRV_DIFF_PRESS_DEVTYPE_MS5525 0x48 -#define DRV_DIFF_PRESS_DEVTYPE_SDP31 0x49 -#define DRV_DIFF_PRESS_DEVTYPE_SDP32 0x4A -#define DRV_DIFF_PRESS_DEVTYPE_SDP33 0x4B + +#define DRV_DIFF_PRESS_DEVTYPE_ETS3 0x46 +#define DRV_DIFF_PRESS_DEVTYPE_MS4515 0x47 +#define DRV_DIFF_PRESS_DEVTYPE_MS4525DO 0x48 +#define DRV_DIFF_PRESS_DEVTYPE_MS5525DSO 0x49 +#define DRV_DIFF_PRESS_DEVTYPE_SDP31 0x4A +#define DRV_DIFF_PRESS_DEVTYPE_SDP32 0x4B +#define DRV_DIFF_PRESS_DEVTYPE_SDP33 0x4C + #define DRV_BARO_DEVTYPE_LPS33HW 0x4C #define DRV_BARO_DEVTYPE_TCBP001TA 0x4D - -#define DRV_BARO_DEVTYPE_MS5837 0x4E -#define DRV_BARO_DEVTYPE_SPL06 0x4F +#define DRV_BARO_DEVTYPE_MS5837 0x4E +#define DRV_BARO_DEVTYPE_SPL06 0x4F #define DRV_BARO_DEVTYPE_MPL3115A2 0x51 #define DRV_ACC_DEVTYPE_FXOS8701C 0x52 diff --git a/src/lib/drivers/CMakeLists.txt b/src/lib/drivers/CMakeLists.txt index eafe7f2926..00c45648d4 100644 --- a/src/lib/drivers/CMakeLists.txt +++ b/src/lib/drivers/CMakeLists.txt @@ -32,7 +32,6 @@ ############################################################################ add_subdirectory(accelerometer) -add_subdirectory(airspeed) add_subdirectory(device) add_subdirectory(gyroscope) add_subdirectory(led) diff --git a/src/lib/drivers/airspeed/CMakeLists.txt b/src/lib/drivers/airspeed/CMakeLists.txt deleted file mode 100644 index 4161c5faca..0000000000 --- a/src/lib/drivers/airspeed/CMakeLists.txt +++ /dev/null @@ -1,35 +0,0 @@ -############################################################################ -# -# Copyright (c) 2015 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(drivers__airspeed airspeed.cpp) -target_link_libraries(drivers__airspeed PRIVATE drivers__device) diff --git a/src/lib/parameters/param_translation.cpp b/src/lib/parameters/param_translation.cpp index d024a303b9..f709f12bf1 100644 --- a/src/lib/parameters/param_translation.cpp +++ b/src/lib/parameters/param_translation.cpp @@ -178,5 +178,20 @@ bool param_modify_on_import(bson_node_t node) } } + // 2022-04-25 (v1.13 alpha): translate MS4525->MS4525DO and MS5525->MS5525DSO + { + if (strcmp("SENS_EN_MS4525", node->name) == 0) { + strcpy(node->name, "SENS_EN_MS4525DO"); + PX4_INFO("copying %s -> %s", "SENS_EN_MS4525", "SENS_EN_MS4525DO"); + return true; + } + + if (strcmp("SENS_EN_MS5525", node->name) == 0) { + strcpy(node->name, "SENS_EN_MS5525DS"); + PX4_INFO("copying %s -> %s", "SENS_EN_MS5525", "SENS_EN_MS5525DS"); + return true; + } + } + return false; } diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 51d6721229..6321c457e2 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -49,12 +49,14 @@ #include #include #include -#include +#include #include #include #include #include +using namespace time_literals; + static const char *sensor_name = "airspeed"; static void feedback_calibration_failed(orb_advert_t *mavlink_log_pub) @@ -69,7 +71,7 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub) int result = PX4_OK; unsigned calibration_counter = 0; - const unsigned maxcount = 2400; + const unsigned maxcount = 500; /* give directions */ calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name); @@ -78,15 +80,10 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub) float diff_pres_offset = 0.0f; - if (param_set(param_find("SENS_DPRES_OFF"), &diff_pres_offset) != PX4_OK) { - calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG); - return PX4_ERROR; - } - calibration_log_critical(mavlink_log_pub, "[cal] Ensure sensor is not measuring wind"); px4_usleep(500 * 1000); - uORB::SubscriptionBlocking diff_pres_sub{ORB_ID(differential_pressure)}; + uORB::SubscriptionData diff_pres_sub{ORB_ID(differential_pressure)}; while (calibration_counter < calibration_count) { @@ -94,31 +91,24 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub) return PX4_ERROR; } - differential_pressure_s diff_pres; + if (diff_pres_sub.update()) { - if (diff_pres_sub.updateBlocking(diff_pres, 1000000)) { + const differential_pressure_s &diff_pres = diff_pres_sub.get(); diff_pres_offset += diff_pres.differential_pressure_pa; calibration_counter++; - /* any differential pressure failure a reason to abort */ - if (diff_pres.error_count != 0) { - calibration_log_critical(mavlink_log_pub, "[cal] Airspeed sensor is reporting errors (%" PRIu32 ")", - diff_pres.error_count); - calibration_log_critical(mavlink_log_pub, "[cal] Check wiring, reboot vehicle, and try again"); - feedback_calibration_failed(mavlink_log_pub); - return PX4_ERROR; - } - if (calibration_counter % (calibration_count / 20) == 0) { calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, (calibration_counter * 80) / calibration_count); } + } - } else { - /* any poll failure for 1s is a reason to abort */ + if (hrt_elapsed_time(&calibration_started) > 30_s) { feedback_calibration_failed(mavlink_log_pub); return PX4_ERROR; } + + px4_usleep(10000); } diff_pres_offset = diff_pres_offset / calibration_count; @@ -132,7 +122,7 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub) diff_pres_offset = 0.00000001f; } - if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { + if (param_set(param_find("SENS_DPRES_OFF"), &diff_pres_offset)) { calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG); return PX4_ERROR; } @@ -161,9 +151,9 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub) return PX4_ERROR; } - differential_pressure_s diff_pres; + if (diff_pres_sub.update()) { + const differential_pressure_s &diff_pres = diff_pres_sub.get(); - if (diff_pres_sub.updateBlocking(diff_pres, 1000000)) { differential_pressure_sum += diff_pres.differential_pressure_pa; differential_pressure_sum_count++; @@ -197,7 +187,7 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub) } } - if (calibration_counter % 500 == 0) { + if (calibration_counter % 300 == 0) { calibration_log_info(mavlink_log_pub, "[cal] Create air pressure! (got %d, wanted: 50 Pa)", (int)differential_pressure_pa); tune_neutral(true); @@ -208,12 +198,14 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub) } calibration_counter++; + } - } else { - /* any poll failure for 1s is a reason to abort */ + if (hrt_elapsed_time(&calibration_started) > 90_s) { feedback_calibration_failed(mavlink_log_pub); return PX4_ERROR; } + + px4_usleep(10000); } if (calibration_counter == maxcount) { @@ -227,7 +219,7 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub) tune_neutral(true); // This give a chance for the log messages to go out of the queue before someone else stomps on then - px4_sleep(1); + px4_usleep(200000); return result; } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 370bf87e1e..84471a2287 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2312,7 +2312,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) sensor_baro_s sensor_baro{}; sensor_baro.timestamp_sample = timestamp; sensor_baro.device_id = 6620172; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION - sensor_baro.pressure = hil_sensor.abs_pressure; + sensor_baro.pressure = hil_sensor.abs_pressure * 100.0f; // hPa to Pa sensor_baro.temperature = hil_sensor.temperature; sensor_baro.error_count = 0; sensor_baro.timestamp = hrt_absolute_time(); @@ -2323,9 +2323,9 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) if ((hil_sensor.fields_updated & SensorSource::DIFF_PRESS) == SensorSource::DIFF_PRESS) { differential_pressure_s report{}; report.timestamp_sample = timestamp; - report.device_id = 0; // TODO + report.device_id = 1377548; // 1377548: DRV_DIFF_PRESS_DEVTYPE_SIM, BUS: 1, ADDR: 5, TYPE: SIMULATION report.temperature = hil_sensor.temperature; - report.differential_pressure_pa = hil_sensor.diff_pressure * 100.0f; // convert from millibar to bar; + report.differential_pressure_pa = hil_sensor.diff_pressure * 100.0f; // hPa to Pa report.timestamp = hrt_absolute_time(); _differential_pressure_pub.publish(report); } @@ -2653,6 +2653,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) /* airspeed */ { airspeed_s airspeed{}; + airspeed.timestamp_sample = timestamp_sample; airspeed.indicated_airspeed_m_s = hil_state.ind_airspeed * 1e-2f; airspeed.true_airspeed_m_s = hil_state.true_airspeed * 1e-2f; airspeed.air_temperature_celsius = 15.f; diff --git a/src/modules/mavlink/streams/SCALED_PRESSURE.hpp b/src/modules/mavlink/streams/SCALED_PRESSURE.hpp index 2b2b57e941..7ae9908316 100644 --- a/src/modules/mavlink/streams/SCALED_PRESSURE.hpp +++ b/src/modules/mavlink/streams/SCALED_PRESSURE.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2021-2022 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 @@ -72,8 +72,8 @@ private: if (_sensor_baro_sub.copy(&sensor_baro)) { msg.time_boot_ms = sensor_baro.timestamp / 1000; - msg.press_abs = sensor_baro.pressure; // millibar to hPa - msg.temperature = roundf(sensor_baro.temperature * 100.f); // centidegrees + msg.press_abs = sensor_baro.pressure * 0.01f; // Pa to hPa + msg.temperature = roundf(sensor_baro.temperature * 100.f); // cdegC (centidegrees) } differential_pressure_s differential_pressure; @@ -83,8 +83,8 @@ private: msg.time_boot_ms = differential_pressure.timestamp / 1000; } - msg.press_diff = differential_pressure.differential_pressure_pa * 100.f; // Pa to hPa - msg.temperature_press_diff = roundf(differential_pressure.temperature * 100.f); // centidegrees + msg.press_diff = differential_pressure.differential_pressure_pa * 0.01f; // Pa to hPa + msg.temperature_press_diff = roundf(differential_pressure.temperature * 100.f); // cdegC (centidegrees) } mavlink_msg_scaled_pressure_send_struct(_mavlink->get_channel(), &msg); diff --git a/src/modules/mavlink/streams/SCALED_PRESSURE2.hpp b/src/modules/mavlink/streams/SCALED_PRESSURE2.hpp index d57925bd48..f453c4ff18 100644 --- a/src/modules/mavlink/streams/SCALED_PRESSURE2.hpp +++ b/src/modules/mavlink/streams/SCALED_PRESSURE2.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2021-2022 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 @@ -72,8 +72,8 @@ private: if (_sensor_baro_sub.copy(&sensor_baro)) { msg.time_boot_ms = sensor_baro.timestamp / 1000; - msg.press_abs = sensor_baro.pressure; // millibar to hPa - msg.temperature = roundf(sensor_baro.temperature * 100.f); // centidegrees + msg.press_abs = sensor_baro.pressure * 0.01f; // Pa to hPa + msg.temperature = roundf(sensor_baro.temperature * 100.f); // cdegC (centidegrees) } differential_pressure_s differential_pressure; @@ -83,8 +83,8 @@ private: msg.time_boot_ms = differential_pressure.timestamp / 1000; } - msg.press_diff = differential_pressure.differential_pressure_pa * 100.f; // Pa to hPa - msg.temperature_press_diff = roundf(differential_pressure.temperature * 100.f); // centidegrees + msg.press_diff = differential_pressure.differential_pressure_pa * 0.01f; // Pa to hPa + msg.temperature_press_diff = roundf(differential_pressure.temperature * 100.f); // cdegC (centidegrees) } mavlink_msg_scaled_pressure2_send_struct(_mavlink->get_channel(), &msg); diff --git a/src/modules/mavlink/streams/SCALED_PRESSURE3.hpp b/src/modules/mavlink/streams/SCALED_PRESSURE3.hpp index bff511645e..7b3011bb66 100644 --- a/src/modules/mavlink/streams/SCALED_PRESSURE3.hpp +++ b/src/modules/mavlink/streams/SCALED_PRESSURE3.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2021-2022 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 @@ -72,8 +72,8 @@ private: if (_sensor_baro_sub.copy(&sensor_baro)) { msg.time_boot_ms = sensor_baro.timestamp / 1000; - msg.press_abs = sensor_baro.pressure; // millibar to hPa - msg.temperature = roundf(sensor_baro.temperature * 100.f); // centidegrees + msg.press_abs = sensor_baro.pressure * 0.01f; // Pa to hPa + msg.temperature = roundf(sensor_baro.temperature * 100.f); // cdegC (centidegrees) } differential_pressure_s differential_pressure; @@ -83,8 +83,8 @@ private: msg.time_boot_ms = differential_pressure.timestamp / 1000; } - msg.press_diff = differential_pressure.differential_pressure_pa * 100.f; // Pa to hPa - msg.temperature_press_diff = roundf(differential_pressure.temperature * 100.f); // centidegrees + msg.press_diff = differential_pressure.differential_pressure_pa * 0.01f; // Pa to hPa + msg.temperature_press_diff = roundf(differential_pressure.temperature * 100.f); // cdegC (centidegrees) } mavlink_msg_scaled_pressure3_send_struct(_mavlink->get_channel(), &msg); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 2c1a99f0a1..30ba6aeaac 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -439,7 +439,7 @@ void Sensors::diff_pres_poll() _baro_pressure_sum += air_data.baro_pressure_pa; _diff_pres_count++; - if ((_diff_pres_count > 0) && hrt_elapsed_time(&_airspeed_last_publish) >= 100_ms) { + 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; diff --git a/src/modules/sih/sih.cpp b/src/modules/sih/sih.cpp index 4408862a9e..77b3e13819 100644 --- a/src/modules/sih/sih.cpp +++ b/src/modules/sih/sih.cpp @@ -522,12 +522,13 @@ void Sih::send_gps() void Sih::send_airspeed() { - airspeed_s airspeed{}; - airspeed.timestamp = _now; + airspeed_s airspeed{}; + airspeed.timestamp_sample = _now; airspeed.true_airspeed_m_s = fmaxf(0.1f, _v_B(0) + generate_wgn() * 0.2f); airspeed.indicated_airspeed_m_s = airspeed.true_airspeed_m_s * sqrtf(_wing_l.get_rho() / RHO); airspeed.air_temperature_celsius = _baro_temp_c; airspeed.confidence = 0.7f; + airspeed.timestamp = hrt_absolute_time(); _airspeed_pub.publish(airspeed); } diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index c4ce824f18..8cdd807672 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -161,8 +161,6 @@ private: static Simulator *_instance; - static constexpr float hPa2Pa = 100.0f; // hectopascal to pascal multiplier - // simulated sensor instances static constexpr uint8_t ACCEL_COUNT_MAX = 3; PX4Accelerometer _px4_accel[ACCEL_COUNT_MAX] { diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index b62b138544..d7eebd68ff 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -328,7 +328,7 @@ void Simulator::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor if ((sensors.fields_updated & SensorSource::BARO) == SensorSource::BARO && !_baro_blocked) { if (!_baro_stuck) { - _last_baro_pressure = sensors.abs_pressure * hPa2Pa; // convert hPa to Pa + _last_baro_pressure = sensors.abs_pressure * 100.f; // hPa to Pa _last_baro_temperature = sensors.temperature; } @@ -353,8 +353,8 @@ void Simulator::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor if ((sensors.fields_updated & SensorSource::DIFF_PRESS) == SensorSource::DIFF_PRESS && !_airspeed_blocked) { differential_pressure_s report{}; report.timestamp_sample = time; - report.device_id = 0; // TODO - report.differential_pressure_pa = sensors.diff_pressure * hPa2Pa; // convert hPa to Pa; + report.device_id = 1377548; // 1377548: DRV_DIFF_PRESS_DEVTYPE_SIM, BUS: 1, ADDR: 5, TYPE: SIMULATION + report.differential_pressure_pa = sensors.diff_pressure * 100.f; // hPa to Pa; report.temperature = _sensors_temperature; report.timestamp = hrt_absolute_time(); _differential_pressure_pub.publish(report);