From 53bd545facc03294a8fff78956def59443165cb9 Mon Sep 17 00:00:00 2001 From: Rishabh Date: Sat, 5 Jun 2021 16:15:13 +0530 Subject: [PATCH] AC_PrecLand: Account for vertical camera offset in alt calculations --- libraries/AC_PrecLand/AC_PrecLand.cpp | 24 +++++++++++++++++------- 1 file changed, 17 insertions(+), 7 deletions(-) diff --git a/libraries/AC_PrecLand/AC_PrecLand.cpp b/libraries/AC_PrecLand/AC_PrecLand.cpp index 1c66d7700a..5f71f7dc1e 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand.cpp @@ -428,25 +428,35 @@ bool AC_PrecLand::construct_pos_meas_using_rangefinder(float rangefinder_alt_m, if (retrieve_los_meas(target_vec_unit_body)) { const struct inertial_data_frame_s *inertial_data_delayed = (*_inertial_history)[0]; - Vector3f target_vec_unit_ned = inertial_data_delayed->Tbn * target_vec_unit_body; - bool target_vec_valid = target_vec_unit_ned.z > 0.0f; - bool alt_valid = (rangefinder_alt_valid && rangefinder_alt_m > 0.0f) || (_backend->distance_to_target() > 0.0f); + const Vector3f target_vec_unit_ned = inertial_data_delayed->Tbn * target_vec_unit_body; + const bool target_vec_valid = target_vec_unit_ned.z > 0.0f; + const bool alt_valid = (rangefinder_alt_valid && rangefinder_alt_m > 0.0f) || (_backend->distance_to_target() > 0.0f); if (target_vec_valid && alt_valid) { float dist, alt; + // figure out ned camera orientation w.r.t its offset + Vector3f cam_pos_ned; + if (!_cam_offset.get().is_zero()) { + // user has specifed offset for camera + // take its height into account while calculating distance + cam_pos_ned = inertial_data_delayed->Tbn * _cam_offset; + } if (_backend->distance_to_target() > 0.0f) { + // sensor has provided distance to landing target dist = _backend->distance_to_target(); alt = dist * target_vec_unit_ned.z; } else { - alt = MAX(rangefinder_alt_m, 0.0f); + // sensor only knows the horizontal location of the landing target + // rely on rangefinder for the vertical target + alt = MAX(rangefinder_alt_m - cam_pos_ned.z, 0.0f); dist = alt / target_vec_unit_ned.z; } // Compute camera position relative to IMU - Vector3f accel_body_offset = AP::ins().get_imu_pos_offset(AP::ahrs().get_primary_accel_index()); - Vector3f cam_pos_ned = inertial_data_delayed->Tbn * (_cam_offset.get() - accel_body_offset); + const Vector3f accel_pos_ned = inertial_data_delayed->Tbn * AP::ins().get_imu_pos_offset(AP::ahrs().get_primary_accel_index()); + const Vector3f cam_pos_ned_rel_imu = cam_pos_ned - accel_pos_ned; // Compute target position relative to IMU - _target_pos_rel_meas_NED = Vector3f(target_vec_unit_ned.x*dist, target_vec_unit_ned.y*dist, alt) + cam_pos_ned; + _target_pos_rel_meas_NED = Vector3f{target_vec_unit_ned.x*dist, target_vec_unit_ned.y*dist, alt} + cam_pos_ned_rel_imu; return true; } }