Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 02:55:07 +08:00
Code Issues Actions 19 Packages Projects Releases Wiki Activity

Sensors should now take into account the orientation of an external mag

Browse Source
This commit is contained in:
Julian Oes
2013-08-18 09:33:59 +02:00
parent 408eaf0ad1
commit bc717f1715
1 changed files with 16 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/sensors.cpp
+16 -4
View File
@@ -284,6 +284,7 @@ private:
math::Matrix _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ 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 */ 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 { struct {
float min[_rc_max_chan_count]; float min[_rc_max_chan_count];
@@ -537,7 +538,8 @@ Sensors::Sensors() :
_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")), _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")),
_board_rotation(3,3), _board_rotation(3,3),
_external_mag_rotation(3,3) _external_mag_rotation(3,3),
_mag_is_external(false)
{ {
/* basic r/c parameters */ /* basic r/c parameters */
@@ -948,6 +950,14 @@ Sensors::mag_init()
/* set the driver to poll at 150Hz */ /* set the driver to poll at 150Hz */
ioctl(fd, SENSORIOCSPOLLRATE, 150); 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); close(fd);
} }
@@ -1044,10 +1054,12 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report); 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}; 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[0] = vect(0);
raw.magnetometer_ga[1] = vect(1); raw.magnetometer_ga[1] = vect(1);