// get distance vector to target (in meters) and target's velocity all in NED frame
bool AP_Follow::get_target_dist_and_vel_ned(Vector3f &dist_ned, Vector3f &dist_with_offs, Vector3f &vel_ned)
{
// get our location
Location current_loc;
if (!AP::ahrs().get_position(current_loc)) {
clear_dist_and_bearing_to_target();
return false;
}
// get target location and velocity
Location target_loc;
Vector3f veh_vel;
if (!get_target_location_and_velocity(target_loc, veh_vel)) {
clear_dist_and_bearing_to_target();
return false;
}
// change to altitude above home if relative altitude is being used
if (target_loc.relative_alt == 1) {
current_loc.alt -= AP::ahrs().get_home().alt;
}
// calculate difference
const Vector3f dist_vec = current_loc.get_distance_NED(target_loc);
// fail if too far
if (is_positive(_dist_max.get()) && (dist_vec.length() > _dist_max)) {
clear_dist_and_bearing_to_target();
return false;
}
// initialise offsets from distance vector if required
init_offsets_if_required(dist_vec);
// get offsets
Vector3f offsets;
if (!get_offsets_ned(offsets)) {
clear_dist_and_bearing_to_target();
return false;
}
// calculate results
dist_ned = dist_vec;
dist_with_offs = dist_vec + offsets;
vel_ned = veh_vel;
// record distance and heading for reporting purposes
if (is_zero(dist_with_offs.x) && is_zero(dist_with_offs.y)) {
clear_dist_and_bearing_to_target();
} else {
_dist_to_target = safe_sqrt(sq(dist_with_offs.x) + sq(dist_with_offs.y));
_bearing_to_target = degrees(atan2f(dist_with_offs.y, dist_with_offs.x));
}
return true;
}
// get distance vector to target (in meters) and target's velocity all in NED frame
bool AP_Follow::get_target_dist_and_vel_ned(Vector3f &dist_ned, Vector3f &dist_with_offs, Vector3f &vel_ned)
{
// get our location
Location current_loc;
if (!AP::ahrs().get_position(current_loc)) {
return false;
}
// get target location and velocity
Location target_loc;
Vector3f veh_vel;
if (!get_target_location_and_velocity(target_loc, veh_vel)) {
return false;
}
// change to altitude above home if relative altitude is being used
if (target_loc.flags.relative_alt == 1) {
current_loc.alt -= AP::ahrs().get_home().alt;
}
// calculate difference
const Vector3f dist_vec = location_3d_diff_NED(current_loc, target_loc);
// fail if too far
if (is_positive(_dist_max.get()) && (dist_vec.length() > _dist_max)) {
return false;
}
// initialise offsets from distance vector if required
init_offsets_if_required(dist_vec);
// get offsets
Vector3f offsets;
if (!get_offsets_ned(offsets)) {
return false;
}
// return results
dist_ned = dist_vec;
dist_with_offs = dist_vec + offsets;
vel_ned = veh_vel;
return true;
}