diff options
author | Julian Oes <julian@oes.ch> | 2013-08-18 09:33:59 +0200 |
---|---|---|
committer | Julian Oes <julian@oes.ch> | 2013-08-18 09:33:59 +0200 |
commit | bc717f1715590a6915c223dde53fe6e1139f92d2 (patch) | |
tree | 53fc67d583168b33c2aaaa6a365c93c1d873ff7f /src/modules | |
parent | 408eaf0ad1c8aa87c74f83281de3a7c25fc4b4e6 (diff) | |
download | px4-firmware-bc717f1715590a6915c223dde53fe6e1139f92d2.tar.gz px4-firmware-bc717f1715590a6915c223dde53fe6e1139f92d2.tar.bz2 px4-firmware-bc717f1715590a6915c223dde53fe6e1139f92d2.zip |
Sensors should now take into account the orientation of an external mag
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/sensors/sensors.cpp | 20 |
1 files changed, 16 insertions, 4 deletions
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index c47f6bb7d..198da9f0a 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]; @@ -537,7 +538,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 */ @@ -948,6 +950,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); } @@ -1044,10 +1054,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); |