fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

differential_pressure sensors: use driver base class

Browse Source
This commit is contained in:
Beat Küng 2020-03-03 19:02:11 +01:00 committed by Daniel Agar
parent 0924ff7fcf
commit 3b7b2dc871
14 changed files with 248 additions and 890 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -24,7 +24,8 @@ if [ ${VEHICLE_TYPE} = fw -o ${VEHICLE_TYPE} = vtol ]
then
if param compare CBRK_AIRSPD_CHK 0
then
sdp3x_airspeed start -a
sdp3x_airspeed start -X
sdp3x_airspeed start -X -a 0x22
# Pixhawk 2.1 has a MS5611 on I2C which gets wrongly
# detected as MS5525 because the chip manufacturer was so
@ -32,14 +33,14 @@ then
# register.
if [ $BOARD_FMUV3 = 21 ]
then
ms5525_airspeed start -b 2
ms5525_airspeed start -X -b 2
else
ms5525_airspeed start -a
ms5525_airspeed start -X
fi
ms4525_airspeed start -a
ms4525_airspeed start -X
ets_airspeed start -a
ets_airspeed start -X
fi
fi

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

@ -21,31 +21,19 @@ if adc start
then
fi
if sdp3x_airspeed start
if sdp3x_airspeed start -X
then
fi
if ms5525_airspeed start
if ms5525_airspeed start -X
then
fi
if ms5525_airspeed start -b 2
if ms4525_airspeed start -X
then
fi
if ms4525_airspeed start
then
fi
if ms4525_airspeed start -b 2
then
fi
if ets_airspeed start
then
fi
if ets_airspeed start -b 1
if ets_airspeed start -X
then
fi

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

@ -11,7 +11,7 @@ lps22hb -s start
lsm303agr -R 4 start
ms4525_airspeed -T 4515 -b 3 start
ms4525_airspeed -T 4515 -I -b 3 start
if ! param greater SENS_EN_PMW3901 0
then

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

@ -30,7 +30,7 @@ adc start
battery_status start
gps start -d /dev/spidev0.0 -i spi -p ubx
ms4525_airspeed start
ms4525_airspeed start -X
rc_update start
sensors start
commander start

242
src/drivers/differential_pressure/ets/ets_airspeed.cpp
View File

@ -42,6 +42,8 @@
#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 */
@ -59,21 +61,22 @@
/* Measurement rate is 100Hz */
#define CONVERSION_INTERVAL (1000000 / 100) /* microseconds */
class ETSAirspeed : public Airspeed
class ETSAirspeed : public Airspeed, public I2CSPIDriver<ETSAirspeed>
{
public:
ETSAirspeed(int bus, int address = I2C_ADDRESS, const char *path = ETS_PATH);
ETSAirspeed(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address = I2C_ADDRESS,
const char *path = ETS_PATH);
virtual ~ETSAirspeed() = default;
static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance);
static void print_usage();
void RunImpl();
protected:
/**
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*/
void Run() override;
int measure() override;
int collect() override;
};
/*
@ -81,8 +84,9 @@ protected:
*/
extern "C" __EXPORT int ets_airspeed_main(int argc, char *argv[]);
ETSAirspeed::ETSAirspeed(int bus, int address, const char *path) : Airspeed(bus, address,
CONVERSION_INTERVAL, path)
ETSAirspeed::ETSAirspeed(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address, const char *path)
: Airspeed(bus, bus_frequency, address, CONVERSION_INTERVAL, path),
I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, address)
{
_device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_MS4525;
}
@ -157,7 +161,7 @@ ETSAirspeed::collect()
}
void
ETSAirspeed::Run()
ETSAirspeed::RunImpl()
{
int ret;
@ -170,8 +174,9 @@ ETSAirspeed::Run()
if (OK != ret) {
perf_count(_comms_errors);
/* restart the measurement state machine */
start();
_collect_phase = false;
_sensor_ok = false;
ScheduleNow();
return;
}
@ -206,206 +211,63 @@ ETSAirspeed::Run()
ScheduleDelayed(CONVERSION_INTERVAL);
}
/**
* Local functions in support of the shell command.
*/
namespace ets_airspeed
I2CSPIDriverBase *ETSAirspeed::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance)
{
ETSAirspeed *instance = new ETSAirspeed(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency);
ETSAirspeed *g_dev = nullptr;
int start();
int start_bus(int i2c_bus);
int stop();
int reset();
int info();
/**
* Attempt to start driver on all available I2C busses.
*
* This function will return as soon as the first sensor
* is detected on one of the available busses or if no
* sensors are detected.
*
*/
int
start()
{
for (unsigned i = 0; i < NUM_I2C_BUS_OPTIONS; i++) {
if (start_bus(i2c_bus_options[i]) == PX4_OK) {
return PX4_OK;
}
if (instance == nullptr) {
PX4_ERR("alloc failed");
return nullptr;
}
return PX4_ERROR;
if (instance->init() != PX4_OK) {
delete instance;
return nullptr;
}
instance->ScheduleNow();
return instance;
}
/**
* Start the driver on a specific bus.
*
* This function only returns if the sensor is up and running
* or could not be detected successfully.
*/
int
start_bus(int i2c_bus)
void
ETSAirspeed::print_usage()
{
int fd;
if (g_dev != nullptr) {
PX4_ERR("already started");
return PX4_ERROR;
}
/* create the driver */
g_dev = new ETSAirspeed(i2c_bus);
if (g_dev == nullptr) {
goto fail;
}
if (OK != g_dev->init()) {
goto fail;
}
/* set the poll rate to default, starts automatic data collection */
fd = px4_open(AIRSPEED0_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
goto fail;
}
if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
goto fail;
}
return PX4_OK;
fail:
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
}
return PX4_ERROR;
}
/**
* Stop the driver
*/
int
stop()
{
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
} else {
PX4_ERR("driver not running");
return PX4_ERROR;
}
return PX4_OK;
}
/**
* Reset the driver.
*/
int
reset()
{
int fd = px4_open(ETS_PATH, O_RDONLY);
if (fd < 0) {
PX4_ERR("failed");
return PX4_ERROR;
}
if (px4_ioctl(fd, SENSORIOCRESET, 0) < 0) {
PX4_ERR("driver reset failed");
return PX4_ERROR;
}
if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
PX4_ERR("driver poll restart failed");
return PX4_ERROR;
}
return PX4_OK;
}
} // namespace
static void
ets_airspeed_usage()
{
PX4_INFO("usage: ets_airspeed command [options]");
PX4_INFO("options:");
PX4_INFO("\t-b --bus i2cbus (%d)", PX4_I2C_BUS_DEFAULT);
PX4_INFO("\t-a --all");
PX4_INFO("command:");
PX4_INFO("\tstart|stop|reset|info");
PRINT_MODULE_USAGE_NAME("ets_airspeed", "driver");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
}
int
ets_airspeed_main(int argc, char *argv[])
{
int i2c_bus = PX4_I2C_BUS_DEFAULT;
using ThisDriver = ETSAirspeed;
BusCLIArguments cli{true, false};
cli.default_i2c_frequency = 100000;
int myoptind = 1;
int ch;
const char *myoptarg = nullptr;
bool start_all = false;
const char *verb = cli.parseDefaultArguments(argc, argv);
while ((ch = px4_getopt(argc, argv, "ab:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'b':
i2c_bus = atoi(myoptarg);
break;
case 'a':
start_all = true;
break;
default:
ets_airspeed_usage();
return 0;
}
}
if (myoptind >= argc) {
ets_airspeed_usage();
if (!verb) {
ThisDriver::print_usage();
return -1;
}
/*
* Start/load the driver.
*/
if (!strcmp(argv[myoptind], "start")) {
if (start_all) {
return ets_airspeed::start();
BusInstanceIterator iterator(MODULE_NAME, cli, DRV_DIFF_PRESS_DEVTYPE_ETS3);
} else {
return ets_airspeed::start_bus(i2c_bus);
}
if (!strcmp(verb, "start")) {
return ThisDriver::module_start(cli, iterator);
}
/*
* Stop the driver
*/
if (!strcmp(argv[myoptind], "stop")) {
return ets_airspeed::stop();
if (!strcmp(verb, "stop")) {
return ThisDriver::module_stop(iterator);
}
/*
* Reset the driver.
*/
if (!strcmp(argv[myoptind], "reset")) {
return ets_airspeed::reset();
if (!strcmp(verb, "status")) {
return ThisDriver::module_status(iterator);
}
ets_airspeed_usage();
return 0;
ThisDriver::print_usage();
return -1;
}

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

@ -51,6 +51,8 @@
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/i2c_spi_buses.h>
#include <uORB/topics/system_power.h>
#include <drivers/airspeed/airspeed.h>
@ -74,18 +76,22 @@ enum MS_DEVICE_TYPE {
#define CONVERSION_INTERVAL (1000000 / MEAS_RATE) /* microseconds */
class MEASAirspeed : public Airspeed
class MEASAirspeed : public Airspeed, public I2CSPIDriver<MEASAirspeed>
{
public:
MEASAirspeed(int bus, int address = I2C_ADDRESS_MS4525DO, const char *path = PATH_MS4525);
MEASAirspeed(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address = I2C_ADDRESS_MS4525DO,
const char *path = PATH_MS4525);
virtual ~MEASAirspeed() = default;
static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance);
static void print_usage();
void RunImpl();
protected:
/**
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*/
void Run() override;
int measure() override;
int collect() override;
@ -105,7 +111,9 @@ protected:
*/
extern "C" __EXPORT int ms4525_airspeed_main(int argc, char *argv[]);
MEASAirspeed::MEASAirspeed(int bus, int address, const char *path) : Airspeed(bus, address, CONVERSION_INTERVAL, path)
MEASAirspeed::MEASAirspeed(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address, const char *path)
: Airspeed(bus, bus_frequency, address, CONVERSION_INTERVAL, path),
I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, address)
{
_device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_MS4525;
}
@ -221,7 +229,7 @@ MEASAirspeed::collect()
}
void
MEASAirspeed::Run()
MEASAirspeed::RunImpl()
{
int ret;
@ -233,8 +241,9 @@ MEASAirspeed::Run()
if (OK != ret) {
/* restart the measurement state machine */
start();
_collect_phase = false;
_sensor_ok = false;
ScheduleNow();
return;
}
@ -336,216 +345,83 @@ MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature)
#endif // defined(ADC_SCALED_V5_SENSE)
}
/**
* Local functions in support of the shell command.
*/
namespace meas_airspeed
I2CSPIDriverBase *MEASAirspeed::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance)
{
MEASAirspeed *instance = new MEASAirspeed(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency,
cli.i2c_address);
MEASAirspeed *g_dev = nullptr;
int start();
int start_bus(int i2c_bus, int address);
int stop();
int reset();
/**
* Attempt to start driver on all available I2C busses.
*
* This function will return as soon as the first sensor
* is detected on one of the available busses or if no
* sensors are detected.
*
*/
int
start()
{
for (unsigned i = 0; i < NUM_I2C_BUS_OPTIONS; i++) {
if (start_bus(i2c_bus_options[i], I2C_ADDRESS_MS4525DO) == PX4_OK) {
return PX4_OK;
}
if (instance == nullptr) {
PX4_ERR("alloc failed");
return nullptr;
}
return PX4_ERROR;
if (instance->init() != PX4_OK) {
delete instance;
return nullptr;
}
instance->ScheduleNow();
return instance;
}
/**
* Start the driver on a specific bus.
*
* This function call only returns once the driver is up and running
* or failed to detect the sensor.
*/
int
start_bus(int i2c_bus, int address)
void
MEASAirspeed::print_usage()
{
int fd;
if (g_dev != nullptr) {
PX4_ERR("already started");
return PX4_ERROR;
}
/* create the driver, try the MS4525DO first */
g_dev = new MEASAirspeed(i2c_bus, address, PATH_MS4525);
/* check if the MS4525DO was instantiated */
if (g_dev == nullptr) {
goto fail;
}
if (OK != g_dev->init()) {
goto fail;
}
/* set the poll rate to default, starts automatic data collection */
fd = px4_open(PATH_MS4525, O_RDONLY);
if (fd < 0) {
goto fail;
}
if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
goto fail;
}
return PX4_OK;
fail:
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
}
return PX4_ERROR;
}
/**
* Stop the driver
*/
int
stop()
{
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
} else {
PX4_ERR("driver not running");
return PX4_ERROR;
}
return PX4_OK;
}
/**
* Reset the driver.
*/
int
reset()
{
int fd = px4_open(PATH_MS4525, O_RDONLY);
if (fd < 0) {
PX4_ERR("failed");
return PX4_ERROR;
}
if (px4_ioctl(fd, SENSORIOCRESET, 0) < 0) {
PX4_ERR("driver reset failed");
return PX4_ERROR;
}
if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
PX4_ERR("driver poll restart failed");
return PX4_ERROR;
}
return PX4_OK;
}
} // namespace
static void
meas_airspeed_usage()
{
PX4_INFO("usage: ms4525 command [options]");
PX4_INFO("options:");
PX4_INFO("\t-b --bus i2cbus (%d)", PX4_I2C_BUS_DEFAULT);
PX4_INFO("\t-a --all");
PX4_INFO("command:");
PX4_INFO("\tstart|stop|reset");
PRINT_MODULE_USAGE_NAME("ms4525_airspeed", "driver");
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 i2c_bus = PX4_I2C_BUS_DEFAULT;
int myoptind = 1;
int ch;
const char *myoptarg = nullptr;
using ThisDriver = MEASAirspeed;
BusCLIArguments cli{true, false};
cli.default_i2c_frequency = 100000;
int device_type = DEVICE_TYPE_MS4525;
bool start_all = false;
while ((ch = px4_getopt(argc, argv, "ab:T:", &myoptind, &myoptarg)) != EOF) {
while ((ch = cli.getopt(argc, argv, "T:")) != EOF) {
switch (ch) {
case 'b':
i2c_bus = atoi(myoptarg);
break;
case 'a':
start_all = true;
break;
case 'T':
device_type = atoi(myoptarg);
device_type = atoi(cli.optarg());
break;
default:
meas_airspeed_usage();
return 0;
}
}
if (myoptind >= argc) {
meas_airspeed_usage();
const char *verb = cli.optarg();
if (!verb) {
ThisDriver::print_usage();
return -1;
}
/*
* Start/load the driver.
*/
if (!strcmp(argv[myoptind], "start")) {
if (start_all) {
return meas_airspeed::start();
if (device_type == DEVICE_TYPE_MS4525) {
cli.i2c_address = I2C_ADDRESS_MS4525DO;
} else if (device_type == DEVICE_TYPE_MS4515) {
return meas_airspeed::start_bus(i2c_bus, I2C_ADDRESS_MS4515DO);
} else if (device_type == DEVICE_TYPE_MS4525) {
return meas_airspeed::start_bus(i2c_bus, I2C_ADDRESS_MS4525DO);
}
} else {
cli.i2c_address = I2C_ADDRESS_MS4515DO;
}
/*
* Stop the driver
*/
if (!strcmp(argv[myoptind], "stop")) {
return meas_airspeed::stop();
BusInstanceIterator iterator(MODULE_NAME, cli,
DRV_DIFF_PRESS_DEVTYPE_MS4525);
if (!strcmp(verb, "start")) {
return ThisDriver::module_start(cli, iterator);
}
/*
* Reset the driver.
*/
if (!strcmp(argv[myoptind], "reset")) {
return meas_airspeed::reset();
if (!strcmp(verb, "stop")) {
return ThisDriver::module_stop(iterator);
}
meas_airspeed_usage();
return 0;
if (!strcmp(verb, "status")) {
return ThisDriver::module_status(iterator);
}
ThisDriver::print_usage();
return -1;
}

5
src/drivers/differential_pressure/ms5525/MS5525.cpp
View File

@ -269,7 +269,7 @@ MS5525::collect()
}
void
MS5525::Run()
MS5525::RunImpl()
{
int ret = PX4_ERROR;
@ -280,8 +280,9 @@ MS5525::Run()
if (OK != ret) {
/* restart the measurement state machine */
start();
_collect_phase = false;
_sensor_ok = false;
ScheduleNow();
return;
}

29
src/drivers/differential_pressure/ms5525/MS5525.hpp
View File

@ -31,13 +31,14 @@
*
****************************************************************************/
#ifndef DRIVERS_MS5525_AIRSPEED_HPP_
#define DRIVERS_MS5525_AIRSPEED_HPP_
#pragma once
#include <drivers/airspeed/airspeed.h>
#include <math.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#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;
@ -49,24 +50,26 @@ static constexpr unsigned MEAS_RATE = 100;
static constexpr float MEAS_DRIVER_FILTER_FREQ = 1.2f;
static constexpr int64_t CONVERSION_INTERVAL = (1000000 / MEAS_RATE); /* microseconds */
class MS5525 : public Airspeed
class MS5525 : public Airspeed, public I2CSPIDriver<MS5525>
{
public:
MS5525(uint8_t bus, uint8_t address = I2C_ADDRESS_1_MS5525DSO, const char *path = PATH_MS5525) :
Airspeed(bus, address, CONVERSION_INTERVAL, path)
MS5525(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address = I2C_ADDRESS_1_MS5525DSO,
const char *path = PATH_MS5525) :
Airspeed(bus, bus_frequency, address, CONVERSION_INTERVAL, path),
I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, address)
{
}
~MS5525() override = default;
virtual ~MS5525() = default;
static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance);
static void print_usage();
void RunImpl();
private:
/**
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*/
void Run() override;
int measure() override;
int collect() override;
@ -127,5 +130,3 @@ private:
uint8_t prom_crc4(uint16_t n_prom[]) const;
};
#endif /* DRIVERS_MS5525_AIRSPEED_HPP_ */

205
src/drivers/differential_pressure/ms5525/MS5525_main.cpp
View File

@ -33,199 +33,66 @@
#include "MS5525.hpp"
// Driver 'main' command.
extern "C" __EXPORT int ms5525_airspeed_main(int argc, char *argv[]);
// Local functions in support of the shell command.
namespace ms5525_airspeed
I2CSPIDriverBase *MS5525::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance)
{
MS5525 *g_dev = nullptr;
MS5525 *instance = new MS5525(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency);
int start();
int start_bus(uint8_t i2c_bus);
int stop();
int reset();
/**
* Attempt to start driver on all available I2C busses.
*
* This function will return as soon as the first sensor
* is detected on one of the available busses or if no
* sensors are detected.
*
*/
int
start()
{
for (unsigned i = 0; i < NUM_I2C_BUS_OPTIONS; i++) {
if (start_bus(i2c_bus_options[i]) == PX4_OK) {
return PX4_OK;
}
if (instance == nullptr) {
PX4_ERR("alloc failed");
return nullptr;
}
return PX4_ERROR;
if (instance->init() != PX4_OK) {
delete instance;
return nullptr;
}
instance->ScheduleNow();
return instance;
}
/**
* Start the driver on a specific bus.
*
* This function call only returns once the driver is up and running
* or failed to detect the sensor.
*/
int
start_bus(uint8_t i2c_bus)
void
MS5525::print_usage()
{
int fd = -1;
if (g_dev != nullptr) {
PX4_ERR("already started");
goto fail;
}
g_dev = new MS5525(i2c_bus, I2C_ADDRESS_1_MS5525DSO, PATH_MS5525);
/* check if the MS4525DO was instantiated */
if (g_dev == nullptr) {
goto fail;
}
/* try to initialize */
if (g_dev->init() != PX4_OK) {
goto fail;
}
/* set the poll rate to default, starts automatic data collection */
fd = px4_open(PATH_MS5525, O_RDONLY);
if (fd < 0) {
goto fail;
}
if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
goto fail;
}
return PX4_OK;
fail:
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
}
return PX4_ERROR;
}
// stop the driver
int stop()
{
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
} else {
PX4_ERR("driver not running");
return PX4_ERROR;
}
return PX4_OK;
}
// reset the driver
int reset()
{
int fd = px4_open(PATH_MS5525, O_RDONLY);
if (fd < 0) {
PX4_ERR("failed ");
return PX4_ERROR;
}
if (px4_ioctl(fd, SENSORIOCRESET, 0) < 0) {
PX4_ERR("driver reset failed");
return PX4_ERROR;
}
if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
PX4_ERR("driver poll restart failed");
return PX4_ERROR;
}
return PX4_OK;
}
} // namespace ms5525_airspeed
static void
ms5525_airspeed_usage()
{
PX4_INFO("usage: ms5525_airspeed command [options]");
PX4_INFO("options:");
PX4_INFO("\t-b --bus i2cbus (%d)", PX4_I2C_BUS_DEFAULT);
PX4_INFO("\t-a --all");
PX4_INFO("command:");
PX4_INFO("\tstart|stop|reset");
PRINT_MODULE_USAGE_NAME("ms5525_airspeed", "driver");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
}
int
ms5525_airspeed_main(int argc, char *argv[])
{
uint8_t i2c_bus = PX4_I2C_BUS_DEFAULT;
using ThisDriver = MS5525;
BusCLIArguments cli{true, false};
cli.default_i2c_frequency = 100000;
int myoptind = 1;
int ch;
const char *myoptarg = nullptr;
bool start_all = false;
const char *verb = cli.parseDefaultArguments(argc, argv);
while ((ch = px4_getopt(argc, argv, "ab:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'b':
i2c_bus = atoi(myoptarg);
break;
case 'a':
start_all = true;
break;
default:
ms5525_airspeed_usage();
return 0;
}
}
if (myoptind >= argc) {
ms5525_airspeed_usage();
if (!verb) {
ThisDriver::print_usage();
return -1;
}
/*
* Start/load the driver.
*/
if (!strcmp(argv[myoptind], "start")) {
if (start_all) {
return ms5525_airspeed::start();
BusInstanceIterator iterator(MODULE_NAME, cli,
DRV_DIFF_PRESS_DEVTYPE_MS5525);
} else {
return ms5525_airspeed::start_bus(i2c_bus);
}
if (!strcmp(verb, "start")) {
return ThisDriver::module_start(cli, iterator);
}
/*
* Stop the driver
*/
if (!strcmp(argv[myoptind], "stop")) {
return ms5525_airspeed::stop();
if (!strcmp(verb, "stop")) {
return ThisDriver::module_stop(iterator);
}
/*
* Reset the driver.
*/
if (!strcmp(argv[myoptind], "reset")) {
return ms5525_airspeed::reset();
if (!strcmp(verb, "status")) {
return ThisDriver::module_status(iterator);
}
ms5525_airspeed_usage();
return 0;
ThisDriver::print_usage();
return -1;
}

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

@ -166,7 +166,7 @@ SDP3X::collect()
}
void
SDP3X::Run()
SDP3X::RunImpl()
{
int ret = PX4_ERROR;

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

@ -39,13 +39,14 @@
* Datasheet: https://www.sensirion.com/fileadmin/user_upload/customers/sensirion/Dokumente/8_Differential_Pressure/Sensirion_Differential_Pressure_Sensors_SDP3x_Digital_Datasheet_V0.8.pdf
*/
#ifndef DRIVERS_SDP3X_AIRSPEED_HPP_
#define DRIVERS_SDP3X_AIRSPEED_HPP_
#pragma once
#include <drivers/airspeed/airspeed.h>
#include <math.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/i2c_spi_buses.h>
#define I2C_ADDRESS_1_SDP3X 0x21
#define I2C_ADDRESS_2_SDP3X 0x22
@ -67,21 +68,26 @@
#define SDP3X_MEAS_DRIVER_FILTER_FREQ 3.0f
#define CONVERSION_INTERVAL (1000000 / SPD3X_MEAS_RATE) /* microseconds */
class SDP3X : public Airspeed
class SDP3X : public Airspeed, public I2CSPIDriver<SDP3X>
{
public:
SDP3X(int bus, int address = I2C_ADDRESS_1_SDP3X, const char *path = PATH_SDP3X) :
Airspeed(bus, address, CONVERSION_INTERVAL, path)
SDP3X(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address = I2C_ADDRESS_1_SDP3X,
const char *path = PATH_SDP3X) :
Airspeed(bus, bus_frequency, address, CONVERSION_INTERVAL, path),
I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, address)
{
}
virtual ~SDP3X() = default;
static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance);
static void print_usage();
void RunImpl();
private:
/**
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*/
void Run() override;
int measure() override { return 0; }
int collect() override;
int probe() override;
@ -102,5 +108,3 @@ private:
uint16_t _scale{0};
};
#endif /* DRIVERS_SDP3X_AIRSPEED_HPP_ */

220
src/drivers/differential_pressure/sdp3x/SDP3X_main.cpp
View File

@ -33,213 +33,67 @@
#include "SDP3X.hpp"
// Driver 'main' command.
extern "C" __EXPORT int sdp3x_airspeed_main(int argc, char *argv[]);
// Local functions in support of the shell command.
namespace sdp3x_airspeed
I2CSPIDriverBase *SDP3X::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance)
{
SDP3X *g_dev = nullptr;
SDP3X *instance = new SDP3X(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency, cli.i2c_address);
int start();
int start_bus(uint8_t i2c_bus);
int stop();
int reset();
/**
* Attempt to start driver on all available I2C busses.
*
* This function will return as soon as the first sensor
* is detected on one of the available busses or if no
* sensors are detected.
*
*/
int
start()
{
for (unsigned i = 0; i < NUM_I2C_BUS_OPTIONS; i++) {
if (start_bus(i2c_bus_options[i]) == PX4_OK) {
return PX4_OK;
}
if (instance == nullptr) {
PX4_ERR("alloc failed");
return nullptr;
}
return PX4_ERROR;
if (instance->init() != PX4_OK) {
delete instance;
return nullptr;
}
instance->ScheduleNow();
return instance;
}
/**
* Start the driver on a specific bus.
*
* This function call only returns once the driver is up and running
* or failed to detect the sensor.
*/
int
start_bus(uint8_t i2c_bus)
void
SDP3X::print_usage()
{
int fd = -1;
if (g_dev != nullptr) {
PX4_WARN("already started");
return PX4_ERROR;
}
g_dev = new SDP3X(i2c_bus, I2C_ADDRESS_1_SDP3X, PATH_SDP3X);
/* check if the SDP3XDSO was instantiated */
if (g_dev == nullptr) {
goto fail;
}
/* try the next SDP3XDSO if init fails */
if (g_dev->init() != PX4_OK) {
delete g_dev;
g_dev = new SDP3X(i2c_bus, I2C_ADDRESS_2_SDP3X, PATH_SDP3X);
/* check if the SDP3XDSO was instantiated */
if (g_dev == nullptr) {
PX4_ERR("SDP3X was not instantiated (RAM)");
goto fail;
}
/* both versions failed if the init for the SDP3XDSO fails, give up */
if (g_dev->init() != PX4_OK) {
goto fail;
}
}
/* set the poll rate to default, starts automatic data collection */
fd = px4_open(PATH_SDP3X, O_RDONLY);
if (fd < 0) {
goto fail;
}
if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
goto fail;
}
return PX4_OK;
fail:
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
}
return PX4_ERROR;
}
// stop the driver
int stop()
{
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
} else {
PX4_ERR("driver not running");
return PX4_ERROR;
}
return PX4_OK;
}
// reset the driver
int reset()
{
int fd = px4_open(PATH_SDP3X, O_RDONLY);
if (fd < 0) {
PX4_ERR("failed ");
return PX4_ERROR;
}
if (px4_ioctl(fd, SENSORIOCRESET, 0) < 0) {
PX4_ERR("driver reset failed");
return PX4_ERROR;
}
if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
PX4_ERR("driver poll restart failed");
return PX4_ERROR;
}
return PX4_OK;
}
} // namespace sdp3x_airspeed
static void
sdp3x_airspeed_usage()
{
PX4_INFO("usage: sdp3x_airspeed command [options]");
PX4_INFO("options:");
PX4_INFO("\t-b --bus i2cbus (%d)", PX4_I2C_BUS_DEFAULT);
PX4_INFO("\t-a --all");
PX4_INFO("command:");
PX4_INFO("\tstart|stop|reset");
PRINT_MODULE_USAGE_NAME("sdp3x_airspeed", "driver");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x21);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
}
int
sdp3x_airspeed_main(int argc, char *argv[])
{
uint8_t i2c_bus = PX4_I2C_BUS_DEFAULT;
using ThisDriver = SDP3X;
BusCLIArguments cli{true, false};
cli.default_i2c_frequency = 100000;
cli.i2c_address = I2C_ADDRESS_1_SDP3X;
int myoptind = 1;
int ch;
const char *myoptarg = nullptr;
bool start_all = false;
const char *verb = cli.parseDefaultArguments(argc, argv);
while ((ch = px4_getopt(argc, argv, "ab:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'b':
i2c_bus = atoi(myoptarg);
break;
case 'a':
start_all = true;
break;
default:
sdp3x_airspeed_usage();
return 0;
}
}
if (myoptind >= argc) {
sdp3x_airspeed_usage();
if (!verb) {
ThisDriver::print_usage();
return -1;
}
BusInstanceIterator iterator(MODULE_NAME, cli, DRV_DIFF_PRESS_DEVTYPE_SDP31);
/*
* Start/load the driver.
*/
if (!strcmp(argv[myoptind], "start")) {
if (start_all) {
return sdp3x_airspeed::start();
} else {
return sdp3x_airspeed::start_bus(i2c_bus);
}
if (!strcmp(verb, "start")) {
return ThisDriver::module_start(cli, iterator);
}
/*
* Stop the driver
*/
if (!strcmp(argv[myoptind], "stop")) {
return sdp3x_airspeed::stop();
if (!strcmp(verb, "stop")) {
return ThisDriver::module_stop(iterator);
}
/*
* Reset the driver.
*/
if (!strcmp(argv[myoptind], "reset")) {
return sdp3x_airspeed::reset();
if (!strcmp(verb, "status")) {
return ThisDriver::module_status(iterator);
}
sdp3x_airspeed_usage();
return 0;
ThisDriver::print_usage();
return -1;
}

76
src/lib/drivers/airspeed/airspeed.cpp
View File

@ -56,11 +56,10 @@
#include <drivers/airspeed/airspeed.h>
Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const char *path) :
I2C("Airspeed", path, bus, address, 100000),
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
Airspeed::Airspeed(int bus, int bus_frequency, int address, unsigned conversion_interval, const char *path) :
I2C("Airspeed", path, bus, address, bus_frequency),
_sensor_ok(false),
_measure_interval(0),
_measure_interval(conversion_interval),
_collect_phase(false),
_diff_pres_offset(0.0f),
_airspeed_orb_class_instance(-1),
@ -73,9 +72,6 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const cha
Airspeed::~Airspeed()
{
/* make sure we are truly inactive */
stop();
if (_class_instance != -1) {
unregister_class_devname(AIRSPEED_BASE_DEVICE_PATH, _class_instance);
}
@ -121,57 +117,6 @@ int
Airspeed::ioctl(device::file_t *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case SENSORIOCSPOLLRATE: {
switch (arg) {
/* zero would be bad */
case 0:
return -EINVAL;
/* set default polling rate */
case SENSOR_POLLRATE_DEFAULT: {
/* do we need to start internal polling? */
bool want_start = (_measure_interval == 0);
/* set interval for next measurement to minimum legal value */
_measure_interval = USEC2TICK(_conversion_interval);
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}
return OK;
}
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_measure_interval == 0);
/* convert hz to tick interval via microseconds */
unsigned interval = (1000000 / arg);
/* check against maximum rate */
if (interval < _conversion_interval) {
return -EINVAL;
}
/* update interval for next measurement */
_measure_interval = interval;
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}
return OK;
}
}
}
break;
case AIRSPEEDIOCSSCALE: {
struct airspeed_scale *s = (struct airspeed_scale *)arg;
_diff_pres_offset = s->offset_pa;
@ -184,18 +129,3 @@ Airspeed::ioctl(device::file_t *filp, int cmd, unsigned long arg)
}
}
void
Airspeed::start()
{
/* reset the report ring and state machine */
_collect_phase = false;
/* schedule a cycle to start things */
ScheduleNow();
}
void
Airspeed::stop()
{
ScheduleClear();
}

40
src/lib/drivers/airspeed/airspeed.h
View File

@ -42,34 +42,28 @@
#include <perf/perf_counter.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/PublicationMulti.hpp>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
/* Default I2C bus */
static constexpr uint8_t PX4_I2C_BUS_DEFAULT = PX4_I2C_BUS_EXPANSION;
class __EXPORT Airspeed : public device::I2C, public px4::ScheduledWorkItem
class __EXPORT Airspeed : public device::I2C
{
public:
Airspeed(int bus, int address, unsigned conversion_interval, const char *path);
Airspeed(int bus, int bus_frequency, int address, unsigned conversion_interval, const char *path);
virtual ~Airspeed();
virtual int init();
int init() override;
virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg);
int ioctl(device::file_t *filp, int cmd, unsigned long arg) override;
private:
/* this class has pointer data members and should not be copied */
Airspeed(const Airspeed &);
Airspeed &operator=(const Airspeed &);
Airspeed(const Airspeed &) = delete;
Airspeed &operator=(const Airspeed &) = delete;
protected:
virtual int probe();
int probe() override;
/**
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*/
virtual void Run() = 0;
virtual int measure() = 0;
virtual int collect() = 0;
@ -88,26 +82,6 @@ protected:
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
/**
* Initialise the automatic measurement state machine and start it.
*
* @note This function is called at open and error time. It might make sense
* to make it more aggressive about resetting the bus in case of errors.
*/
void start();
/**
* Stop the automatic measurement state machine.
*/
void stop();
/**
* add a new report to the reports queue
*
* @param report differential_pressure_s report
*/
void new_report(const differential_pressure_s &report);
};