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

AC_PrecLand: rotate target vector based on sensor orient 

Since, the frontend takes care of rotating sensor frame target vector to body frame, the  sitl backend should return unit
vector in sensor frame instead of body frame. This is to have homogeneity among backends.
This commit is contained in:
Shiv Tyagi 2022-06-24 18:33:39 +05:30 committed by Randy Mackay
parent 7b573fa2c4
commit 8bd26dffac
1 changed files with 9 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AC_PrecLand/AC_PrecLand_SITL.cpp
View File

@ -21,6 +21,15 @@ void AC_PrecLand_SITL::update()
_los_meas_body = body_to_ned.mul_transpose(-position).tofloat(); _los_meas_body = body_to_ned.mul_transpose(-position).tofloat();
_distance_to_target = _sitl->precland_sim.option_enabled(SITL::SIM_Precland::Option::ENABLE_TARGET_DISTANCE) ? _los_meas_body.length() : 0.0f; _distance_to_target = _sitl->precland_sim.option_enabled(SITL::SIM_Precland::Option::ENABLE_TARGET_DISTANCE) ? _los_meas_body.length() : 0.0f;
_los_meas_body /= _los_meas_body.length(); _los_meas_body /= _los_meas_body.length();
if (_frontend._orient != Rotation::ROTATION_PITCH_270) {
// rotate body frame vector based on orientation
// this is done to have homogeneity among backends
// frontend rotates it back to get correct body frame vector
_los_meas_body.rotate_inverse(_frontend._orient);
_los_meas_body.rotate_inverse(ROTATION_PITCH_90);
}
_have_los_meas = true; _have_los_meas = true;
_los_meas_time_ms = _sitl->precland_sim.last_update_ms(); _los_meas_time_ms = _sitl->precland_sim.last_update_ms();
} else { } else {