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