fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-02 05:31:37 +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-04-01 10:07:08 -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 # Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions # modification, are permitted provided that the following conditions
@@ -31,15 +31,15 @@
# #
############################################################################ ############################################################################
px4_add_module( px4_add_module(
MODULE drivers__lis3mdl MODULE drivers__magnetometer__lis3mdl
MAIN lis3mdl MAIN lis3mdl
COMPILE_FLAGS COMPILE_FLAGS
-Wno-cast-align # TODO: fix and enable
SRCS SRCS
lis3mdl_i2c.cpp lis3mdl_i2c.cpp
lis3mdl_spi.cpp lis3mdl_spi.cpp
lis3mdl_main.cpp lis3mdl_main.cpp
lis3mdl.cpp lis3mdl.cpp
DEPENDS DEPENDS
conversion drivers_magnetometer
px4_work_queue
) )
src/drivers/magnetometer/lis3mdl/lis3mdl.cpp
+43 -555
View File
@@ -43,29 +43,20 @@
#include "lis3mdl.h" #include "lis3mdl.h"
LIS3MDL::LIS3MDL(device::Device *interface, enum Rotation rotation, I2CSPIBusOption bus_option, int bus) : 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), 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), _interface(interface),
_reports(nullptr), _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms_errors")),
_scale{}, _conf_errors(perf_alloc(PC_COUNT, MODULE_NAME": conf_errors")),
_last_report{}, _range_errors(perf_alloc(PC_COUNT, MODULE_NAME": range_errors")),
_mag_topic(nullptr), _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
_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),
_continuous_mode_set(false), _continuous_mode_set(false),
_mode(CONTINUOUS), _mode(CONTINUOUS),
_rotation(rotation),
_measure_interval(0), _measure_interval(0),
_class_instance(-1),
_orb_class_instance(-1),
_range_ga(4.0f), _range_ga(4.0f),
_range_scale(0), // default range scale from counts to gauss */
_check_state_cnt(0), _check_state_cnt(0),
_cntl_reg1( _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_reg2(CNTL_REG2_DEFAULT), // 4 gauss FS range, reboot settings default
_cntl_reg3(CNTL_REG3_DEFAULT), // operating mode CONTINUOUS! _cntl_reg3(CNTL_REG3_DEFAULT), // operating mode CONTINUOUS!
_cntl_reg4(CNTL_REG4_DEFAULT), // Z-axis ultra high performance mode _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_counter(0),
_temperature_error_count(0) _temperature_error_count(0)
{ {
// set the device type from the interface _interface->set_device_type(DRV_MAG_DEVTYPE_IST8310);
_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;
// default scaling _px4_mag.set_device_type(DRV_MAG_DEVTYPE_IST8310);
_scale.x_offset = 0; _px4_mag.set_external(_interface->external());
_scale.x_scale = 1.0f;
_scale.y_offset = 0;
_scale.y_scale = 1.0f;
_scale.z_offset = 0;
_scale.z_scale = 1.0f;
} }
LIS3MDL::~LIS3MDL() 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 // free perf counters
perf_free(_sample_perf); perf_free(_sample_perf);
perf_free(_comms_errors); perf_free(_comms_errors);
@@ -110,332 +80,61 @@ LIS3MDL::~LIS3MDL()
perf_free(_conf_errors); perf_free(_conf_errors);
} }
int int LIS3MDL::collect()
LIS3MDL::calibrate(struct file *file_pointer, unsigned enable)
{ {
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 { struct {
uint8_t x[2]; uint8_t x[2];
uint8_t y[2]; uint8_t y[2];
uint8_t z[2]; uint8_t z[2];
} lis_report; } lis_report{};
struct { struct {
int16_t x; int16_t x;
int16_t y; int16_t y;
int16_t z; int16_t z;
int16_t t; int16_t t;
} report; } report{};
#pragma pack(pop)
int ret = 0; uint8_t buf_rx[2] {};
uint8_t buf_rx[2] = {0};
float xraw_f; _px4_mag.set_error_count(perf_event_count(_comms_errors));
float yraw_f;
float zraw_f;
sensor_mag_s new_mag_report;
bool sensor_is_onboard = false;
perf_begin(_sample_perf); perf_begin(_sample_perf);
new_mag_report.timestamp = hrt_absolute_time(); const hrt_abstime timestamp_sample = hrt_absolute_time();
new_mag_report.error_count = perf_event_count(_comms_errors); _interface->read(ADDR_OUT_X_L, (uint8_t *)&lis_report, sizeof(lis_report));
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));
/** /**
* Silicon Bug: the X axis will be read instead of the temperature registers if you do a sequential read through XYZ. * 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. * 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) { if (ret != OK) {
perf_end(_sample_perf);
perf_count(_comms_errors); perf_count(_comms_errors);
PX4_WARN("Register read error."); PX4_WARN("Register read error.");
return ret; return ret;
} }
perf_end(_sample_perf);
report.x = (int16_t)((lis_report.x[1] << 8) | lis_report.x[0]); 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.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.z = (int16_t)((lis_report.z[1] << 8) | lis_report.z[0]);
report.t = (int16_t)((buf_rx[1] << 8) | buf_rx[0]); report.t = (int16_t)((buf_rx[1] << 8) | buf_rx[0]);
float temperature = report.t; float temperature = 25.0f + (report.t / 8.0f);
new_mag_report.temperature = 25.0f + (temperature / 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; return PX4_OK;
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;
} }
void void LIS3MDL::RunImpl()
LIS3MDL::RunImpl()
{ {
/* _measure_interval == 0 is used as _task_should_exit */ /* _measure_interval == 0 is used as _task_should_exit */
if (_measure_interval == 0) { if (_measure_interval == 0) {
@@ -444,15 +143,14 @@ LIS3MDL::RunImpl()
/* Collect last measurement at the start of every cycle */ /* Collect last measurement at the start of every cycle */
if (collect() != OK) { if (collect() != OK) {
DEVICE_DEBUG("collection error"); PX4_DEBUG("collection error");
/* restart the measurement state machine */ /* restart the measurement state machine */
start(); start();
return; return;
} }
if (measure() != OK) { if (measure() != OK) {
DEVICE_DEBUG("measure error"); PX4_DEBUG("measure error");
} }
if (_measure_interval > 0) { if (_measure_interval > 0) {
@@ -461,122 +159,18 @@ LIS3MDL::RunImpl()
} }
} }
int int LIS3MDL::init()
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 the device configuration */
reset(); reset();
_class_instance = register_class_devname(MAG_BASE_DEVICE_PATH);
_measure_interval = LIS3MDL_CONVERSION_INTERVAL; _measure_interval = LIS3MDL_CONVERSION_INTERVAL;
start(); start();
return PX4_OK; return PX4_OK;
} }
int int LIS3MDL::measure()
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 ret = 0; int ret = 0;
@@ -590,7 +184,6 @@ LIS3MDL::measure()
_continuous_mode_set = false; _continuous_mode_set = false;
} }
if (ret != OK) { if (ret != OK) {
perf_count(_comms_errors); perf_count(_comms_errors);
} }
@@ -598,23 +191,18 @@ LIS3MDL::measure()
return ret; return ret;
} }
void void LIS3MDL::print_status()
LIS3MDL::print_status()
{ {
I2CSPIDriverBase::print_status(); I2CSPIDriverBase::print_status();
perf_print_counter(_sample_perf); perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors); perf_print_counter(_comms_errors);
PX4_INFO("poll interval: %u", _measure_interval); PX4_INFO("poll interval: %u", _measure_interval);
print_message(_last_report); _px4_mag.print_status();
_reports->print_info("report queue");
} }
int int LIS3MDL::reset()
LIS3MDL::reset()
{ {
int ret = 0; int ret = set_default_register_values();
ret = set_default_register_values();
if (ret != OK) { if (ret != OK) {
return PX4_ERROR; return PX4_ERROR;
@@ -629,64 +217,6 @@ LIS3MDL::reset()
return PX4_OK; 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 int
LIS3MDL::set_default_register_values() LIS3MDL::set_default_register_values()
{ {
@@ -699,69 +229,33 @@ LIS3MDL::set_default_register_values()
return PX4_OK; return PX4_OK;
} }
int int LIS3MDL::set_range(unsigned range)
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)
{ {
if (range <= 4) { if (range <= 4) {
_range_bits = 0x00; _range_bits = 0x00;
_range_scale = 1.0f / 6842.0f; _px4_mag.set_scale(1.0f / 6842.0f);
_range_ga = 4.0f; _range_ga = 4.0f;
} else if (range <= 8) { } else if (range <= 8) {
_range_bits = 0x01; _range_bits = 0x01;
_range_scale = 1.0f / 3421.0f; _px4_mag.set_scale(1.0f / 3421.0f);
_range_ga = 8.0f; _range_ga = 8.0f;
} else if (range <= 12) { } else if (range <= 12) {
_range_bits = 0x02; _range_bits = 0x02;
_range_scale = 1.0f / 2281.0f; _px4_mag.set_scale(1.0f / 2281.0f);
_range_ga = 12.0f; _range_ga = 12.0f;
} else { } else {
_range_bits = 0x03; _range_bits = 0x03;
_range_scale = 1.0f / 1711.0f; _px4_mag.set_scale(1.0f / 1711.0f);
_range_ga = 16.0f; _range_ga = 16.0f;
} }
int ret = 0;
/* /*
* Send the command to set the range * 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) { if (ret != OK) {
perf_count(_comms_errors); perf_count(_comms_errors);
@@ -782,20 +276,15 @@ LIS3MDL::set_range(unsigned range)
} }
} }
void void LIS3MDL::start()
LIS3MDL::start()
{ {
/* reset the report ring and state machine */
_reports->flush();
set_default_register_values(); set_default_register_values();
/* schedule a cycle to start things */ /* schedule a cycle to start things */
ScheduleNow(); ScheduleNow();
} }
int int LIS3MDL::read_reg(uint8_t reg, uint8_t &val)
LIS3MDL::read_reg(uint8_t reg, uint8_t &val)
{ {
uint8_t buf = val; uint8_t buf = val;
int ret = _interface->read(reg, &buf, 1); int ret = _interface->read(reg, &buf, 1);
@@ -803,8 +292,7 @@ LIS3MDL::read_reg(uint8_t reg, uint8_t &val)
return ret; return ret;
} }
int int LIS3MDL::write_reg(uint8_t reg, uint8_t val)
LIS3MDL::write_reg(uint8_t reg, uint8_t val)
{ {
uint8_t buf = val; uint8_t buf = val;
return _interface->write(reg, &buf, 1); return _interface->write(reg, &buf, 1);
src/drivers/magnetometer/lis3mdl/lis3mdl.h
+5 -72
View File
@@ -39,19 +39,13 @@
#pragma once #pragma once
#include <float.h>
#include <drivers/device/i2c.h> #include <drivers/device/i2c.h>
#include <drivers/device/ringbuffer.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <drivers/drv_mag.h> #include <drivers/drv_mag.h>
#include <lib/conversion/rotation.h>
#include <systemlib/err.h>
#include <px4_platform_common/i2c_spi_buses.h> #include <px4_platform_common/i2c_spi_buses.h>
#include <lib/perf/perf_counter.h>
#include <perf/perf_counter.h>
#include <px4_platform_common/defines.h> #include <px4_platform_common/defines.h>
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
/** /**
* LIS3MDL internal constants and data structures. * LIS3MDL internal constants and data structures.
@@ -99,8 +93,7 @@ enum OPERATING_MODE {
SINGLE SINGLE
}; };
class LIS3MDL : public I2CSPIDriver<LIS3MDL>
class LIS3MDL : public device::CDev, public I2CSPIDriver<LIS3MDL>
{ {
public: public:
LIS3MDL(device::Device *interface, enum Rotation rotation, I2CSPIBusOption bus_option, int bus); LIS3MDL(device::Device *interface, enum Rotation rotation, I2CSPIBusOption bus_option, int bus);
@@ -114,10 +107,6 @@ public:
virtual int init(); 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; void print_status() override;
/** /**
@@ -127,18 +116,10 @@ public:
void RunImpl(); void RunImpl();
protected:
Device *_interface;
private: private:
PX4Magnetometer _px4_mag;
ringbuffer::RingBuffer *_reports; device::Device *_interface;
struct mag_calibration_s _scale;
sensor_mag_s _last_report {}; /**< used for info() */
orb_advert_t _mag_topic;
perf_counter_t _comms_errors; perf_counter_t _comms_errors;
perf_counter_t _conf_errors; perf_counter_t _conf_errors;
@@ -146,19 +127,13 @@ private:
perf_counter_t _sample_perf; perf_counter_t _sample_perf;
/* status reporting */ /* status reporting */
bool _calibrated; /**< the calibration is valid */
bool _continuous_mode_set; bool _continuous_mode_set;
enum OPERATING_MODE _mode; enum OPERATING_MODE _mode;
enum Rotation _rotation;
unsigned int _measure_interval; unsigned int _measure_interval;
int _class_instance;
int _orb_class_instance;
float _range_ga; float _range_ga;
float _range_scale;
uint8_t _check_state_cnt; uint8_t _check_state_cnt;
uint8_t _cntl_reg1; uint8_t _cntl_reg1;
@@ -170,37 +145,11 @@ private:
uint8_t _temperature_counter; uint8_t _temperature_counter;
uint8_t _temperature_error_count; 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. * Collect the result of the most recent measurement.
*/ */
int collect(); 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. * Issue a measurement command.
* *
@@ -221,18 +170,6 @@ private:
*/ */
void start(); 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. * @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); 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 }; // 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); LIS3MDL_I2C(int bus, int bus_frequency);
virtual ~LIS3MDL_I2C() = default; virtual ~LIS3MDL_I2C() = default;
virtual int ioctl(unsigned operation, unsigned &arg);
virtual int read(unsigned address, void *data, unsigned count); virtual int read(unsigned address, void *data, unsigned count);
virtual int write(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; _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LIS3MDL;
} }
int int LIS3MDL_I2C::probe()
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()
{ {
uint8_t data = 0; uint8_t data = 0;
@@ -125,15 +107,13 @@ LIS3MDL_I2C::probe()
return OK; return OK;
} }
int int LIS3MDL_I2C::read(unsigned address, void *data, unsigned count)
LIS3MDL_I2C::read(unsigned address, void *data, unsigned count)
{ {
uint8_t cmd = address; uint8_t cmd = address;
return transfer(&cmd, 1, (uint8_t *)data, count); return transfer(&cmd, 1, (uint8_t *)data, count);
} }
int int LIS3MDL_I2C::write(unsigned address, void *data, unsigned count)
LIS3MDL_I2C::write(unsigned address, void *data, unsigned count)
{ {
uint8_t buf[32]; 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 ~LIS3MDL_SPI() = default;
virtual int init(); virtual int init();
virtual int ioctl(unsigned operation, unsigned &arg);
virtual int read(unsigned address, void *data, unsigned count); virtual int read(unsigned address, void *data, unsigned count);
virtual int write(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 int
LIS3MDL_SPI::init() LIS3MDL_SPI::init()
{ {
int ret; int ret = SPI::init();
ret = SPI::init();
if (ret != OK) { if (ret != OK) {
DEVICE_DEBUG("SPI init failed"); DEVICE_DEBUG("SPI init failed");
@@ -114,34 +111,7 @@ LIS3MDL_SPI::init()
return OK; return OK;
} }
int int LIS3MDL_SPI::read(unsigned address, void *data, unsigned count)
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)
{ {
uint8_t buf[32]; uint8_t buf[32];
@@ -156,8 +126,7 @@ LIS3MDL_SPI::read(unsigned address, void *data, unsigned count)
return ret; return ret;
} }
int int LIS3MDL_SPI::write(unsigned address, void *data, unsigned count)
LIS3MDL_SPI::write(unsigned address, void *data, unsigned count)
{ {
uint8_t buf[32]; uint8_t buf[32];