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

refactor bmm150: use driver base class

This commit is contained in:
Beat Küng 2020-03-11 18:56:46 +01:00 committed by Daniel Agar
parent e4bf535595
commit 47b329cc54
4 changed files with 137 additions and 353 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
boards/nxp/fmuk66-v3/init/rc.board_sensors
View File

@ -12,7 +12,7 @@ ist8310 -X start
qmc5883 -X start
# Internal Mag I2C bus roll 180, yaw 90
bmm150 -R 10 start
bmm150 -I -R 10 start
# Onboard I2C baros
bmp280 -I start

2
boards/px4/fmu-v5x/init/rc.board_sensors
View File

@ -27,7 +27,7 @@ hmc5883 -C -T -X start
qmc5883 -X start
# Possible internal compass
bmm150 start
bmm150 -I start
# Possible internal Baro
bmp388 -I start

410
src/drivers/magnetometer/bmm150/bmm150.cpp
View File

@ -38,221 +38,13 @@
#include "bmm150.hpp"
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
/** driver 'main' command */
extern "C" { __EXPORT int bmm150_main(int argc, char *argv[]); }
extern "C" __EXPORT int bmm150_main(int argc, char *argv[]);
namespace bmm150
{
BMM150 *g_dev_ext;
BMM150 *g_dev_int;
void start(bool, enum Rotation);
void test(bool);
void reset(bool);
void info(bool);
void regdump(bool external_bus);
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, enum Rotation rotation)
{
int fd;
BMM150 **g_dev_ptr = (external_bus ? &g_dev_ext : &g_dev_int);
const char *path = (external_bus ? BMM150_DEVICE_PATH_MAG_EXT : BMM150_DEVICE_PATH_MAG);
if (*g_dev_ptr != nullptr)
/* if already started, the still command succeeded */
{
PX4_ERR("already started");
exit(0);
}
/* create the driver */
if (external_bus) {
#if defined(PX4_I2C_BUS_EXPANSION)
*g_dev_ptr = new BMM150(PX4_I2C_BUS_EXPANSION, path, rotation);
#else
PX4_ERR("External I2C not available");
exit(0);
#endif
} else {
#if defined(PX4_I2C_BUS_ONBOARD)
*g_dev_ptr = new BMM150(PX4_I2C_BUS_ONBOARD, path, rotation);
#else
PX4_ERR("Internal I2C not available");
exit(0);
#endif
}
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;
}
PX4_ERR("driver start failed");
exit(1);
}
void test(bool external_bus)
{
int fd = -1;
const char *path = (external_bus ? BMM150_DEVICE_PATH_MAG_EXT : BMM150_DEVICE_PATH_MAG);
sensor_mag_s m_report;
ssize_t sz;
/* get the driver */
fd = open(path, O_RDONLY);
if (fd < 0) {
PX4_ERR("%s open failed (try 'bmm150 start' if the driver is not running)", path);
exit(1);
}
/* reset to default polling rate*/
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
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)) {
PX4_ERR("immediate mag read failed");
exit(1);
}
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);
PX4_ERR("PASS");
exit(0);
}
void
reset(bool external_bus)
{
const char *path = external_bus ? BMM150_DEVICE_PATH_MAG_EXT : BMM150_DEVICE_PATH_MAG;
int fd = open(path, O_RDONLY);
if (fd < 0) {
PX4_ERR("failed");
exit(1);
}
if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
PX4_ERR("driver reset failed");
exit(1);
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
PX4_ERR("driver poll restart failed");
exit(1);
}
exit(0);
}
void
info(bool external_bus)
{
BMM150 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int;
if (*g_dev_ptr == nullptr) {
PX4_ERR("driver not running");
exit(1);
}
printf("state @ %p\n", *g_dev_ptr);
(*g_dev_ptr)->print_info();
exit(0);
}
/**
* Dump the register information
*/
void
regdump(bool external_bus)
{
BMM150 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int;
if (*g_dev_ptr == nullptr) {
PX4_ERR("driver not running");
exit(1);
}
printf("regdump @ %p\n", *g_dev_ptr);
(*g_dev_ptr)->print_registers();
exit(0);
}
void
usage()
{
PX4_WARN("missing command: try 'start', 'info', 'test', 'stop',\n'reset', 'regdump'");
PX4_WARN("options:");
PX4_WARN(" -X (external bus)");
}
} // namespace bmm150
BMM150::BMM150(int bus, const char *path, enum Rotation rotation) :
I2C("BMM150", path, bus, BMM150_SLAVE_ADDRESS, BMM150_BUS_SPEED),
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
_running(false),
BMM150::BMM150(I2CSPIBusOption bus_option, const int bus, int bus_frequency, enum Rotation rotation) :
I2C("BMM150", nullptr, bus, BMM150_SLAVE_ADDRESS, bus_frequency),
I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus),
_call_interval(0),
_reports(nullptr),
_collect_phase(false),
@ -297,9 +89,6 @@ BMM150::BMM150(int bus, const char *path, enum Rotation rotation) :
BMM150::~BMM150()
{
/* make sure we are truly inactive */
stop();
/* free any existing reports */
if (_reports != nullptr) {
delete _reports;
@ -349,9 +138,9 @@ int BMM150::init()
up_udelay(10000);
/* check id*/
/* check id*/
if (read_reg(BMM150_CHIP_ID_REG) != BMM150_CHIP_ID) {
PX4_WARN("id of magnetometer is not: 0x%02x", BMM150_CHIP_ID);
DEVICE_DEBUG("id of magnetometer is not: 0x%02x", BMM150_CHIP_ID);
return -EIO;
}
@ -382,9 +171,13 @@ int BMM150::init()
&_orb_class_instance, (external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH);
if (_topic == nullptr) {
PX4_WARN("ADVERT FAIL");
PX4_ERR("ADVERT FAIL");
return -ENOMEM;
}
_call_interval = 1000000 / BMM150_MAX_DATA_RATE;
start();
out:
return ret;
}
@ -408,20 +201,12 @@ BMM150::start()
{
/* reset the report ring and state machine */
_collect_phase = false;
_running = true;
_reports->flush();
/* schedule a cycle to start things */
ScheduleNow();
}
void
BMM150::stop()
{
_running = false;
ScheduleClear();
}
ssize_t
BMM150::read(struct file *filp, char *buffer, size_t buflen)
{
@ -484,29 +269,24 @@ BMM150::read(struct file *filp, char *buffer, size_t buflen)
}
void
BMM150::Run()
BMM150::RunImpl()
{
if (_collect_phase) {
collect();
unsigned wait_gap = _call_interval - BMM150_CONVERSION_INTERVAL;
if ((wait_gap != 0) && (_running)) {
if (wait_gap != 0) {
// need to wait some time before new measurement
ScheduleDelayed(wait_gap);
return;
}
}
measure();
if ((_running)) {
/* schedule a fresh cycle call when the measurement is done */
ScheduleDelayed(BMM150_CONVERSION_INTERVAL);
}
/* schedule a fresh cycle call when the measurement is done */
ScheduleDelayed(BMM150_CONVERSION_INTERVAL);
}
int
@ -708,45 +488,6 @@ BMM150::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case SENSORIOCSPOLLRATE: {
switch (arg) {
/* zero would be bad */
case 0:
return -EINVAL;
/* set default polling rate */
case SENSOR_POLLRATE_DEFAULT:
return ioctl(filp, SENSORIOCSPOLLRATE, BMM150_MAX_DATA_RATE);
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_call_interval == 0);
/* convert hz to tick interval via microseconds */
unsigned interval = (1000000 / arg);
/* check against maximum rate */
if (interval < BMM150_CONVERSION_INTERVAL) {
return -EINVAL;
}
/* update interval for next measurement */
_call_interval = interval;
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}
return OK;
}
}
}
case SENSORIOCRESET:
return reset();
case MAGIOCSSCALE:
return OK;
@ -841,7 +582,7 @@ BMM150::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
int BMM150 :: set_power_mode(uint8_t power_mode)
int BMM150::set_power_mode(uint8_t power_mode)
{
uint8_t setbits = 0;
uint8_t clearbits = BMM150_POWER_MASK;
@ -870,7 +611,7 @@ int BMM150 :: set_power_mode(uint8_t power_mode)
}
int BMM150 :: set_data_rate(uint8_t data_rate)
int BMM150::set_data_rate(uint8_t data_rate)
{
uint8_t setbits = 0;
uint8_t clearbits = BMM150_OUTPUT_DATA_RATE_MASK;
@ -920,7 +661,7 @@ int BMM150 :: set_data_rate(uint8_t data_rate)
}
int BMM150 :: init_trim_registers(void)
int BMM150::init_trim_registers(void)
{
int ret = OK;
uint8_t data[2] = {0};
@ -1023,8 +764,9 @@ int BMM150::set_presetmode(uint8_t presetmode)
}
void
BMM150::print_info()
BMM150::print_status()
{
I2CSPIDriverBase::print_status();
perf_print_counter(_sample_perf);
perf_print_counter(_bad_transfers);
perf_print_counter(_good_transfers);
@ -1061,75 +803,97 @@ BMM150::print_registers()
}
void
BMM150::print_usage()
{
PRINT_MODULE_USAGE_NAME("bmm150", "driver");
PRINT_MODULE_USAGE_SUBCATEGORY("magnetometer");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
PRINT_MODULE_USAGE_COMMAND("reset");
PRINT_MODULE_USAGE_COMMAND("regdump");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
}
void
BMM150::custom_method(const BusCLIArguments &cli)
{
switch (cli.custom1) {
case 0: reset();
break;
case 1: print_registers();
break;
}
}
I2CSPIDriverBase *BMM150::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance)
{
BMM150 *interface = new BMM150(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency, cli.rotation);
if (interface == nullptr) {
PX4_ERR("alloc failed");
return nullptr;
}
if (interface->init() != OK) {
delete interface;
PX4_DEBUG("no device on bus %i (devid 0x%x)", iterator.bus(), iterator.devid());
return nullptr;
}
return interface;
}
int
bmm150_main(int argc, char *argv[])
{
int myoptind = 1;
int ch;
const char *myoptarg = nullptr;
bool external_bus = false;
enum Rotation rotation = ROTATION_NONE;
using ThisDriver = BMM150;
BusCLIArguments cli{true, false};
cli.default_i2c_frequency = BMM150_BUS_SPEED;
while ((ch = px4_getopt(argc, argv, "XR:", &myoptind, &myoptarg)) != EOF) {
while ((ch = cli.getopt(argc, argv, "R:")) != EOF) {
switch (ch) {
case 'X':
external_bus = true;
break;
case 'R':
rotation = (enum Rotation)atoi(myoptarg);
cli.rotation = (enum Rotation)atoi(cli.optarg());
break;
default:
bmm150::usage();
return 0;
}
}
if (myoptind >= argc) {
bmm150::usage();
const char *verb = cli.optarg();
if (!verb) {
ThisDriver::print_usage();
return -1;
}
const char *verb = argv[myoptind];
BusInstanceIterator iterator(MODULE_NAME, cli, DRV_MAG_DEVTYPE_BMM150);
/*
* Start/load the driver.
*/
if (!strcmp(verb, "start")) {
bmm150::start(external_bus, rotation);
return ThisDriver::module_start(cli, iterator);
}
/*
* Test the driver/device.
*/
if (!strcmp(verb, "test")) {
bmm150::test(external_bus);
if (!strcmp(verb, "stop")) {
return ThisDriver::module_stop(iterator);
}
if (!strcmp(verb, "status")) {
return ThisDriver::module_status(iterator);
}
/*
* Reset the driver.
*/
if (!strcmp(verb, "reset")) {
bmm150::reset(external_bus);
cli.custom1 = 0;
return ThisDriver::module_custom_method(cli, iterator);
}
/*
* Print driver information.
*/
if (!strcmp(verb, "info")) {
bmm150::info(external_bus);
}
/*
* Print register information.
*/
if (!strcmp(verb, "regdump")) {
bmm150::regdump(external_bus);
cli.custom1 = 1;
return ThisDriver::module_custom_method(cli, iterator);
}
bmm150::usage();
ThisDriver::print_usage();
return -1;
}

76
src/drivers/magnetometer/bmm150/bmm150.hpp
View File

@ -1,5 +1,36 @@
#ifndef BMM150_HPP_
#define BMM150_HPP_
/****************************************************************************
*
* Copyright (c) 2020 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.
*
****************************************************************************/
#pragma once
#include <px4_platform_common/px4_config.h>
@ -19,8 +50,7 @@
#include <px4_platform_common/log.h>
#include <perf/perf_counter.h>
#include <systemlib/err.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <px4_platform_common/i2c_spi_buses.h>
#include <systemlib/conversions.h>
#include <nuttx/arch.h>
@ -37,10 +67,6 @@
#include <lib/conversion/rotation.h>
#define BMM150_DEVICE_PATH_MAG "/dev/bmm150_i2c_int"
#define BMM150_DEVICE_PATH_MAG_EXT "/dev/bmm150_i2c_ext"
#define BMM150_SLAVE_ADDRESS 0x10
#define BMM150_BUS_SPEED 1000*100
@ -191,35 +217,33 @@ struct bmm150_data {
};
class BMM150 : public device::I2C, public px4::ScheduledWorkItem
class BMM150 : public device::I2C, public I2CSPIDriver<BMM150>
{
public:
BMM150(int bus, const char *path, enum Rotation rotation);
BMM150(I2CSPIBusOption bus_option, const int bus, int bus_frequency, enum Rotation rotation);
virtual ~BMM150();
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);
static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance);
static void print_usage();
/**
* Stop automatic measurement.
*/
void stop();
int init() override;
ssize_t read(struct file *filp, char *buffer, size_t buflen) override;
int ioctl(struct file *filp, int cmd, unsigned long arg) override;
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info();
void print_status() override;
void print_registers();
void RunImpl();
void custom_method(const BusCLIArguments &cli) override;
protected:
virtual int probe();
int probe() override;
private:
bool _running;
/* altitude conversion calibration */
unsigned _call_interval;
@ -277,8 +301,6 @@ private:
int measure(); //start measure
int collect(); //get results and publish
void Run() override;
/**
* Read the specified number of bytes from BMM150.
*
@ -363,5 +385,3 @@ private:
};
#endif /* BMM150_HPP_ */