Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-25 00:31:36 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

rm3100: move to PX4Magnetometer and cleanup

Browse Source
This commit is contained in:
Daniel Agar
2020-03-30 15:17:11 -04:00
parent a59aa865da
commit cfc087c6ad
5 changed files with 59 additions and 416 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/magnetometer/rm3100/CMakeLists.txt
+3 -1
View File
@@ -34,10 +34,12 @@ px4_add_module(
MODULE drivers__rm3100
MAIN rm3100
COMPILE_FLAGS
-Wno-cast-align # TODO: fix and enable
SRCS
rm3100_i2c.cpp
rm3100_spi.cpp
rm3100_main.cpp
rm3100.cpp
DEPENDS
drivers_magnetometer
px4_work_queue
)
src/drivers/magnetometer/rm3100/rm3100.cpp
+40 -313
View File
@@ -42,52 +42,28 @@
#include "rm3100.h"
RM3100::RM3100(device::Device *interface, enum Rotation rotation, I2CSPIBusOption bus_option, int bus) :
CDev("RM3100", nullptr),
I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus),
_px4_mag(interface->get_device_id(), interface->external() ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT, rotation),
_interface(interface),
_reports(nullptr),
_scale{},
_last_report{},
_mag_topic(nullptr),
_comms_errors(perf_alloc(PC_COUNT, "rm3100_comms_errors")),
_conf_errors(perf_alloc(PC_COUNT, "rm3100_conf_errors")),
_range_errors(perf_alloc(PC_COUNT, "rm3100_range_errors")),
_sample_perf(perf_alloc(PC_ELAPSED, "rm3100_read")),
_calibrated(false),
_comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms_errors")),
_conf_errors(perf_alloc(PC_COUNT, MODULE_NAME": conf_errors")),
_range_errors(perf_alloc(PC_COUNT, MODULE_NAME": range_errors")),
_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
_continuous_mode_set(false),
_mode(SINGLE),
_rotation(rotation),
_measure_interval(0),
_class_instance(-1),
_orb_class_instance(-1),
_range_scale(1.0f / (RM3100_SENSITIVITY * UTESLA_TO_GAUSS)),
_check_state_cnt(0)
{
// set the device type from the interface
_device_id.devid_s.bus_type = _interface->get_device_bus_type();
_device_id.devid_s.bus = _interface->get_device_bus();
_device_id.devid_s.address = _interface->get_device_address();
_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_RM3100;
_interface->set_device_type(DRV_MAG_DEVTYPE_RM3100);
// default scaling
_scale.x_offset = 0;
_scale.x_scale = 1.0f;
_scale.y_offset = 0;
_scale.y_scale = 1.0f;
_scale.z_offset = 0;
_scale.z_scale = 1.0f;
_px4_mag.set_device_type(DRV_MAG_DEVTYPE_RM3100);
_px4_mag.set_external(_interface->external());
_px4_mag.set_scale(1.f / (RM3100_SENSITIVITY * UTESLA_TO_GAUSS));
}
RM3100::~RM3100()
{
if (_reports != nullptr) {
delete _reports;
}
if (_class_instance != -1) {
unregister_class_devname(MAG_BASE_DEVICE_PATH, _class_instance);
}
// free perf counters
perf_free(_sample_perf);
perf_free(_comms_errors);
@@ -95,8 +71,7 @@ RM3100::~RM3100()
perf_free(_conf_errors);
}
int
RM3100::self_test()
int RM3100::self_test()
{
/* Chances are that a poll event was triggered, so wait for conversion and read registers in order to clear DRDY bit */
usleep(RM3100_CONVERSION_INTERVAL);
@@ -141,13 +116,10 @@ RM3100::self_test()
return ret;
}
int
RM3100::check_measurement()
int RM3100::check_measurement()
{
uint8_t status = 0;
int ret = -1;
ret = _interface->read(ADDR_STATUS, &status, 1);
int ret = _interface->read(ADDR_STATUS, &status, 1);
if (ret != 0) {
return ret;
@@ -156,120 +128,55 @@ RM3100::check_measurement()
return !((status & STATUS_DRDY) == STATUS_DRDY) ;
}
int
RM3100::collect()
int RM3100::collect()
{
/* Check whether a measurement is available or not, otherwise return immediately */
if (check_measurement() != 0) {
DEVICE_DEBUG("No measurement available");
PX4_DEBUG("No measurement available");
return 0;
}
#pragma pack(push, 1)
struct {
uint8_t x[3];
uint8_t y[3];
uint8_t z[3];
} rm_report;
#pragma pack(pop)
} rm_report{};
int ret = 0;
int32_t xraw;
int32_t yraw;
int32_t zraw;
float xraw_f;
float yraw_f;
float zraw_f;
sensor_mag_s new_mag_report;
bool sensor_is_onboard = false;
_px4_mag.set_error_count(perf_event_count(_comms_errors));
perf_begin(_sample_perf);
new_mag_report.timestamp = hrt_absolute_time();
new_mag_report.error_count = perf_event_count(_comms_errors);
new_mag_report.scaling = _range_scale;
new_mag_report.device_id = _device_id.devid;
ret = _interface->read(ADDR_MX, (uint8_t *)&rm_report, sizeof(rm_report));
const hrt_abstime timestamp_sample = hrt_absolute_time();
int ret = _interface->read(ADDR_MX, (uint8_t *)&rm_report, sizeof(rm_report));
if (ret != OK) {
perf_end(_sample_perf);
perf_count(_comms_errors);
PX4_WARN("Register read error.");
return ret;
}
perf_end(_sample_perf);
/* Rearrange mag data */
xraw = ((rm_report.x[0] << 16) | (rm_report.x[1] << 8) | rm_report.x[2]);
yraw = ((rm_report.y[0] << 16) | (rm_report.y[1] << 8) | rm_report.y[2]);
zraw = ((rm_report.z[0] << 16) | (rm_report.z[1] << 8) | rm_report.z[2]);
int32_t xraw = ((rm_report.x[0] << 16) | (rm_report.x[1] << 8) | rm_report.x[2]);
int32_t yraw = ((rm_report.y[0] << 16) | (rm_report.y[1] << 8) | rm_report.y[2]);
int32_t zraw = ((rm_report.z[0] << 16) | (rm_report.z[1] << 8) | rm_report.z[2]);
/* Convert 24 bit signed values to 32 bit signed values */
convert_signed(&xraw);
convert_signed(&yraw);
convert_signed(&zraw);
/* There is no temperature sensor */
new_mag_report.temperature = 0.0f;
// XXX revisit for SPI part, might require a bus type IOCTL
unsigned dummy = 0;
sensor_is_onboard = !_interface->ioctl(MAGIOCGEXTERNAL, dummy);
new_mag_report.is_external = !sensor_is_onboard;
/**
* RAW outputs
* As we only have 16 bits to store raw data, the following values are not correct
*/
new_mag_report.x_raw = (int16_t)(xraw >> 8);
new_mag_report.y_raw = (int16_t)(yraw >> 8);
new_mag_report.z_raw = (int16_t)(zraw >> 8);
xraw_f = xraw;
yraw_f = yraw;
zraw_f = zraw;
/* apply user specified rotation */
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
new_mag_report.x = ((xraw_f * _range_scale) - _scale.x_offset) * _scale.x_scale;
new_mag_report.y = ((yraw_f * _range_scale) - _scale.y_offset) * _scale.y_scale;
new_mag_report.z = ((zraw_f * _range_scale) - _scale.z_offset) * _scale.z_scale;
if (!(_pub_blocked)) {
if (_mag_topic != nullptr) {
/* publish it */
orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_mag_report);
} else {
_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &new_mag_report,
&_orb_class_instance, (sensor_is_onboard) ? ORB_PRIO_HIGH : ORB_PRIO_MAX);
if (_mag_topic == nullptr) {
DEVICE_DEBUG("ADVERT FAIL");
}
}
}
_last_report = new_mag_report;
/* post a report to the ring */
_reports->force(&new_mag_report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
_px4_mag.update(timestamp_sample, xraw, yraw, zraw);
ret = OK;
perf_end(_sample_perf);
return ret;
}
void
RM3100::convert_signed(int32_t *n)
void RM3100::convert_signed(int32_t *n)
{
/* Sensor returns values as 24 bit signed values, so we need to manually convert to 32 bit signed values */
if ((*n & (1 << 23)) == (1 << 23)) {
@@ -277,8 +184,7 @@ RM3100::convert_signed(int32_t *n)
}
}
void
RM3100::RunImpl()
void RM3100::RunImpl()
{
/* _measure_interval == 0 is used as _task_should_exit */
if (_measure_interval == 0) {
@@ -287,15 +193,14 @@ RM3100::RunImpl()
/* Collect last measurement at the start of every cycle */
if (collect() != OK) {
DEVICE_DEBUG("collection error");
PX4_DEBUG("collection error");
/* restart the measurement state machine */
start();
return;
}
if (measure() != OK) {
DEVICE_DEBUG("measure error");
PX4_DEBUG("measure error");
}
if (_measure_interval > 0) {
@@ -304,31 +209,12 @@ RM3100::RunImpl()
}
}
int
RM3100::init()
int RM3100::init()
{
int ret = PX4_ERROR;
ret = CDev::init();
if (ret != OK) {
DEVICE_DEBUG("CDev init failed");
return ret;
}
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_mag_s));
if (_reports == nullptr) {
return PX4_ERROR;
}
/* reset the device configuration */
reset();
_class_instance = register_class_devname(MAG_BASE_DEVICE_PATH);
ret = self_test();
int ret = self_test();
if (ret != PX4_OK) {
PX4_ERR("self test failed");
@@ -340,97 +226,7 @@ RM3100::init()
return ret;
}
int
RM3100::ioctl(struct file *file_pointer, int cmd, unsigned long arg)
{
unsigned dummy = 0;
switch (cmd) {
case SENSORIOCSPOLLRATE: {
switch (arg) {
/* zero would be bad */
case 0:
return -EINVAL;
case SENSOR_POLLRATE_DEFAULT: {
/* do we need to start internal polling? */
bool not_started = (_measure_interval == 0);
/* set interval for next measurement to minimum legal value */
_measure_interval = (RM3100_CONVERSION_INTERVAL);
/* if we need to start the poll state machine, do it */
if (not_started) {
start();
}
return PX4_OK;
}
/* Uses arg (hz) for a custom poll rate */
default: {
/* do we need to start internal polling? */
bool not_started = (_measure_interval == 0);
/* convert hz to tick interval via microseconds */
unsigned interval = (1000000 / arg);
/* check against maximum rate */
if (interval < RM3100_CONVERSION_INTERVAL) {
return -EINVAL;
}
/* update interval for next measurement */
_measure_interval = interval;
/* if we need to start the poll state machine, do it */
if (not_started) {
start();
}
return PX4_OK;
}
}
}
case SENSORIOCRESET:
return reset();
case MAGIOCSRANGE:
/* field measurement range cannot be configured for this sensor (8 Gauss) */
return OK;
case MAGIOCSSCALE:
/* set new scale factors */
memcpy(&_scale, (struct mag_calibration_s *)arg, sizeof(_scale));
return 0;
case MAGIOCGSCALE:
/* copy out scale factors */
memcpy((struct mag_calibration_s *)arg, &_scale, sizeof(_scale));
return 0;
case MAGIOCCALIBRATE:
/* This is left for compatibility with the IOCTL call in mag calibration */
return OK;
case MAGIOCGEXTERNAL:
DEVICE_DEBUG("MAGIOCGEXTERNAL in main driver");
return _interface->ioctl(cmd, dummy);
case DEVIOCGDEVICEID:
return _interface->ioctl(cmd, dummy);
default:
/* give it to the superclass */
return CDev::ioctl(file_pointer, cmd, arg);
}
}
int
RM3100::measure()
int RM3100::measure()
{
int ret = 0;
uint8_t cmd = 0;
@@ -467,23 +263,18 @@ RM3100::measure()
return ret;
}
void
RM3100::print_status()
void RM3100::print_status()
{
I2CSPIDriverBase::print_status();
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
PX4_INFO("poll interval: %u", _measure_interval);
print_message(_last_report);
_reports->print_info("report queue");
_px4_mag.print_status();
}
int
RM3100::reset()
int RM3100::reset()
{
int ret = 0;
ret = set_default_register_values();
int ret = set_default_register_values();
if (ret != OK) {
return PX4_ERROR;
@@ -492,66 +283,7 @@ RM3100::reset()
return PX4_OK;
}
int
RM3100::read(struct file *file_pointer, char *buffer, size_t buffer_len)
{
unsigned count = buffer_len / sizeof(sensor_mag_s);
sensor_mag_s *mag_buf = reinterpret_cast<sensor_mag_s *>(buffer);
int ret = 0;
/* buffer must be large enough */
if (count < 1) {
return -ENOSPC;
}
/* if automatic measurement is enabled */
if (_measure_interval > 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(mag_buf)) {
ret += sizeof(sensor_mag_s);
mag_buf++;
}
}
/* if there was no data, warn the caller */
return ret ? ret : -EAGAIN;
}
/* manual measurement - run one conversion */
/* XXX really it'd be nice to lock against other readers here */
do {
_reports->flush();
/* trigger a measurement */
if (measure() != OK) {
ret = -EIO;
break;
}
/* wait for it to complete */
usleep(RM3100_CONVERSION_INTERVAL);
/* run the collection phase */
if (collect() != OK) {
ret = -EIO;
break;
}
if (_reports->get(mag_buf)) {
ret = sizeof(sensor_mag_s);
}
} while (0);
return ret;
}
int
RM3100::set_default_register_values()
int RM3100::set_default_register_values()
{
uint8_t cmd[2] = {0, 0};
@@ -579,15 +311,10 @@ RM3100::set_default_register_values()
return PX4_OK;
}
void
RM3100::start()
void RM3100::start()
{
/* reset the report ring and state machine */
_reports->flush();
set_default_register_values();
/* schedule a cycle to start things */
ScheduleNow();
}
src/drivers/magnetometer/rm3100/rm3100.h
+6 -36
View File
@@ -39,20 +39,13 @@
#pragma once
#include <float.h>
#include <drivers/device/i2c.h>
#include <drivers/device/ringbuffer.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_mag.h>
#include <lib/conversion/rotation.h>
#include <perf/perf_counter.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/defines.h>
#include <systemlib/err.h>
#include <px4_platform_common/i2c_spi_buses.h>
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
/**
* RM3100 internal constants and data structures.
@@ -105,8 +98,7 @@ enum OPERATING_MODE {
SINGLE
};
class RM3100 : public device::CDev, public I2CSPIDriver<RM3100>
class RM3100 : public I2CSPIDriver<RM3100>
{
public:
RM3100(device::Device *interface, enum Rotation rotation, I2CSPIBusOption bus_option, int bus);
@@ -118,11 +110,7 @@ public:
void custom_method(const BusCLIArguments &cli) override;
int init() override;
int ioctl(struct file *file_pointer, int cmd, unsigned long arg) override;
int read(struct file *file_pointer, char *buffer, size_t buffer_len) override;
int init();
void print_status() override;
@@ -132,18 +120,11 @@ public:
int set_default_register_values();
void RunImpl();
protected:
Device *_interface;
private:
PX4Magnetometer _px4_mag;
ringbuffer::RingBuffer *_reports;
struct mag_calibration_s _scale;
sensor_mag_s _last_report {}; /**< used for info() */
orb_advert_t _mag_topic;
device::Device *_interface;
perf_counter_t _comms_errors;
perf_counter_t _conf_errors;
@@ -151,19 +132,12 @@ private:
perf_counter_t _sample_perf;
/* status reporting */
bool _calibrated; /**< the calibration is valid */
bool _continuous_mode_set;
enum OPERATING_MODE _mode;
enum Rotation _rotation;
unsigned int _measure_interval;
int _class_instance;
int _orb_class_instance;
float _range_scale;
uint8_t _check_state_cnt;
/**
@@ -210,8 +184,4 @@ private:
*/
void start();
/* this class has pointer data members, do not allow copying it */
RM3100(const RM3100 &);
RM3100 operator=(const RM3100 &);
}; // class RM3100
src/drivers/magnetometer/rm3100/rm3100_i2c.cpp
+7 -38
View File
@@ -48,8 +48,6 @@
#include <sys/types.h>
#include <unistd.h>
#include <drivers/device/i2c.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_device.h>
@@ -63,16 +61,13 @@ class RM3100_I2C : public device::I2C
{
public:
RM3100_I2C(int bus, int bus_frequency);
virtual ~RM3100_I2C() = default;
~RM3100_I2C() override = default;
virtual int init();
virtual int ioctl(unsigned operation, unsigned &arg);
virtual int read(unsigned address, void *data, unsigned count);
virtual int write(unsigned address, void *data, unsigned count);
int read(unsigned address, void *data, unsigned count) override;
int write(unsigned address, void *data, unsigned count) override;
protected:
virtual int probe();
int probe() override;
};
device::Device *
@@ -90,31 +85,7 @@ RM3100_I2C::RM3100_I2C(int bus, int bus_frequency) :
_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_RM3100;
}
int
RM3100_I2C::init()
{
/* this will call probe() */
return I2C::init();
}
int
RM3100_I2C::ioctl(unsigned operation, unsigned &arg)
{
switch (operation) {
case MAGIOCGEXTERNAL:
return external();
case DEVIOCGDEVICEID:
return CDev::ioctl(nullptr, operation, arg);
default:
return -EINVAL;
}
}
int
RM3100_I2C::probe()
int RM3100_I2C::probe()
{
uint8_t data = 0;
@@ -135,8 +106,7 @@ RM3100_I2C::probe()
return OK;
}
int
RM3100_I2C::read(unsigned address, void *data, unsigned count)
int RM3100_I2C::read(unsigned address, void *data, unsigned count)
{
uint8_t cmd = address;
int ret;
@@ -154,8 +124,7 @@ RM3100_I2C::read(unsigned address, void *data, unsigned count)
return ret;
}
int
RM3100_I2C::write(unsigned address, void *data, unsigned count)
int RM3100_I2C::write(unsigned address, void *data, unsigned count)
{
uint8_t buf[32];
src/drivers/magnetometer/rm3100/rm3100_spi.cpp
+3 -28
View File
@@ -66,7 +66,6 @@ public:
virtual ~RM3100_SPI() = default;
virtual int init();
virtual int ioctl(unsigned operation, unsigned &arg);
virtual int read(unsigned address, void *data, unsigned count);
virtual int write(unsigned address, void *data, unsigned count);
};
@@ -86,8 +85,7 @@ RM3100_SPI::RM3100_SPI(int bus, uint32_t devid, int bus_frequency, spi_mode_e sp
_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_RM3100;
}
int
RM3100_SPI::init()
int RM3100_SPI::init()
{
int ret;
@@ -113,29 +111,7 @@ RM3100_SPI::init()
return OK;
}
int
RM3100_SPI::ioctl(unsigned operation, unsigned &arg)
{
int ret;
switch (operation) {
case MAGIOCGEXTERNAL:
return external();
case DEVIOCGDEVICEID:
return CDev::ioctl(nullptr, operation, arg);
default: {
ret = -EINVAL;
}
}
return ret;
}
int
RM3100_SPI::read(unsigned address, void *data, unsigned count)
int RM3100_SPI::read(unsigned address, void *data, unsigned count)
{
uint8_t buf[32];
@@ -150,8 +126,7 @@ RM3100_SPI::read(unsigned address, void *data, unsigned count)
return ret;
}
int
RM3100_SPI::write(unsigned address, void *data, unsigned count)
int RM3100_SPI::write(unsigned address, void *data, unsigned count)
{
uint8_t buf[32];