From 8bd26dffaccf4dc35c0c0035801eb2bca8f1b811 Mon Sep 17 00:00:00 2001 From: Shiv Tyagi Date: Fri, 24 Jun 2022 18:33:39 +0530 Subject: [PATCH] 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. --- libraries/AC_PrecLand/AC_PrecLand_SITL.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/libraries/AC_PrecLand/AC_PrecLand_SITL.cpp b/libraries/AC_PrecLand/AC_PrecLand_SITL.cpp index 99a3261a29..b71a4d1c26 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_SITL.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand_SITL.cpp @@ -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 {