Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Plane follow script Applet #28546

Open
wants to merge 2 commits into
base: master
Choose a base branch
from
+1,430 −21
Open

Plane follow script Applet #28546

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
a0279f1
AP_Follow: changes to support scripted plane follow
timtuxworth Nov 5, 2024
b9de7f9
AP_Scripting: scripted plane follow
timtuxworth Nov 8, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
82 changes: 72 additions & 10 deletions libraries/AP_Follow/AP_Follow.cpp
View file
Open in desktop
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
Open in desktop
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
Loading
Loading