// 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;
}
// handle mavlink DISTANCE_SENSOR messages
void AP_Follow::handle_msg(const mavlink_message_t &msg)
{
// exit immediately if not enabled
if (!_enabled) {
return;
}
// skip our own messages
if (msg.sysid == mavlink_system.sysid) {
return;
}
// skip message if not from our target
if (_sysid != 0 && msg.sysid != _sysid) {
if (_automatic_sysid) {
// maybe timeout who we were following...
if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_SYSID_TIMEOUT_MS)) {
_sysid.set(0);
}
}
return;
}
// decode global-position-int message
if (msg.msgid == MAVLINK_MSG_ID_GLOBAL_POSITION_INT) {
// get estimated location and velocity (for logging)
Location loc_estimate{};
Vector3f vel_estimate;
UNUSED_RESULT(get_target_location_and_velocity(loc_estimate, vel_estimate));
// decode message
mavlink_global_position_int_t packet;
mavlink_msg_global_position_int_decode(&msg, &packet);
// ignore message if lat and lon are (exactly) zero
if ((packet.lat == 0 && packet.lon == 0)) {
return;
}
_target_location.lat = packet.lat;
_target_location.lng = packet.lon;
// select altitude source based on FOLL_ALT_TYPE param
if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
// relative altitude
_target_location.alt = packet.relative_alt / 10; // convert millimeters to cm
_target_location.relative_alt = 1; // set relative_alt flag
} else {
// absolute altitude
_target_location.alt = packet.alt / 10; // convert millimeters to cm
_target_location.relative_alt = 0; // reset relative_alt flag
}
_target_velocity_ned.x = packet.vx * 0.01f; // velocity north
_target_velocity_ned.y = packet.vy * 0.01f; // velocity east
_target_velocity_ned.z = packet.vz * 0.01f; // velocity down
// get a local timestamp with correction for transport jitter
_last_location_update_ms = _jitter.correct_offboard_timestamp_msec(packet.time_boot_ms, AP_HAL::millis());
if (packet.hdg <= 36000) { // heading (UINT16_MAX if unknown)
_target_heading = packet.hdg * 0.01f; // convert centi-degrees to degrees
_last_heading_update_ms = _last_location_update_ms;
}
// initialise _sysid if zero to sender's id
if (_sysid == 0) {
_sysid.set(msg.sysid);
_automatic_sysid = true;
}
// log lead's estimated vs reported position
AP::logger().Write("FOLL",
"TimeUS,Lat,Lon,Alt,VelN,VelE,VelD,LatE,LonE,AltE", // labels
"sDUmnnnDUm", // units
"F--B000--B", // mults
"QLLifffLLi", // fmt
AP_HAL::micros64(),
_target_location.lat,
_target_location.lng,
_target_location.alt,
(double)_target_velocity_ned.x,
(double)_target_velocity_ned.y,
(double)_target_velocity_ned.z,
loc_estimate.lat,
loc_estimate.lng,
loc_estimate.alt
);
}
}
// handle mavlink DISTANCE_SENSOR messages
void AP_Follow::handle_msg(const mavlink_message_t &msg)
{
// exit immediately if not enabled
if (!_enabled) {
return;
}
// skip our own messages
if (msg.sysid == mavlink_system.sysid) {
return;
}
// skip message if not from our target
if ((_sysid_to_follow != 0) && (msg.sysid != _sysid_to_follow)) {
if (_sysid == 0) {
// maybe timeout who we were following...
if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_SYSID_TIMEOUT_MS)) {
_sysid_to_follow = 0;
}
}
return;
}
// decode global-position-int message
if (msg.msgid == MAVLINK_MSG_ID_GLOBAL_POSITION_INT) {
const uint32_t now = AP_HAL::millis();
// get estimated location and velocity (for logging)
Location loc_estimate{};
Vector3f vel_estimate;
UNUSED_RESULT(get_target_location_and_velocity(loc_estimate, vel_estimate));
// decode message
mavlink_global_position_int_t packet;
mavlink_msg_global_position_int_decode(&msg, &packet);
// ignore message if lat and lon are (exactly) zero
if ((packet.lat == 0 && packet.lon == 0)) {
return;
}
_target_location.lat = packet.lat;
_target_location.lng = packet.lon;
// select altitude source based on FOLL_ALT_TYPE param
if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
// relative altitude
_target_location.alt = packet.relative_alt / 10; // convert millimeters to cm
_target_location.flags.relative_alt = 1; // set relative_alt flag
} else {
// absolute altitude
_target_location.alt = packet.alt / 10; // convert millimeters to cm
_target_location.flags.relative_alt = 0; // reset relative_alt flag
}
_target_velocity_ned.x = packet.vx * 0.01f; // velocity north
_target_velocity_ned.y = packet.vy * 0.01f; // velocity east
_target_velocity_ned.z = packet.vz * 0.01f; // velocity down
_last_location_update_ms = now;
if (packet.hdg <= 36000) { // heading (UINT16_MAX if unknown)
_target_heading = packet.hdg * 0.01f; // convert centi-degrees to degrees
_last_heading_update_ms = now;
}
// initialise _sysid if zero to sender's id
if (_sysid_to_follow == 0) {
_sysid_to_follow = msg.sysid;
}
if ((now - _last_location_sent_to_gcs) > AP_GCS_INTERVAL_MS) {
_last_location_sent_to_gcs = now;
gcs().send_text(MAV_SEVERITY_INFO, "Foll: %u %ld %ld %4.2f\n",
(unsigned)_sysid_to_follow,
(long)_target_location.lat,
(long)_target_location.lng,
(double)(_target_location.alt * 0.01f)); // cm to m
}
// log lead's estimated vs reported position
DataFlash_Class::instance()->Log_Write("FOLL",
"TimeUS,Lat,Lon,Alt,VelN,VelE,VelD,LatE,LonE,AltE", // labels
"sDUmnnnDUm", // units
"F--B000--B", // mults
"QLLifffLLi", // fmt
AP_HAL::micros64(),
_target_location.lat,
_target_location.lng,
_target_location.alt,
(double)_target_velocity_ned.x,
(double)_target_velocity_ned.y,
(double)_target_velocity_ned.z,
loc_estimate.lat,
loc_estimate.lng,
loc_estimate.alt
);
}
}
