fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-06 06:40:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

lis3mdl: move to PX4Magnetometer and cleanup

Browse Source
This commit is contained in:
Daniel Agar
2020-03-30 15:15:51 -04:00
parent cf12969a29
commit 5f6f1c554b
5 changed files with 58 additions and 688 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/magnetometer/lis3mdl/CMakeLists.txt
+4 -4
View File
@@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2016 PX4 Development Team. All rights reserved.
# Copyright (c) 2016-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
@@ -31,15 +31,15 @@
#
############################################################################
px4_add_module(
MODULE drivers__lis3mdl
MODULE drivers__magnetometer__lis3mdl
MAIN lis3mdl
COMPILE_FLAGS
-Wno-cast-align # TODO: fix and enable
SRCS
lis3mdl_i2c.cpp
lis3mdl_spi.cpp
lis3mdl_main.cpp
lis3mdl.cpp
DEPENDS
conversion
drivers_magnetometer
px4_work_queue
)
src/drivers/magnetometer/lis3mdl/lis3mdl.cpp
+43 -555
View File
@@ -43,29 +43,20 @@
#include "lis3mdl.h"
LIS3MDL::LIS3MDL(device::Device *interface, enum Rotation rotation, I2CSPIBusOption bus_option, int bus) :
CDev("LIS3MDL", 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, "lis3mdl_comms_errors")),
_conf_errors(perf_alloc(PC_COUNT, "lis3mdl_conf_errors")),
_range_errors(perf_alloc(PC_COUNT, "lis3mdl_range_errors")),
_sample_perf(perf_alloc(PC_ELAPSED, "lis3mdl_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(CONTINUOUS),
_rotation(rotation),
_measure_interval(0),
_class_instance(-1),
_orb_class_instance(-1),
_range_ga(4.0f),
_range_scale(0), // default range scale from counts to gauss */
_check_state_cnt(0),
_cntl_reg1(
CNTL_REG1_DEFAULT), // 1 11 111 0 0 | temp-en, ultra high performance (XY), fast_odr disabled, self test disabled
CNTL_REG1_DEFAULT), // 1 11 111 0 0 | temp-en, ultra high performance (XY), fast_odr disabled, self test disabled
_cntl_reg2(CNTL_REG2_DEFAULT), // 4 gauss FS range, reboot settings default
_cntl_reg3(CNTL_REG3_DEFAULT), // operating mode CONTINUOUS!
_cntl_reg4(CNTL_REG4_DEFAULT), // Z-axis ultra high performance mode
@@ -74,35 +65,14 @@ LIS3MDL::LIS3MDL(device::Device *interface, enum Rotation rotation, I2CSPIBusOpt
_temperature_counter(0),
_temperature_error_count(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_LIS3MDL;
_interface->set_device_type(DRV_MAG_DEVTYPE_IST8310);
// 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_IST8310);
_px4_mag.set_external(_interface->external());
}
LIS3MDL::~LIS3MDL()
{
if (_mag_topic != nullptr) {
orb_unadvertise(_mag_topic);
}
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);
@@ -110,332 +80,61 @@ LIS3MDL::~LIS3MDL()
perf_free(_conf_errors);
}
int
LIS3MDL::calibrate(struct file *file_pointer, unsigned enable)
int LIS3MDL::collect()
{
sensor_mag_s report;
ssize_t sz;
int ret = 1;
uint8_t num_samples = 10;
// XXX do something smarter here
int fd = (int)enable;
float sum_excited[3] = {0.0f, 0.0f, 0.0f};
float sum_non_excited[3] = {0.0f, 0.0f, 0.0f};
/* start the sensor polling at 50 Hz */
if (ioctl(file_pointer, SENSORIOCSPOLLRATE, 50) != OK) {
warn("FAILED: SENSORIOCSPOLLRATE 50Hz");
ret = 1;
goto out;
}
/* Set to 12 Gauss */
if (ioctl(file_pointer, MAGIOCSRANGE, 12) != OK) {
PX4_WARN("FAILED: MAGIOCSRANGE 12 Ga");
ret = 1;
goto out;
}
px4_usleep(20000);
/* discard 10 samples to let the sensor settle */
for (uint8_t i = 0; i < num_samples; i++) {
struct pollfd fds;
/* wait for data to be ready */
fds.fd = fd;
fds.events = POLLIN;
ret = ::poll(&fds, 1, 2000);
if (ret != 1) {
warn("ERROR: TIMEOUT 1");
goto out;
}
/* now go get it */
sz = ::read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
warn("ERROR: READ 1");
ret = -EIO;
goto out;
}
}
/* read the sensor up to 10x */
for (uint8_t i = 0; i < num_samples; i++) {
struct pollfd fds;
/* wait for data to be ready */
fds.fd = fd;
fds.events = POLLIN;
ret = ::poll(&fds, 1, 2000);
if (ret != 1) {
warn("ERROR: TIMEOUT 2");
goto out;
}
/* now go get it */
sz = ::read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
warn("ERROR: READ 2");
ret = -EIO;
goto out;
}
sum_non_excited[0] += report.x;
sum_non_excited[1] += report.y;
sum_non_excited[2] += report.z;
}
sum_non_excited[0] /= num_samples;
sum_non_excited[1] /= num_samples;
sum_non_excited[2] /= num_samples;
/* excite strap and take measurements */
if (ioctl(file_pointer, MAGIOCEXSTRAP, 1) != OK) {
PX4_WARN("FAILED: MAGIOCEXSTRAP 1");
ret = 1;
goto out;
}
px4_usleep(60000);
/* discard 10 samples to let the sensor settle */
for (uint8_t i = 0; i < num_samples; i++) {
struct pollfd fds;
/* wait for data to be ready */
fds.fd = fd;
fds.events = POLLIN;
ret = ::poll(&fds, 1, 2000);
if (ret != 1) {
warn("ERROR: TIMEOUT 1");
goto out;
}
/* now go get it */
sz = ::read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
warn("ERROR: READ 1");
ret = -EIO;
goto out;
}
}
/* read the sensor up to 10x */
for (uint8_t i = 0; i < 10; i++) {
struct pollfd fds;
/* wait for data to be ready */
fds.fd = fd;
fds.events = POLLIN;
ret = ::poll(&fds, 1, 2000);
if (ret != 1) {
warn("ERROR: TIMEOUT 2");
goto out;
}
/* now go get it */
sz = ::read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
warn("ERROR: READ 2");
ret = -EIO;
goto out;
}
sum_excited[0] += report.x;
sum_excited[1] += report.y;
sum_excited[2] += report.z;
}
sum_excited[0] /= num_samples;
sum_excited[1] /= num_samples;
sum_excited[2] /= num_samples;
if (1.0f < fabsf(sum_excited[0] - sum_non_excited[0]) && fabsf(sum_excited[0] - sum_non_excited[0]) < 3.0f &&
1.0f < fabsf(sum_excited[1] - sum_non_excited[1]) && fabsf(sum_excited[1] - sum_non_excited[1]) < 3.0f &&
0.1f < fabsf(sum_excited[2] - sum_non_excited[2]) && fabsf(sum_excited[2] - sum_non_excited[2]) < 1.0f) {
ret = OK;
} else {
ret = -EIO;
goto out;
}
out:
/* set back to normal mode */
set_range(4);
set_default_register_values();
px4_usleep(20000);
return ret;
}
int
LIS3MDL::check_offset()
{
bool offset_valid;
if ((-2.0f * FLT_EPSILON < _scale.x_offset && _scale.x_offset < 2.0f * FLT_EPSILON) &&
(-2.0f * FLT_EPSILON < _scale.y_offset && _scale.y_offset < 2.0f * FLT_EPSILON) &&
(-2.0f * FLT_EPSILON < _scale.z_offset && _scale.z_offset < 2.0f * FLT_EPSILON)) {
/* offset is zero */
offset_valid = false;
} else {
offset_valid = true;
}
/* return 0 if calibrated, 1 else */
return !offset_valid;
}
int
LIS3MDL::check_scale()
{
bool scale_valid;
if ((-FLT_EPSILON + 1.0f < _scale.x_scale && _scale.x_scale < FLT_EPSILON + 1.0f) &&
(-FLT_EPSILON + 1.0f < _scale.y_scale && _scale.y_scale < FLT_EPSILON + 1.0f) &&
(-FLT_EPSILON + 1.0f < _scale.z_scale && _scale.z_scale < FLT_EPSILON + 1.0f)) {
/* scale is one */
scale_valid = false;
} else {
scale_valid = true;
}
/* return 0 if calibrated, 1 else */
return !scale_valid;
}
int
LIS3MDL::collect()
{
#pragma pack(push, 1)
struct {
uint8_t x[2];
uint8_t y[2];
uint8_t z[2];
} lis_report;
} lis_report{};
struct {
int16_t x;
int16_t y;
int16_t z;
int16_t t;
} report;
#pragma pack(pop)
} report{};
int ret = 0;
uint8_t buf_rx[2] = {0};
uint8_t buf_rx[2] {};
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_OUT_X_L, (uint8_t *)&lis_report, sizeof(lis_report));
const hrt_abstime timestamp_sample = hrt_absolute_time();
_interface->read(ADDR_OUT_X_L, (uint8_t *)&lis_report, sizeof(lis_report));
/**
* Silicon Bug: the X axis will be read instead of the temperature registers if you do a sequential read through XYZ.
* The temperature registers must be addressed directly.
*/
ret = _interface->read(ADDR_OUT_T_L, (uint8_t *)&buf_rx, sizeof(buf_rx));
int ret = _interface->read(ADDR_OUT_T_L, (uint8_t *)&buf_rx, sizeof(buf_rx));
if (ret != OK) {
perf_end(_sample_perf);
perf_count(_comms_errors);
PX4_WARN("Register read error.");
return ret;
}
perf_end(_sample_perf);
report.x = (int16_t)((lis_report.x[1] << 8) | lis_report.x[0]);
report.y = (int16_t)((lis_report.y[1] << 8) | lis_report.y[0]);
report.z = (int16_t)((lis_report.z[1] << 8) | lis_report.z[0]);
report.t = (int16_t)((buf_rx[1] << 8) | buf_rx[0]);
float temperature = report.t;
new_mag_report.temperature = 25.0f + (temperature / 8.0f);
float temperature = 25.0f + (report.t / 8.0f);
_px4_mag.set_temperature(temperature);
// XXX revisit for SPI part, might require a bus type IOCTL
_px4_mag.update(timestamp_sample, report.x, report.y, report.z);
unsigned dummy = 0;
sensor_is_onboard = !_interface->ioctl(MAGIOCGEXTERNAL, dummy);
new_mag_report.is_external = !sensor_is_onboard;
/**
* RAW outputs
*/
new_mag_report.x_raw = report.x;
new_mag_report.y_raw = report.y;
new_mag_report.z_raw = report.z;
xraw_f = report.x;
yraw_f = report.y;
zraw_f = report.z;
// 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;
/* flip axes and negate value for y */
new_mag_report.y = ((yraw_f * _range_scale) - _scale.y_offset) * _scale.y_scale;
/* z remains z */
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);
ret = OK;
perf_end(_sample_perf);
return ret;
return PX4_OK;
}
void
LIS3MDL::RunImpl()
void LIS3MDL::RunImpl()
{
/* _measure_interval == 0 is used as _task_should_exit */
if (_measure_interval == 0) {
@@ -444,15 +143,14 @@ LIS3MDL::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) {
@@ -461,122 +159,18 @@ LIS3MDL::RunImpl()
}
}
int
LIS3MDL::init()
int LIS3MDL::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);
_measure_interval = LIS3MDL_CONVERSION_INTERVAL;
start();
return PX4_OK;
}
int
LIS3MDL::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 = (LIS3MDL_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);
/* 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:
return set_range(arg);
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:
return calibrate(file_pointer, arg);
case MAGIOCEXSTRAP:
return set_excitement(arg);
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
LIS3MDL::measure()
int LIS3MDL::measure()
{
int ret = 0;
@@ -590,7 +184,6 @@ LIS3MDL::measure()
_continuous_mode_set = false;
}
if (ret != OK) {
perf_count(_comms_errors);
}
@@ -598,23 +191,18 @@ LIS3MDL::measure()
return ret;
}
void
LIS3MDL::print_status()
void LIS3MDL::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
LIS3MDL::reset()
int LIS3MDL::reset()
{
int ret = 0;
ret = set_default_register_values();
int ret = set_default_register_values();
if (ret != OK) {
return PX4_ERROR;
@@ -629,64 +217,6 @@ LIS3MDL::reset()
return PX4_OK;
}
int
LIS3MDL::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 */
px4_usleep(LIS3MDL_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
LIS3MDL::set_default_register_values()
{
@@ -699,69 +229,33 @@ LIS3MDL::set_default_register_values()
return PX4_OK;
}
int
LIS3MDL::set_excitement(unsigned enable)
{
int ret;
/* arm the excitement strap */
ret = read_reg(ADDR_CTRL_REG1, _cntl_reg1);
if (ret != OK) {
perf_count(_comms_errors);
}
_cntl_reg1 &= ~0x01; // reset previous excitement mode
if (enable > 0) {
_cntl_reg1 |= 0x01;
}
::printf("set_excitement enable=%d cntl1=0x%x\n", (int)enable, (unsigned)_cntl_reg1);
ret = write_reg(ADDR_CTRL_REG1, _cntl_reg1);
if (ret != OK) {
perf_count(_comms_errors);
}
uint8_t conf_reg_ret = 0;
read_reg(ADDR_CTRL_REG1, conf_reg_ret);
//print_info();
return !(_cntl_reg1 == conf_reg_ret);
}
int
LIS3MDL::set_range(unsigned range)
int LIS3MDL::set_range(unsigned range)
{
if (range <= 4) {
_range_bits = 0x00;
_range_scale = 1.0f / 6842.0f;
_px4_mag.set_scale(1.0f / 6842.0f);
_range_ga = 4.0f;
} else if (range <= 8) {
_range_bits = 0x01;
_range_scale = 1.0f / 3421.0f;
_px4_mag.set_scale(1.0f / 3421.0f);
_range_ga = 8.0f;
} else if (range <= 12) {
_range_bits = 0x02;
_range_scale = 1.0f / 2281.0f;
_px4_mag.set_scale(1.0f / 2281.0f);
_range_ga = 12.0f;
} else {
_range_bits = 0x03;
_range_scale = 1.0f / 1711.0f;
_px4_mag.set_scale(1.0f / 1711.0f);
_range_ga = 16.0f;
}
int ret = 0;
/*
* Send the command to set the range
*/
ret = write_reg(ADDR_CTRL_REG2, (_range_bits << 5));
int ret = write_reg(ADDR_CTRL_REG2, (_range_bits << 5));
if (ret != OK) {
perf_count(_comms_errors);
@@ -782,20 +276,15 @@ LIS3MDL::set_range(unsigned range)
}
}
void
LIS3MDL::start()
void LIS3MDL::start()
{
/* reset the report ring and state machine */
_reports->flush();
set_default_register_values();
/* schedule a cycle to start things */
ScheduleNow();
}
int
LIS3MDL::read_reg(uint8_t reg, uint8_t &val)
int LIS3MDL::read_reg(uint8_t reg, uint8_t &val)
{
uint8_t buf = val;
int ret = _interface->read(reg, &buf, 1);
@@ -803,8 +292,7 @@ LIS3MDL::read_reg(uint8_t reg, uint8_t &val)
return ret;
}
int
LIS3MDL::write_reg(uint8_t reg, uint8_t val)
int LIS3MDL::write_reg(uint8_t reg, uint8_t val)
{
uint8_t buf = val;
return _interface->write(reg, &buf, 1);
src/drivers/magnetometer/lis3mdl/lis3mdl.h
+5 -72
View File
@@ -39,19 +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 <systemlib/err.h>
#include <px4_platform_common/i2c_spi_buses.h>
#include <perf/perf_counter.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/defines.h>
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
/**
* LIS3MDL internal constants and data structures.
@@ -99,8 +93,7 @@ enum OPERATING_MODE {
SINGLE
};
class LIS3MDL : public device::CDev, public I2CSPIDriver<LIS3MDL>
class LIS3MDL : public I2CSPIDriver<LIS3MDL>
{
public:
LIS3MDL(device::Device *interface, enum Rotation rotation, I2CSPIBusOption bus_option, int bus);
@@ -114,10 +107,6 @@ public:
virtual int init();
virtual int ioctl(struct file *file_pointer, int cmd, unsigned long arg);
virtual int read(struct file *file_pointer, char *buffer, size_t buffer_len);
void print_status() override;
/**
@@ -127,18 +116,10 @@ public:
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;
@@ -146,19 +127,13 @@ 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_ga;
float _range_scale;
uint8_t _check_state_cnt;
uint8_t _cntl_reg1;
@@ -170,37 +145,11 @@ private:
uint8_t _temperature_counter;
uint8_t _temperature_error_count;
/**
* @brief Performs the on-sensor scale calibration routine.
*
* @note The sensor will continue to provide measurements, these
* will however reflect the uncalibrated sensor state until
* the calibration routine has been completed.
*
* @param enable set to 1 to enable self-test strap, 0 to disable
*/
int calibrate(struct file *file_pointer, unsigned enable);
/**
* Collect the result of the most recent measurement.
*/
int collect();
/**
* Check the current scale calibration
*
* @return 0 if scale calibration is ok, 1 else
*/
int check_scale();
/**
* Check the current offset calibration
*
* @return 0 if offset calibration is ok, 1 else
*/
int check_offset();
/**
* Issue a measurement command.
*
@@ -221,18 +170,6 @@ private:
*/
void start();
/**
* @brief Performs the on-sensor scale calibration routine.
*
* @note The sensor will continue to provide measurements, these
* will however reflect the uncalibrated sensor state until
* the calibration routine has been completed.
*
* @param enable set to 1 to enable self-test positive strap, -1 to enable
* negative strap, 0 to set to normal mode
*/
int set_excitement(unsigned enable);
/**
* @brief Sets the sensor internal range to handle at least the argument in Gauss.
*
@@ -258,8 +195,4 @@ private:
*/
int write_reg(uint8_t reg, uint8_t val);
/* this class has pointer data members, do not allow copying it */
LIS3MDL(const LIS3MDL &);
LIS3MDL operator=(const LIS3MDL &);
}; // class LIS3MDL
src/drivers/magnetometer/lis3mdl/lis3mdl_i2c.cpp
+3 -23
View File
@@ -63,7 +63,6 @@ public:
LIS3MDL_I2C(int bus, int bus_frequency);
virtual ~LIS3MDL_I2C() = default;
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);
@@ -87,24 +86,7 @@ LIS3MDL_I2C::LIS3MDL_I2C(int bus, int bus_frequency) :
_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LIS3MDL;
}
int
LIS3MDL_I2C::ioctl(unsigned operation, unsigned &arg)
{
switch (operation) {
case MAGIOCGEXTERNAL:
return external();
case DEVIOCGDEVICEID:
return CDev::ioctl(nullptr, operation, arg);
default:
return -EINVAL;
}
}
int
LIS3MDL_I2C::probe()
int LIS3MDL_I2C::probe()
{
uint8_t data = 0;
@@ -125,15 +107,13 @@ LIS3MDL_I2C::probe()
return OK;
}
int
LIS3MDL_I2C::read(unsigned address, void *data, unsigned count)
int LIS3MDL_I2C::read(unsigned address, void *data, unsigned count)
{
uint8_t cmd = address;
return transfer(&cmd, 1, (uint8_t *)data, count);
}
int
LIS3MDL_I2C::write(unsigned address, void *data, unsigned count)
int LIS3MDL_I2C::write(unsigned address, void *data, unsigned count)
{
uint8_t buf[32];
src/drivers/magnetometer/lis3mdl/lis3mdl_spi.cpp
+3 -34
View File
@@ -67,7 +67,6 @@ public:
virtual ~LIS3MDL_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);
};
@@ -90,9 +89,7 @@ LIS3MDL_SPI::LIS3MDL_SPI(int bus, uint32_t devid, int bus_frequency, spi_mode_e
int
LIS3MDL_SPI::init()
{
int ret;
ret = SPI::init();
int ret = SPI::init();
if (ret != OK) {
DEVICE_DEBUG("SPI init failed");
@@ -114,34 +111,7 @@ LIS3MDL_SPI::init()
return OK;
}
int
LIS3MDL_SPI::ioctl(unsigned operation, unsigned &arg)
{
int ret;
switch (operation) {
case MAGIOCGEXTERNAL:
/*
* Even if this sensor is on the external SPI
* bus it is still internal to the autopilot
* assembly, so always return 0 for internal.
*/
return 0;
case DEVIOCGDEVICEID:
return CDev::ioctl(nullptr, operation, arg);
default: {
ret = -EINVAL;
}
}
return ret;
}
int
LIS3MDL_SPI::read(unsigned address, void *data, unsigned count)
int LIS3MDL_SPI::read(unsigned address, void *data, unsigned count)
{
uint8_t buf[32];
@@ -156,8 +126,7 @@ LIS3MDL_SPI::read(unsigned address, void *data, unsigned count)
return ret;
}
int
LIS3MDL_SPI::write(unsigned address, void *data, unsigned count)
int LIS3MDL_SPI::write(unsigned address, void *data, unsigned count)
{
uint8_t buf[32];