dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

drivers/differential_pressure: remove lib/drivers/airspeed dependency and cleanup 

- split ms4525_airspeed into separate ms4515 and ms4525 drivers
This commit is contained in:
Daniel Agar 2021-12-27 11:01:25 -05:00
parent d1d15a6f6d
commit f390f52058
50 changed files with 1350 additions and 879 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
ROMFS/px4fmu_common/init.d/rc.sensors
View File

@ -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

6
ROMFS/px4fmu_test/init.d/rc.sensors
View File

@ -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

16
boards/ark/cannode/init/rc.board_sensors
View File

@ -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

2
boards/av/x-v1/init/rc.board_sensors
View File

@ -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

2
boards/px4/fmu-v2/fixedwing.px4board
View File

@ -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

2
posix-configs/rpi/pilotpi_fw.config
View File

@ -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

2
posix-configs/rpi/px4_fw.config
View File

@ -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

5
src/drivers/differential_pressure/Kconfig
View File

@ -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

10
src/drivers/differential_pressure/ets/CMakeLists.txt
View File

@ -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
)

164
src/drivers/differential_pressure/ets/ets_airspeed.cpp → src/drivers/differential_pressure/ets/ETSAirspeed.cpp
View File

@ -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 <float.h>
#include <drivers/airspeed/airspeed.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/i2c_spi_buses.h>
/* 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>
{
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),
ETSAirspeed::ETSAirspeed(const I2CSPIDriverConfig &config) :
I2C(config),
I2CSPIDriver(config)
{
_device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_MS4525;
}
ETSAirspeed::~ETSAirspeed()
{
perf_free(_sample_perf);
perf_free(_comms_errors);
}
int ETSAirspeed::probe()
{
_retries = 1;
int ret = measure();
return ret;
}
int ETSAirspeed::init()
{
int ret = Airspeed::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;
}

103
src/lib/drivers/airspeed/airspeed.cpp → src/drivers/differential_pressure/ets/ETSAirspeed.hpp
View File

@ -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 <simon@px4.io>
* @author Lorenz Meier <lorenz@px4.io>
*
* Driver for the Eagle Tree Airspeed V3 connected via I2C.
*/
#include <px4_platform_common/px4_config.h>
#include <drivers/device/device.h>
#pragma once
#include <drivers/device/i2c.h>
#include <systemlib/err.h>
#include <parameters/param.h>
#include <perf/perf_counter.h>
#include <float.h>
#include <drivers/drv_hrt.h>
#include <lib/drivers/device/i2c.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/i2c_spi_buses.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/differential_pressure.h>
#include <drivers/airspeed/airspeed.h>
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 */
Airspeed::~Airspeed()
{
// free perf counters
perf_free(_sample_perf);
perf_free(_comms_errors);
}
int
Airspeed::init()
{
/* do I2C init (and probe) first */
if (I2C::init() != PX4_OK) {
return PX4_ERROR;
}
/* advertise sensor topic, measure manually to initialize valid report */
measure();
return PX4_OK;
}
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
/**
* 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.
*/
_retries = 1;
int ret = measure();
#define MIN_ACCURATE_DIFF_PRES_PA 0
return ret;
}
/* Measurement rate is 100Hz */
#define CONVERSION_INTERVAL (1000000 / 100) /* microseconds */
class ETSAirspeed : public device::I2C, public I2CSPIDriver<ETSAirspeed>
{
public:
ETSAirspeed(const I2CSPIDriverConfig &config);
~ETSAirspeed() 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};
bool _sensor_ok{false};
bool _collect_phase{false};
uORB::PublicationMulti<differential_pressure_s> _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")};
};

77
src/drivers/differential_pressure/ets/ets_airspeed_main.cpp Normal file
View File

@ -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 <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
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;
}

14
src/drivers/differential_pressure/ms4525/CMakeLists.txt → src/drivers/differential_pressure/ms4515/CMakeLists.txt
View File

@ -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
)

5
src/drivers/differential_pressure/ms4515/Kconfig Normal file
View File

@ -0,0 +1,5 @@
menuconfig DRIVERS_DIFFERENTIAL_PRESSURE_MS4515
bool "ms4515"
default n
---help---
Enable support for ms4515

222
src/drivers/differential_pressure/ms4515/MS4515.cpp Normal file
View File

@ -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);
}

97
src/drivers/differential_pressure/ms4515/MS4515.hpp Normal file
View File

@ -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 <drivers/drv_hrt.h>
#include <lib/drivers/device/i2c.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/i2c_spi_buses.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/differential_pressure.h>
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<MS4515>
{
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_s> _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")};
};

77
src/drivers/differential_pressure/ms4515/ms4515_main.cpp Normal file
View File

@ -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 <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
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;
}

4
src/drivers/differential_pressure/ms4525/parameters.c → src/drivers/differential_pressure/ms4515/parameters.c
View File

@ -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);

5
src/drivers/differential_pressure/ms4525/Kconfig
View File

@ -1,5 +0,0 @@
menuconfig DRIVERS_DIFFERENTIAL_PRESSURE_MS4525
bool "ms4525"
default n
---help---
Enable support for ms4525

327
src/drivers/differential_pressure/ms4525/ms4525_airspeed.cpp
View File

@ -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 <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/i2c_spi_buses.h>
#include <drivers/airspeed/airspeed.h>
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<MEASAirspeed>
{
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;
}

15
src/drivers/differential_pressure/ms5525/CMakeLists.txt → src/drivers/differential_pressure/ms4525do/CMakeLists.txt
View File

@ -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
)

5
src/drivers/differential_pressure/ms4525do/Kconfig Normal file
View File

@ -0,0 +1,5 @@
menuconfig DRIVERS_DIFFERENTIAL_PRESSURE_MS4525DO
bool "ms4525do"
default n
---help---
Enable support for ms4525do

221
src/drivers/differential_pressure/ms4525do/MS4525DO.cpp Normal file
View File

@ -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;
}
}

97
src/drivers/differential_pressure/ms4525do/MS4525DO.hpp Normal file
View File

@ -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 <drivers/drv_hrt.h>
#include <lib/drivers/device/i2c.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/i2c_spi_buses.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/differential_pressure.h>
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<MS4525DO>
{
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_s> _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")};
};

77
src/drivers/differential_pressure/ms4525do/ms4525do_main.cpp Normal file
View File

@ -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 <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
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;
}

6
src/drivers/differential_pressure/ms5525/parameters.c → src/drivers/differential_pressure/ms4525do/parameters.c
View File

@ -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);

5
src/drivers/differential_pressure/ms5525/Kconfig
View File

@ -1,5 +0,0 @@
menuconfig DRIVERS_DIFFERENTIAL_PRESSURE_MS5525
bool "ms5525"
default n
---help---
Enable support for ms5525

17
src/drivers/differential_pressure/CMakeLists.txt → src/drivers/differential_pressure/ms5525dso/CMakeLists.txt
View File

@ -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)
px4_add_module(
MODULE drivers__differential_pressure__ms5525dso
MAIN ms5525dso
COMPILE_FLAGS
SRCS
ms5525dso_main.cpp
MS5525DSO.cpp
MS5525DSO.hpp
DEPENDS
px4_work_queue
)

5
src/drivers/differential_pressure/ms5525dso/Kconfig Normal file
View File

@ -0,0 +1,5 @@
menuconfig DRIVERS_DIFFERENTIAL_PRESSURE_MS5525DSO
bool "ms5525dso"
default n
---help---
Enable support for ms5525dso

110
src/drivers/differential_pressure/ms5525/MS5525.cpp → src/drivers/differential_pressure/ms5525dso/MS5525DSO.cpp
View File

@ -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");
}

54
src/drivers/differential_pressure/ms5525/MS5525.hpp → src/drivers/differential_pressure/ms5525dso/MS5525DSO.hpp
View File

@ -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 <drivers/airspeed/airspeed.h>
#include <math.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/i2c_spi_buses.h>
/* 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 <drivers/drv_hrt.h>
#include <lib/drivers/device/i2c.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/i2c_spi_buses.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/differential_pressure.h>
/* 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<MS5525>
class MS5525DSO : public device::I2C, public I2CSPIDriver<MS5525DSO>
{
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();
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_s> _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")};
};

29
src/drivers/differential_pressure/ms5525/MS5525_main.cpp → src/drivers/differential_pressure/ms5525dso/ms5525dso_main.cpp
View File

@ -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 <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
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);
}

54
src/lib/drivers/airspeed/airspeed.h → src/drivers/differential_pressure/ms5525dso/parameters.c
View File

@ -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 <string.h>
#include <drivers/device/i2c.h>
#include <drivers/drv_hrt.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>
#include <perf/perf_counter.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/PublicationMulti.hpp>
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.
/**
* TE MS5525DSO differential pressure sensor (external I2C)
*
* @reboot_required true
* @group Sensors
* @boolean
*/
virtual int measure() = 0;
virtual int collect() = 0;
bool _sensor_ok;
int _measure_interval;
bool _collect_phase;
uORB::PublicationMulti<differential_pressure_s> _airspeed_pub{ORB_ID(differential_pressure)};
unsigned _conversion_interval;
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
};
PARAM_DEFINE_INT32(SENS_EN_MS5525DS, 0);

13
src/drivers/differential_pressure/sdp3x/CMakeLists.txt
View File

@ -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
)

102
src/drivers/differential_pressure/sdp3x/SDP3X.cpp
View File

@ -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<float>(P) / static_cast<float>(_scale);
float temperature_c = temp / static_cast<float>(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;
}

39
src/drivers/differential_pressure/sdp3x/SDP3X.hpp
View File

@ -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,16 +40,19 @@
#pragma once
#include <drivers/airspeed/airspeed.h>
#include <math.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
#include <drivers/drv_hrt.h>
#include <lib/drivers/device/i2c.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/i2c_spi_buses.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/differential_pressure.h>
#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
#define SDP3X_RESET_CMD 0x06
@ -65,34 +67,30 @@
#define SPD3X_MEAS_RATE 100
#define CONVERSION_INTERVAL (1000000 / SPD3X_MEAS_RATE) /* microseconds */
class SDP3X : public Airspeed, public I2CSPIDriver<SDP3X>
class SDP3X : public device::I2C, public I2CSPIDriver<SDP3X>
{
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();
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 collect();
int configure();
int read_scale();
@ -111,4 +109,9 @@ private:
uint16_t _scale{0};
const bool _keep_retrying;
State _state{State::RequireConfig};
uORB::PublicationMulti<differential_pressure_s> _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")};
};

22
src/drivers/differential_pressure/sdp3x/SDP3X_main.cpp → src/drivers/differential_pressure/sdp3x/sdp3x_main.cpp
View File

@ -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 <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
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);
}

15
src/drivers/drv_sensor.h
View File

@ -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,15 +100,17 @@
#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_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

1
src/lib/drivers/CMakeLists.txt
View File

@ -32,7 +32,6 @@
############################################################################
add_subdirectory(accelerometer)
add_subdirectory(airspeed)
add_subdirectory(device)
add_subdirectory(gyroscope)
add_subdirectory(led)

35
src/lib/drivers/airspeed/CMakeLists.txt
View File

@ -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)

15
src/lib/parameters/param_translation.cpp
View File

@ -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;
}

48
src/modules/commander/airspeed_calibration.cpp
View File

@ -49,12 +49,14 @@
#include <fcntl.h>
#include <math.h>
#include <drivers/drv_hrt.h>
#include <uORB/SubscriptionBlocking.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/differential_pressure.h>
#include <systemlib/mavlink_log.h>
#include <parameters/param.h>
#include <systemlib/err.h>
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<differential_pressure_s> diff_pres_sub{ORB_ID(differential_pressure)};
uORB::SubscriptionData<differential_pressure_s> 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;
}

7
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -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;

10
src/modules/mavlink/streams/SCALED_PRESSURE.hpp
View File

@ -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);

10
src/modules/mavlink/streams/SCALED_PRESSURE2.hpp
View File

@ -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);

10
src/modules/mavlink/streams/SCALED_PRESSURE3.hpp
View File

@ -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);

2
src/modules/sensors/sensors.cpp
View File

@ -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;

3
src/modules/sih/sih.cpp
View File

@ -523,11 +523,12 @@ void Sih::send_gps()
void Sih::send_airspeed()
{
airspeed_s airspeed{};
airspeed.timestamp = _now;
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);
}

2
src/modules/simulator/simulator.h
View File

@ -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] {

6
src/modules/simulator/simulator_mavlink.cpp
View File

@ -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);