Skip to content

Commit

Permalink
AP_Follow: changes to support scripted plane follow
Browse files Browse the repository at this point in the history
  • Loading branch information
@timtuxworth
timtuxworth committed Nov 8, 2024
1 parent 02b0578 commit 3cd81ef
Show file tree
Hide file tree
Showing 2 changed files with 85 additions and 10 deletions.
82 changes: 72 additions & 10 deletions libraries/AP_Follow/AP_Follow.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,20 +29,23 @@

extern const AP_HAL::HAL& hal;

#define AP_FOLLOW_TIMEOUT_MS 3000 // position estimate timeout after 1 second
#define AP_FOLLOW_SYSID_TIMEOUT_MS 10000 // forget sysid we are following if we have not heard from them in 10 seconds

#define AP_FOLLOW_OFFSET_TYPE_NED 0 // offsets are in north-east-down frame
#define AP_FOLLOW_OFFSET_TYPE_RELATIVE 1 // offsets are relative to lead vehicle's heading

#define AP_FOLLOW_ALTITUDE_TYPE_RELATIVE 1 // relative altitude is used by default
#define AP_FOLLOW_ALTITUDE_TYPE_RELATIVE 1 // home relative altitude is used by default

#define AP_FOLLOW_POS_P_DEFAULT 0.1f // position error gain default

#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
#define AP_FOLLOW_ALT_TYPE_DEFAULT 0
#define AP_FOLLOW_ALT_TYPE_DEFAULT 0 // absolute/AMSL altitude
#define AP_FOLLOW_ALTITUDE_TYPE_ORIGIN 2 // origin relative altitude
#define AP_FOLLOW_ALTITUDE_TYPE_TERRAIN 3 // terrain relative altitude
#define AP_FOLLOW_TIMEOUT_DEFAULT 600
#else
#define AP_FOLLOW_ALT_TYPE_DEFAULT AP_FOLLOW_ALTITUDE_TYPE_RELATIVE
#define AP_FOLLOW_TIMEOUT_DEFAULT 3000
#endif

AP_Follow *AP_Follow::_singleton;
Expand Down Expand Up @@ -129,7 +132,7 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
// @Param: _ALT_TYPE
// @DisplayName: Follow altitude type
// @Description: Follow altitude type
// @Values: 0:absolute, 1:relative
// @Values: 0:absolute, 1:relative, 2:origin (not used), 3:terrain (plane)
// @User: Standard
AP_GROUPINFO("_ALT_TYPE", 10, AP_Follow, _alt_type, AP_FOLLOW_ALT_TYPE_DEFAULT),
#endif
Expand All @@ -141,6 +144,13 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
// @User: Standard
AP_GROUPINFO("_OPTIONS", 11, AP_Follow, _options, 0),

// @Param: _TIMEOUT
// @DisplayName: Follow timeout
// @Description: Follow position estimate timeout after x milliseconds
// @User: Standard
// @Units: ms
AP_GROUPINFO("_TIMEOUT", 12, AP_Follow, _timeout, AP_FOLLOW_TIMEOUT_DEFAULT),

AP_GROUPEND
};

Expand Down Expand Up @@ -174,7 +184,7 @@ bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ne
}

// check for timeout
if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > (uint32_t)_timeout)) {
return false;
}

Expand Down Expand Up @@ -263,7 +273,7 @@ bool AP_Follow::get_target_heading_deg(float &heading) const
}

// check for timeout
if ((_last_heading_update_ms == 0) || (AP_HAL::millis() - _last_heading_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
if ((_last_heading_update_ms == 0) || (AP_HAL::millis() - _last_heading_update_ms > (uint32_t)_timeout)) {
return false;
}

Expand Down Expand Up @@ -345,14 +355,35 @@ bool AP_Follow::handle_global_position_int_message(const mavlink_message_t &msg)
_target_location.lng = packet.lon;

// select altitude source based on FOLL_ALT_TYPE param
#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
switch(_alt_type) {
case AP_FOLLOW_ALT_TYPE_DEFAULT:
_target_location.set_alt_cm(packet.alt * 0.1, Location::AltFrame::ABSOLUTE);
break;
case AP_FOLLOW_ALTITUDE_TYPE_RELATIVE:
case AP_FOLLOW_ALTITUDE_TYPE_ORIGIN:
_target_location.set_alt_cm(packet.relative_alt * 0.1, static_cast<Location::AltFrame>((int)_alt_type));
break;
case AP_FOLLOW_ALTITUDE_TYPE_TERRAIN: {
/// Altitude comes in AMSL
int32_t terrain_altitude_cm;
_target_location.set_alt_cm(packet.alt * 0.1, Location::AltFrame::ABSOLUTE);
// convert the incoming altitude to terrain altitude
if(_target_location.get_alt_cm(Location::AltFrame::ABOVE_TERRAIN, terrain_altitude_cm)) {
_target_location.set_alt_cm(terrain_altitude_cm, Location::AltFrame::ABOVE_TERRAIN);
}
break;
}
}
#else
if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
// above home alt
_target_location.set_alt_cm(packet.relative_alt / 10, Location::AltFrame::ABOVE_HOME);
} else {
// absolute altitude
_target_location.set_alt_cm(packet.alt / 10, Location::AltFrame::ABSOLUTE);
}

#endif
_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
Expand Down Expand Up @@ -393,8 +424,8 @@ bool AP_Follow::handle_follow_target_message(const mavlink_message_t &msg)

// 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)) {
if (_alt_type != AP_FOLLOW_ALT_TYPE_DEFAULT &&
!new_loc.change_alt_frame(static_cast<Location::AltFrame>((int)_alt_type))) {
return false;
}
_target_location = new_loc;
Expand Down Expand Up @@ -559,11 +590,42 @@ bool AP_Follow::have_target(void) const
}

// check for timeout
if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > (uint32_t)_timeout)) {
gcs().send_text(MAV_SEVERITY_NOTICE, "location timeout %d", (int)(AP_HAL::millis() - _last_location_update_ms));
return false;
}
return true;
}

#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
// create a single method to retrieve all the relevant values in one shot for Lua
/* replaces the following Lua calls
target_distance, target_distance_offsets, target_velocity = follow:get_target_dist_and_vel_ned()
target_location, target_velocity = follow:get_target_location_and_velocity()
target_location_offset, target_velocity = follow:get_target_location_and_velocity_ofs()
local xy_dist = follow:get_distance_to_target() -- this value is set by get_target_dist_and_vel_ned() - why do I have to know this?
local target_heading = follow:get_target_heading_deg()
*/
bool AP_Follow::get_target_info(Vector3f &dist_ned, Vector3f &dist_with_offs,
Vector3f &target_vel_ned, Vector3f &target_vel_ned_ofs,
Location &target_loc, Location &target_loc_ofs,
float &target_dist_ofs
)
{
// The order here is VERY important. This needs to called first because it updates a lot of internal variables
if(!get_target_dist_and_vel_ned(dist_ned, dist_with_offs, target_vel_ned)) {
return false;
}
if(!get_target_location_and_velocity(target_loc,target_vel_ned)) {
return false;
}
if(!get_target_location_and_velocity_ofs(target_loc_ofs, target_vel_ned_ofs)) {
return false;
}
target_dist_ofs = _dist_to_target;
return true;
}
#endif

namespace AP {

Expand Down
13 changes: 13 additions & 0 deletions libraries/AP_Follow/AP_Follow.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,8 @@
#include <AC_PID/AC_P.h>
#include <AP_RTC/JitterCorrection.h>

#include <AP_Vehicle/AP_Vehicle_Type.h>

class AP_Follow
{

Expand Down Expand Up @@ -112,6 +114,16 @@ class AP_Follow
// returns true if a follow option enabled
bool option_is_enabled(Option option) const { return (_options.get() & (uint16_t)option) != 0; }

//
// Lua binding combo function
//
// try to get all the values from a single cycle and return them in a single call to Lua
bool get_target_info(Vector3f &dist_ned, Vector3f &dist_with_offs,
Vector3f &target_vel_ned, Vector3f &target_vel_ned_ofs,
Location &target_loc, Location &target_loc_ofs,
float &target_dist_ofs
);

// parameter list
static const struct AP_Param::GroupInfo var_info[];

Expand Down Expand Up @@ -153,6 +165,7 @@ class AP_Follow
AP_Int8 _alt_type; // altitude source for follow mode
AC_P _p_pos; // position error P controller
AP_Int16 _options; // options for mount behaviour follow mode
AP_Int32 _timeout; // position estimate timeout after x milliseconds

// local variables
uint32_t _last_location_update_ms; // system time of last position update
Expand Down

0 comments on commit 3cd81ef

Please sign in to comment.