Browse Source

AC_PrecLand: Use rotate_xy instead of matrix multiplication

c415-sdk
Rishabh 4 years ago committed by Randy Mackay
parent
commit
08b8a43ab5
  1. 15
      libraries/AC_PrecLand/AC_PrecLand.cpp

15
libraries/AC_PrecLand/AC_PrecLand.cpp

@ -405,17 +405,10 @@ bool AC_PrecLand::retrieve_los_meas(Vector3f& target_vec_unit_body) @@ -405,17 +405,10 @@ bool AC_PrecLand::retrieve_los_meas(Vector3f& target_vec_unit_body)
if (_backend->have_los_meas() && _backend->los_meas_time_ms() != _last_backend_los_meas_ms) {
_last_backend_los_meas_ms = _backend->los_meas_time_ms();
_backend->get_los_body(target_vec_unit_body);
// Apply sensor yaw alignment rotation
float sin_yaw_align = sinf(radians(_yaw_align*0.01f));
float cos_yaw_align = cosf(radians(_yaw_align*0.01f));
Matrix3f Rz = Matrix3f(
cos_yaw_align, -sin_yaw_align, 0,
sin_yaw_align, cos_yaw_align, 0,
0, 0, 1
);
target_vec_unit_body = Rz*target_vec_unit_body;
if (!is_zero(_yaw_align)) {
// Apply sensor yaw alignment rotation
target_vec_unit_body.rotate_xy(radians(_yaw_align*0.01f));
}
return true;
} else {
return false;

Loading…
Cancel
Save