diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-08-19 17:37:22 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-08-19 17:37:22 +0200 |
commit | 24d8ca509250a5bc4d4de1f7514abd96eb5add2b (patch) | |
tree | edf53c179b92aeb9a04b905d9d6fd973a4293b69 /src | |
parent | f4b5a17a7b6385869933cd195afd674fa532e735 (diff) | |
parent | 230c09e2f44e0bcf8613815e2f771b4dffe8b042 (diff) | |
download | px4-firmware-24d8ca509250a5bc4d4de1f7514abd96eb5add2b.tar.gz px4-firmware-24d8ca509250a5bc4d4de1f7514abd96eb5add2b.tar.bz2 px4-firmware-24d8ca509250a5bc4d4de1f7514abd96eb5add2b.zip |
Merge branch 'fmuv2_bringup' of github.com:cvg/Firmware_Private into fmuv2_bringup_new_state_machine_drton
Diffstat (limited to 'src')
-rw-r--r-- | src/drivers/drv_mag.h | 3 | ||||
-rw-r--r-- | src/drivers/hmc5883/hmc5883.cpp | 21 | ||||
-rw-r--r-- | src/drivers/lsm303d/lsm303d.cpp | 15 | ||||
-rw-r--r-- | src/modules/sensors/sensors.cpp | 20 |
4 files changed, 49 insertions, 10 deletions
diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h index 9aab995a1..e95034e8e 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 039b496f4..692f890bd 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,8 +860,9 @@ 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 and invert x and 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; /* z remains z */ @@ -1293,6 +1303,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 117583faf..3e6ce64b8 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)); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 6e57a79a8..b237bd059 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -284,6 +284,7 @@ private: math::Matrix _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ math::Matrix _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ + bool _mag_is_external; /**< true if the active mag is on an external board */ struct { float min[_rc_max_chan_count]; @@ -535,7 +536,8 @@ Sensors::Sensors() : _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")), _board_rotation(3,3), - _external_mag_rotation(3,3) + _external_mag_rotation(3,3), + _mag_is_external(false) { /* basic r/c parameters */ @@ -941,6 +943,14 @@ Sensors::mag_init() /* set the driver to poll at 150Hz */ ioctl(fd, SENSORIOCSPOLLRATE, 150); + int ret = ioctl(fd, MAGIOCGEXTERNAL, 0); + if (ret < 0) + errx(1, "FATAL: unknown if magnetometer is external or onboard"); + else if (ret == 1) + _mag_is_external = true; + else + _mag_is_external = false; + close(fd); } @@ -1037,10 +1047,12 @@ Sensors::mag_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report); - // XXX TODO add support for external mag orientation - math::Vector3 vect = {mag_report.x, mag_report.y, mag_report.z}; - vect = _board_rotation*vect; + + if (_mag_is_external) + vect = _external_mag_rotation*vect; + else + vect = _board_rotation*vect; raw.magnetometer_ga[0] = vect(0); raw.magnetometer_ga[1] = vect(1); |