yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

PNI RM3100 magnetometer driver (#10302)

Browse Source 
* tested on SPI (px4fmu-v4pro)
 * WIP I2C support
This commit is contained in:
Kevin Lopez Alvarez
2018-08-28 17:44:34 +02:00
committed by Daniel Agar
parent 124a34e8f6
commit c10ea132b4
11 changed files with 1790 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
ROMFS/px4fmu_common/init.d/rc.sensors
View File

@@ -322,6 +322,9 @@ then
# Possible external compasses
hmc5883 -C -T -X start
#RM3100
rm3100 start
fi
if ver hwcmp PX4FMU_V5

4
src/drivers/boards/px4fmu-v4pro/board_config.h
View File

@@ -153,6 +153,8 @@
#define PX4_SPI_BUS_EXT0 5
#define PX4_SPI_BUS_EXT1 6
#define PX4_SPI_BUS_EXT PX4_SPI_BUS_EXT0
/* Use these in place of the uint32_t enumeration to select a specific SPI device on SPI1 */
#define PX4_SPIDEV_GYRO PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 1)
@@ -171,6 +173,8 @@
#define PX4_SPIDEV_EXT0 PX4_MK_SPI_SEL(PX4_SPI_BUS_EXT0, 1)
#define PX4_SPIDEV_EXT1 PX4_MK_SPI_SEL(PX4_SPI_BUS_EXT1, 1)
#define PX4_SPIDEV_RM_EXT PX4_SPIDEV_EXT0
/* I2C busses */
#define PX4_I2C_BUS_ONBOARD 1
#define PX4_I2C_BUS_EXPANSION 2

1
src/drivers/drv_sensor.h
View File

@@ -58,6 +58,7 @@
#define DRV_MAG_DEVTYPE_MPU9250 0x04
#define DRV_MAG_DEVTYPE_LIS3MDL 0x05
#define DRV_MAG_DEVTYPE_IST8310 0x06
#define DRV_MAG_DEVTYPE_RM3100 0x07
#define DRV_ACC_DEVTYPE_LSM303D 0x11
#define DRV_ACC_DEVTYPE_BMA180 0x12
#define DRV_ACC_DEVTYPE_MPU6000 0x13

1
src/drivers/magnetometer/CMakeLists.txt
View File

@@ -36,3 +36,4 @@ add_subdirectory(hmc5883)
add_subdirectory(ist8310)
add_subdirectory(lis3mdl)
add_subdirectory(lsm303agr)
add_subdirectory(rm3100)

44
src/drivers/magnetometer/rm3100/CMakeLists.txt Normal file
View File

@@ -0,0 +1,44 @@
############################################################################
#
# Copyright (c) 2018 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.
#
############################################################################
px4_add_module(
MODULE drivers__rm3100
MAIN rm3100
STACK_MAIN 1200
COMPILE_FLAGS
SRCS
rm3100_i2c.cpp
rm3100_spi.cpp
rm3100_main.cpp
rm3100.cpp
DEPENDS
)

662
src/drivers/magnetometer/rm3100/rm3100.cpp Normal file
View File

@@ -0,0 +1,662 @@
/****************************************************************************
*
* Copyright (c) 2018 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 rm3100.cpp
*
* Driver for the RM3100 magnetometer connected via I2C or SPI.
*
* Based on the lis3mdl driver.
*/
#include "rm3100.h"
RM3100::RM3100(device::Device *interface, const char *path, enum Rotation rotation) :
CDev("RM3100", path),
_interface(interface),
_work{},
_reports(nullptr),
_scale{},
_last_report{},
_mag_topic(nullptr),
_comms_errors(perf_alloc(PC_COUNT, "rm3100_comms_errors")),
_conf_errors(perf_alloc(PC_COUNT, "rm3100_conf_errors")),
_range_errors(perf_alloc(PC_COUNT, "rm3100_range_errors")),
_sample_perf(perf_alloc(PC_ELAPSED, "rm3100_read")),
_calibrated(false),
_continuous_mode_set(false),
_mode(SINGLE),
_rotation(rotation),
_measure_ticks(0),
_class_instance(-1),
_orb_class_instance(-1),
_range_scale(1.0f / (RM3100_SENSITIVITY * UTESLA_TO_GAUSS)),
_check_state_cnt(0)
{
// set the device type from the interface
_device_id.devid_s.bus_type = _interface->get_device_bus_type();
_device_id.devid_s.bus = _interface->get_device_bus();
_device_id.devid_s.address = _interface->get_device_address();
_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_RM3100;
// enable debug() calls
_debug_enabled = false;
// 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;
// work_cancel in the dtor will explode if we don't do this...
memset(&_work, 0, sizeof(_work));
}
RM3100::~RM3100()
{
/* make sure we are truly inactive */
stop();
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
RM3100::self_test()
{
/* Stop current measurements */
stop();
/* Chances are that a poll event was triggered, so wait for conversion and read registers in order to clear DRDY bit */
usleep(RM3100_CONVERSION_INTERVAL);
collect();
/* Fail if calibration is not good */
int ret = 0;
uint8_t cmd = 0;
/* Configure mag into self test mode */
cmd = BIST_SELFTEST;
ret = _interface->write(ADDR_BIST, &cmd, 1);
if (ret != PX4_OK) {
return ret;
}
/* Now we need to write to POLL to launch self test */
cmd = POLL_XYZ;
ret = _interface->write(ADDR_POLL, &cmd, 1);
if (ret != PX4_OK) {
return ret;
}
/* Now wait for status register */
usleep(RM3100_CONVERSION_INTERVAL);
if (check_measurement() != PX4_OK) {
return -1;;
}
/* Now check BIST register to see whether self test is ok or not*/
ret = _interface->read(ADDR_BIST, &cmd, 1);
if (ret != PX4_OK) {
return ret;
}
ret = !((cmd & BIST_XYZ_OK) == BIST_XYZ_OK);
/* Restart measurement state machine */
start();
return ret;
}
int
RM3100::check_measurement()
{
uint8_t status = 0;
int ret = -1;
ret = _interface->read(ADDR_STATUS, &status, 1);
if (ret != 0) {
return ret;
}
return !((status & STATUS_DRDY) == STATUS_DRDY) ;
}
int
RM3100::collect()
{
/* Check whether a measurement is available or not, otherwise return immediately */
if (check_measurement() != 0) {
DEVICE_DEBUG("No measurement available");
return 0;
}
#pragma pack(push, 1)
struct {
uint8_t x[3];
uint8_t y[3];
uint8_t z[3];
} rm_report;
#pragma pack(pop)
int ret = 0;
int32_t xraw;
int32_t yraw;
int32_t zraw;
float xraw_f;
float yraw_f;
float zraw_f;
struct mag_report new_mag_report;
bool sensor_is_onboard = false;
perf_begin(_sample_perf);
new_mag_report.timestamp = hrt_absolute_time();
new_mag_report.error_count = perf_event_count(_comms_errors);
new_mag_report.scaling = _range_scale;
new_mag_report.device_id = _device_id.devid;
ret = _interface->read(ADDR_MX, (uint8_t *)&rm_report, sizeof(rm_report));
if (ret != OK) {
perf_count(_comms_errors);
PX4_WARN("Register read error.");
return ret;
}
/* Rearrange mag data */
xraw = ((rm_report.x[0] << 16) | (rm_report.x[1] << 8) | rm_report.x[2]);
yraw = ((rm_report.y[0] << 16) | (rm_report.y[1] << 8) | rm_report.y[2]);
zraw = ((rm_report.z[0] << 16) | (rm_report.z[1] << 8) | rm_report.z[2]);
/* Convert 24 bit signed values to 32 bit signed values */
convert_signed(&xraw);
convert_signed(&yraw);
convert_signed(&zraw);
/* There is no temperature sensor */
new_mag_report.temperature = 0.0f;
// XXX revisit for SPI part, might require a bus type IOCTL
unsigned dummy = 0;
sensor_is_onboard = !_interface->ioctl(MAGIOCGEXTERNAL, dummy);
new_mag_report.is_external = !sensor_is_onboard;
/**
* RAW outputs
* As we only have 16 bits to store raw data, the following values are not correct
*/
new_mag_report.x_raw = (int16_t)(xraw >> 8);
new_mag_report.y_raw = (int16_t)(yraw >> 8);
new_mag_report.z_raw = (int16_t)(zraw >> 8);
xraw_f = xraw;
yraw_f = yraw;
zraw_f = zraw;
/* apply user specified rotation */
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
new_mag_report.x = ((xraw_f * _range_scale) - _scale.x_offset) * _scale.x_scale;
new_mag_report.y = ((yraw_f * _range_scale) - _scale.y_offset) * _scale.y_scale;
new_mag_report.z = ((zraw_f * _range_scale) - _scale.z_offset) * _scale.z_scale;
if (!(_pub_blocked)) {
if (_mag_topic != nullptr) {
/* publish it */
orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_mag_report);
} else {
_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &new_mag_report,
&_orb_class_instance, (sensor_is_onboard) ? ORB_PRIO_HIGH : ORB_PRIO_MAX);
if (_mag_topic == nullptr) {
DEVICE_DEBUG("ADVERT FAIL");
}
}
}
_last_report = new_mag_report;
/* post a report to the ring */
_reports->force(&new_mag_report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
ret = OK;
perf_end(_sample_perf);
return ret;
}
void
RM3100::convert_signed(int32_t *n)
{
/* Sensor returns values as 24 bit signed values, so we need to manually convert to 32 bit signed values */
if ((*n & (1 << 23)) == (1 << 23)) {
*n |= 0xFF000000;
}
}
void
RM3100::cycle()
{
/* _measure_ticks == 0 is used as _task_should_exit */
if (_measure_ticks == 0) {
return;
}
/* Collect last measurement at the start of every cycle */
if (collect() != OK) {
DEVICE_DEBUG("collection error");
/* restart the measurement state machine */
start();
return;
}
if (measure() != OK) {
DEVICE_DEBUG("measure error");
}
if (_measure_ticks > 0) {
/* schedule a fresh cycle call when the measurement is done */
work_queue(HPWORK,
&_work,
(worker_t)&RM3100::cycle_trampoline,
this,
_measure_ticks);
}
}
void
RM3100::cycle_trampoline(void *arg)
{
RM3100 *dev = (RM3100 *)arg;
dev->cycle();
}
int
RM3100::init()
{
int ret = PX4_ERROR;
ret = CDev::init();
if (ret != OK) {
DEVICE_DEBUG("CDev init failed");
return ret;
}
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report));
if (_reports == nullptr) {
return PX4_ERROR;
}
/* reset the device configuration */
reset();
_class_instance = register_class_devname(MAG_BASE_DEVICE_PATH);
ret = self_test();
if (ret != PX4_OK) {
PX4_ERR("self test failed");
}
return ret;
}
int
RM3100::ioctl(struct file *file_pointer, int cmd, unsigned long arg)
{
unsigned dummy = 0;
switch (cmd) {
case SENSORIOCSPOLLRATE: {
switch (arg) {
/* switching to manual polling */
case SENSOR_POLLRATE_MANUAL:
stop();
_measure_ticks = 0;
return PX4_OK;
/* zero would be bad */
case 0:
return -EINVAL;
case SENSOR_POLLRATE_DEFAULT: {
/* do we need to start internal polling? */
bool not_started = (_measure_ticks == 0);
/* set interval for next measurement to minimum legal value */
_measure_ticks = USEC2TICK(RM3100_CONVERSION_INTERVAL);
/* if we need to start the poll state machine, do it */
if (not_started) {
start();
}
return PX4_OK;
}
/* Uses arg (hz) for a custom poll rate */
default: {
/* do we need to start internal polling? */
bool not_started = (_measure_ticks == 0);
/* convert hz to tick interval via microseconds */
unsigned ticks = USEC2TICK(1000000 / arg);
/* check against maximum rate */
if (ticks < USEC2TICK(RM3100_CONVERSION_INTERVAL)) {
return -EINVAL;
}
/* update interval for next measurement */
_measure_ticks = ticks;
/* if we need to start the poll state machine, do it */
if (not_started) {
start();
}
return PX4_OK;
}
}
}
case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100)) {
return -EINVAL;
}
irqstate_t flags = px4_enter_critical_section();
if (!_reports->resize(arg)) {
px4_leave_critical_section(flags);
return -ENOMEM;
}
px4_leave_critical_section(flags);
return PX4_OK;
}
case SENSORIOCRESET:
return reset();
case MAGIOCSSAMPLERATE:
/* same as pollrate because device is in single measurement mode*/
return ioctl(file_pointer, SENSORIOCSPOLLRATE, arg);
case MAGIOCGSAMPLERATE:
/* same as pollrate because device is in single measurement mode*/
return 1000000 / TICK2USEC(_measure_ticks);
case MAGIOCSRANGE:
/* field measurement range cannot be configured for this sensor (8 Gauss) */
return OK;
case MAGIOCGRANGE:
/* field measurement range cannot be configured for this sensor (8 Gauss) */
return 8;
case MAGIOCSSCALE:
/* set new scale factors */
memcpy(&_scale, (struct mag_calibration_s *)arg, sizeof(_scale));
return 0;
case MAGIOCGSCALE:
/* copy out scale factors */
memcpy((struct mag_calibration_s *)arg, &_scale, sizeof(_scale));
return 0;
case MAGIOCCALIBRATE:
/* This is left for compatibility with the IOCTL call in mag calibration */
return OK;
case MAGIOCGEXTERNAL:
DEVICE_DEBUG("MAGIOCGEXTERNAL in main driver");
return _interface->ioctl(cmd, dummy);
case DEVIOCGDEVICEID:
return _interface->ioctl(cmd, dummy);
default:
/* give it to the superclass */
return CDev::ioctl(file_pointer, cmd, arg);
}
}
int
RM3100::measure()
{
int ret = 0;
uint8_t cmd = 0;
/* Send the command to begin a measurement. */
if ((_mode == CONTINUOUS) && !_continuous_mode_set) {
cmd = (CMM_DEFAULT | CONTINUOUS_MODE);
ret = _interface->write(ADDR_CMM, &cmd, 1);
_continuous_mode_set = true;
} else if (_mode == SINGLE) {
if (_continuous_mode_set) {
/* This is needed for polling mode */
cmd = (CMM_DEFAULT | POLLING_MODE);
ret = _interface->write(ADDR_CMM, &cmd, 1);
if (ret != OK) {
perf_count(_comms_errors);
return ret;
}
_continuous_mode_set = false;
}
cmd = POLL_XYZ;
ret = _interface->write(ADDR_POLL, &cmd, 1);
}
if (ret != OK) {
perf_count(_comms_errors);
}
return ret;
}
void
RM3100::print_info()
{
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
PX4_INFO("poll interval: %u ticks", _measure_ticks);
print_message(_last_report);
_reports->print_info("report queue");
}
int
RM3100::reset()
{
int ret = 0;
ret = set_default_register_values();
if (ret != OK) {
return PX4_ERROR;
}
return PX4_OK;
}
int
RM3100::read(struct file *file_pointer, char *buffer, size_t buffer_len)
{
unsigned count = buffer_len / sizeof(struct mag_report);
struct mag_report *mag_buf = reinterpret_cast<struct mag_report *>(buffer);
int ret = 0;
/* buffer must be large enough */
if (count < 1) {
return -ENOSPC;
}
/* if automatic measurement is enabled */
if (_measure_ticks > 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(struct mag_report);
mag_buf++;
}
}
/* if there was no data, warn the caller */
return ret ? ret : -EAGAIN;
}
/* manual measurement - run one conversion */
/* XXX really it'd be nice to lock against other readers here */
do {
_reports->flush();
/* trigger a measurement */
if (measure() != OK) {
ret = -EIO;
break;
}
/* wait for it to complete */
usleep(RM3100_CONVERSION_INTERVAL);
/* run the collection phase */
if (collect() != OK) {
ret = -EIO;
break;
}
if (_reports->get(mag_buf)) {
ret = sizeof(struct mag_report);
}
} while (0);
return ret;
}
int
RM3100::set_default_register_values()
{
uint8_t cmd[2] = {0, 0};
cmd[0] = CCX_DEFAULT_MSB;
cmd[1] = CCX_DEFAULT_LSB;
_interface->write(ADDR_CCX, cmd, 2);
cmd[0] = CCY_DEFAULT_MSB;
cmd[1] = CCY_DEFAULT_LSB;
_interface->write(ADDR_CCY, cmd, 2);
cmd[0] = CCZ_DEFAULT_MSB;
cmd[1] = CCZ_DEFAULT_LSB;
_interface->write(ADDR_CCZ, cmd, 2);
cmd[0] = CMM_DEFAULT;
_interface->write(ADDR_CMM, cmd, 1);
cmd[0] = TMRC_DEFAULT;
_interface->write(ADDR_TMRC, cmd, 1);
cmd[0] = BIST_DEFAULT;
_interface->write(ADDR_BIST, cmd, 1);
return PX4_OK;
}
void
RM3100::start()
{
/* reset the report ring and state machine */
_reports->flush();
set_default_register_values();
_measure_ticks = USEC2TICK(RM3100_CONVERSION_INTERVAL);
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&RM3100::cycle_trampoline, this, 1);
}
void
RM3100::stop()
{
if (_measure_ticks > 0) {
/* ensure no new items are queued while we cancel this one */
_measure_ticks = 0;
work_cancel(HPWORK, &_work);
}
}

252
src/drivers/magnetometer/rm3100/rm3100.h Normal file
View File

@@ -0,0 +1,252 @@
/****************************************************************************
*
* Copyright (c) 2018 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 rm3100.h
*
* Shared defines for the RM3100 driver.
*/
#pragma once
#include <float.h>
#include <drivers/device/i2c.h>
#include <drivers/device/ringbuffer.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_mag.h>
#include <lib/conversion/rotation.h>
#include <perf/perf_counter.h>
#include <px4_defines.h>
#ifndef CONFIG_SCHED_WORKQUEUE
# error This requires CONFIG_SCHED_WORKQUEUE.
#endif
/**
* RM3100 internal constants and data structures.
*/
#define RM3100_CONVERSION_INTERVAL 6850 // Microseconds, corresponds to 146 Hz (cycle count 200 on 3 axis)
#define UTESLA_TO_GAUSS 100.0f
#define RM3100_SENSITIVITY 75.0f
#define ADDR_POLL 0x00
#define ADDR_CMM 0x01
#define ADDR_CCX 0x04
#define ADDR_CCY 0x06
#define ADDR_CCZ 0x08
#define ADDR_TMRC 0x0B
#define ADDR_MX 0x24
#define ADDR_MY 0x27
#define ADDR_MZ 0x2A
#define ADDR_BIST 0x33
#define ADDR_STATUS 0x34
#define ADDR_HSHAKE 0x35
#define ADDR_REVID 0x36
#define CCX_DEFAULT_MSB 0x00
#define CCX_DEFAULT_LSB 0xC8
#define CCY_DEFAULT_MSB CCX_DEFAULT_MSB
#define CCY_DEFAULT_LSB CCX_DEFAULT_LSB
#define CCZ_DEFAULT_MSB CCX_DEFAULT_MSB
#define CCZ_DEFAULT_LSB CCX_DEFAULT_LSB
#define CMM_DEFAULT 0x70 // No continuous mode
#define CONTINUOUS_MODE (1 << 0)
#define POLLING_MODE (0 << 0)
#define TMRC_DEFAULT 0x94
#define BIST_SELFTEST 0x8F
#define BIST_DEFAULT 0x00
#define BIST_XYZ_OK ((1 << 4) | (1 << 5) | (1 << 6))
#define STATUS_DRDY (1 << 7)
#define POLL_XYZ 0x70
#define RM3100_REVID 0x22
#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0]))
/* interface factories */
extern device::Device *RM3100_SPI_interface(int bus);
extern device::Device *RM3100_I2C_interface(int bus);
typedef device::Device *(*RM3100_constructor)(int);
enum RM3100_BUS {
RM3100_BUS_ALL = 0,
RM3100_BUS_I2C_INTERNAL,
RM3100_BUS_I2C_EXTERNAL,
RM3100_BUS_SPI_INTERNAL,
RM3100_BUS_SPI_EXTERNAL
};
enum OPERATING_MODE {
CONTINUOUS = 0,
SINGLE
};
class RM3100 : public device::CDev
{
public:
RM3100(device::Device *interface, const char *path, enum Rotation rotation);
virtual ~RM3100();
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);
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info();
/**
* Configures the device with default register values.
*/
int set_default_register_values();
/**
* Stop the automatic measurement state machine.
*/
void stop();
protected:
Device *_interface;
private:
work_s _work;
ringbuffer::RingBuffer *_reports;
struct mag_calibration_s _scale;
struct mag_report _last_report {}; /**< used for info() */
orb_advert_t _mag_topic;
perf_counter_t _comms_errors;
perf_counter_t _conf_errors;
perf_counter_t _range_errors;
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_ticks;
int _class_instance;
int _orb_class_instance;
float _range_scale;
uint8_t _check_state_cnt;
/**
* Collect the result of the most recent measurement.
*/
int collect();
/**
* Run sensor self-test
*
* @return 0 if self-test is ok, 1 else
*/
int self_test();
/**
* Check whether new data is available or not
*
* @return 0 if new data is available, 1 else
*/
int check_measurement();
/**
* Converts int24_t stored in 32-bit container to int32_t
*/
void convert_signed(int32_t *n);
/**
* @brief Performs a poll cycle; collect from the previous measurement
* and start a new one.
*
* This is the heart of the measurement state machine. This function
* alternately starts a measurement, or collects the data from the
* previous measurement.
*
* When the interval between measurements is greater than the minimum
* measurement interval, a gap is inserted between collection
* and measurement to provide the most recent measurement possible
* at the next interval.
*/
void cycle();
/**
* @brief Static trampoline from the workq context; because we don't have a
* generic workq wrapper yet.
*
* @param arg Instance pointer for the driver that is polling.
*/
static void cycle_trampoline(void *arg);
/**
* Issue a measurement command.
*
* @return OK if the measurement command was successful.
*/
int measure();
/**
* @brief Resets the device
*/
int reset();
/**
* @brief Initialises 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();
/* this class has pointer data members, do not allow copying it */
RM3100(const RM3100 &);
RM3100 operator=(const RM3100 &);
}; // class RM3100

165
src/drivers/magnetometer/rm3100/rm3100_i2c.cpp Normal file
View File

@@ -0,0 +1,165 @@
/****************************************************************************
*
* Copyright (c) 2018 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 rm3100_i2c.cpp
*
* I2C interface for RM3100
*/
#include <px4_config.h>
#include <assert.h>
#include <debug.h>
#include <errno.h>
#include <stdint.h>
#include <stdbool.h>
#include <string.h>
#include <sys/types.h>
#include <unistd.h>
#include <arch/board/board.h>
#include <drivers/device/i2c.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_device.h>
#include "board_config.h"
#include "rm3100.h"
#if defined(PX4_I2C_BUS_ONBOARD) || defined(PX4_I2C_BUS_EXPANSION)
#define RM3100_ADDRESS 0x20
#define DIR_READ (1<<7)
#define DIR_WRITE (0<<7)
class RM3100_I2C : public device::I2C
{
public:
RM3100_I2C(int bus);
virtual ~RM3100_I2C() = 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);
protected:
virtual int probe();
};
device::Device *
RM3100_I2C_interface(int bus);
device::Device *
RM3100_I2C_interface(int bus)
{
return new RM3100_I2C(bus);
}
RM3100_I2C::RM3100_I2C(int bus) :
I2C("RM300_I2C", nullptr, bus, RM3100_ADDRESS, 400000)
{
_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_RM3100;
}
int
RM3100_I2C::init()
{
/* this will call probe() */
return I2C::init();
}
int
RM3100_I2C::ioctl(unsigned operation, unsigned &arg)
{
switch (operation) {
case MAGIOCGEXTERNAL:
return external();
case DEVIOCGDEVICEID:
return CDev::ioctl(nullptr, operation, arg);
default:
return -EINVAL;
}
}
int
RM3100_I2C::probe()
{
uint8_t data = 0;
_retries = 10;
if (read(ADDR_REVID, &data, 1)) {
DEVICE_DEBUG("RM3100 read_reg fail");
return -EIO;
}
_retries = 2;
if (data != RM3100_REVID) {
DEVICE_DEBUG("RM3100 bad ID: %02x", data);
return -EIO;
}
return OK;
}
int
RM3100_I2C::read(unsigned address, void *data, unsigned count)
{
uint8_t cmd = address | DIR_READ;
return transfer(&cmd, 1, (uint8_t *)data, count);
}
int
RM3100_I2C::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], count + 1, nullptr, 0);
}
#endif /* PX4_I2C_OBDEV_RM3100 */

360
src/drivers/magnetometer/rm3100/rm3100_main.cpp Normal file
View File

@@ -0,0 +1,360 @@
/****************************************************************************
*
* Copyright (c) 2018 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 rm3100_main.cpp
*
* Driver for the RM3100 magnetometer connected via I2C or SPI.
*/
#include "rm3100_main.h"
#include <px4_getopt.h>
/**
* Driver 'main' command.
*/
extern "C" __EXPORT int rm3100_main(int argc, char *argv[]);
int
rm3100::info(RM3100_BUS bus_id)
{
struct rm3100_bus_option &bus = find_bus(bus_id);
PX4_WARN("running on bus: %u (%s)\n", (unsigned)bus.bus_id, bus.devpath);
bus.dev->print_info();
return 1;
}
bool
rm3100::init(RM3100_BUS bus_id)
{
struct rm3100_bus_option &bus = find_bus(bus_id);
const char *path = bus.devpath;
int fd = open(path, O_RDONLY);
if (fd < 0) {
return false;
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
close(fd);
errx(1, "Failed to setup poll rate");
return false;
} else {
PX4_INFO("Poll rate set to max (146 Hz)");
}
close(fd);
return true;
}
bool
rm3100::start_bus(struct rm3100_bus_option &bus, Rotation rotation)
{
if (bus.dev != nullptr) {
errx(1, "bus option already started");
return false;
}
device::Device *interface = bus.interface_constructor(bus.busnum);
if (interface->init() != OK) {
delete interface;
warnx("no device on bus %u", (unsigned)bus.bus_id);
return false;
}
bus.dev = new RM3100(interface, bus.devpath, rotation);
if (bus.dev != nullptr &&
bus.dev->init() != OK) {
delete bus.dev;
bus.dev = NULL;
return false;
}
return true;
}
int
rm3100::start(RM3100_BUS bus_id, Rotation rotation)
{
bool started = false;
for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
if (bus_id == RM3100_BUS_ALL && bus_options[i].dev != NULL) {
// this device is already started
continue;
}
if (bus_id != RM3100_BUS_ALL && bus_options[i].bus_id != bus_id) {
// not the one that is asked for
continue;
}
started |= start_bus(bus_options[i], rotation);
//init(bus_id);
}
return started;
}
int
rm3100::stop()
{
bool stopped = false;
for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
if (bus_options[i].dev != nullptr) {
bus_options[i].dev->stop();
delete bus_options[i].dev;
bus_options[i].dev = nullptr;
stopped = true;
}
}
return !stopped;
}
bool
rm3100::test(RM3100_BUS bus_id)
{
struct rm3100_bus_option &bus = find_bus(bus_id);
struct mag_report report;
ssize_t sz;
int ret;
const char *path = bus.devpath;
int fd = open(path, O_RDONLY);
if (fd < 0) {
PX4_WARN("%s open failed (try 'rm3100 start')", path);
return 1;
}
/* do a simple demand read */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
PX4_WARN("immediate read failed");
return 1;
}
print_message(report);
/* check if mag is onboard or external */
if (ioctl(fd, MAGIOCGEXTERNAL, 0) < 0) {
PX4_WARN("failed to get if mag is onboard or external");
return 1;
}
/* set the queue depth to 5 */
if (ioctl(fd, SENSORIOCSQUEUEDEPTH, 10) != OK) {
PX4_WARN("failed to set queue depth");
return 1;
}
/* start the sensor polling at 2Hz */
if (ioctl(fd, SENSORIOCSPOLLRATE, 2) != OK) {
PX4_WARN("failed to set 2Hz poll rate");
return 1;
}
struct pollfd fds;
/* read the sensor 5x and report each value */
for (unsigned i = 0; i < 5; i++) {
/* wait for data to be ready */
fds.fd = fd;
fds.events = POLLIN;
ret = poll(&fds, 1, 2000);
if (ret != 1) {
PX4_WARN("timed out waiting for sensor data");
return 1;
}
/* now go get it */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
PX4_WARN("periodic read failed");
return 1;
}
print_message(report);
}
PX4_INFO("PASS");
return 1;
}
bool
rm3100::reset(RM3100_BUS bus_id)
{
struct rm3100_bus_option &bus = find_bus(bus_id);
const char *path = bus.devpath;
int fd = open(path, O_RDONLY);
if (fd < 0) {
PX4_WARN("open failed ");
return 1;
}
if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
PX4_WARN("driver reset failed");
return 1;
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
PX4_WARN("driver poll restart failed");
return 1;
}
return 0;
}
void
rm3100::usage()
{
PX4_WARN("missing command: try 'start', 'info', 'test', 'reset', 'info'");
PX4_WARN("options:");
PX4_WARN(" -R rotation");
PX4_WARN(" -X external I2C bus");
PX4_WARN(" -I internal I2C bus");
PX4_WARN(" -S external SPI bus");
PX4_WARN(" -s internal SPI bus");
}
int
rm3100_main(int argc, char *argv[])
{
int myoptind = 1;
int ch;
const char *myoptarg = nullptr;
enum RM3100_BUS bus_id = RM3100_BUS_ALL;
enum Rotation rotation = ROTATION_NONE;
while ((ch = px4_getopt(argc, argv, "XISR:T", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'R':
rotation = (enum Rotation)atoi(myoptarg);
break;
case 'I':
bus_id = RM3100_BUS_I2C_INTERNAL;
break;
case 'X':
bus_id = RM3100_BUS_I2C_EXTERNAL;
break;
case 'S':
bus_id = RM3100_BUS_SPI_EXTERNAL;
break;
case 's':
bus_id = RM3100_BUS_SPI_INTERNAL;
break;
default:
rm3100::usage();
return 0;
}
}
if (myoptind >= argc) {
rm3100::usage();
return -1;
}
const char *verb = argv[myoptind];
// Start/load the driver
if (!strcmp(verb, "start")) {
if (rm3100::start(bus_id, rotation)) {
rm3100::init(bus_id);
return 0;
} else {
return 1;
}
}
// Stop the driver
if (!strcmp(verb, "stop")) {
return rm3100::stop();
}
// Test the driver/device
if (!strcmp(verb, "test")) {
return rm3100::test(bus_id);
}
// Reset the driver
if (!strcmp(verb, "reset")) {
return rm3100::reset(bus_id);
}
// Print driver information
if (!strcmp(verb, "info") ||
!strcmp(verb, "status")) {
return rm3100::info(bus_id);
}
PX4_INFO("unrecognized command, try 'start', 'test', 'reset' or 'info'");
return 1;
}
struct
rm3100::rm3100_bus_option &rm3100::find_bus(RM3100_BUS bus_id)
{
for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
if ((bus_id == RM3100_BUS_ALL ||
bus_id == bus_options[i].bus_id) && bus_options[i].dev != NULL) {
return bus_options[i];
}
}
errx(1, "bus %u not started", (unsigned)bus_id);
}

122
src/drivers/magnetometer/rm3100/rm3100_main.h Normal file
View File

@@ -0,0 +1,122 @@
/****************************************************************************
*
* Copyright (c) 2015 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 rm3100_main.h
*/
#pragma once
#include "rm3100.h"
namespace rm3100
{
/**
* @struct List of supported bus configurations
*/
struct rm3100_bus_option {
RM3100_BUS bus_id;
const char *devpath;
RM3100_constructor interface_constructor;
uint8_t busnum;
RM3100 *dev;
} bus_options[] = {
#ifdef PX4_I2C_BUS_EXPANSION
{ RM3100_BUS_I2C_EXTERNAL, "/dev/rm3100_ext", &RM3100_I2C_interface, PX4_I2C_BUS_EXPANSION, NULL },
#endif /* PX4_I2C_BUS_EXPANSION */
#ifdef PX4_I2C_BUS_EXPANSION1
{ RM3100_BUS_I2C_EXTERNAL, "/dev/rm3100_ext1", &RM3100_I2C_interface, PX4_I2C_BUS_EXPANSION1, NULL },
#endif /* PX4_I2C_BUS_EXPANSION1 */
#ifdef PX4_I2C_BUS_EXPANSION2
{ RM3100_BUS_I2C_EXTERNAL, "/dev/rm3100_ext2", &RM3100_I2C_interface, PX4_I2C_BUS_EXPANSION2, NULL },
#endif /* PX4_I2C_BUS_EXPANSION2 */
#ifdef PX4_I2C_BUS_ONBOARD
{ RM3100_BUS_I2C_INTERNAL, "/dev/rm3100_int", &RM3100_I2C_interface, PX4_I2C_BUS_ONBOARD, NULL },
#endif /* PX4_I2C_BUS_ONBOARD */
#ifdef PX4_SPIDEV_RM
{ RM3100_BUS_SPI_INTERNAL, "/dev/rm3100_spi_int", &RM3100_SPI_interface, PX4_SPI_BUS_SENSORS, NULL },
#endif /* PX4_SPIDEV_RM */
#ifdef PX4_SPIDEV_RM_EXT
{ RM3100_BUS_SPI_EXTERNAL, "/dev/rm3100_spi_ext", &RM3100_SPI_interface, PX4_SPI_BUS_EXT, NULL },
#endif /* PX4_SPIDEV_RM_EXT */
};
/**
* @brief Finds a bus structure for a bus_id
*/
rm3100_bus_option &find_bus(RM3100_BUS bus_id);
/**
* @brief Prints info about the driver.
*/
int info(RM3100_BUS bus_id);
/**
* @brief Initializes the driver -- sets defaults and starts a cycle
*/
bool init(RM3100_BUS bus_id);
/**
* @brief Resets the driver.
*/
bool reset(RM3100_BUS bus_id);
/**
* @brief Starts the driver for a specific bus option
*/
bool start_bus(struct rm3100_bus_option &bus, Rotation rotation);
/**
* @brief Starts the driver. This function call only returns once the driver
* is either successfully up and running or failed to start.
*/
int start(RM3100_BUS bus_id, Rotation rotation);
/**
* @brief Stop the driver.
*/
int stop();
/**
* @brief Perform some basic functional tests on the driver;
* make sure we can collect data from the sensor in polled
* and automatic modes.
*/
bool test(RM3100_BUS bus_id);
/**
* @brief Prints info about the driver argument usage.
*/
void usage();
} // namespace RM3100

176
src/drivers/magnetometer/rm3100/rm3100_spi.cpp Normal file
View File

@@ -0,0 +1,176 @@
/****************************************************************************
*
* Copyright (c) 2018 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 rm3100_spi.cpp
*
* SPI interface for RM3100
*/
#include <px4_config.h>
#include <assert.h>
#include <debug.h>
#include <errno.h>
#include <stdint.h>
#include <stdbool.h>
#include <string.h>
#include <sys/types.h>
#include <unistd.h>
#include <arch/board/board.h>
#include <drivers/device/spi.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_device.h>
#include "board_config.h"
#include "rm3100.h"
#if defined(PX4_SPIDEV_RM) || defined (PX4_SPIDEV_RM_EXT)
/* SPI protocol address bits */
#define DIR_READ (1<<7)
#define DIR_WRITE (0<<7)
class RM3100_SPI : public device::SPI
{
public:
RM3100_SPI(int bus, uint32_t device);
virtual ~RM3100_SPI() = default;
virtual int init();
virtual int ioctl(unsigned operation, unsigned &arg);
virtual int read(unsigned address, void *data, unsigned count);
virtual int write(unsigned address, void *data, unsigned count);
};
device::Device *
RM3100_SPI_interface(int bus);
device::Device *
RM3100_SPI_interface(int bus)
{
#ifdef PX4_SPIDEV_RM_EXT
return new RM3100_SPI(bus, PX4_SPIDEV_RM_EXT);
#else
return new RM3100_SPI(bus, PX4_SPIDEV_RM);
#endif
}
RM3100_SPI::RM3100_SPI(int bus, uint32_t device) :
SPI("RM3100_SPI", nullptr, bus, device, SPIDEV_MODE3, 1 * 1000 * 1000 /* */)
{
_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_RM3100;
}
int
RM3100_SPI::init()
{
int ret;
ret = SPI::init();
if (ret != OK) {
DEVICE_DEBUG("SPI init failed");
return -EIO;
}
// Read REV_ID value
uint8_t data = 0;
if (read(ADDR_REVID, &data, 1)) {
DEVICE_DEBUG("RM3100 read_reg fail");
}
if (data != RM3100_REVID) {
DEVICE_DEBUG("RM3100 ID: %02x", data);
return -EIO;
}
return OK;
}
int
RM3100_SPI::ioctl(unsigned operation, unsigned &arg)
{
int ret;
switch (operation) {
case MAGIOCGEXTERNAL:
return external();
case DEVIOCGDEVICEID:
return CDev::ioctl(nullptr, operation, arg);
default: {
ret = -EINVAL;
}
}
return ret;
}
int
RM3100_SPI::read(unsigned address, void *data, unsigned count)
{
uint8_t buf[32];
if (sizeof(buf) < (count + 1)) {
return -EIO;
}
buf[0] = address | DIR_READ;
int ret = transfer(&buf[0], &buf[0], count + 1);
memcpy(data, &buf[1], count);
return ret;
}
int
RM3100_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);
}
#endif /* PX4_SPIDEV_RM || PX4_SPIDEV_RM_EXT */