Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_PrecLand: Use rotate_xy instead of matrix multiplication

This commit is contained in:
Rishabh 2021-06-06 00:13:27 +05:30 committed by Andrew Tridgell
parent 7528d6ee7e
commit 36259bc195
1 changed files with 4 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AC_PrecLand/AC_PrecLand.cpp
View File

@ -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) { 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(); _last_backend_los_meas_ms = _backend->los_meas_time_ms();
_backend->get_los_body(target_vec_unit_body); _backend->get_los_body(target_vec_unit_body);
if (!is_zero(_yaw_align)) {
// Apply sensor yaw alignment rotation // Apply sensor yaw alignment rotation
float sin_yaw_align = sinf(radians(_yaw_align*0.01f)); target_vec_unit_body.rotate_xy(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;
return true; return true;
} else { } else {
return false; return false;