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

AP_Follow: support the FOLLOW_TARGET mavlink message 

this is used by qgroundcontrol, allowing follow on a mobile device

note that you must set the qgc option to "always send follow" and also
must set FOLL_ALT_TYPE=2 in ArduPilot
This commit is contained in:
Andrew Tridgell 2022-02-26 13:04:46 +11:00 committed by Randy Mackay
parent 36f68c8b64
commit 4233bf5fda
1 changed files with 68 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

79
libraries/AP_Follow/AP_Follow.cpp
View File

@ -275,13 +275,10 @@ void AP_Follow::handle_msg(const mavlink_message_t &msg)
}
// 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));
bool updated = false;
switch (msg.msgid) {
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: {
// decode message
mavlink_global_position_int_t packet;
mavlink_msg_global_position_int_decode(&msg, &packet);
@ -296,13 +293,11 @@ void AP_Follow::handle_msg(const mavlink_message_t &msg)
// 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
// above home alt
_target_location.set_alt_cm(packet.relative_alt / 10, Location::AltFrame::ABOVE_HOME);
} else {
// absolute altitude
_target_location.alt = packet.alt / 10; // convert millimeters to cm
_target_location.relative_alt = 0; // reset relative_alt flag
_target_location.set_alt_cm(packet.alt / 10, Location::AltFrame::ABSOLUTE);
}
_target_velocity_ned.x = packet.vx * 0.01f; // velocity north
@ -320,6 +315,68 @@ void AP_Follow::handle_msg(const mavlink_message_t &msg)
_sysid.set(msg.sysid);
_automatic_sysid = true;
}
updated = true;
break;
}
case MAVLINK_MSG_ID_FOLLOW_TARGET: {
// decode message
mavlink_follow_target_t packet;
mavlink_msg_follow_target_decode(&msg, &packet);
// ignore message if lat and lon are (exactly) zero
if ((packet.lat == 0 && packet.lon == 0)) {
return;
}
// require at least position
if ((packet.est_capabilities & (1<<0)) == 0) {
return;
}
Location new_loc = _target_location;
new_loc.lat = packet.lat;
new_loc.lng = packet.lon;
new_loc.set_alt_cm(packet.alt*100, Location::AltFrame::ABSOLUTE);
// FOLLOW_TARGET is always AMSL, change the provided alt to
// above home if we are configured for relative alt
if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE &&
!new_loc.change_alt_frame(Location::AltFrame::ABOVE_HOME)) {
return;
}
_target_location = new_loc;
if (packet.est_capabilities & (1<<1)) {
_target_velocity_ned.x = packet.vel[0]; // velocity north
_target_velocity_ned.y = packet.vel[1]; // velocity east
_target_velocity_ned.z = packet.vel[2]; // velocity down
}
// get a local timestamp with correction for transport jitter
_last_location_update_ms = _jitter.correct_offboard_timestamp_msec(packet.timestamp, AP_HAL::millis());
if (packet.est_capabilities & (1<<3)) {
Quaternion q{packet.attitude_q[0], packet.attitude_q[1], packet.attitude_q[2], packet.attitude_q[3]};
float r, p, y;
q.to_euler(r,p,y);
_target_heading = degrees(y);
_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;
}
updated = true;
break;
}
}
if (updated) {
// get estimated location and velocity
Location loc_estimate{};
Vector3f vel_estimate;
UNUSED_RESULT(get_target_location_and_velocity(loc_estimate, vel_estimate));
// log lead's estimated vs reported position
// @LoggerMessage: FOLL