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

AC_PrecLand: Account for vertical camera offset in alt calculations

This commit is contained in:
Rishabh 2021-06-05 16:15:13 +05:30 committed by Randy Mackay
parent f1e4f6dbc8
commit 53bd545fac
1 changed files with 17 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AC_PrecLand/AC_PrecLand.cpp
View File

@ -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;
}
}