aboutsummaryrefslogtreecommitdiff
path: root/src/drivers/hmc5883/hmc5883.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/drivers/hmc5883/hmc5883.cpp')
-rw-r--r--src/drivers/hmc5883/hmc5883.cpp373
1 files changed, 277 insertions, 96 deletions
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index 4c85c0cda..4aef43102 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -71,13 +71,16 @@
#include <uORB/topics/subsystem_info.h>
#include <float.h>
+#include <getopt.h>
+#include <lib/conversion/rotation.h>
/*
* HMC5883 internal constants and data structures.
*/
#define HMC5883L_ADDRESS PX4_I2C_OBDEV_HMC5883
-#define HMC5883L_DEVICE_PATH "/dev/hmc5883"
+#define HMC5883L_DEVICE_PATH_INT "/dev/hmc5883_int"
+#define HMC5883L_DEVICE_PATH_EXT "/dev/hmc5883_ext"
/* Max measurement rate is 160Hz, however with 160 it will be set to 166 Hz, therefore workaround using 150 */
#define HMC5883_CONVERSION_INTERVAL (1000000 / 150) /* microseconds */
@@ -130,7 +133,7 @@ static const int ERROR = -1;
class HMC5883 : public device::I2C
{
public:
- HMC5883(int bus);
+ HMC5883(int bus, const char *path, enum Rotation rotation);
virtual ~HMC5883();
virtual int init();
@@ -158,16 +161,25 @@ private:
int _class_instance;
orb_advert_t _mag_topic;
+ orb_advert_t _subsystem_pub;
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
perf_counter_t _buffer_overflows;
+ perf_counter_t _range_errors;
+ perf_counter_t _conf_errors;
/* status reporting */
bool _sensor_ok; /**< sensor was found and reports ok */
bool _calibrated; /**< the calibration is valid */
int _bus; /**< the bus the device is connected to */
+ enum Rotation _rotation;
+
+ struct mag_report _last_report; /**< used for info() */
+
+ uint8_t _range_bits;
+ uint8_t _conf_reg;
/**
* Test whether the device supported by the driver is present at a
@@ -227,6 +239,23 @@ private:
int set_range(unsigned range);
/**
+ * check the sensor range.
+ *
+ * checks that the range of the sensor is correctly set, to
+ * cope with communication errors causing the range to change
+ */
+ void check_range(void);
+
+ /**
+ * 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);
+
+ /**
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*
@@ -316,21 +345,31 @@ private:
extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]);
-HMC5883::HMC5883(int bus) :
- I2C("HMC5883", HMC5883L_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000),
+HMC5883::HMC5883(int bus, const char *path, enum Rotation rotation) :
+ I2C("HMC5883", path, bus, HMC5883L_ADDRESS, 400000),
_measure_ticks(0),
_reports(nullptr),
_range_scale(0), /* default range scale from counts to gauss */
_range_ga(1.3f),
- _mag_topic(-1),
+ _collect_phase(false),
_class_instance(-1),
+ _mag_topic(-1),
+ _subsystem_pub(-1),
_sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")),
_comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")),
_buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")),
+ _range_errors(perf_alloc(PC_COUNT, "hmc5883_range_errors")),
+ _conf_errors(perf_alloc(PC_COUNT, "hmc5883_conf_errors")),
_sensor_ok(false),
_calibrated(false),
- _bus(bus)
+ _bus(bus),
+ _rotation(rotation),
+ _last_report{0},
+ _range_bits(0),
+ _conf_reg(0)
{
+ _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883;
+
// enable debug() calls
_debug_enabled = false;
@@ -361,6 +400,8 @@ HMC5883::~HMC5883()
perf_free(_sample_perf);
perf_free(_comms_errors);
perf_free(_buffer_overflows);
+ perf_free(_range_errors);
+ perf_free(_conf_errors);
}
int
@@ -391,45 +432,43 @@ out:
int HMC5883::set_range(unsigned range)
{
- uint8_t range_bits;
-
if (range < 1) {
- range_bits = 0x00;
+ _range_bits = 0x00;
_range_scale = 1.0f / 1370.0f;
_range_ga = 0.88f;
} else if (range <= 1) {
- range_bits = 0x01;
+ _range_bits = 0x01;
_range_scale = 1.0f / 1090.0f;
_range_ga = 1.3f;
} else if (range <= 2) {
- range_bits = 0x02;
+ _range_bits = 0x02;
_range_scale = 1.0f / 820.0f;
_range_ga = 1.9f;
} else if (range <= 3) {
- range_bits = 0x03;
+ _range_bits = 0x03;
_range_scale = 1.0f / 660.0f;
_range_ga = 2.5f;
} else if (range <= 4) {
- range_bits = 0x04;
+ _range_bits = 0x04;
_range_scale = 1.0f / 440.0f;
_range_ga = 4.0f;
} else if (range <= 4.7f) {
- range_bits = 0x05;
+ _range_bits = 0x05;
_range_scale = 1.0f / 390.0f;
_range_ga = 4.7f;
} else if (range <= 5.6f) {
- range_bits = 0x06;
+ _range_bits = 0x06;
_range_scale = 1.0f / 330.0f;
_range_ga = 5.6f;
} else {
- range_bits = 0x07;
+ _range_bits = 0x07;
_range_scale = 1.0f / 230.0f;
_range_ga = 8.1f;
}
@@ -439,7 +478,7 @@ int HMC5883::set_range(unsigned range)
/*
* Send the command to set the range
*/
- ret = write_reg(ADDR_CONF_B, (range_bits << 5));
+ ret = write_reg(ADDR_CONF_B, (_range_bits << 5));
if (OK != ret)
perf_count(_comms_errors);
@@ -450,7 +489,53 @@ int HMC5883::set_range(unsigned range)
if (OK != ret)
perf_count(_comms_errors);
- return !(range_bits_in == (range_bits << 5));
+ return !(range_bits_in == (_range_bits << 5));
+}
+
+/**
+ check that the range register has the right value. This is done
+ periodically to cope with I2C bus noise causing the range of the
+ compass changing.
+ */
+void HMC5883::check_range(void)
+{
+ int ret;
+
+ uint8_t range_bits_in;
+ ret = read_reg(ADDR_CONF_B, range_bits_in);
+ if (OK != ret) {
+ perf_count(_comms_errors);
+ return;
+ }
+ if (range_bits_in != (_range_bits<<5)) {
+ perf_count(_range_errors);
+ ret = write_reg(ADDR_CONF_B, (_range_bits << 5));
+ if (OK != ret)
+ perf_count(_comms_errors);
+ }
+}
+
+/**
+ 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 HMC5883::check_conf(void)
+{
+ int ret;
+
+ uint8_t conf_reg_in;
+ ret = read_reg(ADDR_CONF_A, conf_reg_in);
+ if (OK != ret) {
+ perf_count(_comms_errors);
+ return;
+ }
+ if (conf_reg_in != _conf_reg) {
+ perf_count(_conf_errors);
+ ret = write_reg(ADDR_CONF_A, _conf_reg);
+ if (OK != ret)
+ perf_count(_comms_errors);
+ }
}
int
@@ -713,7 +798,7 @@ HMC5883::cycle()
/* perform collection */
if (OK != collect()) {
- log("collection error");
+ debug("collection error");
/* restart the measurement state machine */
start();
return;
@@ -740,7 +825,7 @@ HMC5883::cycle()
/* measurement phase */
if (OK != measure())
- log("measure error");
+ debug("measure error");
/* next phase is collection */
_collect_phase = true;
@@ -784,7 +869,7 @@ HMC5883::collect()
} report;
int ret = -EIO;
uint8_t cmd;
-
+ uint8_t check_counter;
perf_begin(_sample_perf);
struct mag_report new_report;
@@ -857,6 +942,9 @@ HMC5883::collect()
/* z remains z */
new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
+ // apply user specified rotation
+ rotate_3f(_rotation, new_report.x, new_report.y, new_report.z);
+
if (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) {
if (_mag_topic != -1) {
@@ -870,6 +958,8 @@ HMC5883::collect()
}
}
+ _last_report = new_report;
+
/* post a report to the ring */
if (_reports->force(&new_report)) {
perf_count(_buffer_overflows);
@@ -878,6 +968,21 @@ HMC5883::collect()
/* notify anyone waiting for data */
poll_notify(POLLIN);
+ /*
+ periodically check the range register and 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_range();
+ }
+ if (check_counter == 128) {
+ check_conf();
+ }
+
ret = OK;
out:
@@ -1042,31 +1147,28 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
warnx("axes scaling: %.6f %.6f %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]);
- /* set back to normal mode */
- /* Set to 1.1 Gauss */
- if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) {
- warnx("failed to set 1.1 Ga range");
- goto out;
- }
-
- if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) {
- warnx("failed to disable sensor calibration mode");
- goto out;
- }
-
/* set scaling in device */
mscale_previous.x_scale = scaling[0];
mscale_previous.y_scale = scaling[1];
mscale_previous.z_scale = scaling[2];
+ ret = OK;
+
+out:
+
if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) {
warn("WARNING: failed to set new scale / offsets for mag");
- goto out;
}
- ret = OK;
+ /* set back to normal mode */
+ /* Set to 1.1 Gauss */
+ if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) {
+ warnx("failed to set 1.1 Ga range");
+ }
-out:
+ if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) {
+ warnx("failed to disable sensor calibration mode");
+ }
if (ret == OK) {
if (!check_scale()) {
@@ -1136,13 +1238,12 @@ int HMC5883::check_calibration()
true,
_calibrated,
SUBSYSTEM_TYPE_MAG};
- static orb_advert_t pub = -1;
if (!(_pub_blocked)) {
- if (pub > 0) {
- orb_publish(ORB_ID(subsystem_info), pub, &info);
+ if (_subsystem_pub > 0) {
+ orb_publish(ORB_ID(subsystem_info), _subsystem_pub, &info);
} else {
- pub = orb_advertise(ORB_ID(subsystem_info), &info);
+ _subsystem_pub = orb_advertise(ORB_ID(subsystem_info), &info);
}
}
}
@@ -1155,25 +1256,24 @@ int HMC5883::set_excitement(unsigned enable)
{
int ret;
/* arm the excitement strap */
- uint8_t conf_reg;
- ret = read_reg(ADDR_CONF_A, conf_reg);
+ ret = read_reg(ADDR_CONF_A, _conf_reg);
if (OK != ret)
perf_count(_comms_errors);
if (((int)enable) < 0) {
- conf_reg |= 0x01;
+ _conf_reg |= 0x01;
} else if (enable > 0) {
- conf_reg |= 0x02;
+ _conf_reg |= 0x02;
} else {
- conf_reg &= ~0x03;
+ _conf_reg &= ~0x03;
}
- // ::printf("set_excitement enable=%d regA=0x%x\n", (int)enable, (unsigned)conf_reg);
+ // ::printf("set_excitement enable=%d regA=0x%x\n", (int)enable, (unsigned)_conf_reg);
- ret = write_reg(ADDR_CONF_A, conf_reg);
+ ret = write_reg(ADDR_CONF_A, _conf_reg);
if (OK != ret)
perf_count(_comms_errors);
@@ -1183,7 +1283,7 @@ int HMC5883::set_excitement(unsigned enable)
//print_info();
- return !(conf_reg == conf_reg_ret);
+ return !(_conf_reg == conf_reg_ret);
}
int
@@ -1221,10 +1321,11 @@ HMC5883::print_info()
perf_print_counter(_comms_errors);
perf_print_counter(_buffer_overflows);
printf("poll interval: %u ticks\n", _measure_ticks);
+ printf("output (%.2f %.2f %.2f)\n", (double)_last_report.x, (double)_last_report.y, (double)_last_report.z);
printf("offsets (%.2f %.2f %.2f)\n", (double)_scale.x_offset, (double)_scale.y_offset, (double)_scale.z_offset);
printf("scaling (%.2f %.2f %.2f) 1/range_scale %.2f range_ga %.2f\n",
(double)_scale.x_scale, (double)_scale.y_scale, (double)_scale.z_scale,
- (double)1.0/_range_scale, (double)_range_ga);
+ (double)(1.0f/_range_scale), (double)_range_ga);
_reports->print_info("report queue");
}
@@ -1240,63 +1341,87 @@ namespace hmc5883
#endif
const int ERROR = -1;
-HMC5883 *g_dev;
+HMC5883 *g_dev_int;
+HMC5883 *g_dev_ext;
-void start();
-void test();
-void reset();
-void info();
-int calibrate();
+void start(int bus, enum Rotation rotation);
+void test(int bus);
+void reset(int bus);
+void info(int bus);
+int calibrate(int bus);
+void usage();
/**
* Start the driver.
*/
void
-start()
+start(int bus, enum Rotation rotation)
{
int fd;
- if (g_dev != nullptr)
- /* if already started, the still command succeeded */
- errx(0, "already started");
-
/* create the driver, attempt expansion bus first */
- g_dev = new HMC5883(PX4_I2C_BUS_EXPANSION);
- if (g_dev != nullptr && OK != g_dev->init()) {
- delete g_dev;
- g_dev = nullptr;
+ if (bus == -1 || bus == PX4_I2C_BUS_EXPANSION) {
+ if (g_dev_ext != nullptr)
+ errx(0, "already started external");
+ g_dev_ext = new HMC5883(PX4_I2C_BUS_EXPANSION, HMC5883L_DEVICE_PATH_EXT, rotation);
+ if (g_dev_ext != nullptr && OK != g_dev_ext->init()) {
+ delete g_dev_ext;
+ g_dev_ext = nullptr;
+ }
}
#ifdef PX4_I2C_BUS_ONBOARD
/* if this failed, attempt onboard sensor */
- if (g_dev == nullptr) {
- g_dev = new HMC5883(PX4_I2C_BUS_ONBOARD);
- if (g_dev != nullptr && OK != g_dev->init()) {
+ if (bus == -1 || bus == PX4_I2C_BUS_ONBOARD) {
+ if (g_dev_int != nullptr)
+ errx(0, "already started internal");
+ g_dev_int = new HMC5883(PX4_I2C_BUS_ONBOARD, HMC5883L_DEVICE_PATH_INT, rotation);
+ if (g_dev_int != nullptr && OK != g_dev_int->init()) {
+ if (bus == PX4_I2C_BUS_ONBOARD) {
+ goto fail;
+ }
+ }
+ if (g_dev_int == nullptr && bus == PX4_I2C_BUS_ONBOARD) {
goto fail;
}
}
#endif
- if (g_dev == nullptr)
+ if (g_dev_int == nullptr && g_dev_ext == nullptr)
goto fail;
/* set the poll rate to default, starts automatic data collection */
- fd = open(HMC5883L_DEVICE_PATH, O_RDONLY);
+ if (g_dev_int != nullptr) {
+ fd = open(HMC5883L_DEVICE_PATH_INT, O_RDONLY);
+ if (fd < 0)
+ goto fail;
- if (fd < 0)
- goto fail;
+ if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+ goto fail;
+ close(fd);
+ }
- if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
- goto fail;
+ if (g_dev_ext != nullptr) {
+ fd = open(HMC5883L_DEVICE_PATH_EXT, O_RDONLY);
+ if (fd < 0)
+ goto fail;
+
+ if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+ goto fail;
+ close(fd);
+ }
exit(0);
fail:
-
- if (g_dev != nullptr) {
- delete g_dev;
- g_dev = nullptr;
+ if (g_dev_int != nullptr && (bus == -1 || bus == PX4_I2C_BUS_ONBOARD)) {
+ delete g_dev_int;
+ g_dev_int = nullptr;
+ }
+ if (g_dev_ext != nullptr && (bus == -1 || bus == PX4_I2C_BUS_EXPANSION)) {
+ delete g_dev_ext;
+ g_dev_ext = nullptr;
}
errx(1, "driver start failed");
@@ -1308,16 +1433,17 @@ fail:
* and automatic modes.
*/
void
-test()
+test(int bus)
{
struct mag_report report;
ssize_t sz;
int ret;
+ const char *path = (bus==PX4_I2C_BUS_ONBOARD?HMC5883L_DEVICE_PATH_INT:HMC5883L_DEVICE_PATH_EXT);
- int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY);
+ int fd = open(path, O_RDONLY);
if (fd < 0)
- err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", HMC5883L_DEVICE_PATH);
+ err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", path);
/* do a simple demand read */
sz = read(fd, &report, sizeof(report));
@@ -1410,14 +1536,15 @@ test()
* 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 calibrate()
+int calibrate(int bus)
{
int ret;
+ const char *path = (bus==PX4_I2C_BUS_ONBOARD?HMC5883L_DEVICE_PATH_INT:HMC5883L_DEVICE_PATH_EXT);
- int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY);
+ int fd = open(path, O_RDONLY);
if (fd < 0)
- err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", HMC5883L_DEVICE_PATH);
+ err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", path);
if (OK != (ret = ioctl(fd, MAGIOCCALIBRATE, fd))) {
warnx("failed to enable sensor calibration mode");
@@ -1437,9 +1564,11 @@ int calibrate()
* Reset the driver.
*/
void
-reset()
+reset(int bus)
{
- int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY);
+ const char *path = (bus==PX4_I2C_BUS_ONBOARD?HMC5883L_DEVICE_PATH_INT:HMC5883L_DEVICE_PATH_EXT);
+
+ int fd = open(path, O_RDONLY);
if (fd < 0)
err(1, "failed ");
@@ -1457,8 +1586,9 @@ reset()
* Print a little info about the driver.
*/
void
-info()
+info(int bus)
{
+ HMC5883 *g_dev = (bus == PX4_I2C_BUS_ONBOARD?g_dev_int:g_dev_ext);
if (g_dev == nullptr)
errx(1, "driver not running");
@@ -1468,40 +1598,91 @@ info()
exit(0);
}
+void
+usage()
+{
+ warnx("missing command: try 'start', 'info', 'test', 'reset', 'info', 'calibrate'");
+ warnx("options:");
+ warnx(" -R rotation");
+ warnx(" -C calibrate on start");
+ warnx(" -X only external bus");
+#ifdef PX4_I2C_BUS_ONBOARD
+ warnx(" -I only internal bus");
+#endif
+}
+
} // namespace
int
hmc5883_main(int argc, char *argv[])
{
+ int ch;
+ int bus = -1;
+ enum Rotation rotation = ROTATION_NONE;
+ bool calibrate = false;
+
+ while ((ch = getopt(argc, argv, "XIR:C")) != EOF) {
+ switch (ch) {
+ case 'R':
+ rotation = (enum Rotation)atoi(optarg);
+ break;
+#ifdef PX4_I2C_BUS_ONBOARD
+ case 'I':
+ bus = PX4_I2C_BUS_ONBOARD;
+ break;
+#endif
+ case 'X':
+ bus = PX4_I2C_BUS_EXPANSION;
+ break;
+ case 'C':
+ calibrate = true;
+ break;
+ default:
+ hmc5883::usage();
+ exit(0);
+ }
+ }
+
+ const char *verb = argv[optind];
+
/*
* Start/load the driver.
*/
- if (!strcmp(argv[1], "start"))
- hmc5883::start();
+ if (!strcmp(verb, "start")) {
+ hmc5883::start(bus, rotation);
+ if (calibrate) {
+ if (hmc5883::calibrate(bus) == 0) {
+ errx(0, "calibration successful");
+
+ } else {
+ errx(1, "calibration failed");
+ }
+ }
+ }
/*
* Test the driver/device.
*/
- if (!strcmp(argv[1], "test"))
- hmc5883::test();
+ if (!strcmp(verb, "test"))
+ hmc5883::test(bus);
/*
* Reset the driver.
*/
- if (!strcmp(argv[1], "reset"))
- hmc5883::reset();
+ if (!strcmp(verb, "reset"))
+ hmc5883::reset(bus);
/*
* Print driver information.
*/
- if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status"))
- hmc5883::info();
+ if (!strcmp(verb, "info") || !strcmp(verb, "status"))
+ hmc5883::info(bus);
/*
* Autocalibrate the scaling
*/
- if (!strcmp(argv[1], "calibrate")) {
- if (hmc5883::calibrate() == 0) {
+ if (!strcmp(verb, "calibrate")) {
+ if (hmc5883::calibrate(bus) == 0) {
errx(0, "calibration successful");
} else {
generated by cgit v1.2.3 (git 2.39.1) at 2024-06-12 08:54:17 +0000