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

airspeed sensor startup improvements (#7903)

This commit is contained in:
Daniel Agar 2017-10-05 14:29:44 -07:00 committed by GitHub
parent 123a0b584a
commit 38f45d1a9d
10 changed files with 124 additions and 358 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1,6 +1,6 @@
#!nsh
#
# Standard startup script for PX4FMU v2, v3, v4 onboard sensor drivers.
# Standard startup script for sensor drivers.
#
if ver hwcmp AEROFC_V1
@ -68,8 +68,6 @@ if ver hwcmp PX4FMU_V2
then
# External I2C bus
hmc5883 -C -T -X start
# External I2C bus
lis3mdl -X start
# Internal I2C bus
@ -117,6 +115,7 @@ then
hmc5883 -C -T -S -R 8 start
fi
if [ $BOARD_FMUV3 == 21 ]
then
# v2.1 internal MPU9250 is rotated 180 deg roll, 270 deg yaw
@ -156,51 +155,63 @@ if ver hwcmp PX4FMU_V4
then
# External I2C bus
hmc5883 -C -T -X start
lis3mdl -X start
bmp280 -I start
lis3mdl -R 2 start
# expansion i2c used for BMM150 rotated by 90deg
bmm150 -R 2 start
# Internal SPI bus is rotated 90 deg yaw
hmc5883 -C -T -S -R 2 start
# Internal SPI bus ICM-20608-G is rotated 90 deg yaw
mpu6000 -R 2 -T 20608 start
# Internal SPI bus ICM-20602-G is rotated 90 deg yaw
mpu6000 -R 2 -T 20602 start
# Start either MPU9250 or BMI160. They are both connected to the same SPI bus and use the same
# chip select pin. There are different boards with either one of them and the WHO_AM_I register
# will prevent the incorrect driver from a successful initialization.
# Internal SPI bus mpu9250 is rotated 90 deg yaw
mpu9250 -R 2 start
# Internal SPI bus BMI160
bmi160 start
if hmc5883 -C -T -S -R 2 start
then
# hmc5883 internal SPI bus is rotated 90 deg yaw
else
if lis3mdl -R 2 start
then
# lis3mdl internal SPI bus is rotated 90 deg yaw
else
# BMI055 gyro internal SPI bus
bmi055 -G start
fi
fi
# Start either ICM2060X or BMI055. They are both connected to the same SPI bus and use the same
# chip select pin. There are different boards with either one of them and the WHO_AM_I register
# will prevent the incorrect driver from a successful initialization.
# Internal SPI bus BMI055_ACC
if mpu6000 -R 2 -T 20602 start
then
# ICM20602 internal SPI bus ICM-20608-G is rotated 90 deg yaw
else
if mpu6000 -R 2 -T 20608 start
then
# ICM20608 internal SPI bus ICM-20602-G is rotated 90 deg yaw
else
# BMI055 accel internal SPI bus
bmi055 -A start
fi
fi
# Internal SPI bus BMI055_GYR
bmi055 -G start
# Start either MPU9250 or BMI160. They are both connected to the same SPI bus and use the same
# chip select pin. There are different boards with either one of them and the WHO_AM_I register
# will prevent the incorrect driver from a successful initialization.
# expansion i2c used for BMM150 rotated by 90deg
bmm150 -R 2 start
# expansion i2c used for BMP280
bmp280 -I start
if mpu9250 -R 2 start
then
# mpu9250 internal SPI bus mpu9250 is rotated 90 deg yaw
else
# BMI160 internal SPI bus
bmi160 start
fi
fi
if ver hwcmp MINDPX_V2
then
# External I2C bus
hmc5883 -C -T -X start
# Internal I2C bus
hmc5883 -C -T -I -R 12 start
mpu6000 -s -R 8 start
mpu9250 -s -R 8 start
lsm303d -R 10 start
@ -219,16 +230,13 @@ fi
if ver hwcmp AEROFC_V1
then
ms5611 -T 0 start
mpu9250 -s -R 14 start
# Possible external compasses
hmc5883 -X start
ist8310 -C -b 1 -R 4 start
aerofc_adc start
ll40ls start i2c
fi
@ -287,26 +295,41 @@ fi
# Optional drivers
#
sdp3x_airspeed start
sdp3x_airspeed start -b 2
# Pixhawk 2.1 has a MS5611 on I2C which gets wrongly
# detected as MS5525 because the chip manufacturer was so
# clever to assign the same I2C address and skip a WHO_AM_I
# register.
if [ $BOARD_FMUV3 == 21 ]
if [ ${VEHICLE_TYPE} == fw -o ${VEHICLE_TYPE} == vtol ]
then
if param compare CBRK_AIRSPD_CHK 0
then
if sdp3x_airspeed start
then
else
sdp3x_airspeed start -b 2
fi
# Pixhawk 2.1 has a MS5611 on I2C which gets wrongly
# detected as MS5525 because the chip manufacturer was so
# clever to assign the same I2C address and skip a WHO_AM_I
# register.
if [ $BOARD_FMUV3 == 21 ]
then
ms5525_airspeed start -b 2
else
else
ms5525_airspeed start
fi
if ms4525_airspeed start
then
else
ms4525_airspeed start -b 2
fi
if ets_airspeed start
then
else
ets_airspeed start -b 1
fi
fi
fi
ms4525_airspeed start
ms4525_airspeed start -b 2
ets_airspeed start
ets_airspeed start -b 1
# Sensors on the PWM interface bank
if param compare SENS_EN_LL40LS 1
then
@ -323,34 +346,29 @@ then
fi
# lightware serial lidar sensor
if param compare SENS_EN_SF0X 0
if param greater SENS_EN_SF0X 0
then
else
sf0x start
fi
# lightware i2c lidar sensor
if param compare SENS_EN_SF1XX 0
if param greater SENS_EN_SF1XX 0
then
else
sf1xx start
fi
# mb12xx sonar sensor
if param compare SENS_EN_MB12XX 1
if param greater SENS_EN_MB12XX 0
then
mb12xx start
fi
# teraranger one tof sensor
if param compare SENS_EN_TRANGER 0
if param greater SENS_EN_TRANGER 0
then
else
teraranger start
fi
# Wait 20 ms for sensors (because we need to wait for the HRT and work queue callbacks to fire)
usleep 20000
if sensors start
then
fi
sensors start

130
src/drivers/airspeed/airspeed.cpp
View File

@ -60,7 +60,6 @@
Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const char *path) :
I2C("Airspeed", path, bus, address, 100000),
_reports(nullptr),
_sensor_ok(false),
_last_published_sensor_ok(true), /* initialize differently to force publication */
_measure_ticks(0),
@ -90,11 +89,6 @@ Airspeed::~Airspeed()
unregister_class_devname(AIRSPEED_BASE_DEVICE_PATH, _class_instance);
}
/* free any existing reports */
if (_reports != nullptr) {
delete _reports;
}
// free perf counters
perf_free(_sample_perf);
perf_free(_comms_errors);
@ -103,18 +97,9 @@ Airspeed::~Airspeed()
int
Airspeed::init()
{
int ret = PX4_ERROR;
/* do I2C init (and probe) first */
if (I2C::init() != OK) {
goto out;
}
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(differential_pressure_s));
if (_reports == nullptr) {
goto out;
if (I2C::init() != PX4_OK) {
return PX4_ERROR;
}
/* register alternate interfaces if we have to */
@ -122,8 +107,7 @@ Airspeed::init()
/* advertise sensor topic, measure manually to initialize valid report */
measure();
differential_pressure_s arp;
_reports->get(&arp);
differential_pressure_s arp = {};
/* measurement will have generated a report, publish */
_airspeed_pub = orb_advertise_multi(ORB_ID(differential_pressure), &arp, &_airspeed_orb_class_instance,
@ -133,10 +117,7 @@ Airspeed::init()
PX4_WARN("uORB started?");
}
ret = OK;
out:
return ret;
return PX4_OK;
}
int
@ -217,6 +198,7 @@ Airspeed::ioctl(device::file_t *filp, int cmd, unsigned long arg)
}
}
}
break;
case SENSORIOCGPOLLRATE:
if (_measure_ticks == 0) {
@ -225,27 +207,6 @@ Airspeed::ioctl(device::file_t *filp, int cmd, unsigned long arg)
return (1000 / _measure_ticks);
case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100)) {
return -EINVAL;
}
ATOMIC_ENTER;
if (!_reports->resize(arg)) {
ATOMIC_LEAVE;
return -ENOMEM;
}
ATOMIC_LEAVE;
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _reports->size();
case SENSORIOCRESET:
/* XXX implement this */
return -EINVAL;
@ -269,72 +230,11 @@ Airspeed::ioctl(device::file_t *filp, int cmd, unsigned long arg)
}
}
ssize_t
Airspeed::read(device::file_t *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(differential_pressure_s);
differential_pressure_s *abuf = reinterpret_cast<differential_pressure_s *>(buffer);
int ret = 0;
/* buffer must be large enough */
if (count < 1) {
return -ENOSPC;
}
/* if automatic measurement is enabled */
if (_measure_ticks > 0) {
/*
* While there is space in the caller's buffer, and reports, copy them.
* Note that we may be pre-empted by the workq thread while we are doing this;
* we are careful to avoid racing with them.
*/
while (count--) {
if (_reports->get(abuf)) {
ret += sizeof(*abuf);
abuf++;
}
}
/* if there was no data, warn the caller */
return ret ? ret : -EAGAIN;
}
/* manual measurement - run one conversion */
do {
_reports->flush();
/* trigger a measurement */
if (OK != measure()) {
ret = -EIO;
break;
}
/* wait for it to complete */
usleep(_conversion_interval);
/* run the collection phase */
if (OK != collect()) {
ret = -EIO;
break;
}
/* state machine will have generated a report, copy it out */
if (_reports->get(abuf)) {
ret = sizeof(*abuf);
}
} while (0);
return ret;
}
void
Airspeed::start()
{
/* reset the report ring and state machine */
_collect_phase = false;
_reports->flush();
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&Airspeed::cycle_trampoline, this, 1);
@ -372,25 +272,7 @@ void
Airspeed::cycle_trampoline(void *arg)
{
Airspeed *dev = (Airspeed *)arg;
dev->cycle();
// XXX we do not know if this is
// really helping - do not update the
// subsys state right now
//dev->update_status();
}
void
Airspeed::print_info()
{
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
warnx("poll interval: %u ticks", _measure_ticks);
_reports->print_info("report queue");
}
void
Airspeed::new_report(const differential_pressure_s &report)
{
_reports->force(&report);
dev->update_status();
}

32
src/drivers/airspeed/airspeed.h
View File

@ -31,23 +31,13 @@
*
****************************************************************************/
/**
* @file airspeed.h
* @author Simon Wilks
*
* Generic driver for airspeed sensors connected via I2C.
*/
#include <drivers/device/i2c.h>
#include <drivers/device/ringbuffer.h>
#include <drivers/drv_airspeed.h>
#include <drivers/drv_hrt.h>
#include <px4_config.h>
#include <px4_defines.h>
#include <px4_workqueue.h>
#include <systemlib/airspeed.h>
#include <systemlib/err.h>
#include <systemlib/param/param.h>
#include <systemlib/perf_counter.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/subsystem_info.h>
@ -56,10 +46,6 @@
/* Default I2C bus */
static constexpr uint8_t PX4_I2C_BUS_DEFAULT = PX4_I2C_BUS_EXPANSION;
#ifndef CONFIG_SCHED_WORKQUEUE
# error This requires CONFIG_SCHED_WORKQUEUE.
#endif
class __EXPORT Airspeed : public device::I2C
{
public:
@ -68,17 +54,9 @@ public:
virtual int init();
virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen);
virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg);
/**
* Diagnostics - print some basic information about the driver.
*/
virtual void print_info();
private:
ringbuffer::RingBuffer *_reports;
/* this class has pointer data members and should not be copied */
Airspeed(const Airspeed &);
Airspeed &operator=(const Airspeed &);
@ -118,16 +96,6 @@ protected:
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
/**
* Test whether the device supported by the driver is present at a
* specific address.
*
* @param address The I2C bus address to probe.
* @return True if the device is present.
*/
int probe_address(uint8_t address);
/**
* Initialise the automatic measurement state machine and start it.
*

44
src/drivers/ets_airspeed/ets_airspeed.cpp
View File

@ -165,11 +165,6 @@ ETSAirspeed::collect()
orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
}
new_report(report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
ret = OK;
perf_end(_sample_perf);
@ -272,7 +267,7 @@ start(int i2c_bus)
goto fail;
}
if (OK != g_dev->Airspeed::init()) {
if (OK != g_dev->init()) {
goto fail;
}
@ -296,7 +291,8 @@ fail:
g_dev = nullptr;
}
PX4_WARN("no ETS airspeed sensor connected on bus %d", i2c_bus);
PX4_WARN("not started on bus %d", i2c_bus);
return PX4_ERROR;
}
@ -346,7 +342,7 @@ test()
}
PX4_INFO("single read");
PX4_INFO("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa);
PX4_INFO("diff pressure: %d pa", (int)report.differential_pressure_filtered_pa);
/* start the sensor polling at 2Hz */
if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
@ -356,22 +352,23 @@ test()
/* read the sensor 5x and report each value */
for (unsigned i = 0; i < 5; i++) {
struct pollfd fds;
px4_pollfd_struct_t fds;
/* wait for data to be ready */
fds.fd = fd;
fds.events = POLLIN;
ret = poll(&fds, 1, 2000);
ret = px4_poll(&fds, 1, 2000);
if (ret != 1) {
PX4_ERR("timed out waiting for sensor data");
return PX4_ERROR;
}
/* now go get it */
sz = px4_read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
err(1, "periodic read failed");
PX4_ERR("periodic read failed");
}
PX4_INFO("periodic read %u", i);
@ -396,7 +393,7 @@ reset()
int fd = px4_open(ETS_PATH, O_RDONLY);
if (fd < 0) {
PX4_ERR("failed ");
PX4_ERR("failed");
return PX4_ERROR;
}
@ -413,22 +410,6 @@ reset()
return PX4_OK;
}
/**
* Print a little info about the driver.
*/
int
info()
{
if (g_dev == nullptr) {
PX4_ERR("driver not running");
return PX4_ERROR;
}
PX4_INFO("state @ %p", g_dev);
g_dev->print_info();
return PX4_OK;
}
} // namespace
@ -485,13 +466,6 @@ ets_airspeed_main(int argc, char *argv[])
return ets_airspeed::reset();
}
/*
* Print driver information.
*/
if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
return ets_airspeed::info();
}
ets_airspeed_usage();
return PX4_OK;

46
src/drivers/ms4525_airspeed/ms4525_airspeed.cpp
View File

@ -49,13 +49,10 @@
* - Interfacing to MEAS Digital Pressure Modules (http://www.meas-spec.com/downloads/Interfacing_to_MEAS_Digital_Pressure_Modules.pdf)
*/
#include <px4_config.h>
#include <drivers/device/i2c.h>
#include <board_config.h>
#include <systemlib/airspeed.h>
#include <systemlib/err.h>
#include <systemlib/param/param.h>
@ -238,11 +235,6 @@ MEASAirspeed::collect()
orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
}
new_report(report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
ret = OK;
perf_end(_sample_perf);
@ -386,7 +378,6 @@ int start(int i2c_bus);
int stop();
int test();
int reset();
int info();
/**
* Start the driver.
@ -412,8 +403,7 @@ start(int i2c_bus)
goto fail;
}
/* both versions failed if the init for the MS5525DSO fails, give up */
if (OK != g_dev->Airspeed::init()) {
if (OK != g_dev->init()) {
goto fail;
}
@ -437,7 +427,8 @@ fail:
g_dev = nullptr;
}
PX4_WARN("no MS4525 airspeed sensor connected on bus %d", i2c_bus);
PX4_WARN("not started on bus %d", i2c_bus);
return PX4_ERROR;
}
@ -505,7 +496,8 @@ test()
ret = px4_poll(&fds, 1, 2000);
if (ret != 1) {
PX4_ERR("timed out");
PX4_ERR("timed out waiting for sensor data");
return PX4_ERROR;
}
/* now go get it */
@ -539,7 +531,7 @@ reset()
int fd = px4_open(PATH_MS4525, O_RDONLY);
if (fd < 0) {
PX4_ERR("failed ");
PX4_ERR("failed");
return PX4_ERROR;
}
@ -556,23 +548,6 @@ reset()
return PX4_OK;
}
/**
* Print a little info about the driver.
*/
int
info()
{
if (g_dev == nullptr) {
PX4_ERR("driver not running");
return PX4_ERROR;
}
PX4_INFO("state @ %p", g_dev);
g_dev->print_info();
return PX4_OK;
}
} // namespace
@ -583,7 +558,7 @@ meas_airspeed_usage()
PX4_INFO("options:");
PX4_INFO("\t-b --bus i2cbus (%d)", PX4_I2C_BUS_DEFAULT);
PX4_INFO("command:");
PX4_INFO("\tstart|stop|reset|test|info");
PX4_INFO("\tstart|stop|reset|test");
}
int
@ -629,13 +604,6 @@ ms4525_airspeed_main(int argc, char *argv[])
return meas_airspeed::reset();
}
/*
* Print driver information.
*/
if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
return meas_airspeed::info();
}
meas_airspeed_usage();
return PX4_OK;

5
src/drivers/ms5525_airspeed/MS5525.cpp
View File

@ -264,11 +264,6 @@ MS5525::collect()
orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &diff_pressure);
}
new_report(diff_pressure);
/* notify anyone waiting for data */
poll_notify(POLLIN);
ret = OK;
perf_end(_sample_perf);

1
src/drivers/ms5525_airspeed/MS5525.hpp
View File

@ -48,7 +48,6 @@
/* The MS5525DSO address is 111011Cx, where C is the complementary value of the pin CSB */
static constexpr uint8_t I2C_ADDRESS_1_MS5525DSO = 0x76;
static constexpr uint8_t I2C_ADDRESS_2_MS5525DSO = 0x77;
static constexpr const char PATH_MS5525[] = "/dev/ms5525";

20
src/drivers/ms5525_airspeed/MS5525_main.cpp
View File

@ -66,24 +66,11 @@ start(uint8_t i2c_bus)
goto fail;
}
/* try the next MS5525DSO if init fails */
if (OK != g_dev->Airspeed::init()) {
delete g_dev;
g_dev = new MS5525(i2c_bus, I2C_ADDRESS_2_MS5525DSO, PATH_MS5525);
/* check if the MS5525DSO was instantiated */
if (g_dev == nullptr) {
PX4_WARN("MS5525 was not instantiated");
/* try to initialize */
if (g_dev->init() != PX4_OK) {
goto fail;
}
/* both versions failed if the init for the MS5525DSO fails, give up */
if (OK != g_dev->Airspeed::init()) {
goto fail;
}
}
/* set the poll rate to default, starts automatic data collection */
fd = px4_open(PATH_MS5525, O_RDONLY);
@ -104,7 +91,8 @@ fail:
g_dev = nullptr;
}
PX4_WARN("no MS5525 airspeed sensor connected on bus %d", i2c_bus);
PX4_WARN("not started on bus %d", i2c_bus);
return PX4_ERROR;
}

5
src/drivers/sdp3x_airspeed/SDP3X.cpp
View File

@ -160,11 +160,6 @@ SDP3X::collect()
orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
}
new_report(report);
// notify anyone waiting for data
poll_notify(POLLIN);
ret = OK;
perf_end(_sample_perf);

39
src/drivers/sdp3x_airspeed/SDP3X_main.cpp
View File

@ -41,22 +41,21 @@ namespace sdp3x_airspeed
{
SDP3X *g_dev = nullptr;
int start(int i2c_bus);
int start(uint8_t i2c_bus);
int stop();
int test();
int reset();
int info();
// Start the driver.
// This function call only returns once the driver is up and running
// or failed to detect the sensor.
int
start(int i2c_bus)
start(uint8_t i2c_bus)
{
int fd = -1;
if (g_dev != nullptr) {
PX4_WARN("driver already started");
PX4_WARN("already started");
return PX4_ERROR;
}
@ -68,7 +67,7 @@ start(int i2c_bus)
}
/* try the next SDP3XDSO if init fails */
if (OK != g_dev->Airspeed::init()) {
if (g_dev->init() != PX4_OK) {
delete g_dev;
g_dev = new SDP3X(i2c_bus, I2C_ADDRESS_2_SDP3X, PATH_SDP3X);
@ -80,7 +79,7 @@ start(int i2c_bus)
}
/* both versions failed if the init for the SDP3XDSO fails, give up */
if (OK != g_dev->Airspeed::init()) {
if (g_dev->init() != PX4_OK) {
goto fail;
}
}
@ -105,7 +104,8 @@ fail:
g_dev = nullptr;
}
PX4_WARN("no SDP3X airspeed sensor connected on bus %d", i2c_bus);
PX4_WARN("not started on bus %d", i2c_bus);
return PX4_ERROR;
}
@ -213,20 +213,6 @@ int reset()
return PX4_OK;
}
// print a little info about the driver
int
info()
{
if (g_dev == nullptr) {
PX4_ERR("driver not running");
return PX4_ERROR;
}
PX4_INFO("state @ %p", g_dev);
g_dev->print_info();
return PX4_OK;
}
} // namespace sdp3x_airspeed
@ -237,13 +223,13 @@ sdp3x_airspeed_usage()
PX4_WARN("options:");
PX4_WARN("\t-b --bus i2cbus (%d)", PX4_I2C_BUS_DEFAULT);
PX4_WARN("command:");
PX4_WARN("\tstart|stop|reset|test|info");
PX4_WARN("\tstart|stop|reset|test");
}
int
sdp3x_airspeed_main(int argc, char *argv[])
{
int i2c_bus = PX4_I2C_BUS_DEFAULT;
uint8_t i2c_bus = PX4_I2C_BUS_DEFAULT;
for (int i = 1; i < argc; i++) {
if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--bus") == 0) {
@ -281,13 +267,6 @@ sdp3x_airspeed_main(int argc, char *argv[])
return sdp3x_airspeed::reset();
}
/*
* Print driver information.
*/
if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
return sdp3x_airspeed::info();
}
sdp3x_airspeed_usage();
return PX4_OK;