fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 22:53:33 +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-17 23:31:17 -04:00
committed by Daniel Agar
parent 0924ff7fcf
commit 3b7b2dc871
14 changed files with 248 additions and 890 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/differential_pressure/ets/ets_airspeed.cpp
+52 -190
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;
}
src/drivers/differential_pressure/ms4525/ms4525_airspeed.cpp
+66 -190
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;
}
src/drivers/differential_pressure/ms5525/MS5525.cpp
+3 -2
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;
}
src/drivers/differential_pressure/ms5525/MS5525.hpp
+15 -14
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_ */
src/drivers/differential_pressure/ms5525/MS5525_main.cpp
+36 -169
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;
}
src/drivers/differential_pressure/sdp3x/SDP3X.cpp
+1 -1
View File
@@ -166,7 +166,7 @@ SDP3X::collect()
}
void
SDP3X::Run()
SDP3X::RunImpl()
{
int ret = PX4_ERROR;
src/drivers/differential_pressure/sdp3x/SDP3X.hpp
+16 -12
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_ */
src/drivers/differential_pressure/sdp3x/SDP3X_main.cpp
+37 -183
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;
}