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/modules/sensors | |
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/modules/sensors')
-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 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); |