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

changes on drivers according to comments from DavidS

Browse Source
This commit is contained in:
Sergej Scheiermann 2017-04-11 16:01:35 +02:00 committed by Lorenz Meier
parent da31e6e0b5
commit 641a03510c
14 changed files with 422 additions and 1396 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -180,42 +180,37 @@ fi
if ver hwcmp PX4FMU_V4
then
# External I2C bus
#if hmc5883 -C -T -X start
#then
#fi
if hmc5883 -C -T -X start
then
fi
#if lis3mdl -R 2 start
#then
#fi
if lis3mdl -R 2 start
then
fi
# Internal SPI bus is rotated 90 deg yaw
#if hmc5883 -C -T -S -R 2 start
#then
#fi
if hmc5883 -C -T -S -R 2 start
then
fi
# Internal SPI bus ICM-20608-G is rotated 90 deg yaw
#if mpu6000 -R 2 -T 20608 start
#then
#fi
# Internal SPI bus ICM-20602-G is rotated 90 deg yaw
#if mpu6000 -R 2 -T 20602 start
#then
#fi
if mpu6000 -R 2 -T 20608 start
then
fi
# 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
#if mpu9250 -R 2 start
#then
#fi
if mpu9250 -R 2 start
then
fi
# Internal SPI bus BMI160
#if bmi160 start
#then
#fi
if bmi160 start
then
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
@ -231,13 +226,13 @@ then
then
fi
# expansion i2c used for BMM150
if bmm150 start
# expansion i2c used for BMM150 rotated by 90deg
if bmm150 -R 2 start
then
fi
# expansion i2c used for BMP285
if bmp285 start
# expansion i2c used for BMP280
if bmp280 -I start
then
fi
fi

1
cmake/configs/nuttx_px4fmu-v4_default.cmake
View File

@ -53,7 +53,6 @@ set(config_module_list
drivers/bmi160
drivers/bmi055
drivers/bmm150
drivers/bmp285
drivers/tap_esc
drivers/iridiumsbd

159
src/drivers/bmm150/bmm150.cpp
View File

@ -71,7 +71,8 @@ void start(bool external_bus, enum Rotation rotation)
if (*g_dev_ptr != nullptr)
/* if already started, the still command succeeded */
{
errx(0, "already started");
PX4_ERR("already started");
exit(0);
}
/* create the driver */
@ -79,7 +80,8 @@ void start(bool external_bus, enum Rotation rotation)
#if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_BMI)
*g_dev_ptr = new BMM150(PX4_I2C_BUS_BMM150, path, external_bus, rotation);
#else
errx(0, "External I2C not available");
PX4_ERR("External I2C not available");
exit(0);
#endif
} else {
@ -117,7 +119,9 @@ fail:
*g_dev_ptr = nullptr;
}
errx(1, "driver start failed");
PX4_ERR("driver start failed");
exit(1);
}
@ -132,37 +136,41 @@ void test(bool external_bus)
/* get the driver */
fd = open(path, O_RDONLY);
if (fd < 0)
err(1, "%s open failed (try 'bmm150 start' if the driver is not running)",
path);
if (fd < 0) {
PX4_ERR("%s open failed (try 'bmm150 start' if the driver is not running)",
path);
exit(1);
}
/* reset to Max polling rate*/
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0) {
err(1, "reset to Max polling rate");
PX4_ERR("reset to Max polling rate");
exit(1);
}
/* do a simple demand read */
sz = read(fd, &m_report, sizeof(m_report));
if (sz != sizeof(m_report)) {
err(1, "immediate mag read failed");
PX4_ERR("immediate mag read failed");
exit(1);
}
warnx("single read");
warnx("time: %lld", m_report.timestamp);
warnx("mag x: \t%8.4f\t", (double)m_report.x);
warnx("mag y: \t%8.4f\t", (double)m_report.y);
warnx("mag z: \t%8.4f\t", (double)m_report.z);
warnx("mag x: \t%d\traw 0x%0x", (short)m_report.x_raw, (unsigned short)m_report.x_raw);
warnx("mag y: \t%d\traw 0x%0x", (short)m_report.y_raw, (unsigned short)m_report.y_raw);
warnx("mag z: \t%d\traw 0x%0x", (short)m_report.z_raw, (unsigned short)m_report.z_raw);
PX4_WARN("single read");
PX4_WARN("time: %lld", m_report.timestamp);
PX4_WARN("mag x: \t%8.4f\t", (double)m_report.x);
PX4_WARN("mag y: \t%8.4f\t", (double)m_report.y);
PX4_WARN("mag z: \t%8.4f\t", (double)m_report.z);
PX4_WARN("mag x: \t%d\traw 0x%0x", (short)m_report.x_raw, (unsigned short)m_report.x_raw);
PX4_WARN("mag y: \t%d\traw 0x%0x", (short)m_report.y_raw, (unsigned short)m_report.y_raw);
PX4_WARN("mag z: \t%d\traw 0x%0x", (short)m_report.z_raw, (unsigned short)m_report.z_raw);
errx(0, "PASS");
PX4_ERR("PASS");
exit(0);
}
void
reset(bool external_bus)
{
@ -170,15 +178,18 @@ reset(bool external_bus)
int fd = open(path, O_RDONLY);
if (fd < 0) {
err(1, "failed ");
PX4_ERR("failed");
exit(1);
}
if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
err(1, "driver reset failed");
PX4_ERR("driver reset failed");
exit(1);
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
err(1, "driver poll restart failed");
PX4_ERR("driver poll restart failed");
exit(1);
}
exit(0);
@ -191,7 +202,8 @@ info(bool external_bus)
BMM150 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int;
if (*g_dev_ptr == nullptr) {
errx(1, "driver not running");
PX4_ERR("driver not running");
exit(1);
}
printf("state @ %p\n", *g_dev_ptr);
@ -210,7 +222,8 @@ regdump(bool external_bus)
BMM150 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int;
if (*g_dev_ptr == nullptr) {
errx(1, "driver not running");
PX4_ERR("driver not running");
exit(1);
}
printf("regdump @ %p\n", *g_dev_ptr);
@ -222,9 +235,9 @@ regdump(bool external_bus)
void
usage()
{
warnx("missing command: try 'start', 'info', 'test', 'stop',\n'reset', 'regdump'");
warnx("options:");
warnx(" -X (external bus)");
PX4_WARN("missing command: try 'start', 'info', 'test', 'stop',\n'reset', 'regdump'");
PX4_WARN("options:");
PX4_WARN(" -X (external bus)");
}
@ -236,9 +249,11 @@ BMM150 :: BMM150(int bus, const char *path, bool external, enum Rotation rotatio
I2C("BMM150", path, bus, BMM150_SLAVE_ADDRESS, BMM150_BUS_SPEED),
_work{},
_external(false),
_running(false),
_call_interval(0),
_report{0},
_reports(nullptr),
_collect_phase(false),
_scale{},
_range_scale(0.01), /* default range scale from from uT to gauss */
_topic(nullptr),
@ -261,6 +276,8 @@ BMM150 :: BMM150(int bus, const char *path, bool external, enum Rotation rotatio
_sample_perf(perf_alloc(PC_ELAPSED, "bmm150_read")),
_bad_transfers(perf_alloc(PC_COUNT, "bmm150_bad_transfers")),
_good_transfers(perf_alloc(PC_COUNT, "bmm150_good_transfers")),
_measure_perf(perf_alloc(PC_ELAPSED, "bmp280_measure")),
_comms_errors(perf_alloc(PC_COUNT, "bmp280_comms_errors")),
_duplicates(perf_alloc(PC_COUNT, "bmm150_duplicates")),
_rotation(rotation),
_got_duplicate(false),
@ -298,10 +315,17 @@ BMM150 :: ~BMM150()
unregister_class_devname(MAG_BASE_DEVICE_PATH, _class_instance);
}
if (_topic != nullptr) {
orb_unadvertise(_topic);
}
/* delete the perf counter */
perf_free(_sample_perf);
perf_free(_bad_transfers);
perf_free(_good_transfers);
perf_free(_measure_perf);
perf_free(_comms_errors);
perf_free(_duplicates);
}
@ -333,7 +357,7 @@ int BMM150::init()
/* check id*/
if (read_reg(BMM150_CHIP_ID_REG) != BMM150_CHIP_ID) {
warnx("id of magnetometer is not: 0x%02x", BMM150_CHIP_ID);
PX4_WARN("id of magnetometer is not: 0x%02x", BMM150_CHIP_ID);
return -EIO;
}
@ -345,7 +369,15 @@ int BMM150::init()
_class_instance = register_class_devname(MAG_BASE_DEVICE_PATH);
collect();
if (measure()) {
return -EIO;
}
up_udelay(10000);
if (collect()) {
return -EIO;
}
/* advertise sensor topic, measure manually to initialize valid report */
struct mag_report mrb;
@ -356,7 +388,7 @@ int BMM150::init()
&_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_MAX);
if (_topic == nullptr) {
warnx("ADVERT FAIL");
PX4_WARN("ADVERT FAIL");
}
out:
@ -381,18 +413,21 @@ void
BMM150::start()
{
/* reset the report ring and state machine */
_collect_phase = false;
_running = true;
_reports->flush();
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&BMM150::cycle_trampoline, this, 1);
}
void
BMM150::stop()
{
_running = false;
work_cancel(HPWORK, &_work);
}
ssize_t
@ -430,6 +465,15 @@ BMM150::read(struct file *filp, char *buffer, size_t buflen)
do {
_reports->flush();
/* trigger a measurement */
if (OK != measure()) {
ret = -EIO;
break;
}
/* wait for it to complete */
usleep(BMM150_CONVERSION_INTERVAL);
/* run the collection phase */
if (OK != collect()) {
ret = -EIO;
@ -455,21 +499,65 @@ BMM150::cycle_trampoline(void *arg)
/* make measurement */
dev->cycle();
}
void
BMM150::cycle()
{
collect();
work_queue(HPWORK, &_work, (worker_t)&BMM150::cycle_trampoline, this, USEC2TICK(BMM150_CONVERSION_INTERVAL));
if (_collect_phase) {
collect();
unsigned wait_gap = _call_interval - USEC2TICK(BMM150_CONVERSION_INTERVAL);
if ((wait_gap != 0) && (_running)) {
work_queue(HPWORK, &_work, (worker_t)&BMM150::cycle_trampoline, this,
wait_gap); //need to wait some time before new measurement
return;
}
}
measure();
if ((_running)) {
/* schedule a fresh cycle call when the measurement is done */
work_queue(HPWORK,
&_work,
(worker_t)&BMM150::cycle_trampoline,
this,
USEC2TICK(BMM150_CONVERSION_INTERVAL));
}
}
int
BMM150::measure()
{
_collect_phase = true;
perf_begin(_measure_perf);
/* start measure */
int ret = set_power_mode(BMM150_FORCED_MODE);
if (ret != OK) {
perf_count(_comms_errors);
perf_cancel(_measure_perf);
return -EIO;
}
perf_end(_measure_perf);
return OK;
}
int
BMM150::collect()
{
_collect_phase = false;
bool mag_notify = true;
uint8_t mag_data[8], status;
uint16_t resistance, lsb, msb, msblsb;
@ -537,6 +625,7 @@ BMM150::collect()
mrb.z_raw == 0 &&
resistance == 0) {
// all zero data - probably a I2C bus error
perf_count(_comms_errors);
perf_count(_bad_transfers);
perf_end(_sample_perf);
return -EIO;
@ -601,6 +690,9 @@ BMM150::collect()
// should use this sensor based on whether it has had failures
mrb.error_count = perf_event_count(_bad_transfers);
// apply user specified rotation
rotate_3f(_rotation, mrb.x, mrb.y, mrb.z);
/* Scaling the data */
mrb.x = ((mrb.x * _range_scale) - _scale.x_offset) * _scale.x_scale;
@ -1029,7 +1121,6 @@ BMM150::print_info()
(double)(1.0f / _range_scale));
printf("\n");
}
void

22
src/drivers/bmm150/bmm150.hpp
View File

@ -17,6 +17,7 @@
#include <math.h>
#include <unistd.h>
#include <getopt.h>
#include <px4_log.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
@ -128,9 +129,10 @@
/* Preset modes - Data rates */
#define BMM150_LOWPOWER_DR BMM150_DATA_RATE_30HZ
#define BMM150_REGULAR_DR BMM150_DATA_RATE_30HZ
#define BMM150_HIGHACCURACY_DR BMM150_DATA_RATE_25HZ
#define BMM150_HIGHACCURACY_DR BMM150_DATA_RATE_20HZ
#define BMM150_ENHANCED_DR BMM150_DATA_RATE_10HZ
/* Power modes value definitions */
#define BMM150_NORMAL_MODE 0x00
#define BMM150_FORCED_MODE 0x02
@ -201,6 +203,11 @@ public:
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
/**
* Stop automatic measurement.
*/
void stop();
/**
* Diagnostics - print some basic information about the driver.
*/
@ -215,6 +222,8 @@ private:
work_s _work;
bool _external;
bool _running;
/* altitude conversion calibration */
unsigned _call_interval;
@ -222,6 +231,8 @@ private:
struct mag_report _report;
ringbuffer::RingBuffer *_reports;
bool _collect_phase;
struct mag_calibration_s _scale;
float _range_scale;
@ -251,6 +262,8 @@ private:
perf_counter_t _sample_perf;
perf_counter_t _bad_transfers;
perf_counter_t _good_transfers;
perf_counter_t _measure_perf;
perf_counter_t _comms_errors;
perf_counter_t _duplicates;
enum Rotation _rotation;
@ -265,12 +278,7 @@ private:
*/
void start();
/**
* Stop automatic measurement.
*/
void stop();
int measure(); //start measure
int collect(); //get results and publish
static void cycle_trampoline(void *arg);

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

@ -37,6 +37,7 @@ px4_add_module(
COMPILE_FLAGS
SRCS
bmp280_spi.cpp
bmp280_i2c.cpp
bmp280.cpp
DEPENDS
platforms__common

149
src/drivers/bmp280/bmp280.cpp
View File

@ -51,6 +51,7 @@
#include <math.h>
#include <unistd.h>
#include <getopt.h>
#include <px4_log.h>
#include <nuttx/arch.h>
#include <nuttx/wqueue.h>
@ -104,6 +105,8 @@ public:
private:
bmp280::IBMP280 *_interface;
bool _running;
uint8_t _curr_ctrl;
struct work_s _work;
@ -150,6 +153,7 @@ extern "C" __EXPORT int bmp280_main(int argc, char *argv[]);
BMP280::BMP280(bmp280::IBMP280 *interface, const char *path) :
CDev("BMP280", path),
_interface(interface),
_running(false),
_report_ticks(0),
_reports(nullptr),
_collect_phase(false),
@ -161,8 +165,6 @@ BMP280::BMP280(bmp280::IBMP280 *interface, const char *path) :
_measure_perf(perf_alloc(PC_ELAPSED, "bmp280_measure")),
_comms_errors(perf_alloc(PC_COUNT, "bmp280_comms_errors"))
{
_device_id.devid_s.devtype = DRV_BARO_DEVTYPE_BMP280;
// work_cancel in stop_cycle called from the dtor will explode if we don't do this...
memset(&_work, 0, sizeof(_work));
}
@ -181,6 +183,11 @@ BMP280::~BMP280()
delete _reports;
}
if (_baro_topic != nullptr) {
orb_unadvertise(_baro_topic);
}
// free perf counters
perf_free(_sample_perf);
perf_free(_measure_perf);
@ -221,7 +228,7 @@ BMP280::init()
/* check id*/
if (_interface->get_reg(BPM280_ADDR_ID) != BPM280_VALUE_ID) {
warnx("id of your baro is not: 0x%02x", BPM280_VALUE_ID);
PX4_WARN("id of your baro is not: 0x%02x", BPM280_VALUE_ID);
return -EIO;
}
@ -270,7 +277,7 @@ BMP280::init()
&_orb_class_instance, _interface->is_external() ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT);
if (_baro_topic == nullptr) {
warnx("failed to create sensor_baro publication");
PX4_WARN("failed to create sensor_baro publication");
return -ENOMEM;
}
@ -440,6 +447,7 @@ BMP280::start_cycle()
/* reset the report ring and state machine */
_collect_phase = false;
_running = true;
_reports->flush();
/* schedule a cycle to start things */
@ -449,6 +457,7 @@ BMP280::start_cycle()
void
BMP280::stop_cycle()
{
_running = false;
work_cancel(HPWORK, &_work);
}
@ -467,7 +476,7 @@ BMP280::cycle()
collect();
unsigned wait_gap = _report_ticks - _max_mesure_ticks;
if (wait_gap != 0) {
if ((wait_gap != 0) && (_running)) {
work_queue(HPWORK, &_work, (worker_t)&BMP280::cycle_trampoline, this,
wait_gap); //need to wait some time before new measurement
return;
@ -476,7 +485,10 @@ BMP280::cycle()
}
measure();
work_queue(HPWORK, &_work, (worker_t)&BMP280::cycle_trampoline, this, _max_mesure_ticks);
if (_running) {
work_queue(HPWORK, &_work, (worker_t)&BMP280::cycle_trampoline, this, _max_mesure_ticks);
}
}
@ -542,8 +554,6 @@ BMP280::collect()
report.temperature = _T;
report.pressure = _P / 100.0f; // to mbar
/* Get device ID */
report.device_id = _device_id.devid;
/* altitude calculations based on http://www.kansasflyer.org/index.asp?nav=Avi&sec=Alti&tab=Theory&pg=1 */
@ -620,10 +630,10 @@ struct bmp280_bus_option {
{ BMP280_BUS_SPI_INTERNAL, "/dev/bmp280_spi_int", &bmp280_spi_interface, PX4_SPI_BUS_SENSORS, PX4_SPIDEV_BARO, false, NULL },
#endif
#ifdef PX4_I2C_OBDEV_BMP280
{ BMP280_BUS_I2C_INTERNAL, "/dev/bmp280_i2c_int", nullptr, PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_BMP280, false, NULL },
{ BMP280_BUS_I2C_INTERNAL, "/dev/bmp280_i2c_int", &bmp280_i2c_interface, PX4_I2C_BUS_EXPANSION, PX4_I2C_OBDEV_BMP280, false, NULL },
#endif
#if defined(PX4_I2C_BUS_EXPANSION) && defined(PX4_I2C_EXT_OBDEV_BMP280)
{ BMP280_BUS_I2C_EXTERNAL, "/dev/bmp280_i2c_ext", nullptr, PX4_I2C_BUS_EXPANSION, PX4_I2C_EXT_OBDEV_BMP280, true, NULL },
{ BMP280_BUS_I2C_EXTERNAL, "/dev/bmp280_i2c_ext", &bmp280_i2c_interface, PX4_I2C_BUS_EXPANSION, PX4_I2C_EXT_OBDEV_BMP280, true, NULL },
#endif
};
#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0]))
@ -645,14 +655,15 @@ bool
start_bus(struct bmp280_bus_option &bus)
{
if (bus.dev != nullptr) {
errx(1, "bus option already started");
PX4_ERR("bus option already started");
exit(1);
}
bmp280::IBMP280 *interface = bus.interface_constructor(bus.busnum, bus.device, bus.external);
if (interface->init() != OK) {
delete interface;
warnx("no device on bus %u", (unsigned)bus.busid);
PX4_WARN("no device on bus %u", (unsigned)bus.busid);
return false;
}
@ -668,14 +679,17 @@ start_bus(struct bmp280_bus_option &bus)
/* set the poll rate to default, starts automatic data collection */
if (fd == -1) {
errx(1, "can't open baro device");
PX4_ERR("can't open baro device");
exit(1);
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
close(fd);
errx(1, "failed setting default poll rate");
PX4_ERR("failed setting default poll rate");
exit(1);
}
close(fd);
return true;
}
@ -708,7 +722,9 @@ start(enum BMP280_BUS busid)
}
if (!started) {
errx(1, "driver start failed");
PX4_WARN("bus option number is %d", i);
PX4_ERR("driver start failed");
exit(1);
}
// one or more drivers started OK
@ -728,7 +744,8 @@ struct bmp280_bus_option &find_bus(enum BMP280_BUS busid)
}
}
errx(1, "bus %u not started", (unsigned)busid);
PX4_ERR("bus %u not started", (unsigned)busid);
exit(1);
}
/**
@ -749,30 +766,34 @@ test(enum BMP280_BUS busid)
fd = open(bus.devpath, O_RDONLY);
if (fd < 0) {
err(1, "open failed (try 'bmp280 start' if the driver is not running)");
PX4_ERR("open failed (try 'bmp280 start' if the driver is not running)");
exit(1);
}
/* do a simple demand read */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
err(1, "immediate read failed");
PX4_ERR("immediate read failed");
exit(1);
}
warnx("single read");
warnx("pressure: %10.4f", (double)report.pressure);
warnx("altitude: %11.4f", (double)report.altitude);
warnx("temperature: %8.4f", (double)report.temperature);
warnx("time: %lld", report.timestamp);
PX4_WARN("single read");
PX4_WARN("pressure: %10.4f", (double)report.pressure);
PX4_WARN("altitude: %11.4f", (double)report.altitude);
PX4_WARN("temperature: %8.4f", (double)report.temperature);
PX4_WARN("time: %lld", report.timestamp);
/* set the queue depth to 10 */
if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) {
errx(1, "failed to set queue depth");
PX4_ERR("failed to set queue depth");
exit(1);
}
/* start the sensor polling at 2Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
errx(1, "failed to set 2Hz poll rate");
PX4_ERR("failed to set 2Hz poll rate");
exit(1);
}
/* read the sensor 5x and report each value */
@ -785,25 +806,28 @@ test(enum BMP280_BUS busid)
ret = poll(&fds, 1, 2000);
if (ret != 1) {
errx(1, "timed out waiting for sensor data");
PX4_ERR("timed out waiting for sensor data");
exit(1);
}
/* now go get it */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
err(1, "periodic read failed");
PX4_ERR("periodic read failed");
exit(1);
}
warnx("periodic read %u", i);
warnx("pressure: %10.4f", (double)report.pressure);
warnx("altitude: %11.4f", (double)report.altitude);
warnx("temperature K: %8.4f", (double)report.temperature);
warnx("time: %lld", report.timestamp);
PX4_WARN("periodic read %u", i);
PX4_WARN("pressure: %10.4f", (double)report.pressure);
PX4_WARN("altitude: %11.4f", (double)report.altitude);
PX4_WARN("temperature K: %8.4f", (double)report.temperature);
PX4_WARN("time: %lld", report.timestamp);
}
close(fd);
errx(0, "PASS");
PX4_ERR("PASS");
exit(0);
}
/**
@ -818,15 +842,18 @@ reset(enum BMP280_BUS busid)
fd = open(bus.devpath, O_RDONLY);
if (fd < 0) {
err(1, "failed ");
PX4_ERR("failed ");
exit(1);
}
if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
err(1, "driver reset failed");
PX4_ERR("driver reset failed");
exit(1);
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
err(1, "driver poll restart failed");
PX4_ERR("driver poll restart failed");
exit(1);
}
exit(0);
@ -842,7 +869,7 @@ info()
struct bmp280_bus_option &bus = bus_options[i];
if (bus.dev != nullptr) {
warnx("%s", bus.devpath);
PX4_WARN("%s", bus.devpath);
bus.dev->print_info();
}
}
@ -866,12 +893,14 @@ calibrate(unsigned altitude, enum BMP280_BUS busid)
fd = open(bus.devpath, O_RDONLY);
if (fd < 0) {
err(1, "open failed (try 'bmp280 start' if the driver is not running)");
PX4_ERR("open failed (try 'bmp280 start' if the driver is not running)");
exit(1);
}
/* start the sensor polling at max */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX)) {
errx(1, "failed to set poll rate");
PX4_ERR("failed to set poll rate");
exit(1);
}
/* average a few measurements */
@ -888,14 +917,16 @@ calibrate(unsigned altitude, enum BMP280_BUS busid)
ret = poll(&fds, 1, 1000);
if (ret != 1) {
errx(1, "timed out waiting for sensor data");
PX4_ERR("timed out waiting for sensor data");
exit(1);
}
/* now go get it */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
err(1, "sensor read failed");
PX4_ERR("sensor read failed");
exit(1);
}
pressure += report.pressure;
@ -910,17 +941,18 @@ calibrate(unsigned altitude, enum BMP280_BUS busid)
const float g = 9.80665f; /* gravity constant in m/s/s */
const float R = 287.05f; /* ideal gas constant in J/kg/K */
warnx("averaged pressure %10.4fkPa at %um", (double)pressure, altitude);
PX4_WARN("averaged pressure %10.4fkPa at %um", (double)pressure, altitude);
p1 = pressure * (powf(((T1 + (a * (float)altitude)) / T1), (g / (a * R))));
warnx("calculated MSL pressure %10.4fkPa", (double)p1);
PX4_WARN("calculated MSL pressure %10.4fkPa", (double)p1);
/* save as integer Pa */
p1 *= 1000.0f;
if (ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)p1) != OK) {
err(1, "BAROIOCSMSLPRESSURE");
PX4_ERR("BAROIOCSMSLPRESSURE");
exit(1);
}
close(fd);
@ -930,16 +962,23 @@ calibrate(unsigned altitude, enum BMP280_BUS busid)
void
usage()
{
warnx("missing command: try 'start', 'info', 'test', 'test2', 'reset', 'calibrate'");
warnx("options:");
warnx(" -X (external I2C bus TODO)");
warnx(" -I (internal I2C bus TODO)");
warnx(" -S (external SPI bus)");
warnx(" -s (internal SPI bus)");
PX4_WARN("missing command: try 'start', 'info', 'test', 'test2', 'reset', 'calibrate'");
PX4_WARN("options:");
PX4_WARN(" -X (external I2C bus TODO)");
PX4_WARN(" -I (internal I2C bus TODO)");
PX4_WARN(" -S (external SPI bus)");
PX4_WARN(" -s (internal SPI bus)");
}
} // namespace
bmp280::IBMP280::~IBMP280()
{
}
int
bmp280_main(int argc, char *argv[])
{
@ -951,12 +990,12 @@ bmp280_main(int argc, char *argv[])
switch (ch) {
case 'X':
busid = BMP280_BUS_I2C_EXTERNAL;
errx(1, "not supported yet");
break;
case 'I':
busid = BMP280_BUS_I2C_INTERNAL;
errx(1, "not supported yet");
//PX4_ERR("not supported yet");
//exit(1);
break;
case 'S':
@ -1008,7 +1047,8 @@ bmp280_main(int argc, char *argv[])
*/
if (!strcmp(verb, "calibrate")) {
if (argc < 2) {
errx(1, "missing altitude");
PX4_ERR("missing altitude");
exit(1);
}
long altitude = strtol(argv[optind + 1], nullptr, 10);
@ -1016,5 +1056,6 @@ bmp280_main(int argc, char *argv[])
bmp280::calibrate(altitude, busid);
}
errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
PX4_ERR("unrecognized command, try 'start', 'test', 'reset' or 'info'");
exit(1);
}

40
src/drivers/bmp280/bmp280.h
View File

@ -50,28 +50,28 @@
#define BPM280_VALUE_ID 0x58 /* chip id */
#define BPM280_VALUE_RESET 0xB6 /* reset */
#define BPM280_STATUS_MEASURING 1<<3 /* if in process of measure */
#define BPM280_STATUS_COPING 1<<0 /* if in process of data copy */
#define BPM280_STATUS_MEASURING (1<<3) /* if in process of measure */
#define BPM280_STATUS_COPING (1<<0) /* if in process of data copy */
#define BPM280_CTRL_P0 0x0<<2 /* no p measure */
#define BPM280_CTRL_P1 0x1<<2
#define BPM280_CTRL_P2 0x2<<2
#define BPM280_CTRL_P4 0x3<<2
#define BPM280_CTRL_P8 0x4<<2
#define BPM280_CTRL_P16 0x5<<2
#define BPM280_CTRL_P0 (0x0<<2) /* no p measure */
#define BPM280_CTRL_P1 (0x1<<2)
#define BPM280_CTRL_P2 (0x2<<2)
#define BPM280_CTRL_P4 (0x3<<2)
#define BPM280_CTRL_P8 (0x4<<2)
#define BPM280_CTRL_P16 (0x5<<2)
#define BPM280_CTRL_T0 0x0<<5 /* no t measure */
#define BPM280_CTRL_T1 0x1<<5
#define BPM280_CTRL_T2 0x2<<5
#define BPM280_CTRL_T4 0x3<<5
#define BPM280_CTRL_T8 0x4<<5
#define BPM280_CTRL_T16 0x5<<5
#define BPM280_CTRL_T0 (0x0<<5) /* no t measure */
#define BPM280_CTRL_T1 (0x1<<5)
#define BPM280_CTRL_T2 (0x2<<5)
#define BPM280_CTRL_T4 (0x3<<5)
#define BPM280_CTRL_T8 (0x4<<5)
#define BPM280_CTRL_T16 (0x5<<5)
#define BPM280_CONFIG_F0 0x0<<2 /* no filter */
#define BPM280_CONFIG_F2 0x1<<2
#define BPM280_CONFIG_F4 0x2<<2
#define BPM280_CONFIG_F8 0x3<<2
#define BPM280_CONFIG_F16 0x4<<2
#define BPM280_CONFIG_F0 (0x0<<2) /* no filter */
#define BPM280_CONFIG_F2 (0x1<<2)
#define BPM280_CONFIG_F4 (0x2<<2)
#define BPM280_CONFIG_F8 (0x3<<2)
#define BPM280_CONFIG_F16 (0x4<<2)
#define BPM280_CTRL_MODE_SLEEP 0x0
@ -146,6 +146,8 @@ public:
} /* namespace */
/* interface factories */
extern bmp280::IBMP280 *bmp280_spi_interface(uint8_t busnum, uint8_t device, bool external);
extern bmp280::IBMP280 *bmp280_i2c_interface(uint8_t busnum, uint8_t device, bool external);

142
src/drivers/bmp280/bmp280_i2c.cpp Normal file
View File

@ -0,0 +1,142 @@
/****************************************************************************
*
* Copyright (c) 2016 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 bmp280_spi.cpp
*
* SPI interface for BMP280
*/
#include <px4_config.h>
#include <drivers/bmp280/bmp280.h>
#include <drivers/device/i2c.h>
#include "board_config.h"
#if defined(PX4_I2C_OBDEV_BMP280) || defined(PX4_I2C_EXT_OBDEV_BMP280)
class BMP280_I2C: public device::I2C, public bmp280::IBMP280
{
public:
BMP280_I2C(uint8_t bus, uint8_t device, bool external);
~BMP280_I2C();
bool is_external();
int init();
uint8_t get_reg(uint8_t addr);
int set_reg(uint8_t value, uint8_t addr);
bmp280::data_s *get_data(uint8_t addr);
bmp280::calibration_s *get_calibration(uint8_t addr);
private:
struct bmp280::calibration_s _cal;
struct bmp280::data_s _data;
bool _external;
};
bmp280::IBMP280 *bmp280_i2c_interface(uint8_t busnum, uint8_t device, bool external)
{
return new BMP280_I2C(busnum, device, external);
}
BMP280_I2C::BMP280_I2C(uint8_t bus, uint8_t device, bool external) :
I2C("BMP280_I2C", nullptr, bus, device, 100 * 1000)
{
_external = external;
}
BMP280_I2C::~BMP280_I2C()
{
}
bool BMP280_I2C::is_external()
{
return _external;
}
int BMP280_I2C::init()
{
return I2C::init();
}
uint8_t BMP280_I2C::get_reg(uint8_t addr)
{
uint8_t cmd[2] = { (uint8_t)(addr), 0};
transfer(&cmd[0], 1, &cmd[1], 1);
return cmd[1];
}
int BMP280_I2C::set_reg(uint8_t value, uint8_t addr)
{
uint8_t cmd[2] = { (uint8_t)(addr), value};
return transfer(cmd, sizeof(cmd), nullptr, 0);
}
bmp280::data_s *BMP280_I2C::get_data(uint8_t addr)
{
const uint8_t cmd = (uint8_t)(addr);
if (transfer(&cmd, sizeof(cmd), (uint8_t *)&_data, sizeof(struct bmp280::data_s)) == OK) {
return (&_data);
} else {
return nullptr;
}
}
bmp280::calibration_s *BMP280_I2C::get_calibration(uint8_t addr)
{
const uint8_t cmd = (uint8_t)(addr) ;
if (transfer(&cmd, sizeof(cmd), (uint8_t *)&_cal, sizeof(struct bmp280::calibration_s)) == OK) {
return &(_cal);
} else {
return nullptr;
}
}
#endif /* PX4_I2C_OBDEV_BMP280 || PX4_I2C_EXT_OBDEV_BMP280 */

3
src/drivers/bmp280/bmp280_spi.cpp
View File

@ -93,9 +93,6 @@ BMP280_SPI::BMP280_SPI(uint8_t bus, spi_dev_e device, bool external) :
_external = external;
}
bmp280::IBMP280::~IBMP280()
{
}
BMP280_SPI::~BMP280_SPI()
{

44
src/drivers/bmp285/CMakeLists.txt
View File

@ -1,44 +0,0 @@
############################################################################
#
# Copyright (c) 2016 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_module(
MODULE drivers__bmp285
MAIN bmp285
COMPILE_FLAGS
SRCS
bmp285_main.cpp
bmp285.cpp
DEPENDS
platforms__common
)
# vim: set noet ft=cmake fenc=utf-8 ff=unix :

557
src/drivers/bmp285/bmp285.cpp
View File

@ -1,557 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012-2016 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 bmp285.cpp
* Driver for the BMP285 barometric pressure sensor connected via I2C TODO or SPI.
*/
#include <drivers/bmp285/bmp285.hpp>
#ifndef CONFIG_SCHED_WORKQUEUE
# error This requires CONFIG_SCHED_WORKQUEUE.
#endif
BMP285::BMP285(int bus, uint16_t address, const char *path, bool external) :
I2C("BMP285", path, bus, address, BMP285_BUS_SPEED),
_curr_ctrl(0),
_report_ticks(0),
_max_mesure_ticks(0),
_reports(nullptr),
_collect_phase(false),
_msl_pressure(101325),
_baro_topic(nullptr),
_orb_class_instance(-1),
_class_instance(-1),
_sample_perf(perf_alloc(PC_ELAPSED, "bmp285_read")),
_measure_perf(perf_alloc(PC_ELAPSED, "bmp285_measure")),
_comms_errors(perf_alloc(PC_COUNT, "bmp285_comms_errors")),
_buffer_overflows(perf_alloc(PC_COUNT, "bmp285_buffer_overflows")),
_P(0.0f),
_T(0.0f)
{
_external = external;
// work_cancel in stop_cycle called from the dtor will explode if we don't do this...
memset(&_work, 0, sizeof(_work));
}
BMP285::~BMP285()
{
/* make sure we are truly inactive */
stop_cycle();
if (_class_instance != -1) {
unregister_class_devname(get_devname(), _class_instance);
}
/* free any existing reports */
if (_reports != nullptr) {
delete _reports;
}
// free perf counters
perf_free(_sample_perf);
perf_free(_measure_perf);
perf_free(_comms_errors);
perf_free(_buffer_overflows);
}
int
BMP285::init()
{
int ret;
/* do I2C init (and probe) first */
ret = I2C::init();
/* if probe/setup failed, bail now */
if (ret != OK) {
DEVICE_DEBUG("I2C setup failed");
return ret;
}
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(baro_report));
if (_reports == nullptr) {
DEVICE_DEBUG("can't get memory for reports");
ret = -ENOMEM;
return ret;
}
/* register alternate interfaces if we have to */
_class_instance = register_class_devname(BARO_BASE_DEVICE_PATH);
/* reset sensor */
write_reg(BPM285_ADDR_RESET, BPM285_VALUE_RESET);
usleep(10000);
/* check id*/
if (read_reg(BPM285_ADDR_ID) != BPM285_VALUE_ID) {
warnx("id of your baro is not: 0x%02x", BPM285_VALUE_ID);
return -EIO;
}
/* Put sensor in Normal mode */
ret = write_reg(BPM285_ADDR_CTRL, _curr_ctrl | BPM285_CTRL_MODE_NORMAL);
/* set config, recommended settings */
_curr_ctrl = BPM285_CTRL_P16 | BPM285_CTRL_T2;
write_reg(BPM285_ADDR_CTRL, _curr_ctrl);
_max_mesure_ticks = USEC2TICK(BPM285_MT_INIT + BPM285_MT * (16 - 1 + 2 - 1));
/* Configure filter settings */
write_reg(BPM285_ADDR_CONFIG, BPM285_CONFIG_F16);
/* get calibration and pre process them*/
get_calibration(BPM285_ADDR_CAL);
_fcal.t1 = _cal.t1 * powf(2, 4);
_fcal.t2 = _cal.t2 * powf(2, -14);
_fcal.t3 = _cal.t3 * powf(2, -34);
_fcal.p1 = _cal.p1 * (powf(2, 4) / -100000.0f);
_fcal.p2 = _cal.p1 * _cal.p2 * (powf(2, -31) / -100000.0f);
_fcal.p3 = _cal.p1 * _cal.p3 * (powf(2, -51) / -100000.0f);
_fcal.p4 = _cal.p4 * powf(2, 4) - powf(2, 20);
_fcal.p5 = _cal.p5 * powf(2, -14);
_fcal.p6 = _cal.p6 * powf(2, -31);
_fcal.p7 = _cal.p7 * powf(2, -4);
_fcal.p8 = _cal.p8 * powf(2, -19) + 1.0f;
_fcal.p9 = _cal.p9 * powf(2, -35);
/* do a first measurement cycle to populate reports with valid data */
struct baro_report brp;
_reports->flush();
if (measure()) {
return -EIO;
}
usleep(TICK2USEC(_max_mesure_ticks));
if (collect()) {
return -EIO;
}
_reports->get(&brp);
_baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp,
&_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT);
if (_baro_topic == nullptr) {
warnx("failed to create sensor_baro publication");
return -ENOMEM;
}
return OK;
}
ssize_t
BMP285::read(struct file *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(struct baro_report);
struct baro_report *brp = reinterpret_cast<struct baro_report *>(buffer);
int ret = 0;
/* buffer must be large enough */
if (count < 1) {
return -ENOSPC;
}
/* if automatic measurement is enabled */
if (_report_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(brp)) {
ret += sizeof(*brp);
brp++;
}
}
/* if there was no data, warn the caller */
return ret ? ret : -EAGAIN;
}
/* manual measurement - run one conversion */
_reports->flush();
if (measure()) {
return -EIO;
}
usleep(TICK2USEC(_max_mesure_ticks));
if (collect()) {
return -EIO;
}
if (_reports->get(brp)) { //get new generated report
ret = sizeof(*brp);
}
return ret;
}
int
BMP285::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case SENSORIOCSPOLLRATE: {
unsigned ticks = 0;
switch (arg) {
case SENSOR_POLLRATE_MANUAL:
stop_cycle();
_report_ticks = 0;
return OK;
case SENSOR_POLLRATE_EXTERNAL:
case 0:
return -EINVAL;
case SENSOR_POLLRATE_MAX:
case SENSOR_POLLRATE_DEFAULT:
ticks = _max_mesure_ticks;
default: {
if (ticks == 0) {
ticks = USEC2TICK(USEC_PER_SEC / arg);
}
/* do we need to start internal polling? */
bool want_start = (_report_ticks == 0);
/* check against maximum rate */
if (ticks < _max_mesure_ticks) {
return -EINVAL;
}
_report_ticks = ticks;
if (want_start) {
start_cycle();
}
return OK;
}
}
break;
}
case SENSORIOCGPOLLRATE:
if (_report_ticks == 0) {
return SENSOR_POLLRATE_MANUAL;
}
return (USEC_PER_SEC / USEC_PER_TICK / _report_ticks);
case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100)) {
return -EINVAL;
}
irqstate_t flags = px4_enter_critical_section();
if (!_reports->resize(arg)) {
px4_leave_critical_section(flags);
return -ENOMEM;
}
px4_leave_critical_section(flags);
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _reports->size();
case SENSORIOCRESET:
/*
* Since we are initialized, we do not need to do anything, since the
* PROM is correctly read and the part does not need to be configured.
*/
return OK;
case BAROIOCSMSLPRESSURE:
/* range-check for sanity */
if ((arg < 80000) || (arg > 120000)) {
return -EINVAL;
}
_msl_pressure = arg;
return OK;
case BAROIOCGMSLPRESSURE:
return _msl_pressure;
default:
break;
}
return CDev::ioctl(filp, cmd, arg);
}
void
BMP285::start_cycle()
{
/* reset the report ring and state machine */
_collect_phase = false;
_reports->flush();
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&BMP285::cycle_trampoline, this, 1);
}
void
BMP285::stop_cycle()
{
work_cancel(HPWORK, &_work);
}
void
BMP285::cycle_trampoline(void *arg)
{
BMP285 *dev = reinterpret_cast<BMP285 *>(arg);
dev->cycle();
}
void
BMP285::cycle()
{
if (_collect_phase) {
collect();
unsigned wait_gap = _report_ticks - _max_mesure_ticks;
if (wait_gap != 0) {
work_queue(HPWORK, &_work, (worker_t)&BMP285::cycle_trampoline, this,
wait_gap); //need to wait some time before new measurement
return;
}
}
measure();
work_queue(HPWORK, &_work, (worker_t)&BMP285::cycle_trampoline, this, _max_mesure_ticks);
}
int
BMP285::measure()
{
_collect_phase = true;
perf_begin(_measure_perf);
/* start measure */
int ret = write_reg(BPM285_ADDR_CTRL, _curr_ctrl | BPM285_CTRL_MODE_FORCE);
if (ret != OK) {
perf_count(_comms_errors);
perf_cancel(_measure_perf);
return -EIO;
}
perf_end(_measure_perf);
return OK;
}
int
BMP285::collect()
{
_collect_phase = false;
perf_begin(_sample_perf);
struct baro_report report;
/* this should be fairly close to the end of the conversion, so the best approximation of the time */
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
bmp285::data_s *data = get_data(BPM285_ADDR_DATA);
if (data == nullptr) {
perf_count(_comms_errors);
perf_cancel(_sample_perf);
return -EIO;
}
//convert data to number 20 bit
uint32_t p_raw = data->p_msb << 12 | data->p_lsb << 4 | data->p_xlsb >> 4;
uint32_t t_raw = data->t_msb << 12 | data->t_lsb << 4 | data->t_xlsb >> 4;
// Temperature
float ofs = (float) t_raw - _fcal.t1;
float t_fine = (ofs * _fcal.t3 + _fcal.t2) * ofs;
_T = t_fine * (1.0f / 5120.0f);
// Pressure
float tf = t_fine - 128000.0f;
float x1 = (tf * _fcal.p6 + _fcal.p5) * tf + _fcal.p4;
float x2 = (tf * _fcal.p3 + _fcal.p2) * tf + _fcal.p1;
float pf = ((float) p_raw + x1) / x2;
_P = (pf * _fcal.p9 + _fcal.p8) * pf + _fcal.p7;
report.temperature = _T;
report.pressure = _P / 100.0f; // to mbar
/* altitude calculations based on http://www.kansasflyer.org/index.asp?nav=Avi&sec=Alti&tab=Theory&pg=1 */
/* tropospheric properties (0-11km) for standard atmosphere */
const float T1 = 15.0f + 273.15f; /* temperature at base height in Kelvin */
const float a = -6.5f / 1000.0f; /* temperature gradient in degrees per metre */
const float g = 9.80665f; /* gravity constant in m/s/s */
const float R = 287.05f; /* ideal gas constant in J/kg/K */
float pK = _P / _msl_pressure;
/*
* Solve:
*
* / -(aR / g) \
* | (p / p1) . T1 | - T1
* \ /
* h = ------------------------------- + h1
* a
*/
report.altitude = (((powf(pK, (-(a * R) / g))) * T1) - T1) / a;
/* publish it */
if (!(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
}
if (_reports->force(&report)) {
perf_count(_buffer_overflows);
}
/* notify anyone waiting for data */
poll_notify(POLLIN);
perf_end(_sample_perf);
return OK;
}
void
BMP285::print_info()
{
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
perf_print_counter(_buffer_overflows);
printf("poll interval: %u us \n", _report_ticks * USEC_PER_TICK);
_reports->print_info("report queue");
printf("P Pa: %.3f\n", (double)_P);
printf("T: %.3f\n", (double)_T);
printf("MSL pressure Pa: %u\n", _msl_pressure);
}
bool BMP285::is_external()
{
return _external;
};
uint8_t
BMP285::read_reg(unsigned reg)
{
const uint8_t cmd = (uint8_t)(reg) ;
uint8_t result;
transfer(&cmd, sizeof(cmd), &result, 1);
return result;
}
int
BMP285::write_reg(unsigned reg, uint8_t value)
{
uint8_t cmd[2] = {(uint8_t)reg, value};
return transfer(cmd, sizeof(cmd), nullptr, 0);
}
bmp285::data_s *BMP285::get_data(uint8_t reg)
{
const uint8_t cmd = (uint8_t)(reg);
if (transfer(&cmd, sizeof(cmd), (uint8_t *)&_data, sizeof(struct bmp285::data_s)) == OK) {
return (&_data);
} else {
return nullptr;
}
}
bmp285::calibration_s *BMP285::get_calibration(uint8_t reg)
{
const uint8_t cmd = (uint8_t)(reg) ;
if (transfer(&cmd, sizeof(cmd), (uint8_t *)&_cal, sizeof(struct bmp285::calibration_s)) == OK) {
return &(_cal);
} else {
return nullptr;
}
}

278
src/drivers/bmp285/bmp285.hpp
View File

@ -1,278 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012-2016 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 bmp285.h
*
* Shared defines for the bmp285 driver.
*/
#ifndef BMP285_HPP_
#define BMP285_HPP_
#include <px4_config.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdbool.h>
#include <stdlib.h>
#include <semaphore.h>
#include <string.h>
#include <fcntl.h>
#include <poll.h>
#include <errno.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <getopt.h>
#include <nuttx/arch.h>
#include <nuttx/wqueue.h>
#include <nuttx/clock.h>
#include <arch/board/board.h>
#include <board_config.h>
#include <drivers/device/device.h>
#include <drivers/drv_baro.h>
#include <drivers/drv_hrt.h>
#include <drivers/device/ringbuffer.h>
#include <drivers/device/i2c.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
#pragma once
#define BMP285_DEVICE_PATH_PRESSURE "/dev/bmp285_i2c_int"
#define BMP285_DEVICE_PATH_PRESSURE_EXT "/dev/bmp285_i2c_ext"
#define BMP285_SLAVE_ADDRESS PX4_I2C_OBDEV_BMP285
#define BMP285_BUS_SPEED 1000*100
#define BPM285_ADDR_CAL 0x88 /* address of 12x 2 bytes calibration data */
#define BPM285_ADDR_DATA 0xF7 /* address of 2x 3 bytes p-t data */
#define BPM285_ADDR_CONFIG 0xF5 /* configuration */
#define BPM285_ADDR_CTRL 0xF4 /* controll */
#define BPM285_ADDR_STATUS 0xF3 /* state */
#define BPM285_ADDR_RESET 0xE0 /* reset */
#define BPM285_ADDR_ID 0xD0 /* id */
#define BPM285_VALUE_ID 0x58 /* chip id */
#define BPM285_VALUE_RESET 0xB6 /* reset */
#define BPM285_STATUS_MEASURING 1<<3 /* if in process of measure */
#define BPM285_STATUS_COPING 1<<0 /* if in process of data copy */
#define BPM285_CTRL_P0 0x0<<2 /* no p measure */
#define BPM285_CTRL_P1 0x1<<2
#define BPM285_CTRL_P2 0x2<<2
#define BPM285_CTRL_P4 0x3<<2
#define BPM285_CTRL_P8 0x4<<2
#define BPM285_CTRL_P16 0x5<<2
#define BPM285_CTRL_T0 0x0<<5 /* no t measure */
#define BPM285_CTRL_T1 0x1<<5
#define BPM285_CTRL_T2 0x2<<5
#define BPM285_CTRL_T4 0x3<<5
#define BPM285_CTRL_T8 0x4<<5
#define BPM285_CTRL_T16 0x5<<5
#define BPM285_CONFIG_F0 0x0<<2 /* no filter */
#define BPM285_CONFIG_F2 0x1<<2
#define BPM285_CONFIG_F4 0x2<<2
#define BPM285_CONFIG_F8 0x3<<2
#define BPM285_CONFIG_F16 0x4<<2
#define BMP285_CTRL_T_SB0 0x0<<5
#define BMP285_CTRL_T_SB1 0x1<<5
#define BMP285_CTRL_T_SB2 0x2<<5
#define BMP285_CTRL_T_SB3 0x3<<5
#define BMP285_CTRL_T_SB4 0x4<<5
#define BMP285_CTRL_T_SB5 0x5<<5
#define BMP285_CTRL_T_SB6 0x6<<5
#define BMP285_CTRL_T_SB7 0x7<<5
#define BPM285_CTRL_MODE_SLEEP 0x0
#define BPM285_CTRL_MODE_FORCE 0x1 /* on demand, goes to sleep after */
#define BPM285_CTRL_MODE_NORMAL 0x3
#define BPM285_MT_INIT 6400 /* max measure time of initial p + t in us */
#define BPM285_MT 2300 /* max measure time of p or t in us */
namespace bmp285
{
#pragma pack(push,1)
struct calibration_s {
uint16_t t1;
int16_t t2;
int16_t t3;
uint16_t p1;
int16_t p2;
int16_t p3;
int16_t p4;
int16_t p5;
int16_t p6;
int16_t p7;
int16_t p8;
int16_t p9;
}; //calibration data
struct data_s {
uint8_t p_msb;
uint8_t p_lsb;
uint8_t p_xlsb;
uint8_t t_msb;
uint8_t t_lsb;
uint8_t t_xlsb;
}; // data
#pragma pack(pop)
struct fcalibration_s {
float t1;
float t2;
float t3;
float p1;
float p2;
float p3;
float p4;
float p5;
float p6;
float p7;
float p8;
float p9;
};
} /* namespace */
/*
* BMP285 internal constants and data structures.
*/
class BMP285 : public device::I2C
{
public:
BMP285(int bus, uint16_t address, const char *path, bool external);
~BMP285();
bool is_external();
virtual int init();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info();
bmp285::data_s *get_data(uint8_t reg);
bmp285::calibration_s *get_calibration(uint8_t addr);
private:
uint8_t _curr_ctrl;
struct work_s _work;
bool _external;
unsigned _report_ticks; // 0 - no cycling, otherwise period of sending a report
unsigned _max_mesure_ticks; //ticks needed to measure
ringbuffer::RingBuffer *_reports;
bool _collect_phase;
/* altitude conversion calibration */
unsigned _msl_pressure; /* in Pa */
orb_advert_t _baro_topic;
int _orb_class_instance;
int _class_instance;
perf_counter_t _sample_perf;
perf_counter_t _measure_perf;
perf_counter_t _comms_errors;
perf_counter_t _buffer_overflows;
struct bmp285::calibration_s _cal;
struct bmp285::data_s _data;
struct bmp285::fcalibration_s _fcal; //pre processed calibration constants
float _P; /* in Pa */
float _T; /* in K */
/* periodic execution helpers */
void start_cycle();
void stop_cycle();
void cycle(); //main execution
static void cycle_trampoline(void *arg);
/**
* Read a register from the BMP285
*
* @param The register to read.
* @return The value that was read.
*/
uint8_t read_reg(unsigned reg);
/**
* Write a register in the BMP285
*
* @param reg The register to write.
* @param value The new value to write.
* @return The value returned after transfer of data.
*/
int write_reg(unsigned reg, uint8_t value);
int measure(); //start measure
int collect(); //get results and publish
};
#endif

370
src/drivers/bmp285/bmp285_main.cpp
View File

@ -1,370 +0,0 @@
#include <drivers/bmp285/bmp285.hpp>
/** driver 'main' command */
extern "C" { __EXPORT int bmp285_main(int argc, char *argv[]); }
/**
* Local functions in support of the shell command.
*/
namespace bmp285
{
BMP285 *g_dev_int; // on internal bus
BMP285 *g_dev_ext; // on external bus
void start(bool);
void test(bool);
void reset(bool);
void info(bool);
void calibrate(bool, unsigned);
void usage();
/**
* Start the driver.
*
* This function only returns if the driver is up and running
* or failed to detect the sensor.
*/
void
start(bool external_bus)
{
int fd;
BMP285 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int;
const char *path = external_bus ? BMP285_DEVICE_PATH_PRESSURE_EXT : BMP285_DEVICE_PATH_PRESSURE;
if (*g_dev_ptr != nullptr)
/* if already started, the still command succeeded */
{
errx(0, "already started");
}
/* create the driver */
if (external_bus) {
#if defined(PX4_I2C_BUS_EXPANSION) && defined(PX4_I2C_EXT_OBDEV_BMP285)
*g_dev_ptr = new BMP285(PX4_I2C_BUS_EXPANSION, BMP285_SLAVE_ADDRESS, path, external_bus);
#else
errx(0, "External SPI not available");
#endif
} else {
*g_dev_ptr = new BMP285(PX4_I2C_BUS_BMP285, BMP285_SLAVE_ADDRESS, path, external_bus);
}
if (*g_dev_ptr == nullptr) {
goto fail;
}
if (OK != (*g_dev_ptr)->init()) {
goto fail;
}
/* set the poll rate to default, starts automatic data collection */
fd = open(path, O_RDONLY);
if (fd < 0) {
goto fail;
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
goto fail;
}
close(fd);
exit(0);
fail:
if (*g_dev_ptr != nullptr) {
delete (*g_dev_ptr);
*g_dev_ptr = nullptr;
}
errx(1, "driver start failed");
}
/**
* Perform some basic functional tests on the driver;
* make sure we can collect data from the sensor in polled
* and automatic modes.
*/
void
test(bool external_bus)
{
const char *path = external_bus ? BMP285_DEVICE_PATH_PRESSURE_EXT : BMP285_DEVICE_PATH_PRESSURE;
struct baro_report report;
ssize_t sz;
int ret;
/* get the driver */
int fd = open(path, O_RDONLY);
if (fd < 0) {
err(1, "open failed (try 'bmp285 start' if the driver is not running)");
}
/* do a simple demand read */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
err(1, "immediate read failed");
}
warnx("single read");
warnx("pressure: %10.4f", (double)report.pressure);
warnx("altitude: %11.4f", (double)report.altitude);
warnx("temperature: %8.4f", (double)report.temperature);
warnx("time: %lld", report.timestamp);
/* set the queue depth to 10 */
if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) {
errx(1, "failed to set queue depth");
}
/* start the sensor polling at 2Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
errx(1, "failed to set 2Hz poll rate");
}
/* read the sensor 5x and report each value */
for (unsigned i = 0; i < 5; i++) {
struct pollfd fds;
/* wait for data to be ready */
fds.fd = fd;
fds.events = POLLIN;
ret = poll(&fds, 1, 2000);
if (ret != 1) {
errx(1, "timed out waiting for sensor data");
}
/* now go get it */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
err(1, "periodic read failed");
}
warnx("periodic read %u", i);
warnx("pressure: %10.4f", (double)report.pressure);
warnx("altitude: %11.4f", (double)report.altitude);
warnx("temperature K: %8.4f", (double)report.temperature);
warnx("time: %lld", report.timestamp);
}
close(fd);
errx(0, "PASS");
}
/**
* Reset the driver.
*/
void
reset(bool external_bus)
{
const char *path = external_bus ? BMP285_DEVICE_PATH_PRESSURE_EXT : BMP285_DEVICE_PATH_PRESSURE;
int fd = open(path, O_RDONLY);
if (fd < 0) {
err(1, "failed ");
}
if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
err(1, "driver reset failed");
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
err(1, "driver poll restart failed");
}
close(fd);
exit(0);
}
/**
* Print a little info about the driver.
*/
void
info(bool external_bus)
{
BMP285 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int;
if (*g_dev_ptr == nullptr) {
errx(1, "driver not running");
}
printf("state @ %p\n", *g_dev_ptr);
(*g_dev_ptr)->print_info();
exit(0);
}
/**
* Calculate actual MSL pressure given current altitude
*/
void
calibrate(bool external_bus, unsigned altitude)
{
const char *path = external_bus ? BMP285_DEVICE_PATH_PRESSURE_EXT : BMP285_DEVICE_PATH_PRESSURE;
struct baro_report report;
float pressure;
float p1;
/* get the driver */
int fd = open(path, O_RDONLY);
if (fd < 0) {
err(1, "open failed (try 'bmp285 start' if the driver is not running)");
}
/* start the sensor polling at max */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX)) {
errx(1, "failed to set poll rate");
}
/* average a few measurements */
pressure = 0.0f;
for (unsigned i = 0; i < 20; i++) {
struct pollfd fds;
int ret;
ssize_t sz;
/* wait for data to be ready */
fds.fd = fd;
fds.events = POLLIN;
ret = poll(&fds, 1, 1000);
if (ret != 1) {
errx(1, "timed out waiting for sensor data");
}
/* now go get it */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
err(1, "sensor read failed");
}
pressure += report.pressure;
}
pressure /= 20; /* average */
pressure /= 10; /* scale from millibar to kPa */
/* tropospheric properties (0-11km) for standard atmosphere */
const float T1 = 15.0 + 273.15; /* temperature at base height in Kelvin */
const float a = -6.5 / 1000; /* temperature gradient in degrees per metre */
const float g = 9.80665f; /* gravity constant in m/s/s */
const float R = 287.05f; /* ideal gas constant in J/kg/K */
warnx("averaged pressure %10.4fkPa at %um", (double)pressure, altitude);
float value = (powf(((T1 + (a * (float)altitude)) / T1), (g / (a * R))));
warnx("power value is %10.4f", (double)value);
p1 = pressure * (powf(((T1 + (a * (float)altitude)) / T1), (g / (a * R))));
warnx("calculated MSL pressure %10.4fkPa", (double)p1);
/* save as integer Pa */
p1 *= 1000.0f;
warnx("p1 is %10.4fkPa at %um", (double)p1);
if (ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)p1) != OK) {
err(1, "BAROIOCSMSLPRESSURE");
}
close(fd);
exit(0);
}
void
usage()
{
warnx("missing command: try 'start', 'test', 'info',\n'reset', 'calibrate'");
warnx("options:");
warnx(" -X (external bus)");
}
} // namespace
int
bmp285_main(int argc, char *argv[])
{
bool external_bus = false;
int ch;
/* jump over start/off/etc and look at options first */
while ((ch = getopt(argc, argv, "X:")) != EOF) {
switch (ch) {
case 'X':
external_bus = true;
break;
default:
bmp285::usage();
exit(0);
}
}
const char *verb = argv[optind];
/*
* Start/load the driver.
*/
if (!strcmp(verb, "start")) {
bmp285::start(external_bus);
}
/*
* Test the driver/device.
*/
if (!strcmp(verb, "test")) {
bmp285::test(external_bus);
}
/*
* Reset the driver.
*/
if (!strcmp(verb, "reset")) {
bmp285::reset(external_bus);
}
/*
* Print driver information.
*/
if (!strcmp(verb, "info")) {
bmp285::info(external_bus);
}
/*
* Perform MSL pressure calibration given an altitude in metres
*/
if (!strcmp(verb, "calibrate")) {
if (argc < 2) {
errx(1, "missing altitude");
}
long altitude = strtol(argv[optind + 1], nullptr, 10);
bmp285::calibrate(external_bus, altitude);
}
bmp285::usage();
exit(1);
}

3
src/drivers/boards/px4fmu-v4/board_config.h
View File

@ -156,7 +156,6 @@
#define PX4_I2C_BUS_EXPANSION 1
#define PX4_I2C_BUS_LED PX4_I2C_BUS_EXPANSION
#define PX4_I2C_BUS_BMM150 PX4_I2C_BUS_EXPANSION
#define PX4_I2C_BUS_BMP285 PX4_I2C_BUS_EXPANSION
/* Devices on the external bus.
*
@ -166,7 +165,7 @@
#define PX4_I2C_OBDEV_HMC5883 0x1e
#define PX4_I2C_OBDEV_LIS3MDL 0x1e
#define PX4_I2C_OBDEV_BMM150 0x10
#define PX4_I2C_OBDEV_BMP285 0x76
#define PX4_I2C_OBDEV_BMP280 0x76
/*
* ADC channels