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

qmc5883: move to PX4Magnetometer and cleanup

Browse Source
This commit is contained in:
Daniel Agar
2020-03-30 15:16:53 -04:00
parent ade52d612b
commit a59aa865da
4 changed files with 37 additions and 355 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/magnetometer/qmc5883/CMakeLists.txt
+2 -2
View File
@@ -31,15 +31,15 @@
#
############################################################################
px4_add_module(
MODULE drivers__qmc5883
MODULE drivers__magnetometer__qmc5883
MAIN qmc5883
COMPILE_FLAGS
-Wno-cast-align # TODO: fix and enable
SRCS
QMC5883_I2C.cpp
QMC5883.cpp
QMC5883.hpp
qmc5883_main.cpp
DEPENDS
drivers_magnetometer
px4_work_queue
)
src/drivers/magnetometer/qmc5883/QMC5883.cpp
+26 -277
View File
@@ -35,53 +35,26 @@
QMC5883::QMC5883(device::Device *interface, enum Rotation rotation, I2CSPIBusOption bus_option, int bus,
int i2c_address) :
CDev("QMC5883", nullptr),
I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus, i2c_address),
_px4_mag(interface->get_device_id(), interface->external() ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT, rotation),
_interface(interface),
_reports(nullptr),
_scale{},
_range_scale(1.0f / 12000.0f),
_range_ga(2.0f),
_collect_phase(false),
_class_instance(-1),
_orb_class_instance(-1),
_mag_topic(nullptr),
_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
_comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err")),
_range_errors(perf_alloc(PC_COUNT, MODULE_NAME": rng_err")),
_conf_errors(perf_alloc(PC_COUNT, MODULE_NAME": conf_err")),
_sensor_ok(false),
_rotation(rotation),
_range_bits(0),
_conf_reg(0),
_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_QMC5883;
_interface->set_device_type(DRV_MAG_DEVTYPE_QMC5883);
// 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_QMC5883);
_px4_mag.set_external(_interface->external());
}
QMC5883::~QMC5883()
{
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);
@@ -89,39 +62,15 @@ QMC5883::~QMC5883()
perf_free(_conf_errors);
}
int
QMC5883::init()
int QMC5883::init()
{
int ret = PX4_ERROR;
ret = CDev::init();
if (ret != OK) {
DEVICE_DEBUG("CDev init failed");
goto out;
}
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_mag_s));
if (_reports == nullptr) {
goto out;
}
/* reset the device configuration */
reset();
_class_instance = register_class_devname(MAG_BASE_DEVICE_PATH);
ret = OK;
/* sensor is ok */
_sensor_ok = true;
_measure_interval = QMC5883_CONVERSION_INTERVAL;
start();
out:
return ret;
return PX4_OK;
}
/**
@@ -129,12 +78,10 @@ out:
done periodically to cope with I2C bus noise causing the
configuration of the compass to change.
*/
void QMC5883::check_conf(void)
void QMC5883::check_conf()
{
int ret;
uint8_t conf_reg_in = 0;
ret = read_reg(QMC5883_ADDR_CONTROL_1, conf_reg_in);
int ret = read_reg(QMC5883_ADDR_CONTROL_1, conf_reg_in);
if (OK != ret) {
perf_count(_comms_errors);
@@ -151,152 +98,16 @@ void QMC5883::check_conf(void)
}
}
ssize_t
QMC5883::read(struct file *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / 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();
/* wait for it to complete */
px4_usleep(QMC5883_CONVERSION_INTERVAL);
/* run the collection phase */
if (OK != collect()) {
ret = -EIO;
break;
}
if (_reports->get(mag_buf)) {
ret = sizeof(sensor_mag_s);
}
} while (0);
return ret;
}
int
QMC5883::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
{
unsigned dummy = arg;
switch (cmd) {
case SENSORIOCSPOLLRATE: {
switch (arg) {
/* zero would be bad */
case 0:
return -EINVAL;
/* set default polling rate */
case SENSOR_POLLRATE_DEFAULT: {
/* do we need to start internal polling? */
bool want_start = (_measure_interval == 0);
/* set interval for next measurement to minimum legal value */
_measure_interval = QMC5883_CONVERSION_INTERVAL;
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}
return OK;
}
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_measure_interval == 0);
/* convert hz to interval in microseconds */
unsigned interval = (1000000 / arg);
/* check against maximum rate */
if (interval < QMC5883_CONVERSION_INTERVAL) {
return -EINVAL;
}
/* update interval for next measurement */
_measure_interval = interval;
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}
return OK;
}
}
}
case SENSORIOCRESET:
return reset();
case MAGIOCSRANGE:
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 MAGIOCGEXTERNAL:
DEVICE_DEBUG("MAGIOCGEXTERNAL in main driver");
return _interface->ioctl(cmd, dummy);
default:
/* give it to the superclass */
return CDev::ioctl(filp, cmd, arg);
}
}
void
QMC5883::start()
void QMC5883::start()
{
/* reset the report ring and state machine */
_collect_phase = false;
_reports->flush();
/* schedule a cycle to start things */
ScheduleNow();
}
int
QMC5883::reset()
int QMC5883::reset()
{
/* read 0x00 once */
uint8_t data_bits_in = 0;
@@ -308,10 +119,8 @@ QMC5883::reset()
/* set reset period to 0x01 */
write_reg(QMC5883_ADDR_SET_RESET, QMC5883_SET_DEFAULT);
//use fixed range of 2G
_range_scale = 1.0f / 12000.0f; // 12000 LSB/Gauss at +/- 2G range
_range_ga = 2.00f;
_range_bits = 0x00;
// use fixed range of 2G
_px4_mag.set_scale(1.f / 12000.f); // 12000 LSB/Gauss at +/- 2G range
/* set control register */
_conf_reg = QMC5883_MODE_REG_CONTINOUS_MODE |
@@ -323,8 +132,7 @@ QMC5883::reset()
return OK;
}
void
QMC5883::RunImpl()
void QMC5883::RunImpl()
{
if (_measure_interval == 0) {
return;
@@ -335,7 +143,7 @@ QMC5883::RunImpl()
/* perform collection */
if (OK != collect()) {
DEVICE_DEBUG("collection error");
PX4_DEBUG("collection error");
/* restart the measurement state machine */
start();
return;
@@ -364,9 +172,7 @@ QMC5883::RunImpl()
}
}
int
QMC5883::collect()
int QMC5883::collect()
{
struct { /* status register and data as read back from the device */
uint8_t x[2];
@@ -382,18 +188,12 @@ QMC5883::collect()
uint8_t check_counter;
perf_begin(_sample_perf);
sensor_mag_s new_report;
bool sensor_is_onboard = false;
float xraw_f;
float yraw_f;
float zraw_f;
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
new_report.timestamp = hrt_absolute_time();
new_report.error_count = perf_event_count(_comms_errors);
new_report.scaling = _range_scale;
new_report.device_id = _device_id.devid;
_px4_mag.set_error_count(perf_event_count(_comms_errors));
/*
* @note We could read the status register here, which could tell us that
@@ -403,11 +203,12 @@ QMC5883::collect()
*/
/* get measurements from the device */
const hrt_abstime timestamp_sample = hrt_absolute_time();
ret = _interface->read(QMC5883_ADDR_DATA_OUT_X_LSB, (uint8_t *)&qmc_report, sizeof(qmc_report));
if (ret != OK) {
perf_count(_comms_errors);
DEVICE_DEBUG("data/status read error");
PX4_DEBUG("data/status read error");
goto out;
}
@@ -428,8 +229,6 @@ QMC5883::collect()
}
/* get temperature measurements from the device */
new_report.temperature = 0;
if (_temperature_counter++ == 100) {
uint8_t raw_temperature[2];
@@ -441,33 +240,19 @@ QMC5883::collect()
if (ret == OK) {
int16_t temp16 = (((int16_t)raw_temperature[1]) << 8) +
raw_temperature[0];
new_report.temperature = QMC5883_TEMP_OFFSET + temp16 * 1.0f / 100.0f;
float temperature = QMC5883_TEMP_OFFSET + temp16 * 1.0f / 100.0f;
_px4_mag.set_temperature(temperature);
}
} else {
new_report.temperature = _last_report.temperature;
}
/*
* RAW outputs
*
* to align the sensor axes with the board, x and y need to be flipped
* and y needs to be negated
*/
new_report.x_raw = -report.y;
new_report.y_raw = report.x;
/* z remains z */
new_report.z_raw = report.z;
/* scale values for output */
// XXX revisit for SPI part, might require a bus type IOCTL
unsigned dummy;
sensor_is_onboard = !_interface->ioctl(MAGIOCGEXTERNAL, dummy);
new_report.is_external = !sensor_is_onboard;
if (sensor_is_onboard) {
if (!_px4_mag.external()) {
// convert onboard so it matches offboard for the
// scaling below
report.y = -report.y;
@@ -482,38 +267,7 @@ QMC5883::collect()
yraw_f = report.x;
zraw_f = report.z;
// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
new_report.x = ((xraw_f * _range_scale) - _scale.x_offset) * _scale.x_scale;
/* flip axes and negate value for y */
new_report.y = ((yraw_f * _range_scale) - _scale.y_offset) * _scale.y_scale;
/* z remains z */
new_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_report);
} else {
_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &new_report,
&_orb_class_instance, (sensor_is_onboard) ? ORB_PRIO_HIGH : ORB_PRIO_MAX);
if (_mag_topic == nullptr) {
DEVICE_DEBUG("ADVERT FAIL");
}
}
}
_last_report = new_report;
/* post a report to the ring */
_reports->force(&new_report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
_px4_mag.update(timestamp_sample, xraw_f, yraw_f, zraw_f);
/*
periodically check the configuration
@@ -524,7 +278,6 @@ QMC5883::collect()
*/
check_counter = perf_event_count(_sample_perf) % 256;
if (check_counter == 0) {
check_conf();
}
@@ -536,15 +289,13 @@ out:
return ret;
}
int
QMC5883::write_reg(uint8_t reg, uint8_t val)
int QMC5883::write_reg(uint8_t reg, uint8_t val)
{
uint8_t buf = val;
return _interface->write(reg, &buf, 1);
}
int
QMC5883::read_reg(uint8_t reg, uint8_t &val)
int QMC5883::read_reg(uint8_t reg, uint8_t &val)
{
uint8_t buf = val;
int ret = _interface->read(reg, &buf, 1);
@@ -552,13 +303,11 @@ QMC5883::read_reg(uint8_t reg, uint8_t &val)
return ret;
}
void
QMC5883::print_status()
void QMC5883::print_status()
{
I2CSPIDriverBase::print_status();
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
printf("interval: %u us\n", _measure_interval);
print_message(_last_report);
_reports->print_info("report queue");
_px4_mag.print_status();
}
src/drivers/magnetometer/qmc5883/QMC5883.hpp
+6 -47
View File
@@ -42,35 +42,13 @@
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/time.h>
#include <drivers/device/i2c.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdlib.h>
#include <stdbool.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 <px4_platform_common/i2c_spi_buses.h>
#include <lib/perf/perf_counter.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_hrt.h>
#include <drivers/device/ringbuffer.h>
#include <drivers/drv_device.h>
#include <uORB/uORB.h>
#include <float.h>
#include <lib/conversion/rotation.h>
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
#include "qmc5883.h"
@@ -124,7 +102,7 @@
#define QMC5883_TEMP_OFFSET 50 /* deg celsius */
class QMC5883 : public device::CDev, public I2CSPIDriver<QMC5883>
class QMC5883 : public I2CSPIDriver<QMC5883>
{
public:
QMC5883(device::Device *interface, enum Rotation rotation, I2CSPIBusOption bus_option, int bus, int i2c_address);
@@ -136,42 +114,23 @@ public:
void RunImpl();
int init() override;
ssize_t read(cdev::file_t *filp, char *buffer, size_t buflen) override;
int ioctl(cdev::file_t *filp, int cmd, unsigned long arg) override;
int init();
protected:
void print_status() override;
private:
Device *_interface;
PX4Magnetometer _px4_mag;
device::Device *_interface;
unsigned _measure_interval{0};
ringbuffer::RingBuffer *_reports;
struct mag_calibration_s _scale;
float _range_scale;
float _range_ga;
bool _collect_phase;
int _class_instance;
int _orb_class_instance;
orb_advert_t _mag_topic;
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
perf_counter_t _range_errors;
perf_counter_t _conf_errors;
/* status reporting */
bool _sensor_ok; /**< sensor was found and reports ok */
enum Rotation _rotation;
sensor_mag_s _last_report {}; /**< used for info() */
uint8_t _range_bits;
uint8_t _conf_reg;
uint8_t _temperature_counter;
uint8_t _temperature_error_count;
@@ -196,7 +155,7 @@ private:
* cope with communication errors causing the configuration to
* change
*/
void check_conf(void);
void check_conf();
/**
* Write a register.
src/drivers/magnetometer/qmc5883/QMC5883_I2C.cpp
+3 -29
View File
@@ -55,11 +55,8 @@ public:
virtual int read(unsigned address, void *data, unsigned count);
virtual int write(unsigned address, void *data, unsigned count);
virtual int ioctl(unsigned operation, unsigned &arg);
protected:
virtual int probe();
};
device::Device *
@@ -74,28 +71,7 @@ QMC5883_I2C::QMC5883_I2C(int bus, int bus_frequency, int i2c_address) :
_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_QMC5883;
}
int
QMC5883_I2C::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
QMC5883_I2C::probe()
int QMC5883_I2C::probe()
{
uint8_t data[2] = {0, 0};
@@ -140,8 +116,7 @@ QMC5883_I2C::probe()
return -EIO;
}
int
QMC5883_I2C::write(unsigned address, void *data, unsigned count)
int QMC5883_I2C::write(unsigned address, void *data, unsigned count)
{
uint8_t buf[32];
@@ -155,8 +130,7 @@ QMC5883_I2C::write(unsigned address, void *data, unsigned count)
return transfer(&buf[0], count + 1, nullptr, 0);
}
int
QMC5883_I2C::read(unsigned address, void *data, unsigned count)
int QMC5883_I2C::read(unsigned address, void *data, unsigned count)
{
uint8_t cmd = address;
return transfer(&cmd, 1, (uint8_t *)data, count);