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

refactor hmc5883: use driver base class

Browse Source 
Removes the calibration on startup, as these values were overwritten by
the system calibration values anyway.
So the only difference is that if all calibration scales were equal to 1,
the driver startup would have failed.
This commit is contained in:
Beat Küng 2020-03-13 09:05:36 +01:00 committed by Daniel Agar
parent 8f3ba81c4a
commit 8ebde51648
23 changed files with 167 additions and 460 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
boards/airmind/mindpx-v2/init/rc.board_sensors
View File

@ -6,11 +6,11 @@
adc start
# External I2C bus
hmc5883 -C -T -X start
hmc5883 -T -X start
qmc5883 -X start
# Internal I2C bus
hmc5883 -C -T -I -R 12 start
hmc5883 -T -I -R 12 start
mpu6000 -s -R 8 start
mpu9250 -s -R 8 start

2
boards/av/x-v1/init/rc.board_sensors
View File

@ -22,5 +22,5 @@ fi
# Possible external compasses
ist8310 -X start
hmc5883 -C -T -X start
hmc5883 -T -X start
qmc5883 -X start

2
boards/holybro/durandal-v1/init/rc.board_sensors
View File

@ -15,7 +15,7 @@ bmi088 -G -R 10 start
# Possible external compasses
ist8310 -X start
hmc5883 -C -T -X start
hmc5883 -T -X start
qmc5883 -X start
lis3mdl -X start

2
boards/holybro/kakutef7/init/rc.board_sensors
View File

@ -16,5 +16,5 @@ bmp280 -X start
# Possible external compasses
ist8310 -X start
hmc5883 -C -T -X start
hmc5883 -T -X start
qmc5883 -X start

2
boards/modalai/fc-v1/init/rc.board_sensors
View File

@ -23,7 +23,7 @@ bmi088 -G -R 4 start
# Possible external compasses
ist8310 -X start
hmc5883 -C -T -X start
hmc5883 -T -X start
qmc5883 -X start
# Internal I2C Baro

2
boards/mro/ctrl-zero-f7/init/rc.board_sensors
View File

@ -22,6 +22,6 @@ dps310 -s start
# Possible external compasses
ist8310 -X start
hmc5883 -C -T -X start
hmc5883 -T -X start
qmc5883 -X start
lis3mdl -X start

4
boards/mro/x21-777/init/rc.board_sensors
View File

@ -6,7 +6,7 @@
adc start
# External I2C bus
hmc5883 -C -T -X start
hmc5883 -T -X start
lis3mdl -X start
# Internal SPI bus ICM-20608-G is rotated 90 deg yaw
@ -20,6 +20,6 @@ mpu9250 -R 2 start
# Possible external compasses
ist8310 -X start
hmc5883 -C -T -X start
hmc5883 -T -X start
qmc5883 -X start
lis3mdl -X start

2
boards/mro/x21/init/rc.board_sensors
View File

@ -6,7 +6,7 @@
adc start
# External I2C bus
hmc5883 -C -T -X start
hmc5883 -T -X start
lis3mdl -X start
ist8310 -X start
qmc5883 -X start

2
boards/nxp/fmuk66-v3/init/rc.board_sensors
View File

@ -6,7 +6,7 @@
adc start
# Possible external compasses
hmc5883 -C -X start
hmc5883 -X start
lis3mdl -X start
ist8310 -X start
qmc5883 -X start

2
boards/nxp/fmurt1062-v1/init/rc.board_sensors
View File

@ -31,7 +31,7 @@ bmi055 -G -R 10 -s start
# Possible external compasses
ist8310 -X start
hmc5883 -C -T -X start
hmc5883 -T -X start
qmc5883 -X start
lis3mdl -X start

4
boards/px4/fmu-v2/init/rc.board_sensors
View File

@ -8,12 +8,12 @@ rgbled start -I
adc start
# External I2C bus
hmc5883 -C -T -X start
hmc5883 -T -X start
lis3mdl -X start
ist8310 -X start
# Internal I2C bus
hmc5883 -C -T -I -R 4 start
hmc5883 -T -I -R 4 start
# Internal SPI bus ICM-20608-G
mpu6000 -s -T 20608 start

4
boards/px4/fmu-v3/init/rc.board_sensors
View File

@ -8,13 +8,13 @@ rgbled start -I
adc start
# External I2C bus
hmc5883 -C -T -X start
hmc5883 -T -X start
lis3mdl -X start
ist8310 -X start
qmc5883 -X start
# Internal I2C bus
hmc5883 -C -T -I -R 4 start
hmc5883 -T -I -R 4 start
# Internal SPI bus ICM-20608-G
mpu6000 -s -T 20608 start

4
boards/px4/fmu-v4/init/rc.board_sensors
View File

@ -15,7 +15,7 @@ fmu sensor_reset 50
# External I2C bus
hmc5883 -C -T -X start
hmc5883 -T -X start
lis3mdl -X start
ist8310 -X start
qmc5883 -X start
@ -32,7 +32,7 @@ then
fi
# hmc5883 internal SPI bus is rotated 90 deg yaw
if ! hmc5883 -C -T -S -R 2 start
if ! hmc5883 -T -S -R 2 start
then
# lis3mdl internal SPI bus is rotated 90 deg yaw
lis3mdl start

2
boards/px4/fmu-v4pro/init/rc.board_sensors
View File

@ -20,7 +20,7 @@ mpu9250 -R 2 start
lis3mdl -R 0 start
# Possible external compasses
hmc5883 -C -T -X start
hmc5883 -T -X start
qmc5883 -X start
# RM3100

2
boards/px4/fmu-v5/init/rc.board_sensors
View File

@ -25,7 +25,7 @@ bmi055 -G -R 10 -s start
# Possible external compasses
ist8310 -X start
hmc5883 -C -T -X start
hmc5883 -T -X start
qmc5883 -X start
lis3mdl -X start

2
boards/px4/fmu-v5x/init/rc.board_sensors
View File

@ -23,7 +23,7 @@ bmi088 -G -R 12 start
# Possible external compasses
ist8310 -X start
hmc5883 -C -T -X start
hmc5883 -T -X start
qmc5883 -X start
# Possible internal compass

2
posix-configs/ocpoc/px4.config
View File

@ -16,7 +16,7 @@ param set MAV_TYPE 2
param set MAV_SYS_ID 1
mpu9250 -R 10 start
hmc5883 start
hmc5883 -I start
ms5611 -s start
rgbled start -b 1

95
src/drivers/magnetometer/hmc5883/HMC5883.cpp
View File

@ -33,9 +33,9 @@
#include "HMC5883.hpp"
HMC5883::HMC5883(device::Device *interface, const char *path, enum Rotation rotation) :
CDev("HMC5883", path),
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id())),
HMC5883::HMC5883(device::Device *interface, enum Rotation rotation, I2CSPIBusOption bus_option, int bus) :
CDev("HMC5883", nullptr),
I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus),
_interface(interface),
_reports(nullptr),
_scale{},
@ -73,9 +73,6 @@ HMC5883::HMC5883(device::Device *interface, const char *path, enum Rotation rota
HMC5883::~HMC5883()
{
/* make sure we are truly inactive */
stop();
if (_reports != nullptr) {
delete _reports;
}
@ -118,6 +115,10 @@ HMC5883::init()
ret = OK;
/* sensor is ok, but not calibrated */
_sensor_ok = true;
_measure_interval = HMC5883_CONVERSION_INTERVAL;
start();
out:
return ret;
}
@ -399,16 +400,6 @@ HMC5883::start()
ScheduleNow();
}
void
HMC5883::stop()
{
if (_measure_interval > 0) {
/* ensure no new items are queued while we cancel this one */
_measure_interval = 0;
ScheduleClear();
}
}
int
HMC5883::reset()
{
@ -417,7 +408,7 @@ HMC5883::reset()
}
void
HMC5883::Run()
HMC5883::RunImpl()
{
if (_measure_interval == 0) {
return;
@ -676,6 +667,47 @@ out:
return ret;
}
/**
* Automatic scale calibration.
*
* Basic idea:
*
* output = (ext field +- 1.1 Ga self-test) * scale factor
*
* and consequently:
*
* 1.1 Ga = (excited - normal) * scale factor
* scale factor = (excited - normal) / 1.1 Ga
*
* sxy = (excited - normal) / 766 | for conf reg. B set to 0x60 / Gain = 3
* sz = (excited - normal) / 713 | for conf reg. B set to 0x60 / Gain = 3
*
* By subtracting the non-excited measurement the pure 1.1 Ga reading
* can be extracted and the sensitivity of all axes can be matched.
*
* SELF TEST OPERATION
* To check the HMC5883L for proper operation, a self test feature in incorporated
* in which the sensor offset straps are excited to create a nominal field strength
* (bias field) to be measured. To implement self test, the least significant bits
* (MS1 and MS0) of configuration register A are changed from 00 to 01 (positive bias)
* or 10 (negetive bias), e.g. 0x11 or 0x12.
* Then, by placing the mode register into single-measurement mode (0x01),
* two data acquisition cycles will be made on each magnetic vector.
* The first acquisition will be a set pulse followed shortly by measurement
* data of the external field. The second acquisition will have the offset strap
* excited (about 10 mA) in the positive bias mode for X, Y, and Z axes to create
* about a ±1.1 gauss self test field plus the external field. The first acquisition
* values will be subtracted from the second acquisition, and the net measurement
* will be placed into the data output registers.
* Since self test adds ~1.1 Gauss additional field to the existing field strength,
* using a reduced gain setting prevents sensor from being saturated and data registers
* overflowed. For example, if the configuration register B is set to 0x60 (Gain=3),
* values around +766 LSB (1.16 Ga * 660 LSB/Ga) will be placed in the X and Y data
* output registers and around +713 (1.08 Ga * 660 LSB/Ga) will be placed in Z data
* output register. To leave the self test mode, change MS1 and MS0 bit of the
* configuration register A back to 00 (Normal Measurement Mode), e.g. 0x10.
* Using the self test method described above, the user can scale sensor
*/
int HMC5883::calibrate(cdev::file_t *filp, unsigned enable)
{
sensor_mag_s report{};
@ -713,7 +745,7 @@ int HMC5883::calibrate(cdev::file_t *filp, unsigned enable)
/* start the sensor polling at 50 Hz */
if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) {
warn("FAILED: SENSORIOCSPOLLRATE 50Hz");
PX4_ERR("FAILED: SENSORIOCSPOLLRATE 50Hz");
ret = 1;
goto out;
}
@ -721,25 +753,25 @@ int HMC5883::calibrate(cdev::file_t *filp, unsigned enable)
/* Set to 2.5 Gauss. We ask for 3 to get the right part of
* the chained if statement above. */
if (OK != ioctl(filp, MAGIOCSRANGE, 3)) {
warnx("FAILED: MAGIOCSRANGE 2.5 Ga");
PX4_ERR("FAILED: MAGIOCSRANGE 2.5 Ga");
ret = 1;
goto out;
}
if (OK != ioctl(filp, MAGIOCEXSTRAP, 1)) {
warnx("FAILED: MAGIOCEXSTRAP 1");
PX4_ERR("FAILED: MAGIOCEXSTRAP 1");
ret = 1;
goto out;
}
if (OK != ioctl(filp, MAGIOCGSCALE, (long unsigned int)&mscale_previous)) {
warn("FAILED: MAGIOCGSCALE 1");
PX4_ERR("FAILED: MAGIOCGSCALE 1");
ret = 1;
goto out;
}
if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_null)) {
warn("FAILED: MAGIOCSSCALE 1");
PX4_ERR("FAILED: MAGIOCSSCALE 1");
ret = 1;
goto out;
}
@ -754,7 +786,7 @@ int HMC5883::calibrate(cdev::file_t *filp, unsigned enable)
ret = px4_poll(&fds, 1, 2000);
if (ret != 1) {
warn("ERROR: TIMEOUT 1");
PX4_ERR("ERROR: TIMEOUT 1");
goto out;
}
@ -762,7 +794,7 @@ int HMC5883::calibrate(cdev::file_t *filp, unsigned enable)
sz = px4_read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
warn("ERROR: READ 1");
PX4_ERR("ERROR: READ 1");
ret = -EIO;
goto out;
}
@ -778,7 +810,7 @@ int HMC5883::calibrate(cdev::file_t *filp, unsigned enable)
ret = px4_poll(&fds, 1, 2000);
if (ret != 1) {
warn("ERROR: TIMEOUT 2");
PX4_ERR("ERROR: TIMEOUT 2");
goto out;
}
@ -786,7 +818,7 @@ int HMC5883::calibrate(cdev::file_t *filp, unsigned enable)
sz = px4_read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
warn("ERROR: READ 2");
PX4_ERR("ERROR: READ 2");
ret = -EIO;
goto out;
}
@ -827,23 +859,23 @@ int HMC5883::calibrate(cdev::file_t *filp, unsigned enable)
out:
if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) {
warn("FAILED: MAGIOCSSCALE 2");
PX4_ERR("FAILED: MAGIOCSSCALE 2");
}
/* set back to normal mode */
/* Set to 1.9 Gauss */
if (OK != px4_ioctl(fd, MAGIOCSRANGE, 2)) {
warnx("FAILED: MAGIOCSRANGE 1.9 Ga");
PX4_ERR("FAILED: MAGIOCSRANGE 1.9 Ga");
}
if (OK != px4_ioctl(fd, MAGIOCEXSTRAP, 0)) {
warnx("FAILED: MAGIOCEXSTRAP 0");
PX4_ERR("FAILED: MAGIOCEXSTRAP 0");
}
if (ret == OK) {
if (check_scale()) {
/* failed */
warnx("FAILED: SCALE");
PX4_ERR("FAILED: SCALE");
ret = PX4_ERROR;
}
@ -1011,8 +1043,9 @@ HMC5883::meas_to_float(uint8_t in[2])
}
void
HMC5883::print_info()
HMC5883::print_status()
{
I2CSPIDriverBase::print_status();
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
printf("interval: %u us\n", _measure_interval);

44
src/drivers/magnetometer/hmc5883/HMC5883.hpp
View File

@ -52,10 +52,9 @@
#include <math.h>
#include <unistd.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <px4_platform_common/i2c_spi_buses.h>
#include <lib/perf/perf_counter.h>
#include <systemlib/err.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_hrt.h>
@ -65,7 +64,6 @@
#include <uORB/uORB.h>
#include <float.h>
#include <px4_platform_common/getopt.h>
#include <lib/conversion/rotation.h>
#include "hmc5883.h"
@ -110,32 +108,29 @@
#define HMC5983_TEMP_SENSOR_ENABLE (1 << 7)
class HMC5883 : public device::CDev, public px4::ScheduledWorkItem
class HMC5883 : public device::CDev, public I2CSPIDriver<HMC5883>
{
public:
HMC5883(device::Device *interface, const char *path, enum Rotation rotation);
HMC5883(device::Device *interface, enum Rotation rotation, I2CSPIBusOption bus_option, int bus);
virtual ~HMC5883();
virtual int init();
static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance);
static void print_usage();
virtual ssize_t read(cdev::file_t *filp, char *buffer, size_t buflen);
virtual int ioctl(cdev::file_t *filp, int cmd, unsigned long arg);
void RunImpl();
/**
* Stop the automatic measurement state machine.
*/
void stop();
int init() override;
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info();
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:
Device *_interface;
void print_status() override;
private:
Device *_interface;
unsigned _measure_interval{0};
ringbuffer::RingBuffer *_reports;
@ -230,21 +225,6 @@ private:
*/
void check_conf(void);
/**
* Perform 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 Run() override;
/**
* Write a register.
*

6
src/drivers/magnetometer/hmc5883/hmc5883.h
View File

@ -40,6 +40,7 @@
#pragma once
#include <drivers/device/Device.hpp>
#include <drivers/device/spi.h>
#define ADDR_ID_A 0x0a
#define ADDR_ID_B 0x0b
@ -50,6 +51,5 @@
#define ID_C_WHO_AM_I '3'
/* interface factories */
extern device::Device *HMC5883_SPI_interface(int bus);
extern device::Device *HMC5883_I2C_interface(int bus);
typedef device::Device *(*HMC5883_constructor)(int);
extern device::Device *HMC5883_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode);
extern device::Device *HMC5883_I2C_interface(int bus, int bus_frequency);

12
src/drivers/magnetometer/hmc5883/hmc5883_i2c.cpp
View File

@ -46,12 +46,12 @@
#define HMC5883L_ADDRESS 0x1E
device::Device *HMC5883_I2C_interface(int bus);
device::Device *HMC5883_I2C_interface(int bus, int bus_frequency);
class HMC5883_I2C : public device::I2C
{
public:
HMC5883_I2C(int bus);
HMC5883_I2C(int bus, int bus_frequency);
virtual ~HMC5883_I2C() = default;
virtual int read(unsigned address, void *data, unsigned count);
@ -65,13 +65,13 @@ protected:
};
device::Device *
HMC5883_I2C_interface(int bus)
HMC5883_I2C_interface(int bus, int bus_frequency)
{
return new HMC5883_I2C(bus);
return new HMC5883_I2C(bus, bus_frequency);
}
HMC5883_I2C::HMC5883_I2C(int bus) :
I2C("HMC5883_I2C", nullptr, bus, HMC5883L_ADDRESS, 400000)
HMC5883_I2C::HMC5883_I2C(int bus, int bus_frequency) :
I2C("HMC5883_I2C", nullptr, bus, HMC5883L_ADDRESS, bus_frequency)
{
_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883;
}

410
src/drivers/magnetometer/hmc5883/hmc5883_main.cpp
View File

@ -38,411 +38,109 @@
*/
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
#include "HMC5883.hpp"
#include "hmc5883.h"
/*
* Driver 'main' command.
*/
extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]);
enum HMC5883_BUS {
HMC5883_BUS_ALL = 0,
HMC5883_BUS_I2C_INTERNAL,
HMC5883_BUS_I2C_EXTERNAL,
HMC5883_BUS_SPI
};
/**
* Local functions in support of the shell command.
*/
namespace hmc5883
I2CSPIDriverBase *HMC5883::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance)
{
device::Device *interface = nullptr;
/*
list of supported bus configurations
*/
struct hmc5883_bus_option {
enum HMC5883_BUS busid;
const char *devpath;
HMC5883_constructor interface_constructor;
uint8_t busnum;
HMC5883 *dev;
} bus_options[] = {
{ HMC5883_BUS_I2C_EXTERNAL, "/dev/hmc5883_ext", &HMC5883_I2C_interface, PX4_I2C_BUS_EXPANSION, NULL },
#ifdef PX4_I2C_BUS_EXPANSION1
{ HMC5883_BUS_I2C_EXTERNAL, "/dev/hmc5883_ext1", &HMC5883_I2C_interface, PX4_I2C_BUS_EXPANSION1, NULL },
#endif
#ifdef PX4_I2C_BUS_EXPANSION2
{ HMC5883_BUS_I2C_EXTERNAL, "/dev/hmc5883_ext2", &HMC5883_I2C_interface, PX4_I2C_BUS_EXPANSION2, NULL },
#endif
#ifdef PX4_I2C_BUS_ONBOARD
{ HMC5883_BUS_I2C_INTERNAL, "/dev/hmc5883_int", &HMC5883_I2C_interface, PX4_I2C_BUS_ONBOARD, NULL },
#endif
#ifdef PX4_SPIDEV_HMC
{ HMC5883_BUS_SPI, "/dev/hmc5883_spi", &HMC5883_SPI_interface, PX4_SPI_BUS_SENSORS, NULL },
#endif
};
#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0]))
if (iterator.busType() == BOARD_I2C_BUS) {
interface = HMC5883_I2C_interface(iterator.bus(), cli.bus_frequency);
/**
* start driver for a specific bus option
*/
static bool
start_bus(struct hmc5883_bus_option &bus, enum Rotation rotation)
{
if (bus.dev != nullptr) {
PX4_ERR("bus option already started");
} else if (iterator.busType() == BOARD_SPI_BUS) {
interface = HMC5883_SPI_interface(iterator.bus(), iterator.devid(), cli.bus_frequency, cli.spi_mode);
}
device::Device *interface = bus.interface_constructor(bus.busnum);
if (interface == nullptr) {
PX4_ERR("alloc failed");
return nullptr;
}
if (interface->init() != OK) {
delete interface;
PX4_WARN("no device on bus %u (type: %u)", (unsigned)bus.busnum, (unsigned)bus.busid);
return false;
PX4_DEBUG("no device on bus %i (devid 0x%x)", iterator.bus(), iterator.devid());
return nullptr;
}
bus.dev = new HMC5883(interface, bus.devpath, rotation);
HMC5883 *dev = new HMC5883(interface, cli.rotation, iterator.configuredBusOption(), iterator.bus());
if (bus.dev != nullptr && OK != bus.dev->init()) {
delete bus.dev;
bus.dev = NULL;
return false;
if (dev == nullptr) {
delete interface;
return nullptr;
}
int fd = px4_open(bus.devpath, O_RDONLY);
if (fd < 0) {
return false;
if (OK != dev->init()) {
delete dev;
return nullptr;
}
if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
px4_close(fd);
PX4_ERR("Failed to setup poll rate");
return false;
bool enable_temp_compensation = cli.custom1 == 1;
if (enable_temp_compensation) {
dev->set_temperature_compensation(1);
}
px4_close(fd);
return true;
return dev;
}
/**
* Start the driver.
*
* This function call only returns once the driver
* is either successfully up and running or failed to start.
*/
static int
start(enum HMC5883_BUS busid, enum Rotation rotation)
void HMC5883::print_usage()
{
bool started = false;
for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
if (busid == HMC5883_BUS_ALL && bus_options[i].dev != NULL) {
// this device is already started
continue;
}
if (busid != HMC5883_BUS_ALL && bus_options[i].busid != busid) {
// not the one that is asked for
continue;
}
started |= start_bus(bus_options[i], rotation);
}
if (!started) {
return PX4_ERROR;
}
return PX4_OK;
PRINT_MODULE_USAGE_NAME("hmc5883", "driver");
PRINT_MODULE_USAGE_SUBCATEGORY("magnetometer");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, true);
PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
PRINT_MODULE_USAGE_PARAM_FLAG('T', "Enable temperature compensation", true);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
}
static int
stop()
extern "C" int hmc5883_main(int argc, char *argv[])
{
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;
}
/**
* find a bus structure for a busid
*/
static struct hmc5883_bus_option *
find_bus(enum HMC5883_BUS busid)
{
for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
if ((busid == HMC5883_BUS_ALL ||
busid == bus_options[i].busid) && bus_options[i].dev != NULL) {
return &bus_options[i];
}
}
PX4_ERR("bus %u not started", (unsigned)busid);
return nullptr;
}
/**
* Automatic scale calibration.
*
* Basic idea:
*
* output = (ext field +- 1.1 Ga self-test) * scale factor
*
* and consequently:
*
* 1.1 Ga = (excited - normal) * scale factor
* scale factor = (excited - normal) / 1.1 Ga
*
* sxy = (excited - normal) / 766 | for conf reg. B set to 0x60 / Gain = 3
* sz = (excited - normal) / 713 | for conf reg. B set to 0x60 / Gain = 3
*
* By subtracting the non-excited measurement the pure 1.1 Ga reading
* can be extracted and the sensitivity of all axes can be matched.
*
* SELF TEST OPERATION
* To check the HMC5883L for proper operation, a self test feature in incorporated
* in which the sensor offset straps are excited to create a nominal field strength
* (bias field) to be measured. To implement self test, the least significant bits
* (MS1 and MS0) of configuration register A are changed from 00 to 01 (positive bias)
* or 10 (negetive bias), e.g. 0x11 or 0x12.
* Then, by placing the mode register into single-measurement mode (0x01),
* two data acquisition cycles will be made on each magnetic vector.
* The first acquisition will be a set pulse followed shortly by measurement
* data of the external field. The second acquisition will have the offset strap
* excited (about 10 mA) in the positive bias mode for X, Y, and Z axes to create
* about a ±1.1 gauss self test field plus the external field. The first acquisition
* values will be subtracted from the second acquisition, and the net measurement
* will be placed into the data output registers.
* Since self test adds ~1.1 Gauss additional field to the existing field strength,
* using a reduced gain setting prevents sensor from being saturated and data registers
* overflowed. For example, if the configuration register B is set to 0x60 (Gain=3),
* values around +766 LSB (1.16 Ga * 660 LSB/Ga) will be placed in the X and Y data
* output registers and around +713 (1.08 Ga * 660 LSB/Ga) will be placed in Z data
* output register. To leave the self test mode, change MS1 and MS0 bit of the
* configuration register A back to 00 (Normal Measurement Mode), e.g. 0x10.
* Using the self test method described above, the user can scale sensor
*/
static int calibrate(enum HMC5883_BUS busid)
{
struct hmc5883_bus_option *bus = find_bus(busid);
if (bus == nullptr) {
return 0;
}
const char *path = bus->devpath;
int fd = px4_open(path, O_RDONLY);
if (fd < 0) {
PX4_ERR("%s open failed (try 'hmc5883 start' if the driver is not running", path);
return PX4_ERROR;
}
int ret = 0;
if (OK != (ret = px4_ioctl(fd, MAGIOCCALIBRATE, fd))) {
PX4_ERR("failed to enable sensor calibration mode");
}
px4_close(fd);
return ret;
}
/**
* enable/disable temperature compensation
*/
static int
temp_enable(enum HMC5883_BUS busid, bool enable)
{
struct hmc5883_bus_option *bus = find_bus(busid);
if (bus == nullptr) {
return 0;
}
const char *path = bus->devpath;
int fd = px4_open(path, O_RDONLY);
if (fd < 0) {
PX4_ERR("failed ");
return PX4_ERROR;
}
if (px4_ioctl(fd, MAGIOCSTEMPCOMP, (unsigned)enable) < 0) {
PX4_ERR("set temperature compensation failed");
return PX4_ERROR;
}
px4_close(fd);
return 0;
}
/**
* Print a little info about the driver.
*/
static int
info(enum HMC5883_BUS busid)
{
struct hmc5883_bus_option *bus = find_bus(busid);
if (bus == nullptr) {
return 0;
}
if (bus != nullptr) {
PX4_INFO("running on bus: %u (%s)\n", (unsigned)bus->busid, bus->devpath);
bus->dev->print_info();
}
return PX4_OK;
}
static int
usage()
{
warnx("missing command: try 'start', 'info', 'calibrate'");
warnx("options:");
warnx(" -R rotation");
warnx(" -C calibrate on start");
warnx(" -X only external bus");
#if (PX4_I2C_BUS_ONBOARD || PX4_SPIDEV_HMC)
warnx(" -I only internal bus");
#endif
return PX4_OK;
}
} // namespace
int
hmc5883_main(int argc, char *argv[])
{
int myoptind = 1;
using ThisDriver = HMC5883;
int ch;
const char *myoptarg = nullptr;
BusCLIArguments cli{true, true};
cli.default_i2c_frequency = 400000;
cli.default_spi_frequency = 11 * 1000 * 1000;
enum HMC5883_BUS busid = HMC5883_BUS_ALL;
enum Rotation rotation = ROTATION_NONE;
bool calibrate = false;
bool temp_compensation = false;
if (argc < 2) {
return hmc5883::usage();
}
while ((ch = px4_getopt(argc, argv, "XISR:CT", &myoptind, &myoptarg)) != EOF) {
while ((ch = cli.getopt(argc, argv, "R:T")) != EOF) {
switch (ch) {
case 'R':
rotation = (enum Rotation)atoi(myoptarg);
break;
#if (PX4_I2C_BUS_ONBOARD || PX4_SPIDEV_HMC)
case 'I':
busid = HMC5883_BUS_I2C_INTERNAL;
break;
#endif
case 'X':
busid = HMC5883_BUS_I2C_EXTERNAL;
break;
case 'S':
busid = HMC5883_BUS_SPI;
break;
case 'C':
calibrate = true;
cli.rotation = (enum Rotation)atoi(cli.optarg());
break;
case 'T':
temp_compensation = true;
cli.custom1 = 1;
break;
default:
return hmc5883::usage();
}
}
if (myoptind >= argc) {
return hmc5883::usage();
const char *verb = cli.optarg();
if (!verb) {
ThisDriver::print_usage();
return -1;
}
const char *verb = argv[myoptind];
BusInstanceIterator iterator(MODULE_NAME, cli, DRV_MAG_DEVTYPE_HMC5883);
/*
* Start/load the driver.
*/
if (!strcmp(verb, "start")) {
hmc5883::start(busid, rotation);
if (calibrate && hmc5883::calibrate(busid) != 0) {
PX4_ERR("calibration failed");
return -1;
}
if (temp_compensation) {
// we consider failing to setup temperature
// compensation as non-fatal
hmc5883::temp_enable(busid, true);
}
return 0;
return ThisDriver::module_start(cli, iterator);
}
/*
* Stop the driver
*/
if (!strcmp(verb, "stop")) {
return hmc5883::stop();
return ThisDriver::module_stop(iterator);
}
/*
* enable/disable temperature compensation
*/
if (!strcmp(verb, "tempoff")) {
return hmc5883::temp_enable(busid, false);
if (!strcmp(verb, "status")) {
return ThisDriver::module_status(iterator);
}
if (!strcmp(verb, "tempon")) {
return hmc5883::temp_enable(busid, true);
}
/*
* Print driver information.
*/
if (!strcmp(verb, "info") || !strcmp(verb, "status")) {
return hmc5883::info(busid);
}
/*
* Autocalibrate the scaling
*/
if (!strcmp(verb, "calibrate")) {
if (hmc5883::calibrate(busid) == 0) {
PX4_INFO("calibration successful");
return 0;
} else {
PX4_ERR("calibration failed");
return -1;
}
}
return hmc5883::usage();
ThisDriver::print_usage();
return -1;
}

16
src/drivers/magnetometer/hmc5883/hmc5883_spi.cpp
View File

@ -44,8 +44,6 @@
#include "hmc5883.h"
#ifdef PX4_SPIDEV_HMC
/* SPI protocol address bits */
#define DIR_READ (1<<7)
#define DIR_WRITE (0<<7)
@ -54,12 +52,12 @@
#define HMC_MAX_SEND_LEN 4
#define HMC_MAX_RCV_LEN 8
device::Device *HMC5883_SPI_interface(int bus);
device::Device *HMC5883_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode);
class HMC5883_SPI : public device::SPI
{
public:
HMC5883_SPI(int bus, uint32_t device);
HMC5883_SPI(int bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode);
virtual ~HMC5883_SPI() = default;
virtual int init();
@ -71,13 +69,13 @@ public:
};
device::Device *
HMC5883_SPI_interface(int bus)
HMC5883_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode)
{
return new HMC5883_SPI(bus, PX4_SPIDEV_HMC);
return new HMC5883_SPI(bus, devid, bus_frequency, spi_mode);
}
HMC5883_SPI::HMC5883_SPI(int bus, uint32_t device) :
SPI("HMC5883_SPI", nullptr, bus, device, SPIDEV_MODE3, 11 * 1000 * 1000 /* will be rounded to 10.4 MHz */)
HMC5883_SPI::HMC5883_SPI(int bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode) :
SPI("HMC5883_SPI", nullptr, bus, device, spi_mode, bus_frequency)
{
_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883;
}
@ -169,5 +167,3 @@ HMC5883_SPI::read(unsigned address, void *data, unsigned count)
memcpy(data, &buf[1], count);
return ret;
}
#endif /* PX4_SPIDEV_HMC */