Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Follow: changes to support scripted plane follow

Browse Source
This commit is contained in:
Tim Tuxworth 2024-11-05 21:00:35 +08:00
parent cb6907992b
commit 008bd562f5
2 changed files with 86 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

83
libraries/AP_Follow/AP_Follow.cpp
View File

@ -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;
@ -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
@ -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_ms, AP_FOLLOW_TIMEOUT_DEFAULT),
AP_GROUPEND
};
@ -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_ms)) {
return false;
}
@ -263,7 +273,8 @@ 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_ms)) {
gcs().send_text(MAV_SEVERITY_NOTICE, "gthd timeout %d", (int)(AP_HAL::millis() - _last_location_update_ms));
return false;
}
@ -345,6 +356,27 @@ 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);
@ -352,7 +384,7 @@ bool AP_Follow::handle_global_position_int_message(const mavlink_message_t &msg)
// 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
@ -393,8 +425,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;
@ -559,12 +591,43 @@ 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_ms)) {
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 {
AP_Follow &follow()

13
libraries/AP_Follow/AP_Follow.h
View File

@ -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
{
@ -112,6 +114,16 @@ public:
// 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[];
@ -153,6 +165,7 @@ private:
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_ms; // position estimate timeout after x milliseconds
// local variables
uint32_t _last_location_update_ms; // system time of last position update