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.cpp143
1 files changed, 88 insertions, 55 deletions
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index ac3bdc132..d77f03bb7 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -58,7 +58,7 @@
#include <nuttx/wqueue.h>
#include <nuttx/clock.h>
-#include <arch/board/board.h>
+#include <board_config.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
@@ -77,8 +77,8 @@
#define HMC5883L_ADDRESS PX4_I2C_OBDEV_HMC5883
-/* Max measurement rate is 160Hz */
-#define HMC5883_CONVERSION_INTERVAL (1000000 / 160) /* microseconds */
+/* 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 */
#define ADDR_CONF_A 0x00
#define ADDR_CONF_B 0x01
@@ -167,6 +167,8 @@ private:
bool _sensor_ok; /**< sensor was found and reports ok */
bool _calibrated; /**< the calibration is valid */
+ int _bus; /**< the bus the device is connected to */
+
/**
* Test whether the device supported by the driver is present at a
* specific address.
@@ -190,6 +192,11 @@ private:
void stop();
/**
+ * Reset the device
+ */
+ int reset();
+
+ /**
* Perform the on-sensor scale calibration routine.
*
* @note The sensor will continue to provide measurements, these
@@ -326,7 +333,8 @@ HMC5883::HMC5883(int bus) :
_comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")),
_buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")),
_sensor_ok(false),
- _calibrated(false)
+ _calibrated(false),
+ _bus(bus)
{
// enable debug() calls
_debug_enabled = false;
@@ -362,6 +370,9 @@ HMC5883::init()
if (I2C::init() != OK)
goto out;
+ /* reset the device configuration */
+ reset();
+
/* allocate basic report buffers */
_num_reports = 2;
_reports = new struct mag_report[_num_reports];
@@ -378,9 +389,6 @@ HMC5883::init()
if (_mag_topic < 0)
debug("failed to create sensor_mag object");
- /* set range */
- set_range(_range_ga);
-
ret = OK;
/* sensor is ok, but not calibrated */
_sensor_ok = true;
@@ -539,68 +547,67 @@ int
HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
-
case SENSORIOCSPOLLRATE: {
- switch (arg) {
+ switch (arg) {
- /* switching to manual polling */
- case SENSOR_POLLRATE_MANUAL:
- stop();
- _measure_ticks = 0;
- return OK;
+ /* switching to manual polling */
+ case SENSOR_POLLRATE_MANUAL:
+ stop();
+ _measure_ticks = 0;
+ return OK;
- /* external signalling (DRDY) not supported */
- case SENSOR_POLLRATE_EXTERNAL:
+ /* external signalling (DRDY) not supported */
+ case SENSOR_POLLRATE_EXTERNAL:
- /* zero would be bad */
- case 0:
- return -EINVAL;
+ /* zero would be bad */
+ case 0:
+ return -EINVAL;
- /* set default/max polling rate */
- case SENSOR_POLLRATE_MAX:
- case SENSOR_POLLRATE_DEFAULT: {
- /* do we need to start internal polling? */
- bool want_start = (_measure_ticks == 0);
+ /* set default/max polling rate */
+ case SENSOR_POLLRATE_MAX:
+ case SENSOR_POLLRATE_DEFAULT: {
+ /* do we need to start internal polling? */
+ bool want_start = (_measure_ticks == 0);
- /* set interval for next measurement to minimum legal value */
- _measure_ticks = USEC2TICK(HMC5883_CONVERSION_INTERVAL);
+ /* set interval for next measurement to minimum legal value */
+ _measure_ticks = USEC2TICK(HMC5883_CONVERSION_INTERVAL);
- /* if we need to start the poll state machine, do it */
- if (want_start)
- start();
+ /* if we need to start the poll state machine, do it */
+ if (want_start)
+ start();
- return OK;
- }
+ return OK;
+ }
- /* adjust to a legal polling interval in Hz */
- default: {
- /* do we need to start internal polling? */
- bool want_start = (_measure_ticks == 0);
+ /* adjust to a legal polling interval in Hz */
+ default: {
+ /* do we need to start internal polling? */
+ bool want_start = (_measure_ticks == 0);
- /* convert hz to tick interval via microseconds */
- unsigned ticks = USEC2TICK(1000000 / arg);
+ /* convert hz to tick interval via microseconds */
+ unsigned ticks = USEC2TICK(1000000 / arg);
- /* check against maximum rate */
- if (ticks < USEC2TICK(HMC5883_CONVERSION_INTERVAL))
- return -EINVAL;
+ /* check against maximum rate */
+ if (ticks < USEC2TICK(HMC5883_CONVERSION_INTERVAL))
+ return -EINVAL;
- /* update interval for next measurement */
- _measure_ticks = ticks;
+ /* update interval for next measurement */
+ _measure_ticks = ticks;
- /* if we need to start the poll state machine, do it */
- if (want_start)
- start();
+ /* if we need to start the poll state machine, do it */
+ if (want_start)
+ start();
- return OK;
- }
+ return OK;
}
}
+ }
case SENSORIOCGPOLLRATE:
if (_measure_ticks == 0)
return SENSOR_POLLRATE_MANUAL;
- return (1000 / _measure_ticks);
+ return 1000000/TICK2USEC(_measure_ticks);
case SENSORIOCSQUEUEDEPTH: {
/* add one to account for the sentinel in the ring */
@@ -630,17 +637,24 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
return _num_reports - 1;
case SENSORIOCRESET:
- /* XXX implement this */
- return -EINVAL;
+ return reset();
case MAGIOCSSAMPLERATE:
- /* not supported, always 1 sample per poll */
- return -EINVAL;
+ /* same as pollrate because device is in single measurement mode*/
+ return ioctl(filp, SENSORIOCSPOLLRATE, arg);
+
+ case MAGIOCGSAMPLERATE:
+ /* same as pollrate because device is in single measurement mode*/
+ return 1000000/TICK2USEC(_measure_ticks);
case MAGIOCSRANGE:
return set_range(arg);
+ case MAGIOCGRANGE:
+ return _range_ga;
+
case MAGIOCSLOWPASS:
+ case MAGIOCGLOWPASS:
/* not supported, no internal filtering */
return -EINVAL;
@@ -665,6 +679,12 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
case MAGIOCSELFTEST:
return check_calibration();
+ case MAGIOCGEXTERNAL:
+ if (_bus == PX4_I2C_BUS_EXPANSION)
+ return 1;
+ else
+ return 0;
+
default:
/* give it to the superclass */
return I2C::ioctl(filp, cmd, arg);
@@ -688,6 +708,13 @@ HMC5883::stop()
work_cancel(HPWORK, &_work);
}
+int
+HMC5883::reset()
+{
+ /* set range */
+ return set_range(_range_ga);
+}
+
void
HMC5883::cycle_trampoline(void *arg)
{
@@ -851,10 +878,11 @@ HMC5883::collect()
_reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
} else {
#endif
- /* XXX axis assignment of external sensor is yet unknown */
- _reports[_next_report].x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
+ /* 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 */
+ _reports[_next_report].x = ((((report.y == -32768) ? 32767 : -report.y) * _range_scale) - _scale.x_offset) * _scale.x_scale;
/* flip axes and negate value for y */
- _reports[_next_report].y = ((((report.x == -32768) ? 32767 : -report.x) * _range_scale) - _scale.y_offset) * _scale.y_scale;
+ _reports[_next_report].y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale;
/* z remains z */
_reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
#ifdef PX4_I2C_BUS_ONBOARD
@@ -1293,6 +1321,11 @@ test()
warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z);
warnx("time: %lld", report.timestamp);
+ /* check if mag is onboard or external */
+ if ((ret = ioctl(fd, MAGIOCGEXTERNAL, 0)) < 0)
+ errx(1, "failed to get if mag is onboard or external");
+ warnx("device active: %s", ret ? "external" : "onboard");
+
/* set the queue depth to 10 */
if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10))
errx(1, "failed to set queue depth");
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-16 10:53:41 +0000