Browse Source

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.
master_rangefinder
Shiv Tyagi 3 years ago committed by Randy Mackay
parent
commit
8bd26dffac
  1. 9
      libraries/AC_PrecLand/AC_PrecLand_SITL.cpp

9
libraries/AC_PrecLand/AC_PrecLand_SITL.cpp

@ -21,6 +21,15 @@ void AC_PrecLand_SITL::update() @@ -21,6 +21,15 @@ void AC_PrecLand_SITL::update()
_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;
_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;
_los_meas_time_ms = _sitl->precland_sim.last_update_ms();
} else {

Loading…
Cancel
Save