Browse Source

AC_PrecLand: fix usage of distance_to_target

master
Jonathan Challinger 8 years ago committed by Randy Mackay
parent
commit
9979e4bbe5
  1. 7
      libraries/AC_PrecLand/AC_PrecLand.cpp

7
libraries/AC_PrecLand/AC_PrecLand.cpp

@ -337,13 +337,14 @@ bool AC_PrecLand::construct_pos_meas_using_rangefinder(float rangefinder_alt_m, @@ -337,13 +337,14 @@ bool AC_PrecLand::construct_pos_meas_using_rangefinder(float rangefinder_alt_m,
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);
if (target_vec_valid && alt_valid) {
float alt;
float dist, alt;
if (_backend->distance_to_target() > 0.0f) {
alt = _backend->distance_to_target();
dist = _backend->distance_to_target();
alt = dist * target_vec_unit_ned.z;
} else {
alt = MAX(rangefinder_alt_m, 0.0f);
dist = alt / target_vec_unit_ned.z;
}
float dist = alt/target_vec_unit_ned.z;
// Compute camera position relative to IMU
Vector3f accel_body_offset = _ahrs.get_ins().get_imu_pos_offset(_ahrs.get_primary_accel_index());

Loading…
Cancel
Save