forked from Archive/PX4-Autopilot
refactor qmc5883: use driver base class
This commit is contained in:
Daniel Agar
parent
b156fe5787
commit
532ccd18ad
|
|
@ -36,9 +36,10 @@ px4_add_module(
|
|||
COMPILE_FLAGS
|
||||
-Wno-cast-align # TODO: fix and enable
|
||||
SRCS
|
||||
qmc5883_i2c.cpp
|
||||
qmc5883_spi.cpp
|
||||
qmc5883.cpp
|
||||
QMC5883_I2C.cpp
|
||||
QMC5883.cpp
|
||||
QMC5883.hpp
|
||||
qmc5883_main.cpp
|
||||
DEPENDS
|
||||
px4_work_queue
|
||||
)
|
||||
|
|
|
|||
|
|
@ -0,0 +1,564 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2019 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "QMC5883.hpp"
|
||||
|
||||
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),
|
||||
_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;
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
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);
|
||||
perf_free(_range_errors);
|
||||
perf_free(_conf_errors);
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
/**
|
||||
check that the configuration register has the right value. This is
|
||||
done periodically to cope with I2C bus noise causing the
|
||||
configuration of the compass to change.
|
||||
*/
|
||||
void QMC5883::check_conf(void)
|
||||
{
|
||||
int ret;
|
||||
|
||||
uint8_t conf_reg_in = 0;
|
||||
ret = read_reg(QMC5883_ADDR_CONTROL_1, conf_reg_in);
|
||||
|
||||
if (OK != ret) {
|
||||
perf_count(_comms_errors);
|
||||
return;
|
||||
}
|
||||
|
||||
if (conf_reg_in != _conf_reg) {
|
||||
perf_count(_conf_errors);
|
||||
ret = write_reg(QMC5883_ADDR_CONTROL_1, _conf_reg);
|
||||
|
||||
if (OK != ret) {
|
||||
perf_count(_comms_errors);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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()
|
||||
{
|
||||
/* reset the report ring and state machine */
|
||||
_collect_phase = false;
|
||||
_reports->flush();
|
||||
|
||||
/* schedule a cycle to start things */
|
||||
ScheduleNow();
|
||||
}
|
||||
|
||||
int
|
||||
QMC5883::reset()
|
||||
{
|
||||
/* read 0x00 once */
|
||||
uint8_t data_bits_in = 0;
|
||||
read_reg(QMC5883_ADDR_DATA_OUT_X_LSB, data_bits_in);
|
||||
|
||||
/* software reset */
|
||||
write_reg(QMC5883_ADDR_CONTROL_2, QMC5883_SOFT_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;
|
||||
|
||||
/* set control register */
|
||||
_conf_reg = QMC5883_MODE_REG_CONTINOUS_MODE |
|
||||
QMC5883_OUTPUT_DATA_RATE_200 |
|
||||
QMC5883_OVERSAMPLE_512 |
|
||||
QMC5883_OUTPUT_RANGE_2G;
|
||||
write_reg(QMC5883_ADDR_CONTROL_1, _conf_reg);
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
void
|
||||
QMC5883::RunImpl()
|
||||
{
|
||||
if (_measure_interval == 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
/* collection phase? */
|
||||
if (_collect_phase) {
|
||||
|
||||
/* perform collection */
|
||||
if (OK != collect()) {
|
||||
DEVICE_DEBUG("collection error");
|
||||
/* restart the measurement state machine */
|
||||
start();
|
||||
return;
|
||||
}
|
||||
|
||||
/* next phase is measurement */
|
||||
_collect_phase = false;
|
||||
|
||||
/*
|
||||
* Is there a collect->measure gap?
|
||||
*/
|
||||
if (_measure_interval > QMC5883_CONVERSION_INTERVAL) {
|
||||
/* schedule a fresh cycle call when we are ready to measure again */
|
||||
ScheduleDelayed(_measure_interval - QMC5883_CONVERSION_INTERVAL);
|
||||
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
/* next phase is collection */
|
||||
_collect_phase = true;
|
||||
|
||||
if (_measure_interval > 0) {
|
||||
/* schedule a fresh cycle call when the measurement is done */
|
||||
ScheduleDelayed(QMC5883_CONVERSION_INTERVAL);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
int
|
||||
QMC5883::collect()
|
||||
{
|
||||
struct { /* status register and data as read back from the device */
|
||||
uint8_t x[2];
|
||||
uint8_t y[2];
|
||||
uint8_t z[2];
|
||||
} qmc_report{};
|
||||
|
||||
struct {
|
||||
int16_t x, y, z;
|
||||
} report{};
|
||||
|
||||
int ret;
|
||||
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;
|
||||
|
||||
/*
|
||||
* @note We could read the status register here, which could tell us that
|
||||
* we were too early and that the output registers are still being
|
||||
* written. In the common case that would just slow us down, and
|
||||
* we're better off just never being early.
|
||||
*/
|
||||
|
||||
/* get measurements from the device */
|
||||
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");
|
||||
goto out;
|
||||
}
|
||||
|
||||
/* map data we just received LSB, MSB */
|
||||
report.x = (((int16_t)qmc_report.x[1]) << 8) + qmc_report.x[0];
|
||||
report.y = (((int16_t)qmc_report.y[1]) << 8) + qmc_report.y[0];
|
||||
report.z = (((int16_t)qmc_report.z[1]) << 8) + qmc_report.z[0];
|
||||
|
||||
/*
|
||||
* If any of the values are -4096, there was an internal math error in the sensor.
|
||||
* Generalise this to a simple range check that will also catch some bit errors.
|
||||
*/
|
||||
if ((abs(report.x) > QMC5883_MAX_COUNT) ||
|
||||
(abs(report.y) > QMC5883_MAX_COUNT) ||
|
||||
(abs(report.z) > QMC5883_MAX_COUNT)) {
|
||||
perf_count(_comms_errors);
|
||||
goto out;
|
||||
}
|
||||
|
||||
/* get temperature measurements from the device */
|
||||
new_report.temperature = 0;
|
||||
|
||||
if (_temperature_counter++ == 100) {
|
||||
uint8_t raw_temperature[2];
|
||||
|
||||
_temperature_counter = 0;
|
||||
|
||||
ret = _interface->read(QMC5883_ADDR_TEMP_OUT_LSB,
|
||||
raw_temperature, sizeof(raw_temperature));
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
} 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) {
|
||||
// convert onboard so it matches offboard for the
|
||||
// scaling below
|
||||
report.y = -report.y;
|
||||
report.x = -report.x;
|
||||
}
|
||||
|
||||
/* the standard external mag by 3DR has x pointing to the
|
||||
* right, y pointing backwards, and z down, therefore switch x
|
||||
* and y and invert y */
|
||||
//TODO: sort out axes mapping
|
||||
xraw_f = -report.y;
|
||||
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);
|
||||
|
||||
/*
|
||||
periodically check the configuration
|
||||
registers. With a bad I2C cable it is possible for the
|
||||
registers to become corrupt, leading to bad readings. It
|
||||
doesn't happen often, but given the poor cables some
|
||||
vehicles have it is worth checking for.
|
||||
*/
|
||||
check_counter = perf_event_count(_sample_perf) % 256;
|
||||
|
||||
|
||||
if (check_counter == 0) {
|
||||
check_conf();
|
||||
}
|
||||
|
||||
ret = OK;
|
||||
|
||||
out:
|
||||
perf_end(_sample_perf);
|
||||
return ret;
|
||||
}
|
||||
|
||||
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)
|
||||
{
|
||||
uint8_t buf = val;
|
||||
int ret = _interface->read(reg, &buf, 1);
|
||||
val = buf;
|
||||
return ret;
|
||||
}
|
||||
|
||||
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");
|
||||
}
|
||||
|
|
@ -0,0 +1,223 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2019 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file qmc5883.cpp
|
||||
*
|
||||
* Driver for the QMC5883 magnetometer connected via I2C.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#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 "qmc5883.h"
|
||||
|
||||
/*
|
||||
* QMC5883 internal constants and data structures.
|
||||
*/
|
||||
|
||||
/* Max measurement rate is 200Hz */
|
||||
#define QMC5883_CONVERSION_INTERVAL (1000000 / 150) /* microseconds */
|
||||
#define QMC5883_MAX_COUNT 32767
|
||||
|
||||
#define QMC5883_ADDR_DATA_OUT_X_LSB 0x00
|
||||
#define QMC5883_ADDR_DATA_OUT_X_MSB 0x01
|
||||
#define QMC5883_ADDR_DATA_OUT_Y_LSB 0x02
|
||||
#define QMC5883_ADDR_DATA_OUT_Y_MSB 0x03
|
||||
#define QMC5883_ADDR_DATA_OUT_Z_LSB 0x04
|
||||
#define QMC5883_ADDR_DATA_OUT_Z_MSB 0x05
|
||||
#define QMC5883_ADDR_STATUS 0x06
|
||||
#define QMC5883_ADDR_TEMP_OUT_LSB 0x07
|
||||
#define QMC5883_ADDR_TEMP_OUT_MSB 0x08
|
||||
#define QMC5883_ADDR_CONTROL_1 0x09
|
||||
#define QMC5883_ADDR_CONTROL_2 0x0A
|
||||
#define QMC5883_ADDR_SET_RESET 0x0B
|
||||
|
||||
|
||||
#define QMC5883_STATUS_REG_DRDY (1 << 0) /* Data Ready: "0": no new data, "1": new data is ready */
|
||||
#define QMC5883_STATUS_REG_OVL (1 << 1) /* Overflow Flag: "0": normal, "1": data overflow */
|
||||
#define QMC5883_STATUS_REG_DOR (1 << 2) /* Data Skip: "0": normal, "1": data skipped for reading */
|
||||
|
||||
/* Control Register 1 */
|
||||
#define QMC5883_MODE_REG_STANDBY (0 << 0)
|
||||
#define QMC5883_MODE_REG_CONTINOUS_MODE (1 << 0)
|
||||
#define QMC5883_OUTPUT_DATA_RATE_10 (0 << 2) /* Hz */
|
||||
#define QMC5883_OUTPUT_DATA_RATE_50 (1 << 2)
|
||||
#define QMC5883_OUTPUT_DATA_RATE_100 (2 << 2)
|
||||
#define QMC5883_OUTPUT_DATA_RATE_200 (3 << 2)
|
||||
#define QMC5883_OUTPUT_RANGE_2G (0 << 4) /* +/- 2 gauss */
|
||||
#define QMC5883_OUTPUT_RANGE_8G (1 << 4) /* +/- 8 gauss */
|
||||
#define QMC5883_OVERSAMPLE_512 (0 << 6) /* controls digital filter bw - larger OSR -> smaller bw */
|
||||
#define QMC5883_OVERSAMPLE_256 (1 << 6)
|
||||
#define QMC5883_OVERSAMPLE_128 (2 << 6)
|
||||
#define QMC5883_OVERSAMPLE_64 (3 << 6)
|
||||
|
||||
/* Control Register 2 */
|
||||
#define QMC5883_INT_ENB (1 << 0)
|
||||
#define QMC5883_ROL_PNT (1 << 6)
|
||||
#define QMC5883_SOFT_RESET (1 << 7)
|
||||
|
||||
/* Set Register */
|
||||
#define QMC5883_SET_DEFAULT (1 << 0)
|
||||
|
||||
#define QMC5883_TEMP_OFFSET 50 /* deg celsius */
|
||||
|
||||
class QMC5883 : public device::CDev, public I2CSPIDriver<QMC5883>
|
||||
{
|
||||
public:
|
||||
QMC5883(device::Device *interface, enum Rotation rotation, I2CSPIBusOption bus_option, int bus, int i2c_address);
|
||||
virtual ~QMC5883();
|
||||
|
||||
static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
|
||||
int runtime_instance);
|
||||
static void print_usage();
|
||||
|
||||
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;
|
||||
|
||||
protected:
|
||||
void print_status() override;
|
||||
|
||||
private:
|
||||
|
||||
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;
|
||||
|
||||
/**
|
||||
* Initialise the automatic measurement state machine and start it.
|
||||
*
|
||||
* @note This function is called at open and error time. It might make sense
|
||||
* to make it more aggressive about resetting the bus in case of errors.
|
||||
*/
|
||||
void start();
|
||||
|
||||
/**
|
||||
* Reset the device
|
||||
*/
|
||||
int reset();
|
||||
|
||||
/**
|
||||
* check the sensor configuration.
|
||||
*
|
||||
* checks that the config of the sensor is correctly set, to
|
||||
* cope with communication errors causing the configuration to
|
||||
* change
|
||||
*/
|
||||
void check_conf(void);
|
||||
|
||||
/**
|
||||
* Write a register.
|
||||
*
|
||||
* @param reg The register to write.
|
||||
* @param val The value to write.
|
||||
* @return OK on write success.
|
||||
*/
|
||||
int write_reg(uint8_t reg, uint8_t val);
|
||||
|
||||
/**
|
||||
* Read a register.
|
||||
*
|
||||
* @param reg The register to read.
|
||||
* @param val The value read.
|
||||
* @return OK on read success.
|
||||
*/
|
||||
int read_reg(uint8_t reg, uint8_t &val);
|
||||
|
||||
/**
|
||||
* Collect the result of the most recent measurement.
|
||||
*/
|
||||
int collect();
|
||||
};
|
||||
|
|
@ -37,34 +37,19 @@
|
|||
* I2C interface for QMC5883
|
||||
*/
|
||||
|
||||
/* XXX trim includes */
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <string.h>
|
||||
#include <assert.h>
|
||||
#include <debug.h>
|
||||
#include <errno.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include <drivers/device/i2c.h>
|
||||
#include <drivers/drv_mag.h>
|
||||
#include <drivers/drv_device.h>
|
||||
|
||||
#include "qmc5883.h"
|
||||
|
||||
#include "board_config.h"
|
||||
|
||||
#define QMC5883L_ADDRESS 0x0D
|
||||
|
||||
device::Device *QMC5883_I2C_interface(int bus);
|
||||
device::Device *QMC5883_I2C_interface(int bus, int bus_frequency, int i2c_address);
|
||||
|
||||
class QMC5883_I2C : public device::I2C
|
||||
{
|
||||
public:
|
||||
QMC5883_I2C(int bus);
|
||||
QMC5883_I2C(int bus, int bus_frequency, int i2c_address);
|
||||
virtual ~QMC5883_I2C() = default;
|
||||
|
||||
virtual int read(unsigned address, void *data, unsigned count);
|
||||
|
|
@ -78,13 +63,13 @@ protected:
|
|||
};
|
||||
|
||||
device::Device *
|
||||
QMC5883_I2C_interface(int bus)
|
||||
QMC5883_I2C_interface(int bus, int bus_frequency, int i2c_address)
|
||||
{
|
||||
return new QMC5883_I2C(bus);
|
||||
return new QMC5883_I2C(bus, bus_frequency, i2c_address);
|
||||
}
|
||||
|
||||
QMC5883_I2C::QMC5883_I2C(int bus) :
|
||||
I2C("QMC5883_I2C", nullptr, bus, QMC5883L_ADDRESS, 400000)
|
||||
QMC5883_I2C::QMC5883_I2C(int bus, int bus_frequency, int i2c_address) :
|
||||
I2C("QMC5883_I2C", nullptr, bus, i2c_address, bus_frequency)
|
||||
{
|
||||
_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_QMC5883;
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -39,6 +39,8 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <drivers/device/Device.hpp>
|
||||
|
||||
#define ADDR_ID_A 0x0C
|
||||
#define ADDR_ID_B 0x0D
|
||||
|
||||
|
|
@ -47,6 +49,4 @@
|
|||
|
||||
|
||||
/* interface factories */
|
||||
extern device::Device *QMC5883_SPI_interface(int bus);
|
||||
extern device::Device *QMC5883_I2C_interface(int bus);
|
||||
typedef device::Device *(*QMC5883_constructor)(int);
|
||||
extern device::Device *QMC5883_I2C_interface(int bus, int bus_frequency, int i2c_address);
|
||||
|
|
|
|||
|
|
@ -0,0 +1,133 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2019 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file qmc5883.cpp
|
||||
*
|
||||
* Driver for the QMC5883 magnetometer connected via I2C.
|
||||
*/
|
||||
|
||||
#include <px4_platform_common/getopt.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
|
||||
#include "QMC5883.hpp"
|
||||
#include "qmc5883.h"
|
||||
|
||||
extern "C" __EXPORT int qmc5883_main(int argc, char *argv[]);
|
||||
|
||||
I2CSPIDriverBase *QMC5883::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
|
||||
int runtime_instance)
|
||||
{
|
||||
device::Device *interface = nullptr;
|
||||
|
||||
if (iterator.busType() == BOARD_I2C_BUS) {
|
||||
interface = QMC5883_I2C_interface(iterator.bus(), cli.bus_frequency, cli.i2c_address);
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
QMC5883 *dev = new QMC5883(interface, cli.rotation, iterator.configuredBusOption(), iterator.bus(), cli.i2c_address);
|
||||
|
||||
if (dev == nullptr) {
|
||||
delete interface;
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
if (OK != dev->init()) {
|
||||
delete dev;
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
return dev;
|
||||
}
|
||||
|
||||
void QMC5883::print_usage()
|
||||
{
|
||||
PRINT_MODULE_USAGE_NAME("qmc5883", "driver");
|
||||
PRINT_MODULE_USAGE_SUBCATEGORY("magnetometer");
|
||||
PRINT_MODULE_USAGE_COMMAND("start");
|
||||
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
|
||||
PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x0D);
|
||||
PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
|
||||
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
||||
}
|
||||
|
||||
extern "C" int qmc5883_main(int argc, char *argv[])
|
||||
{
|
||||
using ThisDriver = QMC5883;
|
||||
int ch;
|
||||
BusCLIArguments cli{true, false};
|
||||
cli.i2c_address = ADDR_ID_B;
|
||||
cli.default_i2c_frequency = 400000;
|
||||
|
||||
while ((ch = cli.getopt(argc, argv, "R:")) != EOF) {
|
||||
switch (ch) {
|
||||
case 'R':
|
||||
cli.rotation = (enum Rotation)atoi(cli.optarg());
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
const char *verb = cli.optarg();
|
||||
|
||||
if (!verb) {
|
||||
ThisDriver::print_usage();
|
||||
return -1;
|
||||
}
|
||||
|
||||
BusInstanceIterator iterator(MODULE_NAME, cli, DRV_MAG_DEVTYPE_QMC5883);
|
||||
|
||||
if (!strcmp(verb, "start")) {
|
||||
return ThisDriver::module_start(cli, iterator);
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "stop")) {
|
||||
return ThisDriver::module_stop(iterator);
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "status")) {
|
||||
return ThisDriver::module_status(iterator);
|
||||
}
|
||||
|
||||
ThisDriver::print_usage();
|
||||
return -1;
|
||||
}
|
||||
|
|
@ -1,183 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2019 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file QMC5883_SPI.cpp
|
||||
*
|
||||
* SPI interface for HMC5983
|
||||
*/
|
||||
|
||||
/* XXX trim includes */
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <string.h>
|
||||
#include <assert.h>
|
||||
#include <debug.h>
|
||||
#include <errno.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include <drivers/device/spi.h>
|
||||
#include <drivers/drv_mag.h>
|
||||
#include <drivers/drv_device.h>
|
||||
|
||||
#include "qmc5883.h"
|
||||
#include <board_config.h>
|
||||
|
||||
#ifdef PX4_SPIDEV_HMC
|
||||
|
||||
/* SPI protocol address bits */
|
||||
#define DIR_READ (1<<7)
|
||||
#define DIR_WRITE (0<<7)
|
||||
#define ADDR_INCREMENT (1<<6)
|
||||
|
||||
#define HMC_MAX_SEND_LEN 4
|
||||
#define HMC_MAX_RCV_LEN 8
|
||||
|
||||
device::Device *QMC5883_SPI_interface(int bus);
|
||||
|
||||
class QMC5883_SPI : public device::SPI
|
||||
{
|
||||
public:
|
||||
QMC5883_SPI(int bus, uint32_t device);
|
||||
virtual ~QMC5883_SPI() = default;
|
||||
|
||||
virtual int init();
|
||||
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);
|
||||
|
||||
};
|
||||
|
||||
device::Device *
|
||||
QMC5883_SPI_interface(int bus)
|
||||
{
|
||||
return new QMC5883_SPI(bus, PX4_SPIDEV_HMC);
|
||||
}
|
||||
|
||||
QMC5883_SPI::QMC5883_SPI(int bus, uint32_t device) :
|
||||
SPI("QMC5883_SPI", nullptr, bus, device, SPIDEV_MODE3, 11 * 1000 * 1000 /* will be rounded to 10.4 MHz */)
|
||||
{
|
||||
_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_QMC5883;
|
||||
}
|
||||
|
||||
int
|
||||
QMC5883_SPI::init()
|
||||
{
|
||||
int ret;
|
||||
|
||||
ret = SPI::init();
|
||||
|
||||
if (ret != OK) {
|
||||
DEVICE_DEBUG("SPI init failed");
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
// read WHO_AM_I value
|
||||
uint8_t data[2] = {0, 0};
|
||||
|
||||
if (read(ADDR_ID_A, &data[0], 1) ||
|
||||
read(ADDR_ID_B, &data[1], 1)) {
|
||||
DEVICE_DEBUG("read_reg fail");
|
||||
}
|
||||
|
||||
if ((data[0] != ID_A_WHO_AM_I) ||
|
||||
(data[1] != ID_B_WHO_AM_I)) {
|
||||
DEVICE_DEBUG("ID byte mismatch (%02x,%02x)", data[0], data[1]);
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
int
|
||||
QMC5883_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
|
||||
QMC5883_SPI::write(unsigned address, void *data, unsigned count)
|
||||
{
|
||||
uint8_t buf[32];
|
||||
|
||||
if (sizeof(buf) < (count + 1)) {
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
buf[0] = address | DIR_WRITE;
|
||||
memcpy(&buf[1], data, count);
|
||||
|
||||
return transfer(&buf[0], &buf[0], count + 1);
|
||||
}
|
||||
|
||||
int
|
||||
QMC5883_SPI::read(unsigned address, void *data, unsigned count)
|
||||
{
|
||||
uint8_t buf[32];
|
||||
|
||||
if (sizeof(buf) < (count + 1)) {
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
buf[0] = address | DIR_READ | ADDR_INCREMENT;
|
||||
|
||||
int ret = transfer(&buf[0], &buf[0], count + 1);
|
||||
memcpy(data, &buf[1], count);
|
||||
return ret;
|
||||
}
|
||||
|
||||
#endif /* PX4_SPIDEV_HMC */
|
||||
Loading…
Reference in New Issue