dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Pre-compute board orientation offsets on param update.

This commit is contained in:
Darryl Taylor 2014-06-19 13:22:45 +08:00
parent 5ed1cf7e8d
commit 2475efe13d
1 changed files with 8 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
src/modules/sensors/sensors.cpp
View File

@ -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);