diff options
| author | Darryl Taylor <darryl.c.taylor@gmail.com> | 2014-06-19 13:22:45 +0800 |
|---|---|---|
| committer | Darryl Taylor <darryl.c.taylor@gmail.com> | 2014-06-19 13:22:45 +0800 |
| commit | 2475efe13d8f8d9d801276b73b98759676a64db6 (patch) | |
| tree | d5580ffc8d61df0748c1e49c4dc3bd43d5582962 | |
| parent | 5ed1cf7e8d6201f1f2e0115f4941ac551f61d628 (diff) | |
| download | px4-firmware-2475efe13d8f8d9d801276b73b98759676a64db6.tar.gz px4-firmware-2475efe13d8f8d9d801276b73b98759676a64db6.tar.bz2 px4-firmware-2475efe13d8f8d9d801276b73b98759676a64db6.zip | |
Pre-compute board orientation offsets on param update.
| -rw-r--r-- | src/modules/sensors/sensors.cpp | 15 |
1 files changed, 8 insertions, 7 deletions
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index fe741894b..16fcb4c26 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -229,8 +229,6 @@ private: math::Matrix<3, 3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ math::Matrix<3, 3> _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 */ - - math::Matrix<3, 3> _board_rotation_offset; /**< rotation matrix for fine tuning board offset **/ uint64_t _battery_discharged; /**< battery discharged current in mA*ms */ hrt_abstime _battery_current_timestamp; /**< timestamp of last battery current reading */ @@ -807,10 +805,13 @@ Sensors::parameters_update() param_get(_parameter_handles.board_offset[1], &(_parameters.board_offset[1])); param_get(_parameter_handles.board_offset[2], &(_parameters.board_offset[2])); - _board_rotation_offset.from_euler( M_DEG_TO_RAD_F * _parameters.board_offset[0], + /** fine tune board offset on parameter update **/ + math::Matrix<3, 3> board_rotation_offset; + board_rotation_offset.from_euler( M_DEG_TO_RAD_F * _parameters.board_offset[0], M_DEG_TO_RAD_F * _parameters.board_offset[1], M_DEG_TO_RAD_F * _parameters.board_offset[2]); - + + _board_rotation = _board_rotation * board_rotation_offset; return OK; } @@ -990,7 +991,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report); math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); - vect = _board_rotation * _board_rotation_offset * vect; + vect = _board_rotation * vect; raw.accelerometer_m_s2[0] = vect(0); raw.accelerometer_m_s2[1] = vect(1); @@ -1016,7 +1017,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report); math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); - vect = _board_rotation * _board_rotation_offset * vect; + vect = _board_rotation * vect; raw.gyro_rad_s[0] = vect(0); raw.gyro_rad_s[1] = vect(1); @@ -1047,7 +1048,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) vect = _external_mag_rotation * vect; } else { - vect = _board_rotation * _board_rotation_offset * vect; + vect = _board_rotation * vect; } raw.magnetometer_ga[0] = vect(0); |