diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-08-19 08:37:11 -0700 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-08-19 08:37:11 -0700 |
commit | 230c09e2f44e0bcf8613815e2f771b4dffe8b042 (patch) | |
tree | 37f2b731fb9a0097b64c42c59c6a6235c3318c31 /src/modules/sensors/sensors.cpp | |
parent | cdd09333f946e746527c8e9af36e08dc3a29a975 (diff) | |
parent | a95e48c0b25c75548e923657247cdf268e654097 (diff) | |
download | px4-firmware-230c09e2f44e0bcf8613815e2f771b4dffe8b042.tar.gz px4-firmware-230c09e2f44e0bcf8613815e2f771b4dffe8b042.tar.bz2 px4-firmware-230c09e2f44e0bcf8613815e2f771b4dffe8b042.zip |
Merge pull request #11 from cvg/fmuv2_bringup_ext_mag_rotation
Support for external mags with different rotation
Diffstat (limited to 'src/modules/sensors/sensors.cpp')
-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); |