From 408eaf0ad1c8aa87c74f83281de3a7c25fc4b4e6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 18 Aug 2013 09:22:40 +0200 Subject: [PATCH] Add ioctl to find out if mag is external or onboard --- src/drivers/drv_mag.h | 3 +++ src/drivers/hmc5883/hmc5883.cpp | 24 +++++++++++++++++++----- src/drivers/lsm303d/lsm303d.cpp | 15 ++++++++++++--- 3 files changed, 34 insertions(+), 8 deletions(-) diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h index 9aab995a17..e95034e8e3 100644 --- a/src/drivers/drv_mag.h +++ b/src/drivers/drv_mag.h @@ -111,4 +111,7 @@ ORB_DECLARE(sensor_mag); /** perform self test and report status */ #define MAGIOCSELFTEST _MAGIOC(7) +/** determine if mag is external or onboard */ +#define MAGIOCGEXTERNAL _MAGIOC(8) + #endif /* _DRV_MAG_H */ diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 039b496f4b..9e9c067d53 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -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. @@ -326,7 +328,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; @@ -665,6 +668,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); @@ -851,12 +860,12 @@ 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 seems to be rolled 180deg, therefore y and z inverted */ + _reports[_next_report].x = ((report.x * _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.y == -32768) ? 32767 : -report.y) * _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; + _reports[_next_report].z = ((((report.z == -32768) ? 32767 : -report.z) * _range_scale) - _scale.z_offset) * _scale.z_scale; #ifdef PX4_I2C_BUS_ONBOARD } #endif @@ -1293,6 +1302,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"); diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 117583fafe..3e6ce64b81 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -869,6 +869,10 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) // return self_test(); return -EINVAL; + case MAGIOCGEXTERNAL: + /* no external mag board yet */ + return 0; + default: /* give it to the superclass */ return SPI::ioctl(filp, cmd, arg); @@ -1422,7 +1426,7 @@ test() int fd_accel = -1; struct accel_report accel_report; ssize_t sz; - int filter_bandwidth; + int ret; /* get the driver */ fd_accel = open(ACCEL_DEVICE_PATH, O_RDONLY); @@ -1445,10 +1449,10 @@ test() warnx("accel z: \t%d\traw", (int)accel_report.z_raw); warnx("accel range: %8.4f m/s^2", (double)accel_report.range_m_s2); - if (ERROR == (filter_bandwidth = ioctl(fd_accel, ACCELIOCGLOWPASS, 0))) + if (ERROR == (ret = ioctl(fd_accel, ACCELIOCGLOWPASS, 0))) warnx("accel antialias filter bandwidth: fail"); else - warnx("accel antialias filter bandwidth: %d Hz", filter_bandwidth); + warnx("accel antialias filter bandwidth: %d Hz", ret); int fd_mag = -1; struct mag_report m_report; @@ -1459,6 +1463,11 @@ test() if (fd_mag < 0) err(1, "%s open failed", MAG_DEVICE_PATH); + /* check if mag is onboard or external */ + if ((ret = ioctl(fd_mag, MAGIOCGEXTERNAL, 0)) < 0) + errx(1, "failed to get if mag is onboard or external"); + warnx("device active: %s", ret ? "external" : "onboard"); + /* do a simple demand read */ sz = read(fd_mag, &m_report, sizeof(m_report));