diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2015-02-10 22:01:31 +0100 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2015-02-10 22:01:31 +0100 |
| commit | 81c9b9fd9525261a50ae12ba32083fe48671fab9 (patch) | |
| tree | 090d947f9751a7ba13f4e1a9bafbe0571a4e4a53 | |
| parent | d4c7e66212b5f36c1393ca95708495a750cb6e70 (diff) | |
| download | px4-firmware-81c9b9fd9525261a50ae12ba32083fe48671fab9.tar.gz px4-firmware-81c9b9fd9525261a50ae12ba32083fe48671fab9.tar.bz2 px4-firmware-81c9b9fd9525261a50ae12ba32083fe48671fab9.zip | |
Get secondary mag to work, but only if internal
| -rw-r--r-- | src/modules/sensors/sensors.cpp | 20 |
1 files changed, 20 insertions, 0 deletions
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index c5b7a2265..b93b720be 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1199,6 +1199,16 @@ Sensors::mag_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_mag), _mag1_sub, &mag_report); + math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); + + // XXX presume internal mag + + vect = _board_rotation * vect; + + raw.magnetometer1_ga[0] = vect(0); + raw.magnetometer1_ga[1] = vect(1); + raw.magnetometer1_ga[2] = vect(2); + raw.magnetometer1_raw[0] = mag_report.x_raw; raw.magnetometer1_raw[1] = mag_report.y_raw; raw.magnetometer1_raw[2] = mag_report.z_raw; @@ -1214,6 +1224,16 @@ Sensors::mag_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_mag), _mag2_sub, &mag_report); + math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); + + // XXX presume internal mag + + vect = _board_rotation * vect; + + raw.magnetometer2_ga[0] = vect(0); + raw.magnetometer2_ga[1] = vect(1); + raw.magnetometer2_ga[2] = vect(2); + raw.magnetometer2_raw[0] = mag_report.x_raw; raw.magnetometer2_raw[1] = mag_report.y_raw; raw.magnetometer2_raw[2] = mag_report.z_raw; |