From 91aceb10bf23efa298a3e49de91e43f90b451e78 Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Tue, 5 Nov 2024 21:00:35 +0800 Subject: [PATCH 01/14] AP_Follow: changes to support scripted plane follow --- libraries/AP_Follow/AP_Follow.cpp | 83 +++++++++++++++++++++++++++---- libraries/AP_Follow/AP_Follow.h | 13 +++++ 2 files changed, 86 insertions(+), 10 deletions(-) diff --git a/libraries/AP_Follow/AP_Follow.cpp b/libraries/AP_Follow/AP_Follow.cpp index 8792efd2488988..673d0e42915c1d 100644 --- a/libraries/AP_Follow/AP_Follow.cpp +++ b/libraries/AP_Follow/AP_Follow.cpp @@ -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((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((int)_alt_type))) { return false; } _target_location = new_loc; @@ -559,11 +591,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_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 { diff --git a/libraries/AP_Follow/AP_Follow.h b/libraries/AP_Follow/AP_Follow.h index 48c43fc9ff4706..67305254280325 100644 --- a/libraries/AP_Follow/AP_Follow.h +++ b/libraries/AP_Follow/AP_Follow.h @@ -26,6 +26,8 @@ #include #include +#include + class AP_Follow { @@ -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[]; @@ -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_ms; // position estimate timeout after x milliseconds // local variables uint32_t _last_location_update_ms; // system time of last position update From 37a495c1d64d044983f3f32cf17ad3122aafc2b9 Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Tue, 5 Nov 2024 11:15:17 +0800 Subject: [PATCH 02/14] ArduPlane: changes to support scripted plane follow --- ArduPlane/Parameters.cpp | 7 +++++++ ArduPlane/Parameters.h | 2 ++ ArduPlane/commands.cpp | 4 +++- ArduPlane/mode_guided.cpp | 6 +++--- 4 files changed, 15 insertions(+), 4 deletions(-) diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 5d9865625000aa..c8000e0cb143cc 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -1319,6 +1319,13 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @User: Standard AP_GROUPINFO("FWD_BAT_THR_CUT", 37, ParametersG2, fwd_batt_cmp.batt_voltage_throttle_cutoff, 0.0f), + // @Param: GUIDED_UPD_LIM + // @DisplayName: Guided Update Limit + // @Description: The maximum frequency that an guided mode commands sent by external system such as lua or mavlink can update roll, pitch and throttle. + // @Units: ms + // @User: Standard + AP_GROUPINFO("GUIDED_UPD_LIM", 37, ParametersG2, guided_update_frequency_limit, 3000), + AP_GROUPEND }; diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 328d4a0f107ea8..5ec25eb6b76fd1 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -574,6 +574,8 @@ class ParametersG2 { AP_Follow follow; #endif + AP_Int32 guided_update_frequency_limit; + AP_Float fs_ekf_thresh; // min initial climb in RTL diff --git a/ArduPlane/commands.cpp b/ArduPlane/commands.cpp index 84abde3eef7c0f..1e50f2c0144420 100644 --- a/ArduPlane/commands.cpp +++ b/ArduPlane/commands.cpp @@ -87,7 +87,9 @@ void Plane::set_guided_WP(const Location &loc) // used to control FBW and limit the rate of climb // ----------------------------------------------- - set_target_altitude_current(); + if (!control_mode->is_guided_mode()) { + set_target_altitude_current(); + } setup_glide_slope(); setup_turn_angle(); diff --git a/ArduPlane/mode_guided.cpp b/ArduPlane/mode_guided.cpp index a97bedcd7dffbb..f0afd409edafb6 100644 --- a/ArduPlane/mode_guided.cpp +++ b/ArduPlane/mode_guided.cpp @@ -38,7 +38,7 @@ void ModeGuided::update() // Received an external msg that guides roll in the last 3 seconds? if (plane.guided_state.last_forced_rpy_ms.x > 0 && - millis() - plane.guided_state.last_forced_rpy_ms.x < 3000) { + (millis() - plane.guided_state.last_forced_rpy_ms.x) < (uint32_t)plane.g2.guided_update_frequency_limit) { plane.nav_roll_cd = constrain_int32(plane.guided_state.forced_rpy_cd.x, -plane.roll_limit_cd, plane.roll_limit_cd); plane.update_load_factor(); @@ -76,7 +76,7 @@ void ModeGuided::update() } if (plane.guided_state.last_forced_rpy_ms.y > 0 && - millis() - plane.guided_state.last_forced_rpy_ms.y < 3000) { + (millis() - plane.guided_state.last_forced_rpy_ms.y) < (uint32_t)plane.g2.guided_update_frequency_limit) { plane.nav_pitch_cd = constrain_int32(plane.guided_state.forced_rpy_cd.y, plane.pitch_limit_min*100, plane.aparm.pitch_limit_max.get()*100); } else { plane.calc_nav_pitch(); @@ -89,7 +89,7 @@ void ModeGuided::update() } else if (plane.aparm.throttle_cruise > 1 && plane.guided_state.last_forced_throttle_ms > 0 && - millis() - plane.guided_state.last_forced_throttle_ms < 3000) { + (millis() - plane.guided_state.last_forced_throttle_ms) < (uint32_t)plane.g2.guided_update_frequency_limit) { // Received an external msg that guides throttle in the last 3 seconds? SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.guided_state.forced_throttle); From fde86797e14ec526d59260515a78b8f718504321 Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Fri, 8 Nov 2024 17:24:07 +0800 Subject: [PATCH 03/14] AP_Scripting: scripted plane follow --- .../AP_Scripting/applets/plane_follow.lua | 810 ++++++++++++++++++ .../AP_Scripting/applets/plane_follow.md | 143 ++++ libraries/AP_Scripting/docs/docs.lua | 35 +- .../generator/description/bindings.desc | 3 + .../AP_Scripting/modules/mavlink_attitude.lua | 168 ++++ libraries/AP_Scripting/modules/speedpid.lua | 215 +++++ 6 files changed, 1363 insertions(+), 11 deletions(-) create mode 100644 libraries/AP_Scripting/applets/plane_follow.lua create mode 100644 libraries/AP_Scripting/applets/plane_follow.md create mode 100644 libraries/AP_Scripting/modules/mavlink_attitude.lua create mode 100644 libraries/AP_Scripting/modules/speedpid.lua diff --git a/libraries/AP_Scripting/applets/plane_follow.lua b/libraries/AP_Scripting/applets/plane_follow.lua new file mode 100644 index 00000000000000..d557eb21450fad --- /dev/null +++ b/libraries/AP_Scripting/applets/plane_follow.lua @@ -0,0 +1,810 @@ +--[[ + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + + Follow in Plane + Support follow "mode" in plane. This will actually use GUIDED mode with + a scripting switch to allow guided to track the vehicle id in FOLL_SYSID + Uses the AP_Follow library so all of the existing FOLL_* parameters are used + as documented for Copter, + add 3 more for this script + ZPF_EXIT_MODE - the mode to switch to when follow is turned of using the switch + ZPF_FAIL_MODE - the mode to switch to if the target is lost + ZPF_TIMEOUT - number of seconds to try to reaquire the target after losing it before failing + ZPF_OVRSHT_DEG - if the target is more than this many degrees left or right, assume an overshoot + ZPR_TURN_DEG - if the target is more than this many degrees left or right, assume it's turning +--]] + +SCRIPT_VERSION = "4.6.0-044" +SCRIPT_NAME = "Plane Follow" +SCRIPT_NAME_SHORT = "PFollow" + +-- FOLL_ALT_TYPE and Mavlink FRAME use different values +ALT_FRAME = { GLOBAL = 0, RELATIVE = 1, TERRAIN = 3} + +MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} +MAV_FRAME = {GLOBAL = 0, GLOBAL_RELATIVE_ALT = 3, GLOBAL_TERRAIN_ALT = 10} +MAV_CMD_INT = { DO_SET_MODE = 176, DO_CHANGE_SPEED = 178, DO_REPOSITION = 192, + GUIDED_CHANGE_SPEED = 43000, GUIDED_CHANGE_ALTITUDE = 43001, GUIDED_CHANGE_HEADING = 43002 } +MAV_SPEED_TYPE = { AIRSPEED = 0, GROUNDSPEED = 1, CLIMB_SPEED = 2, DESCENT_SPEED = 3 } +MAV_HEADING_TYPE = { COG = 0, HEADING = 1, DEFAULT = 2} -- COG = Course over Ground, i.e. where you want to go, HEADING = which way the vehicle points + +FLIGHT_MODE = {AUTO=10, RTL=11, LOITER=12, GUIDED=15, QHOVER=18, QLOITER=19, QRTL=21} + +local ahrs_eas2tas = ahrs:get_EAS2TAS() +local windspeed_vector = ahrs:wind_estimate() + +local now = millis():tofloat() * 0.001 +local now_target_heading = now +local follow_enabled = false +local too_close_follow_up = 0 +local lost_target_countdown = LOST_TARGET_TIMEOUT +local save_target_heading1 = -400.0 +local save_target_heading2 = -400.0 +local save_target_altitude +local tight_turn = false + +local PARAM_TABLE_KEY = 120 +local PARAM_TABLE_PREFIX = "ZPF_" +local PARAM_TABLE_KEY2 = 121 +local PARAM_TABLE_PREFIX2 = "ZPF2_" + +-- add a parameter and bind it to a variable +local function bind_add_param(name, idx, default_value) + assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), string.format('could not add param %s', name)) + return Parameter(PARAM_TABLE_PREFIX .. name) +end +-- setup follow mode specific parameters +assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 10), 'could not add param table') + +-- add a parameter and bind it to a variable +local function bind_add_param2(name, idx, default_value) + assert(param:add_param(PARAM_TABLE_KEY2, idx, name, default_value), string.format('could not add param %s', name)) + return Parameter(PARAM_TABLE_PREFIX2 .. name) +end +-- setup follow mode specific parameters- second tranche +assert(param:add_table(PARAM_TABLE_KEY2, PARAM_TABLE_PREFIX2, 10), 'could not add param table') + +-- This uses the exisitng FOLL_* parameters and just adds a couple specific to this script +-- but because most of the logic is already in AP_Follow (called by binding to follow:) there +-- is no need to access them in the scriot + +-- we need these existing FOLL_ parametrs +FOLL_ALT_TYPE = Parameter('FOLL_ALT_TYPE') +FOLL_SYSID = Parameter('FOLL_SYSID') +FOLL_TIMEOUT = Parameter('FOLL_TIMEOUT') +FOLL_OFS_Y = Parameter('FOLL_OFS_Y') +local foll_sysid = FOLL_SYSID:get() or -1 +local foll_ofs_y = FOLL_OFS_Y:get() or 0.0 +local foll_alt_type = FOLL_ALT_TYPE:get() or ALT_FRAME.GLOBAL +local foll_timeout = FOLL_TIMEOUT:get() or 1000 + +-- Add these ZPF_ parameters specifically for this script +--[[ + // @Param: ZPF_FAIL_MODE + // @DisplayName: Plane Follow lost target mode + // @Description: Mode to switch to if the target is lost (no signal or > FOLL_DIST_MAX). + // @User: Standard +--]] +ZPF_FAIL_MODE = bind_add_param('FAIL_MODE', 1, FLIGHT_MODE.LOITER) + +--[[ + // @Param: ZPF_EXIT_MODE + // @DisplayName: Plane Follow exit mode + // @Description: Mode to switch to when follow mode is exited normally + // @User: Standard +--]] +ZPF_EXIT_MODE = bind_add_param('EXIT_MODE', 2, FLIGHT_MODE.LOITER) + +--[[ + // @Param: ZPF_ACT_FN + // @DisplayName: Plane Follow Scripting ActivationFunction + // @Description: Setting an RC channel's _OPTION to this value will use it for Plane Follow enable/disable + // @Range: 300 307 +--]] +ZPF_ACT_FN = bind_add_param("ACT_FN", 3, 301) + +--[[ + // @Param: ZPF_TIMEOUT + // @DisplayName: Plane Follow Scripting Timeout + // @Description: How long to try re-aquire a target if lost + // @Range: 0 30 + // @Units: s +--]] +ZPF_TIMEOUT = bind_add_param("TIMEOUT", 4, 10) + +--[[ + // @Param: ZPF_OVRSHT_DEG + // @DisplayName: Plane Follow Scripting Overshoot Angle + // @Description: If the target is greater than this many degrees left or right, assume an overshoot + // @Range: 0 180 + // @Units: deg +--]] +ZPF_OVRSHT_DEG = bind_add_param("OVRSHT_DEG", 5, 75) + +--[[ + // @Param: ZPF_TURN_DEG + // @DisplayName: Plane Follow Scripting Turn Angle + // @Description: If the target is greater than this many degrees left or right, assume it's turning + // @Range: 0 180 + // @Units: deg +--]] +ZPF_TURN_DEG = bind_add_param("TURN_DEG", 6, 15) + +--[[ + // @Param: ZPF_DIST_CLOSE + // @DisplayName: Plane Follow Scripting Close Distance + // @Description: When closer than this distance assume we track by heading + // @Range: 0 100 + // @Units: m +--]] +ZPF_DIST_CLOSE = bind_add_param("DIST_CLOSE", 7, 50) + +--[[ + // @Param: ZPF_WIDE_TURNS + // @DisplayName: Plane Follow Scripting Wide Turns + // @Description: Use wide turns when following a turning target. Alternative is "cutting the corner" + // @Range: 0 1 +--]] +ZPF_WIDE_TURNS = bind_add_param("WIDE_TURNS", 8, 1) + +--[[ + // @Param: ZPF_ALT + // @DisplayName: Plane Follow Scripting Altitude Override + // @Description: When non zero, this altitude value (in FOLL_ALT_TYPE frame) overrides the value sent by the target vehicle + // @Range: 0 1000 + // @Units: m +--]] +ZPF_ALT_OVR = bind_add_param("ALT_OVR", 9, 0) + +--[[ + // @Param: ZPF_D_P + // @DisplayName: Plane Follow Scripting distance P gain + // @Description: P gain for the speed PID controller distance component + // @Range: 0 1 +--]] +ZPF2_D_P = bind_add_param2("D_P", 1, 0.3) + +--[[ + // @Param: ZPF_D_I + // @DisplayName: Plane Follow Scripting distance I gain + // @Description: I gain for the speed PID distance component + // @Range: 0 1 +--]] +ZPF2_D_I = bind_add_param2("D_I", 2, 0.3) + +--[[ + // @Param: ZPF_D_D + // @DisplayName: Plane Follow Scripting distance D gain + // @Description: D gain for the speed PID controller distance component + // @Range: 0 1 +--]] +ZPF2_D_D = bind_add_param2("D_D", 3, 0.05) + +--[[ + // @Param: ZPF_V_P + // @DisplayName: Plane Follow Scripting speed P gain + // @Description: P gain for the speed PID controller velocity component + // @Range: 0 1 +--]] +ZPF2_V_P = bind_add_param2("V_P", 4, 0.3) + +--[[ + // @Param: ZPF_V_I + // @DisplayName: Plane Follow Scripting speed I gain + // @Description: I gain for the speed PID controller velocity component + // @Range: 0 1 +--]] +ZPF2_V_I = bind_add_param2("V_I", 5, 0.3) + +--[[ + // @Param: ZPF_V_D + // @DisplayName: Plane Follow Scripting speed D gain + // @Description: D gain for the speed PID controller velocity component + // @Range: 0 1 +--]] +ZPF2_V_D = bind_add_param2("V_D", 6, 0.05) + +--[[ + // @Param: ZPF_LkAHD + // @DisplayName: Plane Follow Lookahead seconds + // @Description: Time to "lookahead" when calculating distance errors + // @Units: s +--]] +ZPF2_LKAHD = bind_add_param2("LKAHD", 7, 5) + +--[[ + // @Param: ZPF_DIST_FUDGE + // @DisplayName: Plane Follow distance fudge factor + // @Description: THe distance returned by the AP_FOLLOW library might be off by about this factor of airspeed + // @Units: s +--]] +ZPF2_DIST_FUDGE = bind_add_param2("DIST_FUDGE", 8, 0.92) + +REFRESH_RATE = 0.05 -- in seconds, so 20Hz +LOST_TARGET_TIMEOUT = (ZPF_TIMEOUT:get() or 10) / REFRESH_RATE +OVERSHOOT_ANGLE = ZPF_OVRSHT_DEG:get() or 75.0 +TURNING_ANGLE = ZPF_TURN_DEG:get() or 20.0 + +local fail_mode = ZPF_FAIL_MODE:get() or FLIGHT_MODE.QRTL +local exit_mode = ZPF_EXIT_MODE:get() or FLIGHT_MODE.LOITER + +local use_wide_turns = ZPF_WIDE_TURNS:get() or 1 + +local distance_fudge = ZPF2_DIST_FUDGE:get() or 0.92 + +DISTANCE_LOOKAHEAD_SECONDS = ZPF2_LKAHD:get() or 5.0 + +AIRSPEED_MIN = Parameter('AIRSPEED_MIN') +AIRSPEED_MAX = Parameter('AIRSPEED_MAX') +AIRSPEED_CRUISE = Parameter('AIRSPEED_CRUISE') +WP_LOITER_RAD = Parameter('WP_LOITER_RAD') +WINDSPEED_MAX = Parameter('AHRS_WIND_MAX') + +local airspeed_max = AIRSPEED_MAX:get() or 25.0 +local airspeed_min = AIRSPEED_MIN:get() or 12.0 +local airspeed_cruise = AIRSPEED_CRUISE:get() or 18.0 +local windspeed_max = WINDSPEED_MAX:get() or 100.0 + +local function constrain(v, vmin, vmax) + if v < vmin then + v = vmin + end + if v > vmax then + v = vmax + end + return v +end + +local speedpid = require("speedpid") +local pid_controller_distance = speedpid.speed_controller(ZPF2_D_P:get() or 0.3, + ZPF2_D_I:get() or 0.3, + ZPF2_D_D:get() or 0.05, + 5.0, airspeed_min - airspeed_max, airspeed_max - airspeed_min) + +local pid_controller_velocity = speedpid.speed_controller(ZPF2_V_P:get() or 0.3, + ZPF2_V_I:get() or 0.3, + ZPF2_V_D:get() or 0.05, + 5.0, airspeed_min, airspeed_max) + + +local mavlink_attitude = require("mavlink_attitude") +local mavlink_attitude_receiver = mavlink_attitude.mavlink_attitude_receiver() + +local function follow_frame_to_mavlink(follow_frame) + local mavlink_frame = MAV_FRAME.GLOBAL; + if (follow_frame == ALT_FRAME.TERRAIN) then + mavlink_frame = MAV_FRAME.GLOBAL_TERRAIN_ALT + end + if (follow_frame == ALT_FRAME.RELATIVE) then + mavlink_frame = MAV_FRAME.GLOBAL_RELATIVE_ALT + end + return mavlink_frame +end + +-- set_vehicle_target_altitude() Parameters +-- target.alt = new target altitude in meters +-- target.frame = Altitude frame MAV_FRAME, it's very important to get this right! +-- target.alt = altitude in meters to acheive +-- target.speed = z speed of change to altitude (1000.0 = max) +local function set_vehicle_target_altitude(target) + local speed = target.speed or 1000.0 -- default to maximum z acceleration + if target.alt == nil then + gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": set_vehicle_target_altitude no altiude") + return + end + -- GUIDED_CHANGE_ALTITUDE takes altitude in meters + if not gcs:run_command_int(MAV_CMD_INT.GUIDED_CHANGE_ALTITUDE, { + frame = follow_frame_to_mavlink(target.frame), + p3 = speed, + z = target.alt }) then + gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": MAVLink CHANGE_ALTITUDE returned false") + end +end + +-- set_vehicle_heading() Parameters +-- heading.heading_type = MAV_HEADING_TYPE (defaults to HEADING) +-- heading.heading = the target heading in degrees +-- heading.accel = rate/acceleration to acheive the heading 0 = max +local function set_vehicle_heading(heading) + local heading_type = heading.type or MAV_HEADING_TYPE.HEADING + local heading_heading = heading.heading or 0 + local heading_accel = heading.accel or 10.0 + + if heading_heading == nil or heading_heading <= -400 or heading_heading > 360 then + gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": set_vehicle_heading no heading") + return + end + + if not gcs:run_command_int(MAV_CMD_INT.GUIDED_CHANGE_HEADING, { frame = MAV_FRAME.GLOBAL, + p1 = heading_type, + p2 = heading_heading, + p3 = heading_accel }) then + gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": MAVLink GUIDED_CHANGE_HEADING failed") + end +end + +-- set_vehicle_speed() Parameters +-- speed.speed - new target speed +-- speed.type - speed type MAV_SPEED_TYPE +-- speed.throttle - new target throttle (used if speed = 0) +-- speed.slew - specified slew rate to hit the target speed, rather than jumping to it immediately 0 = maximum acceleration +local function set_vehicle_speed(speed) + local new_speed = speed.speed or 0.0 + local speed_type = speed.type or MAV_SPEED_TYPE.AIRSPEED + local throttle = speed.throttle or 0.0 + local slew = speed.slew or 0.0 + local mode = vehicle:get_mode() + + if speed_type == MAV_SPEED_TYPE.AIRSPEED and mode == FLIGHT_MODE.GUIDED then + if not gcs:run_command_int(MAV_CMD_INT.GUIDED_CHANGE_SPEED, { frame = MAV_FRAME.GLOBAL, + p1 = speed_type, + p2 = new_speed, + p3 = slew }) then + gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": MAVLink GUIDED_CHANGE_SPEED failed") + end + else + if not gcs:run_command_int(MAV_CMD_INT.DO_CHANGE_SPEED, { frame = MAV_FRAME.GLOBAL, + p1 = speed_type, + p2 = new_speed, + p3 = throttle }) then + gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": MAVLink DO_CHANGE_SPEED failed") + end + end +end + +-- set_vehicle_target_location() Parameters +-- target.groundspeed (-1 for ignore) +-- target.radius (defaults to 2m) +-- target.yaw - not really yaw - it's the loiter direction 1 = CCW, -1 = CW NaN = default +-- target.lat - latitude in decimal degrees +-- target.lng - longitude in decimal degrees +-- target.alt - target alitude in meters +local function set_vehicle_target_location(target) + local radius = target.radius or 2.0 + local yaw = target.yaw or 1 + -- If we are on the right side of the vehicle make sure any loitering is CCW (moves away from the other plane) + -- yaw > 0 - CCW = turn to the right of the target point + -- yaw < 0 - Clockwise = turn to the left of the target point + -- if following direct we turn on the "outside" + + -- if we were in HEADING mode, need to switch out of it so that REPOSITION will work + -- Note that MAVLink DO_REPOSITION requires altitude in meters + set_vehicle_heading({type = MAV_HEADING_TYPE.DEFAULT}) + if not gcs:run_command_int(MAV_CMD_INT.DO_REPOSITION, { frame = follow_frame_to_mavlink(target.frame), + p1 = target.groundspeed or -1, + p2 = 1, + p3 = radius, + p4 = yaw, + x = target.lat, + y = target.lng, + z = target.alt }) then -- altitude in m + gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": MAVLink DO_REPOSITION returned false") + end +end + +local last_follow_active_state = rc:get_aux_cached(ZPF_ACT_FN:get()) + +--[[ + return true if we are in a state where follow can apply +--]] +local reported_target = true +local function follow_active() + local mode = vehicle:get_mode() + + if mode == FLIGHT_MODE.GUIDED then + if follow_enabled then + if follow:have_target() then + reported_target = true + else + if reported_target then + gcs:send_text(MAV_SEVERITY.WARNING, SCRIPT_NAME_SHORT .. ": no target: " .. follow:get_target_sysid()) + end + reported_target = false + end + end + else + reported_target = false + end + + return reported_target +end + +--[[ + check for user activating follow using an RC switch set HIGH +--]] +local function follow_check() + if ZPF_ACT_FN == nil then + return + end + local foll_act_fn = ZPF_ACT_FN:get() + if foll_act_fn == nil then + return + end + local active_state = rc:get_aux_cached(foll_act_fn) + if (active_state ~= last_follow_active_state) then + if( active_state == 0) then + if follow_enabled then + -- Follow disabled - return to EXIT mode + vehicle:set_mode(exit_mode) + follow_enabled = false + gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. ": disabled") + end + elseif (active_state == 2) then + if not (arming:is_armed()) then + gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. ": must be armed") + end + -- Follow enabled - switch to guided mode + vehicle:set_mode(FLIGHT_MODE.GUIDED) + follow_enabled = true + lost_target_countdown = LOST_TARGET_TIMEOUT + --speed_controller_pid.reset() + pid_controller_distance.reset() + pid_controller_velocity.reset() + gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. ": enabled") + end + -- Don't know what to do with the 3rd switch position right now. + last_follow_active_state = active_state + end +end + +local function wrap_360(angle) + local res = math.fmod(angle, 360.0) + if res < 0 then + res = res + 360.0 + end + return res +end + +local function wrap_180(angle) + local res = wrap_360(angle) + if res > 180 then + res = res - 360 + end + return res +end + +local function calculate_airspeed_from_groundspeed(velocity_vector) + --[[ + This is the code from AHRS.cpp + Vector3f true_airspeed_vec = nav_vel - wind_vel; + + This is the c++ code from AP_AHRS_DCM.cpp and also from AP_AHRS.cpp + float true_airspeed = airspeed_ret * get_EAS2TAS(); + true_airspeed = constrain_float(true_airspeed, + gnd_speed - _wind_max, + gnd_speed + _wind_max); + airspeed_ret = true_airspeed / get_EAS2TAS( + --]] + + local airspeed_vector = velocity_vector - windspeed_vector + local airspeed = airspeed_vector:length() + airspeed = airspeed * ahrs_eas2tas + airspeed = constrain(airspeed, airspeed - windspeed_max, airspeed + windspeed_max) + airspeed = airspeed / ahrs_eas2tas + + return airspeed +end + +-- main update function +local function update() + now = millis():tofloat() * 0.001 + ahrs_eas2tas = ahrs:get_EAS2TAS() + windspeed_vector = ahrs:wind_estimate() + + follow_check() + if not follow_active() then + return + end + + -- set the target frame as per user set parameter - this is fundamental to this working correctly + local close_distance = ZPF_DIST_CLOSE:get() or airspeed_cruise * 2.0 + local long_distance = close_distance * 4.0 + local altitude_override = ZPF_ALT_OVR:get() or 0 + + LOST_TARGET_TIMEOUT = (ZPF_TIMEOUT:get() or 10) / REFRESH_RATE + OVERSHOOT_ANGLE = ZPF_OVRSHT_DEG:get() or 75.0 + TURNING_ANGLE = ZPF_TURN_DEG:get() or 20.0 + foll_ofs_y = FOLL_OFS_Y:get() or 0.0 + foll_alt_type = FOLL_ALT_TYPE:get() or ALT_FRAME.GLOBAL + use_wide_turns = ZPF_WIDE_TURNS:get() or 1 + distance_fudge = ZPF2_DIST_FUDGE:get() or 0.92 + + --[[ + get the current navigation target. + --]] + local target_location -- = Location() of the target + local target_location_offset -- Location of the target with FOLL_OFS_* offsets applied + local target_velocity -- = Vector3f() -- current velocity of lead vehicle + local target_velocity_offset -- Vector3f -- velocity to the offset target_location_offset + local target_distance -- = Vector3f() -- vector to lead vehicle + local target_distance_offset -- vector to the target taking offsets into account + local xy_dist -- distance to target with offsets in meters + local target_heading -- heading of the target vehicle + + local current_location = ahrs:get_location() + if current_location == nil then + return + end + ---@cast current_location -nil + local current_altitude = current_location:alt() * 0.01 + + local vehicle_airspeed = ahrs:airspeed_estimate() + local current_target = vehicle:get_target_location() + + -- because of the methods available on AP_Follow, need to call these multiple methods get_target_dist_and_vel_ned() MUST BE FIRST + -- to get target_location, target_velocity, target distance and target + -- and yes target_offsets (hopefully the same value) is returned by both methods + -- even worse - both internally call get_target_location_and_Velocity, but making a single method + -- in AP_Follow is probably a high flash cost, so we just take the runtime hit + --[[ + target_distance, target_distance_offsets, target_velocity = follow:get_target_dist_and_vel_ned() -- THIS HAS TO BE FIRST + 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() + --]] + + target_distance, target_distance_offset, + target_velocity, target_velocity_offset, + target_location, target_location_offset, + xy_dist = follow:get_target_info() + target_heading = follow:get_target_heading_deg() or -400 + + -- if we lose the target wait for LOST_TARGET_TIMEOUT seconds to try to reaquire it + if target_location == nil or target_location_offset == nil or + target_velocity == nil or target_velocity_offset == nil or + target_distance_offset == nil or current_target == nil or target_distance == nil or xy_dist == nil then + lost_target_countdown = lost_target_countdown - 1 + if lost_target_countdown <= 0 then + follow_enabled = false + vehicle:set_mode(fail_mode) + gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": follow: " .. FOLL_SYSID:get() .. " FAILED") + return + end + + -- maintain the current heading until we re-establish telemetry from the target vehicle -- note that this is not logged, needs fixing + gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": follow: lost " .. FOLL_SYSID:get() .. " FAILED hdg: " .. save_target_heading1) + set_vehicle_heading({heading = save_target_heading1}) + set_vehicle_target_altitude({alt = save_target_altitude, frame = foll_alt_type}) -- pass altitude in meters (location has it in cm) + return + else + -- have a good target so reset the countdown + lost_target_countdown = LOST_TARGET_TIMEOUT + end + + -- target_velocity from MAVLink (via AP_Follow) is groundspeed, need to convert to airspeed, + -- we can only assume the windspeed for the target is the same as the chase plane + local target_airspeed = calculate_airspeed_from_groundspeed(target_velocity_offset) + + local vehicle_heading = math.abs(wrap_360(math.deg(ahrs:get_yaw()))) + local heading_to_target_offset = math.deg(current_location:get_bearing(target_location_offset)) + + -- offset_angle is the difference between the current heading of the follow vehicle and the target_location (with offsets) + local offset_angle = wrap_180(vehicle_heading - heading_to_target_offset) + + -- rotate the target_distance_offsets in NED to the same direction has the follow vehicle, we use this below + local target_distance_rotated = target_distance_offset:copy() + target_distance_rotated:rotate_xy(math.rad(vehicle_heading)) + + -- default the desired heading to the target heading (adjusted for projected turns) - we might change this below + local airspeed_difference = vehicle_airspeed - target_airspeed + + -- distance seem to be out by about 0.92s at approximately current airspeed just eyeballing it. + xy_dist = math.abs(xy_dist) - vehicle_airspeed * distance_fudge + -- xy_dist will always be a positive value. To get -v to represent overshoot, use the offset_angle + -- to decide if the target is behind + if (math.abs(xy_dist) < long_distance) and (math.abs(offset_angle) > OVERSHOOT_ANGLE) then + xy_dist = -xy_dist + end + + -- projected_distance is how far off the target we expect to be if we maintain the current airspeed for DISTANCE_LOOKAHEAD_SECONDS + local projected_distance = xy_dist - airspeed_difference * DISTANCE_LOOKAHEAD_SECONDS + + -- target angle is the difference between the heading of the target and what its heading was 2 seconds ago + local target_angle = 0.0 + if (target_heading ~= nil and target_heading > -400) then + -- want the greatest angle of we might have turned + local angle_diff1 = wrap_180(math.abs(save_target_heading1 - target_heading)) + local angle_diff2 = wrap_180(math.abs(save_target_heading2 - target_heading)) + if angle_diff2 > angle_diff1 then + target_angle = angle_diff2 + else + target_angle = angle_diff1 + end + -- remember the target heading from 2 seconds ago, so we can tell if it is turning or not + if (now - now_target_heading) > 1 then + save_target_altitude = current_altitude + save_target_heading2 = save_target_heading1 + save_target_heading1 = target_heading + now_target_heading = now + end + end + + -- if the target vehicle is starting to roll we need to pre-empt a turn is coming + -- this is fairly simplistic and could probably be improved + -- got the code from Mission Planner - this is how it calculates the turn radius + --[[ + public float radius + { + get + { + if (_groundspeed <= 1) return 0; + return (float)toDistDisplayUnit(_groundspeed * _groundspeed / (9.80665 * Math.Tan(roll * MathHelper.deg2rad))); + } + } + --]] + local turning = (math.abs(projected_distance) < long_distance and math.abs(target_angle) > TURNING_ANGLE) + local turn_starting = false + local target_attitude = mavlink_attitude_receiver.get_attitude(foll_sysid) + local pre_roll_target_heading = target_heading + local desired_heading = target_heading + local angle_adjustment + tight_turn = false + if target_attitude ~= nil then + if (now - target_attitude.timestamp_ms < foll_timeout) and + math.abs(target_attitude.roll) > 0.1 or math.abs(target_attitude.rollspeed) > 1 then + local turn_radius = vehicle_airspeed * vehicle_airspeed / (9.80665 * math.tan(target_attitude.roll + target_attitude.rollspeed)) + + turning = true + -- predict the roll in 1s from now and use that based on rollspeed + -- need some more maths to convert a roll angle into a turn angle - from Mission Planner: + -- turn_radius = vehicle_airspeed * vehicle_airspeed / (9.80665 * math.tan(target_attitude.roll + target_attitude.rollspeed)) + local tangent_angle = wrap_360(math.deg(math.pi/2.0 - vehicle_airspeed / turn_radius)) + + angle_adjustment = tangent_angle * 0.6 + -- if the roll direction is the same as the y offset, then we are turning on the "inside" (a tight turn) + if (target_attitude.roll < 0 and foll_ofs_y < 0) or + (target_attitude.roll > 0 and foll_ofs_y > 0) then + tight_turn = true + end + + -- if the roll direction is the same as the rollspeed then we are heading into a turn, otherwise we are finishing a turn + if foll_ofs_y == 0 or + (target_attitude.roll < 0 and target_attitude.rollspeed < 0) or + (target_attitude.roll > 0 and target_attitude.rollspeed > 0) then + turn_starting = true + target_angle = wrap_360(target_angle - angle_adjustment) + desired_heading = wrap_360(target_heading - angle_adjustment) + -- push the target heading back because it hasn't figured out we are turning yet + save_target_heading1 = save_target_heading2 + end + end + end + + -- don't count it as close if the heading is diverging (i.e. the target plane has overshot the target so extremely that it's pointing in the wrong direction) + local too_close = (projected_distance > 0) and (math.abs(projected_distance) < close_distance) and (offset_angle < OVERSHOOT_ANGLE) + + -- we've overshot if + -- the distance to the target location is not too_close but will be hit in DISTANCE_LOOKAHEAD_SECONDS (projected_distance) + -- or the distance to the target location is already negative AND the target is very close OR + -- the angle to the target plane is effectively backwards + local overshot = not too_close and ( + (projected_distance < 0 or xy_dist < 0) and + (math.abs(xy_dist or 0.0) < close_distance) + or offset_angle > OVERSHOOT_ANGLE + ) + + local distance_error = constrain(math.abs(projected_distance) / (close_distance * DISTANCE_LOOKAHEAD_SECONDS), 0.1, 1.0) + if overshot or too_close or (too_close_follow_up > 0) then + if too_close_follow_up > 0 then + too_close = true + too_close_follow_up = too_close_follow_up - 1 + else + too_close_follow_up = 10 + end + else + too_close_follow_up = 0 + end + + local target_altitude = 0.0 + local frame_type_log = foll_alt_type + + if altitude_override ~= 0 then + target_altitude = altitude_override + frame_type_log = -1 + elseif target_location_offset ~= nil then + -- change the incoming altitude frame from the target_vehicle to the frame this vehicle wants + target_location_offset:change_alt_frame(foll_alt_type) + target_altitude = target_location_offset:alt() * 0.01 + end + + local mechanism = 0 -- for logging 1: position/location 2:heading + local normalized_distance = math.abs(projected_distance) + local close = (normalized_distance < close_distance) + local too_wide = (math.abs(target_distance_rotated:y()) > (close_distance/5) and not turning) + + -- xy_dist < 3.0 is a special case because mode_guided will try to loiter around the target location if within 2m + -- target_heading - vehicle_heading catches the circumstance where the target vehicle is heaidng in completely the opposite direction + if math.abs(xy_dist or 0.0) < 3.0 or + ((turning and ((tight_turn and turn_starting) or use_wide_turns or foll_ofs_y == 0)) or -- turning + ((close or overshot) and not too_wide) -- we are very close to the target + --math.abs(target_heading - vehicle_heading) > 135) -- the target is going the other way + ) then + set_vehicle_heading({heading = desired_heading}) + set_vehicle_target_altitude({alt = target_altitude, frame = foll_alt_type}) -- pass altitude in meters (location has it in cm) + mechanism = 2 -- heading - for logging + elseif target_location_offset ~= nil then + set_vehicle_target_location({lat = target_location_offset:lat(), + lng = target_location_offset:lng(), + alt = target_altitude, + frame = foll_alt_type, + yaw = foll_ofs_y}) + mechanism = 1 -- position/location - for logging + end + -- dv = interim delta velocity based on the pid controller using projected_distance as the error (we want distance == 0) + local dv = pid_controller_distance.update(target_airspeed - vehicle_airspeed, projected_distance) + local airspeed_new = pid_controller_velocity.update(vehicle_airspeed, dv) + + set_vehicle_speed({speed = constrain(airspeed_new, airspeed_min, airspeed_max)}) + + local log_too_close = 0 + local log_too_close_follow_up = 0 + local log_overshot = 0 + if too_close then + log_too_close = 1 + end + if too_close_follow_up then + log_too_close_follow_up = 1 + end + if overshot then + log_overshot = 1 + end + logger:write("ZPF1",'Dst,DstP,DstE,AspT,Asp,AspO,Mech,Cls,ClsF,OSht','ffffffBBBB','mmmnnn----','----------', + xy_dist, + projected_distance, + distance_error, + target_airspeed, + vehicle_airspeed, + airspeed_new, + mechanism, log_too_close, log_too_close_follow_up, log_overshot + ) + logger:write("ZPF2",'AngT,AngO,Alt,AltT,AltFrm,HdgT,Hdg,HdgP,HdgO','ffffbffff','ddmm-dddd','---------', + target_angle, + offset_angle, + current_altitude, + target_altitude, + frame_type_log, + target_heading, + vehicle_heading, + pre_roll_target_heading, + desired_heading + ) +end + +-- wrapper around update(). This calls update() at 1/REFRESH_RATE Hz +-- and if update faults then an error is displayed, but the script is not +-- stopped +local function protected_wrapper() + local success, err = pcall(update) + + if not success then + gcs:send_text(MAV_SEVERITY.ALERT, SCRIPT_NAME_SHORT .. "Internal Error: " .. err) + -- when we fault we run the update function again after 1s, slowing it + -- down a bit so we don't flood the console with errors + return protected_wrapper, 1000 + end + return protected_wrapper, 1000 * REFRESH_RATE +end + +function delayed_start() + gcs:send_text(MAV_SEVERITY.INFO, string.format("%s %s script loaded", SCRIPT_NAME, SCRIPT_VERSION) ) + return protected_wrapper() +end + +-- start running update loop - waiting 20s for the AP to initialize +if FWVersion:type() == 3 then + return delayed_start, 20000 +else + gcs:send_text(MAV_SEVERITY.ERROR, string.format("%s: must run on Plane", SCRIPT_NAME_SHORT)) +end diff --git a/libraries/AP_Scripting/applets/plane_follow.md b/libraries/AP_Scripting/applets/plane_follow.md new file mode 100644 index 00000000000000..f83909b039e39b --- /dev/null +++ b/libraries/AP_Scripting/applets/plane_follow.md @@ -0,0 +1,143 @@ +# Plane Follow + +This script implements follow functionality for Plane. The plane must be +flying in fixed wing mode and will trigger on a scripting switch or AUX function. + +The target plane must be connected via MAVLink and sending mavlink updates to the chase/follow plane running this script. + +The MAVLINK_SYSID of the target must be set in FOLL_SYSID on the follow plane, +and ==must be different== from the MAVLINK_SYSID of the following plane. + +| Parameter | Target Plane | Follow Plane | +| --------- | ------------ | ------------ | +| SYSID_THIS_MAV | 1 | 2 | +| FOLL_SYSID | n/a | 1 | + + +# Parameters + +The script adds the following parameters to control it's behaviour. It uses +the existing FOLL parameters that are used for the Copter FOLLOW mode. In addition +the following "ZPF" parameters are added: Z = scripting, P = Plane, F = Follow, in two +banks ZPF and ZPF2. + +## ZPF_FAIL_MODE + +This is the mode the plane will change to if following fails. Failure happens +if the following plane loses telemetry from the target, or the distance exceeds +FOLL_DIST_MAX. + +## ZPF_EXIT_MODE + +The flight mode the plane will switch to if it exits following. + +## ZPF_ACT_FN + +The scripting action that will trigger the plane to start following. When this +happens the plane will switch to GUIDED mode and the script will use guided mode +commands to steer the plane towards the target. + +## ZPF_TIMEOUT + +If the target is lost, this is the timeout to wait to re-aquire the target before +triggering ZPF_FAIL_MODE + +## ZPF_OVRSHT_DEG + +This is for the heuristic that uses the difference between the target vehicle heading +and the follow vehicle heading to determine if the vehicle has overshot and should slow +down and turn around. 75 degrees is a good start but tune for your circumstances. + +## ZPF_TURN_DEG + +This is for the heuristic that uses the difference between the target vehicle heading +and the follow vehicle heading to determine if the target vehicle is executing a turn. +15 degrees is a good start but tune for your circumstances. + +## ZPF_DIST_CLOSE + +One of the most important heuristics the follow logic uses to match the heading and speed +of the target plane is to trigger different behavior when the target location is "close". +How close is determined by this value, likely a larger number makes more sense for larger +and faster vehicles and lower values for smaller and slower vehicles. Tune for your circumstances. + +## ZPF_ALT_OVR + +The follow logic can have the follow vehicle track the altitude of the target, but setting a value +in ZPF_ALT_OVR allows the follow vehicle to follow at a fixed altitude regardless of the altitude +of the target. The ZPF_ALT_OVR is in meters in FOLL_ALT_TYPE frame. + +## ZPF2_D_P + +The follow logic uses two PID controllers for controlling speed, the first uses distance (D) +as the error. This is the P gain for the "D" PID controller. + +## ZPF2_D_I + +The follow logic uses two PID controllers for controlling speed, the first uses distance (D) +as the error. This is the I gain for the "D" PID controller. + +## ZPF2_D_D + +The follow logic uses two PID controllers for controlling speed, the first uses distance (D) +as the error. This is the D gain for the "D" PID controller. + +## ZPF2_V_P + +The follow logic uses two PID controllers for controlling speed, the first uses velocity (V) +as the error. This is the P gain for the "V" PID controller. + +## ZPF2_V_I + +The follow logic uses two PID controllers for controlling speed, the first uses distance (V) +as the error. This is the I gain for the "V" PID controller. + +## ZPF2_V_D + +The follow logic uses two PID controllers for controlling speed, the first uses distance (V) +as the error. This is the D gain for the "V" PID controller. + +## ZPF2_LKAHD + +Time to "lookahead" when calculating distance errors. + +## ZPF2_DIST_FUDGE + +This parameter might be a bad idea, but it seems the xy_distance between the target offset location +and the follow vehicle returned by AP_Follow appears to be off by a factor of +airspeed * a fudge factor +This allows this fudge factor to be adjusted until a better solution can be found for this problem. + +# Operation +Enable Lua scripting by setting SCR_ENABLE = 1 on the FOLLOW plane. + +Install the lua script in the APM/scripts directory on the flight +controller's microSD card on the FOLLOW plane. + +Install the speedpid.lua, mavlink_attitude.lua and mavlink_msgs.lua files +in the APM/scripts/modules directory on the SD card on the FOLLOW plane. + +No scripts are required on the target plane. + +Review the above parameter descriptions and decide on the right parameter values for your vehicle and operations. + +Most of the follow logic is in AP_Follow library, which is part of the ArduPilot c++ +code, so this script just calls the existing methods to do things like +lookup the SYSID of the vehicle to follow and calculate the direction and distance +to the target, which should ideally be another fixed wing plane, or VTOL in +fixed wing mode. + +The target location the plane will attempt to achieve will be offset from the target +vehicle location by FOLL_OFS_X and FOLL_OFS_Y. FOLL_OFS_Z will be offset against the +target vehicle, but also FOLL_ALT_TYPE will determine the altitude frame that the vehicle +will use when calculating the target altitude. See the definitions of these +parameters to understand how they work. ZPF2_ALT_OVR will override the operation of FOLL_OFS_Z +setting a fixed altitude for the following plane in FOLL_ALT_TYPE frame. + +To ensure the follow plane gets timely updates from the target, the SRx_EXT_STAT and SRx_EXTRA1 +telemetry stream rate parameters should be increased to increase the rate that the POSITION_TARGET_GLOBAL_INT +and ATTITUDE mavlink messages are sent. The default value is 4Hz, a good value is probably 8Hz or 10Hz but +some testing should be done to confirm the best rate for your telemetry radios and vehicles. + +Ideally the connection is direct plane-to-plane and not routing via a Ground Control Station. This has been tested with 2x HolyBro SiK telemetry radios, one in each plane. RFD900 radios should work and LTE or Starlink connections will probably work well, but haven't been tested. Fast telemetry updates from the target to the following plane will give the best +results. diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index c4ba15485c8741..3c0aa2b98a2e99 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -414,7 +414,6 @@ function CAN:get_device(buffer_len) end ---@return ScriptingCANBuffer_ud|nil function CAN:get_device2(buffer_len) end - -- get latest FlexDebug message from a CAN node ---@param bus number -- CAN bus number, 0 for first bus, 1 for 2nd ---@param node number -- CAN node @@ -3851,28 +3850,42 @@ function precland:healthy() end -- desc follow = {} --- desc +-- get the SYSID_THISMAV of the target +---@return integer +function follow:get_target_sysid() end + +-- get target's heading in degrees (0 = north, 90 = east) ---@return number|nil function follow:get_target_heading_deg() end --- desc ----@return Location_ud|nil ----@return Vector3f_ud|nil -function follow:get_target_location_and_velocity_ofs() end - --- desc ----@return Location_ud|nil ----@return Vector3f_ud|nil +-- get target's estimated location and velocity (in NED) +---@return Location_ud|nil -- location +---@return Vector3f_ud|nil -- velocity function follow:get_target_location_and_velocity() end +-- get target's estimated location and velocity (in NED), with offsets added +---@return Location_ud|nil -- location +---@return Vector3f_ud|nil -- velocity +function follow:get_target_location_and_velocity_ofs() end + -- desc ---@return uint32_t_ud function follow:get_last_update_ms() end --- desc +-- true if we have a valid target location estimate ---@return boolean function follow:have_target() end +-- combo function returning all follow values calcuted in a cycle +---@return Vector3f_ud|nil -- dist_ned - distance to the target +---@return Vector3f_ud|nil -- dist_with_offs - distance to the target with offsets +---@return Vector3f_ud|nil -- target_vel_ned - proposed velocity of the target +---@return Vector3f_ud|nil -- target_vel_ned_ofs - proposed velocity of the target with offsets +---@return Location_ud|nil -- target_loc - location of the target +---@return Location_ud|nil -- target_loc_ofs - location of the target with offsets +---@return number|nil -- target_dist_ofs - distance to the target in meters +function follow:get_target_info() end + -- desc scripting = {} diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index 856097a9be470c..c15a49521d7e81 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -759,10 +759,13 @@ include AP_Follow/AP_Follow.h singleton AP_Follow depends AP_FOLLOW_ENABLED && (APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI) singleton AP_Follow rename follow singleton AP_Follow method have_target boolean +singleton AP_Follow method get_target_sysid uint8_t singleton AP_Follow method get_last_update_ms uint32_t singleton AP_Follow method get_target_location_and_velocity boolean Location'Null Vector3f'Null singleton AP_Follow method get_target_location_and_velocity_ofs boolean Location'Null Vector3f'Null singleton AP_Follow method get_target_heading_deg boolean float'Null +singleton AP_Follow method get_target_info boolean Vector3f'Null Vector3f'Null Vector3f'Null Vector3f'Null Location'Null Location'Null float'Null +singleton AP_Follow method get_target_info depends APM_BUILD_TYPE(APM_BUILD_ArduPlane) include AC_PrecLand/AC_PrecLand.h singleton AC_PrecLand depends AC_PRECLAND_ENABLED && (APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI) diff --git a/libraries/AP_Scripting/modules/mavlink_attitude.lua b/libraries/AP_Scripting/modules/mavlink_attitude.lua new file mode 100644 index 00000000000000..a5636988351313 --- /dev/null +++ b/libraries/AP_Scripting/modules/mavlink_attitude.lua @@ -0,0 +1,168 @@ +--[[ + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + + MAVLinkAttitude + A MAVlink message receiver for ATTITUDE messages specifically +--]] + +local MAVLinkAttitude = {} + +MAVLinkAttitude.SCRIPT_VERSION = "4.6.0-004" +MAVLinkAttitude.SCRIPT_NAME = "MAVLink Attitude" +MAVLinkAttitude.SCRIPT_NAME_SHORT = "MAVATT" + +--[[ + import mavlink support for NAMED_VALUE_FLOAT, only used for + DUAL_AIRCRAFT operation +--]] + +ATTITUDE_MESSAGE = "ATTITUDE" + +--[[ + a lua implementation of the jitter correction algorithm from libraries/AP_RTC + + note that the use of a 32 bit float lua number for a uint32_t + milliseconds means we lose accuracy over time. At 9 hours we have + an accuracy of about 1 millisecond +--]] +function MAVLinkAttitude.JitterCorrection(_max_lag_ms, _convergence_loops) + local self = {} + + local max_lag_ms = _max_lag_ms + local convergence_loops = _convergence_loops + local link_offset_ms = 0 + local min_sample_ms = 0 + local initialised = false + local min_sample_counter = 0 + + function self.correct_offboard_timestamp_msec(offboard_ms, local_ms) + local diff_ms = local_ms - offboard_ms + if not initialised or diff_ms < link_offset_ms then + --[[ + this message arrived from the remote system with a + timestamp that would imply the message was from the + future. We know that isn't possible, so we adjust down the + correction value + --]] + link_offset_ms = diff_ms + initialised = true + end + + local estimate_ms = offboard_ms + link_offset_ms + + if estimate_ms + max_lag_ms < local_ms then + --[[ + this implies the message came from too far in the past. clamp the lag estimate + to assume the message had maximum lag + --]] + estimate_ms = local_ms - max_lag_ms + link_offset_ms = estimate_ms - offboard_ms + end + + if min_sample_counter == 0 then + min_sample_ms = diff_ms + end + + min_sample_counter = (min_sample_counter+1) + if diff_ms < min_sample_ms then + min_sample_ms = diff_ms + end + if min_sample_counter == convergence_loops then + --[[ + we have the requested number of samples of the transport + lag for convergence. To account for long term clock drift + we set the diff we will use in future to this value + --]] + link_offset_ms = min_sample_ms + min_sample_counter = 0 + end + return estimate_ms + end + return self + end + +function MAVLinkAttitude.mavlink_attitude_receiver() + local self = {} + local ATTITUDE_map = {} + ATTITUDE_map.id = 30 + ATTITUDE_map.fields = { + { "time_boot_ms", ". + + SpeedPI + A simple "PI" controller for airspeed. Copied from Andrew Tridgell's original + work on the ArduPilot Aerobatics Lua scripts. + + Usage: + 1. drop it in the scripts/modules directory + 2. include in your own script using + local speedpi = requre("speedpi.lua") + 3. create an instance - you may need to tune the gains + local speed_controller = speedpi.speed_controller(0.1, 0.1, 2.5, airspeed_min, airspeed_max) + 4. call it's update() from your update() with the current airspeed and airspeed error + local airspeed_new = speed_controller.update(vehicle_airspeed, desired_airspeed - vehicle_airspeed) + 5. Set the vehicle airspeed based on the airspeed_new value returned from speedpi + +--]] + +local SpeedPID = {} + +SpeedPID.SCRIPT_VERSION = "4.6.0-003" +SpeedPID.SCRIPT_NAME = "Speed PID Controller" +SpeedPID.SCRIPT_NAME_SHORT = "SpeedPID" + +SpeedPID.MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} + +-- constrain a value between limits +function SpeedPID.constrain(v, vmin, vmax) + if v < vmin then + v = vmin + end + if v > vmax then + v = vmax + end + return v +end + +function SpeedPID.is_zero(x) + return math.abs(x) < 1.1920928955078125e-7 +end + +function SpeedPID.PID_controller(kP,kI,kD,iMax,min,max) + -- the new instance. You can put public variables inside this self + -- declaration if you want to + local self = {} + + -- private fields as locals + local _kP = kP or 0.0 + local _kI = kI or 0.0 + local _kD = kD or 0.0 + local _iMax = iMax + local _min = min + local _max = max + local _last_t = nil + local _error = 0.0 + local _derivative = 0.0 + local _reset_filter = true + local _filt_E_hz = 0.01 + local _filt_D_hz = 0.005 + local _D = 0 + local _I = 0 + local _P = 0 + local _total = 0 + local _counter = 0 + local _target = 0 + local _current = 0 + local nowPI = millis():tofloat() * 0.001 + + function self.calc_lowpass_alpha_dt(dt, cutoff_freq) + if (dt <= 0.0 or cutoff_freq <= 0.0) then + --INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result); + return 1.0 + end + if (SpeedPID.is_zero(cutoff_freq)) then + return 1.0 + end + if (SpeedPID.is_zero(dt)) then + return 0.0 + end + local rc = 1.0 / (math.pi * 2.0 * cutoff_freq) + return dt / (dt + rc); + end + + function self.get_filt_E_alpha(dt) + return self.calc_lowpass_alpha_dt(dt, _filt_E_hz); + end + + function self.get_filt_D_alpha(dt) + return self.calc_lowpass_alpha_dt(dt, _filt_D_hz); + end + + -- update the controller. + function self.update(target, current) + local now = micros() + if not _last_t then + _last_t = now + end + local dt = (now - _last_t):tofloat() * 0.000001 + _last_t = now + local err = target - current + _counter = _counter + 1 + + -- reset input filter to value received + if (_reset_filter) then + _reset_filter = false + _error = _target - current + _derivative = 0.0 + else + local error_last = _error; + _error = _error + self.get_filt_E_alpha(dt) * ((_target - current) - _error); + + -- calculate and filter derivative + if (dt >= 0) then + local derivative = (_error - error_last) / dt; + _derivative = _derivative + self.get_filt_D_alpha(dt) * (derivative - _derivative); + end + end + local D = _derivative * _kD + _D = D + + local P = _kP * err + if ((_total < _max and _total > _min) or + (_total >= _max and err < 0) or + (_total <= _min and err > 0)) then + _I = _I + _kI * err * dt + end + if _iMax then + _I = SpeedPID.constrain(_I, -_iMax, iMax) + end + local I = _I + local ret = target + P + I + D + if math.floor(now:tofloat() * 0.000001) ~= math.floor(nowPI) then + nowPI = micros():tofloat() * 0.000001 + end + _target = target + _current = current + _P = P + + ret = SpeedPID.constrain(ret, _min, _max) + _total = ret + return ret + end + + -- reset integrator to an initial value + function self.reset(integrator) + _I = integrator + _reset_filter = true + end + + function self.set_D(D) + _D = D + _D_error = 0 + end + + function self.set_I(I) + _kI = I + end + + function self.set_P(P) + _kP = P + end + + function self.set_Imax(Imax) + _iMax = Imax + end + + -- log the controller internals + function self.log(name, add_total) + -- allow for an external addition to total + -- Targ = Current + error ( target airspeed ) + -- Curr = Current airspeed input to the controller + -- P = calculated Proportional component + -- I = calculated Integral component + -- Total = calculated new Airspeed + -- Add - passed in as 0 + logger:write(name,'Targ,Curr,P,I,D,Total,Add','fffffff',_target,_current,_P,_I,_D,_total,add_total) + end + + -- return the instance + return self + end + +function SpeedPID.speed_controller(kP_param, kI_param, kD_param, iMax, sMin, sMax) + local self = {} + local speedpid = SpeedPID.PID_controller(kP_param, kI_param, kD_param, iMax, sMin, sMax) + + function self.update(spd_current, spd_error) + local adjustment = speedpid.update(spd_current + spd_error, spd_current) + speedpid.log("ZSPI", 0) -- Z = scripted, S = speed, PI = PI controller + return adjustment + end + + function self.reset() + speedpid.reset(0) + end + + return self +end + +gcs:send_text(SpeedPID.MAV_SEVERITY.NOTICE, string.format("%s %s module loaded", SpeedPID.SCRIPT_NAME, SpeedPID.SCRIPT_VERSION) ) + +return SpeedPID From 1ecee67d5219497e338d20068985a0fbbc69716c Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Fri, 17 Jan 2025 22:44:20 -0700 Subject: [PATCH 04/14] AP_Scripting: scripted plane follow --- .../AP_Scripting/applets/plane_follow.lua | 214 ++++++++++++++---- 1 file changed, 175 insertions(+), 39 deletions(-) diff --git a/libraries/AP_Scripting/applets/plane_follow.lua b/libraries/AP_Scripting/applets/plane_follow.lua index d557eb21450fad..6444f87a1a3353 100644 --- a/libraries/AP_Scripting/applets/plane_follow.lua +++ b/libraries/AP_Scripting/applets/plane_follow.lua @@ -25,7 +25,7 @@ ZPR_TURN_DEG - if the target is more than this many degrees left or right, assume it's turning --]] -SCRIPT_VERSION = "4.6.0-044" +SCRIPT_VERSION = "4.7.0-047" SCRIPT_NAME = "Plane Follow" SCRIPT_NAME_SHORT = "PFollow" @@ -34,7 +34,9 @@ ALT_FRAME = { GLOBAL = 0, RELATIVE = 1, TERRAIN = 3} MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} MAV_FRAME = {GLOBAL = 0, GLOBAL_RELATIVE_ALT = 3, GLOBAL_TERRAIN_ALT = 10} -MAV_CMD_INT = { DO_SET_MODE = 176, DO_CHANGE_SPEED = 178, DO_REPOSITION = 192, +MAV_CMD_INT = { ATTITUDE = 30, GLOBAL_POSITION_INT = 33, + DO_SET_MODE = 176, DO_CHANGE_SPEED = 178, DO_REPOSITION = 192, + CMD_SET_MESSAGE_INTERVAL = 511, CMD_REQUEST_MESSAGE = 512, GUIDED_CHANGE_SPEED = 43000, GUIDED_CHANGE_ALTITUDE = 43001, GUIDED_CHANGE_HEADING = 43002 } MAV_SPEED_TYPE = { AIRSPEED = 0, GROUNDSPEED = 1, CLIMB_SPEED = 2, DESCENT_SPEED = 3 } MAV_HEADING_TYPE = { COG = 0, HEADING = 1, DEFAULT = 2} -- COG = Course over Ground, i.e. where you want to go, HEADING = which way the vehicle points @@ -46,6 +48,8 @@ local windspeed_vector = ahrs:wind_estimate() local now = millis():tofloat() * 0.001 local now_target_heading = now +local now_telemetry_request = now +local now_follow_lost = now local follow_enabled = false local too_close_follow_up = 0 local lost_target_countdown = LOST_TARGET_TIMEOUT @@ -56,8 +60,6 @@ local tight_turn = false local PARAM_TABLE_KEY = 120 local PARAM_TABLE_PREFIX = "ZPF_" -local PARAM_TABLE_KEY2 = 121 -local PARAM_TABLE_PREFIX2 = "ZPF2_" -- add a parameter and bind it to a variable local function bind_add_param(name, idx, default_value) @@ -65,15 +67,15 @@ local function bind_add_param(name, idx, default_value) return Parameter(PARAM_TABLE_PREFIX .. name) end -- setup follow mode specific parameters -assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 10), 'could not add param table') +assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 20), 'could not add param table') -- add a parameter and bind it to a variable -local function bind_add_param2(name, idx, default_value) - assert(param:add_param(PARAM_TABLE_KEY2, idx, name, default_value), string.format('could not add param %s', name)) - return Parameter(PARAM_TABLE_PREFIX2 .. name) -end +--local function bind_add_param2(name, idx, default_value) +-- assert(param:add_param(PARAM_TABLE_KEY2, idx, name, default_value), string.format('could not add param %s', name)) +-- return Parameter(PARAM_TABLE_PREFIX2 .. name) +--end -- setup follow mode specific parameters- second tranche -assert(param:add_table(PARAM_TABLE_KEY2, PARAM_TABLE_PREFIX2, 10), 'could not add param table') +-- assert(param:add_table(PARAM_TABLE_KEY2, PARAM_TABLE_PREFIX2, 10), 'could not add param table') -- This uses the exisitng FOLL_* parameters and just adds a couple specific to this script -- but because most of the logic is already in AP_Follow (called by binding to follow:) there @@ -82,12 +84,10 @@ assert(param:add_table(PARAM_TABLE_KEY2, PARAM_TABLE_PREFIX2, 10), 'could not ad -- we need these existing FOLL_ parametrs FOLL_ALT_TYPE = Parameter('FOLL_ALT_TYPE') FOLL_SYSID = Parameter('FOLL_SYSID') -FOLL_TIMEOUT = Parameter('FOLL_TIMEOUT') FOLL_OFS_Y = Parameter('FOLL_OFS_Y') local foll_sysid = FOLL_SYSID:get() or -1 local foll_ofs_y = FOLL_OFS_Y:get() or 0.0 local foll_alt_type = FOLL_ALT_TYPE:get() or ALT_FRAME.GLOBAL -local foll_timeout = FOLL_TIMEOUT:get() or 1000 -- Add these ZPF_ parameters specifically for this script --[[ @@ -116,8 +116,8 @@ ZPF_ACT_FN = bind_add_param("ACT_FN", 3, 301) --[[ // @Param: ZPF_TIMEOUT - // @DisplayName: Plane Follow Scripting Timeout - // @Description: How long to try re-aquire a target if lost + // @DisplayName: Plane Follow Telemetry Timeout + // @Description: How long to try reaquire a target if telemetry from the lead vehicle is lost. // @Range: 0 30 // @Units: s --]] @@ -173,7 +173,7 @@ ZPF_ALT_OVR = bind_add_param("ALT_OVR", 9, 0) // @Description: P gain for the speed PID controller distance component // @Range: 0 1 --]] -ZPF2_D_P = bind_add_param2("D_P", 1, 0.3) +ZPF_D_P = bind_add_param("D_P", 11, 0.3) --[[ // @Param: ZPF_D_I @@ -181,7 +181,7 @@ ZPF2_D_P = bind_add_param2("D_P", 1, 0.3) // @Description: I gain for the speed PID distance component // @Range: 0 1 --]] -ZPF2_D_I = bind_add_param2("D_I", 2, 0.3) +ZPF_D_I = bind_add_param("D_I", 12, 0.3) --[[ // @Param: ZPF_D_D @@ -189,7 +189,7 @@ ZPF2_D_I = bind_add_param2("D_I", 2, 0.3) // @Description: D gain for the speed PID controller distance component // @Range: 0 1 --]] -ZPF2_D_D = bind_add_param2("D_D", 3, 0.05) +ZPF_D_D = bind_add_param("D_D", 13, 0.05) --[[ // @Param: ZPF_V_P @@ -197,7 +197,7 @@ ZPF2_D_D = bind_add_param2("D_D", 3, 0.05) // @Description: P gain for the speed PID controller velocity component // @Range: 0 1 --]] -ZPF2_V_P = bind_add_param2("V_P", 4, 0.3) +ZPF_V_P = bind_add_param("V_P", 14, 0.3) --[[ // @Param: ZPF_V_I @@ -205,7 +205,7 @@ ZPF2_V_P = bind_add_param2("V_P", 4, 0.3) // @Description: I gain for the speed PID controller velocity component // @Range: 0 1 --]] -ZPF2_V_I = bind_add_param2("V_I", 5, 0.3) +ZPF_V_I = bind_add_param("V_I", 15, 0.3) --[[ // @Param: ZPF_V_D @@ -213,7 +213,7 @@ ZPF2_V_I = bind_add_param2("V_I", 5, 0.3) // @Description: D gain for the speed PID controller velocity component // @Range: 0 1 --]] -ZPF2_V_D = bind_add_param2("V_D", 6, 0.05) +ZPF_V_D = bind_add_param("V_D", 16, 0.05) --[[ // @Param: ZPF_LkAHD @@ -221,7 +221,7 @@ ZPF2_V_D = bind_add_param2("V_D", 6, 0.05) // @Description: Time to "lookahead" when calculating distance errors // @Units: s --]] -ZPF2_LKAHD = bind_add_param2("LKAHD", 7, 5) +ZPF_LKAHD = bind_add_param("LKAHD", 17, 5) --[[ // @Param: ZPF_DIST_FUDGE @@ -229,7 +229,15 @@ ZPF2_LKAHD = bind_add_param2("LKAHD", 7, 5) // @Description: THe distance returned by the AP_FOLLOW library might be off by about this factor of airspeed // @Units: s --]] -ZPF2_DIST_FUDGE = bind_add_param2("DIST_FUDGE", 8, 0.92) +ZPF_DIST_FUDGE = bind_add_param("DIST_FUDGE", 18, 0.92) + +--[[ + // @Param: ZPF_SIM_TELF_FN + // @DisplayName: Plane Follow Simulate Telemetry fail function + // @Description: Set this switch to simulate losing telemetry from the other vehicle + // @Range: 300 307 +--]] +ZPF_SIM_TELF_FN = bind_add_param("SIM_TELF_FN", 19, 302) REFRESH_RATE = 0.05 -- in seconds, so 20Hz LOST_TARGET_TIMEOUT = (ZPF_TIMEOUT:get() or 10) / REFRESH_RATE @@ -241,9 +249,11 @@ local exit_mode = ZPF_EXIT_MODE:get() or FLIGHT_MODE.LOITER local use_wide_turns = ZPF_WIDE_TURNS:get() or 1 -local distance_fudge = ZPF2_DIST_FUDGE:get() or 0.92 +local distance_fudge = ZPF_DIST_FUDGE:get() or 0.92 -DISTANCE_LOOKAHEAD_SECONDS = ZPF2_LKAHD:get() or 5.0 +DISTANCE_LOOKAHEAD_SECONDS = ZPF_LKAHD:get() or 5.0 + +local simulate_telemetry_failed = false AIRSPEED_MIN = Parameter('AIRSPEED_MIN') AIRSPEED_MAX = Parameter('AIRSPEED_MAX') @@ -267,14 +277,14 @@ local function constrain(v, vmin, vmax) end local speedpid = require("speedpid") -local pid_controller_distance = speedpid.speed_controller(ZPF2_D_P:get() or 0.3, - ZPF2_D_I:get() or 0.3, - ZPF2_D_D:get() or 0.05, +local pid_controller_distance = speedpid.speed_controller(ZPF_D_P:get() or 0.3, + ZPF_D_I:get() or 0.3, + ZPF_D_D:get() or 0.05, 5.0, airspeed_min - airspeed_max, airspeed_max - airspeed_min) -local pid_controller_velocity = speedpid.speed_controller(ZPF2_V_P:get() or 0.3, - ZPF2_V_I:get() or 0.3, - ZPF2_V_D:get() or 0.05, +local pid_controller_velocity = speedpid.speed_controller(ZPF_V_P:get() or 0.3, + ZPF_V_I:get() or 0.3, + ZPF_V_D:get() or 0.05, 5.0, airspeed_min, airspeed_max) @@ -292,6 +302,107 @@ local function follow_frame_to_mavlink(follow_frame) return mavlink_frame end +local COMMAND_INT_map = {} +COMMAND_INT_map.id = 75 +COMMAND_INT_map.fields = { + { "param1", " 10 then + request_message_interval(0, {sysid = foll_sysid, message_id = MAV_CMD_INT.ATTITUDE, interval = 100}) + request_message_interval(0, {sysid = foll_sysid, message_id = MAV_CMD_INT.GLOBAL_POSITION_INT, interval = 100}) + now_telemetry_request = now + end + -- set the target frame as per user set parameter - this is fundamental to this working correctly local close_distance = ZPF_DIST_CLOSE:get() or airspeed_cruise * 2.0 local long_distance = close_distance * 4.0 @@ -518,7 +648,7 @@ local function update() foll_ofs_y = FOLL_OFS_Y:get() or 0.0 foll_alt_type = FOLL_ALT_TYPE:get() or ALT_FRAME.GLOBAL use_wide_turns = ZPF_WIDE_TURNS:get() or 1 - distance_fudge = ZPF2_DIST_FUDGE:get() or 0.92 + distance_fudge = ZPF_DIST_FUDGE:get() or 0.92 --[[ get the current navigation target. @@ -564,23 +694,29 @@ local function update() -- if we lose the target wait for LOST_TARGET_TIMEOUT seconds to try to reaquire it if target_location == nil or target_location_offset == nil or target_velocity == nil or target_velocity_offset == nil or - target_distance_offset == nil or current_target == nil or target_distance == nil or xy_dist == nil then + target_distance_offset == nil or current_target == nil or target_distance == nil or xy_dist == nil or + simulate_telemetry_failed then lost_target_countdown = lost_target_countdown - 1 if lost_target_countdown <= 0 then follow_enabled = false vehicle:set_mode(fail_mode) - gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": follow: " .. FOLL_SYSID:get() .. " FAILED") + gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. string.format(": follow: %.0f FAILED", FOLL_SYSID:get())) return end -- maintain the current heading until we re-establish telemetry from the target vehicle -- note that this is not logged, needs fixing - gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": follow: lost " .. FOLL_SYSID:get() .. " FAILED hdg: " .. save_target_heading1) - set_vehicle_heading({heading = save_target_heading1}) - set_vehicle_target_altitude({alt = save_target_altitude, frame = foll_alt_type}) -- pass altitude in meters (location has it in cm) + -- gcs:send_text(MAV_SEVERITY.WARNING, SCRIPT_NAME_SHORT .. ": follow: lost " .. (now_follow_lost - now) .. " seconds ") + if (now - now_follow_lost) > 2 then + gcs:send_text(MAV_SEVERITY.WARNING, SCRIPT_NAME_SHORT .. string.format(": follow: lost %.0f set hdg: %.0f", FOLL_SYSID:get(), save_target_heading1)) + now_follow_lost = now + set_vehicle_heading({heading = save_target_heading1}) + set_vehicle_target_altitude({alt = save_target_altitude, frame = foll_alt_type}) -- pass altitude in meters (location has it in cm) + end return else -- have a good target so reset the countdown lost_target_countdown = LOST_TARGET_TIMEOUT + now_follow_lost = now end -- target_velocity from MAVLink (via AP_Follow) is groundspeed, need to convert to airspeed, @@ -652,7 +788,7 @@ local function update() local angle_adjustment tight_turn = false if target_attitude ~= nil then - if (now - target_attitude.timestamp_ms < foll_timeout) and + if (now - (target_attitude.timestamp_ms * 0.001) < LOST_TARGET_TIMEOUT) and math.abs(target_attitude.roll) > 0.1 or math.abs(target_attitude.rollspeed) > 1 then local turn_radius = vehicle_airspeed * vehicle_airspeed / (9.80665 * math.tan(target_attitude.roll + target_attitude.rollspeed)) From 744eda24022b81c5a5e2994322fff6f893d4ad3e Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Fri, 17 Jan 2025 22:44:56 -0700 Subject: [PATCH 05/14] AP_Follow: changes to support scripted plane follow --- libraries/AP_Follow/AP_Follow.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/libraries/AP_Follow/AP_Follow.cpp b/libraries/AP_Follow/AP_Follow.cpp index 673d0e42915c1d..795c10e4d18250 100644 --- a/libraries/AP_Follow/AP_Follow.cpp +++ b/libraries/AP_Follow/AP_Follow.cpp @@ -592,7 +592,6 @@ bool AP_Follow::have_target(void) const // check for timeout 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; From 8ba3e6d2150aaa2b5f5203b2b8f1d2b05101ded3 Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Fri, 24 Jan 2025 14:07:41 -0700 Subject: [PATCH 06/14] AP_Scripting: scripted plane follow --- .../AP_Scripting/applets/plane_follow.lua | 34 +++++--- .../modules/mavlink_command_int.lua | 80 +++++++++++++++++++ 2 files changed, 103 insertions(+), 11 deletions(-) create mode 100644 libraries/AP_Scripting/modules/mavlink_command_int.lua diff --git a/libraries/AP_Scripting/applets/plane_follow.lua b/libraries/AP_Scripting/applets/plane_follow.lua index 6444f87a1a3353..67321938cebad8 100644 --- a/libraries/AP_Scripting/applets/plane_follow.lua +++ b/libraries/AP_Scripting/applets/plane_follow.lua @@ -239,6 +239,14 @@ ZPF_DIST_FUDGE = bind_add_param("DIST_FUDGE", 18, 0.92) --]] ZPF_SIM_TELF_FN = bind_add_param("SIM_TELF_FN", 19, 302) +--[[ + // @Param: ZPF_SR_CH + // @DisplayName: Plane Follow Serial Channel + // @Description: This is the serial/channel where mavlink messages will go to the target vehicle + // @Range: 0 9 +--]] +ZPF_SR_CH = bind_add_param("SR_CH", 20, 0) + REFRESH_RATE = 0.05 -- in seconds, so 20Hz LOST_TARGET_TIMEOUT = (ZPF_TIMEOUT:get() or 10) / REFRESH_RATE OVERSHOOT_ANGLE = ZPF_OVRSHT_DEG:get() or 75.0 @@ -253,6 +261,8 @@ local distance_fudge = ZPF_DIST_FUDGE:get() or 0.92 DISTANCE_LOOKAHEAD_SECONDS = ZPF_LKAHD:get() or 5.0 +local target_serial_channel = ZPF_SR_CH:get() or 0 + local simulate_telemetry_failed = false AIRSPEED_MIN = Parameter('AIRSPEED_MIN') @@ -356,7 +366,7 @@ local mavlink_command_int = require("mavlink_command_int") -- target.message_id = the message id of the requested message -- target.interval = the interval in milliseconds for the target vehicle to send message_id messages local function request_message_interval(channel, target) - local target_sysid = target.sysid or 0 + local target_sysid = target.sysid or foll_sysid if target_sysid == nil then gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": request_message_interval no target") return @@ -629,13 +639,6 @@ local function update() return end - -- Need to request that the follow vehicle sends telemetry at a reasonable rate - -- we send a new request evern 10 seconds, just to make sure the message gets through - if (now - now_telemetry_request) > 10 then - request_message_interval(0, {sysid = foll_sysid, message_id = MAV_CMD_INT.ATTITUDE, interval = 100}) - request_message_interval(0, {sysid = foll_sysid, message_id = MAV_CMD_INT.GLOBAL_POSITION_INT, interval = 100}) - now_telemetry_request = now - end -- set the target frame as per user set parameter - this is fundamental to this working correctly local close_distance = ZPF_DIST_CLOSE:get() or airspeed_cruise * 2.0 @@ -649,12 +652,21 @@ local function update() foll_alt_type = FOLL_ALT_TYPE:get() or ALT_FRAME.GLOBAL use_wide_turns = ZPF_WIDE_TURNS:get() or 1 distance_fudge = ZPF_DIST_FUDGE:get() or 0.92 + target_serial_channel = ZPF_SR_CH:get() or 0 + + -- Need to request that the follow vehicle sends telemetry at a reasonable rate + -- we send a new request every 10 seconds, just to make sure the message gets through + if (now - now_telemetry_request) > 10 then + request_message_interval(target_serial_channel, {sysid = foll_sysid, message_id = MAV_CMD_INT.ATTITUDE, interval = 100}) + request_message_interval(target_serial_channel, {sysid = foll_sysid, message_id = MAV_CMD_INT.GLOBAL_POSITION_INT, interval = 100}) + now_telemetry_request = now + end --[[ get the current navigation target. --]] local target_location -- = Location() of the target - local target_location_offset -- Location of the target with FOLL_OFS_* offsets applied + local target_location_offset -- Location of the target with FOLL_OFS_* offsets applied local target_velocity -- = Vector3f() -- current velocity of lead vehicle local target_velocity_offset -- Vector3f -- velocity to the offset target_location_offset local target_distance -- = Vector3f() -- vector to lead vehicle @@ -704,7 +716,7 @@ local function update() return end - -- maintain the current heading until we re-establish telemetry from the target vehicle -- note that this is not logged, needs fixing + -- maintain the current heading until we re-establish telemetry from the target vehicle -- gcs:send_text(MAV_SEVERITY.WARNING, SCRIPT_NAME_SHORT .. ": follow: lost " .. (now_follow_lost - now) .. " seconds ") if (now - now_follow_lost) > 2 then gcs:send_text(MAV_SEVERITY.WARNING, SCRIPT_NAME_SHORT .. string.format(": follow: lost %.0f set hdg: %.0f", FOLL_SYSID:get(), save_target_heading1)) @@ -933,7 +945,7 @@ local function protected_wrapper() return protected_wrapper, 1000 * REFRESH_RATE end -function delayed_start() +local function delayed_start() gcs:send_text(MAV_SEVERITY.INFO, string.format("%s %s script loaded", SCRIPT_NAME, SCRIPT_VERSION) ) return protected_wrapper() end diff --git a/libraries/AP_Scripting/modules/mavlink_command_int.lua b/libraries/AP_Scripting/modules/mavlink_command_int.lua new file mode 100644 index 00000000000000..effd86138b5240 --- /dev/null +++ b/libraries/AP_Scripting/modules/mavlink_command_int.lua @@ -0,0 +1,80 @@ +--[[ + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + + MAVLink_command_int + A MAVlink message manager for COMMAND_INT messages specifically + Only send is implemented at the moment, but its intended that receive could be added +--]] + +local MAVLink_command_int = {} + +MAVLink_command_int.SCRIPT_VERSION = "4.7.0-001" +MAVLink_command_int.SCRIPT_NAME = "MAVLink Command Int" +MAVLink_command_int.SCRIPT_NAME_SHORT = "MAVCMDINT" + +MAVLink_command_int.COMMAND_INT_MESSAGE = "COMMAND_INT" + +MAVLink_command_int.map = {} +MAVLink_command_int.map.id = 75 +MAVLink_command_int.map.fields = { + { "param1", " Date: Fri, 24 Jan 2025 15:30:59 -0700 Subject: [PATCH 07/14] AP_Scripting: scripted plane follow --- .../AP_Scripting/applets/plane_follow.lua | 37 +++++-------------- 1 file changed, 10 insertions(+), 27 deletions(-) diff --git a/libraries/AP_Scripting/applets/plane_follow.lua b/libraries/AP_Scripting/applets/plane_follow.lua index 67321938cebad8..3cdfa5395419b6 100644 --- a/libraries/AP_Scripting/applets/plane_follow.lua +++ b/libraries/AP_Scripting/applets/plane_follow.lua @@ -388,31 +388,10 @@ local function request_message_interval(channel, target) --local encoded = mavlink_encode(COMMAND_INT_map, msg_fields) --print(encoded) if not mavlink_command_int.send(channel, message) then - gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. " oh no the mavlink buffer is full") + gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. " MAVLink buffer is full") end - ---[[ local request_interval_message {} - request_interval_message.timestamp = millis():toint() - request_interval_message.param1 = - - mavlink:send(chan, mavlink_msgs.encode("MSG_NAME", {param1 = value1, param2 = value2, ...}}) - - mavlink:send_chan(FOLT_MAV_CHAN:get(), mavlink_msgs.encode("FOLLOW_TARGET", follow_target_msg)) - - -- GUIDED_CHANGE_ALTITUDE takes altitude in meters - if not gcs:run_command_int(MAV_CMD_INT.CMD_SET_MESSAGE_INTERVAL, { - p1 = target_sysid, - p3 = speed, - z = target.alt }) then - gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": MAVLink CHANGE_ALTITUDE returned false") - end ---]] - ---- ---- end - -- set_vehicle_target_altitude() Parameters -- target.alt = new target altitude in meters -- target.frame = Altitude frame MAV_FRAME, it's very important to get this right! @@ -518,6 +497,7 @@ end return true if we are in a state where follow can apply --]] local reported_target = true +local lost_target_now = now local function follow_active() local mode = vehicle:get_mode() @@ -525,9 +505,13 @@ local function follow_active() if follow_enabled then if follow:have_target() then reported_target = true - else - if reported_target then - gcs:send_text(MAV_SEVERITY.WARNING, SCRIPT_NAME_SHORT .. ": no target: " .. follow:get_target_sysid()) + lost_target_now = now + else + if reported_target then -- i.e. if we previously reported a target but lost it + if (now - lost_target_now) > 5 then + gcs:send_text(MAV_SEVERITY.WARNING, SCRIPT_NAME_SHORT .. ": lost prior target: " .. follow:get_target_sysid()) + lost_target_now = now + end end reported_target = false end @@ -636,10 +620,9 @@ local function update() follow_check() if not follow_active() then - return + return end - -- set the target frame as per user set parameter - this is fundamental to this working correctly local close_distance = ZPF_DIST_CLOSE:get() or airspeed_cruise * 2.0 local long_distance = close_distance * 4.0 From 72b232bb06bffa6fcb56881c7a85fdc17a4a676e Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Mon, 27 Jan 2025 08:29:17 -0700 Subject: [PATCH 08/14] AP_Follow: changes to support scripted plane follow --- libraries/AP_Follow/AP_Follow.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Follow/AP_Follow.cpp b/libraries/AP_Follow/AP_Follow.cpp index 795c10e4d18250..b671165e676576 100644 --- a/libraries/AP_Follow/AP_Follow.cpp +++ b/libraries/AP_Follow/AP_Follow.cpp @@ -146,7 +146,7 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = { // @Param: _TIMEOUT // @DisplayName: Follow timeout - // @Description: Follow position estimate timeout after x milliseconds + // @Description: Follow position update from lead - timeout after x milliseconds // @User: Standard // @Units: ms AP_GROUPINFO("_TIMEOUT", 12, AP_Follow, _timeout_ms, AP_FOLLOW_TIMEOUT_DEFAULT), From 1effa67dc3070d0b361335fe19fb6850aec656e7 Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Mon, 3 Feb 2025 10:12:57 -0700 Subject: [PATCH 09/14] AP_Follow: changes to support scripted plane follow --- libraries/AP_Scripting/modules/mavlink_attitude.lua | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/libraries/AP_Scripting/modules/mavlink_attitude.lua b/libraries/AP_Scripting/modules/mavlink_attitude.lua index a5636988351313..8944d331783bde 100644 --- a/libraries/AP_Scripting/modules/mavlink_attitude.lua +++ b/libraries/AP_Scripting/modules/mavlink_attitude.lua @@ -19,15 +19,10 @@ local MAVLinkAttitude = {} -MAVLinkAttitude.SCRIPT_VERSION = "4.6.0-004" +MAVLinkAttitude.SCRIPT_VERSION = "4.7.0-005" MAVLinkAttitude.SCRIPT_NAME = "MAVLink Attitude" MAVLinkAttitude.SCRIPT_NAME_SHORT = "MAVATT" ---[[ - import mavlink support for NAMED_VALUE_FLOAT, only used for - DUAL_AIRCRAFT operation ---]] - ATTITUDE_MESSAGE = "ATTITUDE" --[[ @@ -165,4 +160,4 @@ MAVLinkAttitude.mavlink_handler = MAVLinkAttitude.mavlink_attitude_receiver() gcs:send_text(MAV_SEVERITY.NOTICE, string.format("%s %s module loaded", MAVLinkAttitude.SCRIPT_NAME, MAVLinkAttitude.SCRIPT_VERSION) ) -return MAVLinkAttitude \ No newline at end of file +return MAVLinkAttitude From fbb39f9ebd02909160e4595792d22c9c38365a49 Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Fri, 14 Feb 2025 16:38:23 -0700 Subject: [PATCH 10/14] AP_Follow: changes to support scripted plane follow --- .../AP_Scripting/applets/plane_follow.lua | 120 +++++------------- .../AP_Scripting/applets/plane_follow.md | 40 +++--- .../AP_Scripting/modules/mavlink_attitude.lua | 4 +- .../modules/mavlink_command_int.lua | 98 ++++++++++++-- 4 files changed, 143 insertions(+), 119 deletions(-) diff --git a/libraries/AP_Scripting/applets/plane_follow.lua b/libraries/AP_Scripting/applets/plane_follow.lua index 3cdfa5395419b6..078c95c4dce213 100644 --- a/libraries/AP_Scripting/applets/plane_follow.lua +++ b/libraries/AP_Scripting/applets/plane_follow.lua @@ -25,7 +25,7 @@ ZPR_TURN_DEG - if the target is more than this many degrees left or right, assume it's turning --]] -SCRIPT_VERSION = "4.7.0-047" +SCRIPT_VERSION = "4.7.0-050" SCRIPT_NAME = "Plane Follow" SCRIPT_NAME_SHORT = "PFollow" @@ -34,13 +34,16 @@ ALT_FRAME = { GLOBAL = 0, RELATIVE = 1, TERRAIN = 3} MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} MAV_FRAME = {GLOBAL = 0, GLOBAL_RELATIVE_ALT = 3, GLOBAL_TERRAIN_ALT = 10} -MAV_CMD_INT = { ATTITUDE = 30, GLOBAL_POSITION_INT = 33, +MAV_CMD_INT = { ATTITUDE = 30, GLOBAL_POSITION_INT = 33, REQUEST_DATA_STREAM = 66, DO_SET_MODE = 176, DO_CHANGE_SPEED = 178, DO_REPOSITION = 192, CMD_SET_MESSAGE_INTERVAL = 511, CMD_REQUEST_MESSAGE = 512, GUIDED_CHANGE_SPEED = 43000, GUIDED_CHANGE_ALTITUDE = 43001, GUIDED_CHANGE_HEADING = 43002 } MAV_SPEED_TYPE = { AIRSPEED = 0, GROUNDSPEED = 1, CLIMB_SPEED = 2, DESCENT_SPEED = 3 } MAV_HEADING_TYPE = { COG = 0, HEADING = 1, DEFAULT = 2} -- COG = Course over Ground, i.e. where you want to go, HEADING = which way the vehicle points +MAV_DATA_STREAM = { MAV_DATA_STREAM_ALL=0, MAV_DATA_STREAM_RAW_SENSORS=1, MAV_DATA_STREAM_EXTENDED_STATUS=2, MAV_DATA_STREAM_RC_CHANNELS=3, + MAV_DATA_STREAM_RAW_CONTROLLER=4, MAV_DATA_STREAM_POSITION=6, MAV_DATA_STREAM_EXTRA1=10, MAV_DATA_STREAM_EXTRA2=11 } + FLIGHT_MODE = {AUTO=10, RTL=11, LOITER=12, GUIDED=15, QHOVER=18, QLOITER=19, QRTL=21} local ahrs_eas2tas = ahrs:get_EAS2TAS() @@ -52,7 +55,6 @@ local now_telemetry_request = now local now_follow_lost = now local follow_enabled = false local too_close_follow_up = 0 -local lost_target_countdown = LOST_TARGET_TIMEOUT local save_target_heading1 = -400.0 local save_target_heading2 = -400.0 local save_target_altitude @@ -67,7 +69,7 @@ local function bind_add_param(name, idx, default_value) return Parameter(PARAM_TABLE_PREFIX .. name) end -- setup follow mode specific parameters -assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 20), 'could not add param table') +assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 25), 'could not add param table') -- add a parameter and bind it to a variable --local function bind_add_param2(name, idx, default_value) @@ -241,16 +243,28 @@ ZPF_SIM_TELF_FN = bind_add_param("SIM_TELF_FN", 19, 302) --[[ // @Param: ZPF_SR_CH - // @DisplayName: Plane Follow Serial Channel + // @DisplayName: Plane Follow Stream Rate Serial Channel // @Description: This is the serial/channel where mavlink messages will go to the target vehicle // @Range: 0 9 --]] ZPF_SR_CH = bind_add_param("SR_CH", 20, 0) +--[[ + // @Param: ZPF_SR_INT + // @DisplayName: Plane Follow Stream Rate Interval + // @Description: This is the stream rate that the chase plane will request from the lead plane for GLOBAL_POSITION_INT and ATTITUDE telemetry + // @Range: 25 500 + // @Units: ms +--]] +ZPF_SR_INT = bind_add_param("SR_INT", 21, 50) + REFRESH_RATE = 0.05 -- in seconds, so 20Hz LOST_TARGET_TIMEOUT = (ZPF_TIMEOUT:get() or 10) / REFRESH_RATE OVERSHOOT_ANGLE = ZPF_OVRSHT_DEG:get() or 75.0 TURNING_ANGLE = ZPF_TURN_DEG:get() or 20.0 +DISTANCE_LOOKAHEAD_SECONDS = ZPF_LKAHD:get() or 5.0 + +local lost_target_countdown = LOST_TARGET_TIMEOUT local fail_mode = ZPF_FAIL_MODE:get() or FLIGHT_MODE.QRTL local exit_mode = ZPF_EXIT_MODE:get() or FLIGHT_MODE.LOITER @@ -259,8 +273,6 @@ local use_wide_turns = ZPF_WIDE_TURNS:get() or 1 local distance_fudge = ZPF_DIST_FUDGE:get() or 0.92 -DISTANCE_LOOKAHEAD_SECONDS = ZPF_LKAHD:get() or 5.0 - local target_serial_channel = ZPF_SR_CH:get() or 0 local simulate_telemetry_failed = false @@ -312,86 +324,8 @@ local function follow_frame_to_mavlink(follow_frame) return mavlink_frame end -local COMMAND_INT_map = {} -COMMAND_INT_map.id = 75 -COMMAND_INT_map.fields = { - { "param1", " 5 then gcs:send_text(MAV_SEVERITY.WARNING, SCRIPT_NAME_SHORT .. ": lost prior target: " .. follow:get_target_sysid()) lost_target_now = now @@ -640,8 +574,10 @@ local function update() -- Need to request that the follow vehicle sends telemetry at a reasonable rate -- we send a new request every 10 seconds, just to make sure the message gets through if (now - now_telemetry_request) > 10 then - request_message_interval(target_serial_channel, {sysid = foll_sysid, message_id = MAV_CMD_INT.ATTITUDE, interval = 100}) - request_message_interval(target_serial_channel, {sysid = foll_sysid, message_id = MAV_CMD_INT.GLOBAL_POSITION_INT, interval = 100}) + local stream_interval = ZPF_SR_INT:get() or 50 + -- we'd like to get GLOGALOOSITION_INT and ATTITUDE messages from the target vehicle at 20Hz = every 50ms + mavlink_command_int.request_message_interval(target_serial_channel, {sysid = foll_sysid, message_id = MAV_CMD_INT.ATTITUDE, interval_ms = stream_interval}) + mavlink_command_int.request_message_interval(target_serial_channel, {sysid = foll_sysid, message_id = MAV_CMD_INT.GLO, interval_ms = stream_interval}) now_telemetry_request = now end @@ -764,7 +700,7 @@ local function update() -- if the target vehicle is starting to roll we need to pre-empt a turn is coming -- this is fairly simplistic and could probably be improved - -- got the code from Mission Planner - this is how it calculates the turn radius + -- got the code from Mission Planner - this is how it calculates the turn radius in c# --[[ public float radius { @@ -935,7 +871,11 @@ end -- start running update loop - waiting 20s for the AP to initialize if FWVersion:type() == 3 then - return delayed_start, 20000 + if arming:is_armed() then + return delayed_start, 1000 + else + return delayed_start, 20000 + end else gcs:send_text(MAV_SEVERITY.ERROR, string.format("%s: must run on Plane", SCRIPT_NAME_SHORT)) end diff --git a/libraries/AP_Scripting/applets/plane_follow.md b/libraries/AP_Scripting/applets/plane_follow.md index f83909b039e39b..380b86eaa35ba7 100644 --- a/libraries/AP_Scripting/applets/plane_follow.md +++ b/libraries/AP_Scripting/applets/plane_follow.md @@ -18,8 +18,7 @@ and ==must be different== from the MAVLINK_SYSID of the following plane. The script adds the following parameters to control it's behaviour. It uses the existing FOLL parameters that are used for the Copter FOLLOW mode. In addition -the following "ZPF" parameters are added: Z = scripting, P = Plane, F = Follow, in two -banks ZPF and ZPF2. +the following "ZPF" parameters are added: Z = scripting, P = Plane, F = Follow. ## ZPF_FAIL_MODE @@ -67,55 +66,55 @@ The follow logic can have the follow vehicle track the altitude of the target, b in ZPF_ALT_OVR allows the follow vehicle to follow at a fixed altitude regardless of the altitude of the target. The ZPF_ALT_OVR is in meters in FOLL_ALT_TYPE frame. -## ZPF2_D_P +## ZPF_D_P The follow logic uses two PID controllers for controlling speed, the first uses distance (D) as the error. This is the P gain for the "D" PID controller. -## ZPF2_D_I +## ZPF_D_I The follow logic uses two PID controllers for controlling speed, the first uses distance (D) as the error. This is the I gain for the "D" PID controller. -## ZPF2_D_D +## ZPF_D_D The follow logic uses two PID controllers for controlling speed, the first uses distance (D) as the error. This is the D gain for the "D" PID controller. -## ZPF2_V_P +## ZPF_V_P The follow logic uses two PID controllers for controlling speed, the first uses velocity (V) as the error. This is the P gain for the "V" PID controller. -## ZPF2_V_I +## ZPF_V_I The follow logic uses two PID controllers for controlling speed, the first uses distance (V) as the error. This is the I gain for the "V" PID controller. -## ZPF2_V_D +## ZPF_V_D The follow logic uses two PID controllers for controlling speed, the first uses distance (V) as the error. This is the D gain for the "V" PID controller. -## ZPF2_LKAHD +## ZPF_LKAHD Time to "lookahead" when calculating distance errors. -## ZPF2_DIST_FUDGE +## ZPF_DIST_FUDGE This parameter might be a bad idea, but it seems the xy_distance between the target offset location and the follow vehicle returned by AP_Follow appears to be off by a factor of -airspeed * a fudge factor +`airspeed * a fudge factor` This allows this fudge factor to be adjusted until a better solution can be found for this problem. # Operation -Enable Lua scripting by setting SCR_ENABLE = 1 on the FOLLOW plane. +Enable Lua scripting by setting `SCR_ENABLE = 1` on the FOLLOW plane. Install the lua script in the APM/scripts directory on the flight controller's microSD card on the FOLLOW plane. Install the speedpid.lua, mavlink_attitude.lua and mavlink_msgs.lua files -in the APM/scripts/modules directory on the SD card on the FOLLOW plane. +in the `APM/scripts/modules` directory on the SD card on the FOLLOW plane. No scripts are required on the target plane. @@ -134,10 +133,19 @@ will use when calculating the target altitude. See the definitions of these parameters to understand how they work. ZPF2_ALT_OVR will override the operation of FOLL_OFS_Z setting a fixed altitude for the following plane in FOLL_ALT_TYPE frame. -To ensure the follow plane gets timely updates from the target, the SRx_EXT_STAT and SRx_EXTRA1 +The existing FOLL_YAW_BEHAVE and FOLL_POS_P parameters are ignored by Plane Follow. + +To ensure the follow plane gets timely updates from the target, the SRx_POSITION and SRx_EXTRA1 telemetry stream rate parameters should be increased to increase the rate that the POSITION_TARGET_GLOBAL_INT and ATTITUDE mavlink messages are sent. The default value is 4Hz, a good value is probably 8Hz or 10Hz but some testing should be done to confirm the best rate for your telemetry radios and vehicles. -Ideally the connection is direct plane-to-plane and not routing via a Ground Control Station. This has been tested with 2x HolyBro SiK telemetry radios, one in each plane. RFD900 radios should work and LTE or Starlink connections will probably work well, but haven't been tested. Fast telemetry updates from the target to the following plane will give the best -results. +To prevent Mission Planner, QGC, MAVproxy or any other ground control station connected to your plane +from requesting a different stream rate, also set SERIALx_OPTIONS bit 12 for the matching serial port. + +For example if your telemetry radio is connected to Telem1 = SERIAL1 then set +SERIAL1_OPTIONS = +SR1_POSITION = 10 +SR1_EXTRA1 = 10 + +Ideally the connection is direct plane-to-plane and not routed via a Ground Control Station. This has been tested with 2x HolyBro SiK telemetry radios, one in each plane. RFD900 radios might work and LTE or other IP radio based connections will probably work well, but haven't been tested. Fast telemetry updates from the target to the following plane will give the best results. diff --git a/libraries/AP_Scripting/modules/mavlink_attitude.lua b/libraries/AP_Scripting/modules/mavlink_attitude.lua index 8944d331783bde..03fcefdbb95979 100644 --- a/libraries/AP_Scripting/modules/mavlink_attitude.lua +++ b/libraries/AP_Scripting/modules/mavlink_attitude.lua @@ -105,8 +105,8 @@ function MAVLinkAttitude.mavlink_attitude_receiver() local mavlink_msgs = require("mavlink_msgs") local jitter_correction = MAVLinkAttitude.JitterCorrection(5000, 100) - -- initialise mavlink rx with number of messages, and buffer depth - mavlink.init(1, 10) + -- initialise mavlink rx with buffer depth and number of messages + mavlink.init(10, 1) -- register message id to receive --mavlink.register_rx_msgid(ATTITUDE_msgid) diff --git a/libraries/AP_Scripting/modules/mavlink_command_int.lua b/libraries/AP_Scripting/modules/mavlink_command_int.lua index effd86138b5240..44b2557fbc971c 100644 --- a/libraries/AP_Scripting/modules/mavlink_command_int.lua +++ b/libraries/AP_Scripting/modules/mavlink_command_int.lua @@ -15,17 +15,25 @@ MAVLink_command_int A MAVlink message manager for COMMAND_INT messages specifically - Only send is implemented at the moment, but its intended that receive could be added + This is for sending command_int messages to ANOTHER vehicle. To send to the current + vehicle that the Lua is running on use gcs:run_command_int --]] local MAVLink_command_int = {} -MAVLink_command_int.SCRIPT_VERSION = "4.7.0-001" +MAVLink_command_int.SCRIPT_VERSION = "4.7.0-002" MAVLink_command_int.SCRIPT_NAME = "MAVLink Command Int" MAVLink_command_int.SCRIPT_NAME_SHORT = "MAVCMDINT" MAVLink_command_int.COMMAND_INT_MESSAGE = "COMMAND_INT" +MAVLink_command_int.MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} + +MAVLink_command_int.MAV_CMD_INT = { ATTITUDE = 30, GLOBAL_POSITION_INT = 33, REQUEST_DATA_STREAM = 66, + DO_SET_MODE = 176, DO_CHANGE_SPEED = 178, DO_REPOSITION = 192, + CMD_SET_MESSAGE_INTERVAL = 511, CMD_REQUEST_MESSAGE = 512, + GUIDED_CHANGE_SPEED = 43000, GUIDED_CHANGE_ALTITUDE = 43001, GUIDED_CHANGE_HEADING = 43002 } + MAVLink_command_int.map = {} MAVLink_command_int.map.id = 75 MAVLink_command_int.map.fields = { @@ -45,9 +53,22 @@ MAVLink_command_int.map.fields = { } MAVLink_command_int.map[MAVLink_command_int.map.id] = MAVLink_command_int.COMMAND_INT_MESSAGE -function MAVLink_command_int.mavlink_encode(msg_map, message) +function MAVLink_command_int.dump(o) + if type(o) == 'table' then + local s = '{ ' + for k,v in pairs(o) do + if type(k) ~= 'number' then k = '"'..k..'"' end + s = s .. '['..k..'] = ' .. MAVLink_command_int.dump(v) .. ',' + end + return s .. '} ' + else + return tostring(o) + end +end + +function MAVLink_command_int.mavlink_encode(msg_map, msg) local message_map = msg_map - + local packString = "<" local packedTable = {} local packedIndex = 1 @@ -55,7 +76,7 @@ function MAVLink_command_int.mavlink_encode(msg_map, message) if v[3] then packString = (packString .. string.rep(string.sub(v[2], 2), v[3])) for j = 1, v[3] do - packedTable[packedIndex] = message[message_map.fields[i][1]][j] + packedTable[packedIndex] = msg[message_map.fields[i][1]][j] if packedTable[packedIndex] == nil then packedTable[packedIndex] = 0 end @@ -63,18 +84,73 @@ function MAVLink_command_int.mavlink_encode(msg_map, message) end else packString = (packString .. string.sub(v[2], 2)) - packedTable[packedIndex] = message[message_map.fields[i][1]] + packedTable[packedIndex] = msg[message_map.fields[i][1]] packedIndex = packedIndex + 1 end end - return message_map.id, string.pack(packString, table.unpack(packedTable)) -end + if next(packedTable) ~= nil then + return message_map.id, string.pack(packString, table.unpack(packedTable)) + else + return message_map.id, nil + end +end function MAVLink_command_int.send(channel, message) - return mavlink:send_chan(channel, MAVLink_command_int.mavlink_encode(MAVLink_command_int.map, message)) + local command_id, encoded_message = MAVLink_command_int.mavlink_encode(MAVLink_command_int.map, message) + if command_id == nil or encoded_message == nil then + gcs:send_text(MAVLink_command_int.MAV_SEVERITY.WARNING, MAVLink_command_int.SCRIPT_NAME_SHORT .. " MAVLink encode failed") + return false + else + if not mavlink:send_chan(channel, command_id, encoded_message) then + gcs:send_text(MAVLink_command_int.MAV_SEVERITY.WARNING, MAVLink_command_int.SCRIPT_NAME_SHORT .. " MAVLink buffer is full") + return false + end + end + return true +end + + +-- request another vehicle to send specific mavlink messages at a particular interval +-- set_message_interval() Parameters +-- channel = the channel (telemetry link) on this autopilot to send out the request +-- target.sysid = the target vehicle to request messages from +-- target.componentid = the target component, defaults to the autopilot +-- target.message_id = the message id of the requested message +-- target.interval_ms = the interval in milliseconds for the target vehicle to send message_id messages +function MAVLink_command_int.request_message_interval(channel, target) + local target_sysid = target.sysid or -1 + if target_sysid < 1 then + gcs:send_text(MAVLink_command_int.MAV_SEVERITY.ERROR, MAVLink_command_int.SCRIPT_NAME_SHORT .. ": request_message_interval no target") + return + end + + local message = { + command = MAVLink_command_int.MAV_CMD_INT.CMD_SET_MESSAGE_INTERVAL, + param1 = MAVLink_command_int.MAV_CMD_INT.ATTITUDE, + param2 = target.interval_ms * 1000, -- request interval (MAVLink uses microsecond units) + param3 = 0, + param4 = 0, + x = 0, y = 0, z = 0, + frame = 0, current = 0, autocontinue = 0, + target_system = target.sysid, + target_component = (target.componentid or 1), -- the default to the autopilot + confirmation = 0, + } + --return MAVLink_command_int.send(channel, message) + local command_id, encoded = MAVLink_command_int.mavlink_encode(MAVLink_command_int.map, message) + if command_id == nil or encoded == nil then + return false + else + if not mavlink:send_chan(channel, command_id, encoded) then + gcs:send_text(MAVLink_command_int.MAV_SEVERITY.INFO, MAVLink_command_int.SCRIPT_NAME_SHORT .. " MAVLink buffer is full") + return false + end + end + gcs:send_text(MAVLink_command_int.MAV_SEVERITY.INFO, MAVLink_command_int.SCRIPT_NAME_SHORT .. " MAVLink sent to: " .. target.sysid .. " at " .. target.interval_ms) + return true end -gcs:send_text(MAV_SEVERITY.NOTICE, string.format("%s %s module loaded", +gcs:send_text(MAV_SEVERITY.NOTICE, string.format("%s %s module loaded", MAVLink_command_int.SCRIPT_NAME, MAVLink_command_int.SCRIPT_VERSION) ) -return MAVLink_command_int \ No newline at end of file +return MAVLink_command_int From 3a0b2c3a481eb98974dec108f803bb0a80001064 Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Mon, 17 Feb 2025 16:39:09 -0700 Subject: [PATCH 11/14] AP_Follow: changes to support scripted plane follow --- ArduPlane/Parameters.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index c8000e0cb143cc..9c85ab89524331 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -1324,7 +1324,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Description: The maximum frequency that an guided mode commands sent by external system such as lua or mavlink can update roll, pitch and throttle. // @Units: ms // @User: Standard - AP_GROUPINFO("GUIDED_UPD_LIM", 37, ParametersG2, guided_update_frequency_limit, 3000), + AP_GROUPINFO("GUIDED_UPD_LIM", 38, ParametersG2, guided_update_frequency_limit, 3000), AP_GROUPEND }; From 55d6526a77869df59814b87090faca12d020f60f Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Fri, 21 Feb 2025 13:46:24 -0700 Subject: [PATCH 12/14] AP_Follow: changes to support scripted plane follow --- ArduPlane/commands.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/ArduPlane/commands.cpp b/ArduPlane/commands.cpp index 1e50f2c0144420..ad9a0ea4cc95b1 100644 --- a/ArduPlane/commands.cpp +++ b/ArduPlane/commands.cpp @@ -86,6 +86,7 @@ void Plane::set_guided_WP(const Location &loc) next_WP_loc = loc; // used to control FBW and limit the rate of climb + // cannot be applied in Guided as this prevents the guided set altitude from working correctly // ----------------------------------------------- if (!control_mode->is_guided_mode()) { set_target_altitude_current(); From 234efcbad2fc91af7072c80d6a7c9bade8e5191a Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Fri, 21 Feb 2025 13:46:51 -0700 Subject: [PATCH 13/14] AP_Scripting: changes to support scripted plane follow --- .../AP_Scripting/applets/plane_follow.lua | 70 +++++++++++-------- .../modules/mavlink_command_int.lua | 7 +- 2 files changed, 46 insertions(+), 31 deletions(-) diff --git a/libraries/AP_Scripting/applets/plane_follow.lua b/libraries/AP_Scripting/applets/plane_follow.lua index 078c95c4dce213..3f16a9973fb2de 100644 --- a/libraries/AP_Scripting/applets/plane_follow.lua +++ b/libraries/AP_Scripting/applets/plane_follow.lua @@ -25,7 +25,7 @@ ZPR_TURN_DEG - if the target is more than this many degrees left or right, assume it's turning --]] -SCRIPT_VERSION = "4.7.0-050" +SCRIPT_VERSION = "4.7.0-051" SCRIPT_NAME = "Plane Follow" SCRIPT_NAME_SHORT = "PFollow" @@ -175,7 +175,7 @@ ZPF_ALT_OVR = bind_add_param("ALT_OVR", 9, 0) // @Description: P gain for the speed PID controller distance component // @Range: 0 1 --]] -ZPF_D_P = bind_add_param("D_P", 11, 0.3) +ZPF_D_P = bind_add_param("D_P", 11, 0.02) --[[ // @Param: ZPF_D_I @@ -183,7 +183,7 @@ ZPF_D_P = bind_add_param("D_P", 11, 0.3) // @Description: I gain for the speed PID distance component // @Range: 0 1 --]] -ZPF_D_I = bind_add_param("D_I", 12, 0.3) +ZPF_D_I = bind_add_param("D_I", 12, 0.02) --[[ // @Param: ZPF_D_D @@ -191,7 +191,7 @@ ZPF_D_I = bind_add_param("D_I", 12, 0.3) // @Description: D gain for the speed PID controller distance component // @Range: 0 1 --]] -ZPF_D_D = bind_add_param("D_D", 13, 0.05) +ZPF_D_D = bind_add_param("D_D", 13, 0.015) --[[ // @Param: ZPF_V_P @@ -199,7 +199,7 @@ ZPF_D_D = bind_add_param("D_D", 13, 0.05) // @Description: P gain for the speed PID controller velocity component // @Range: 0 1 --]] -ZPF_V_P = bind_add_param("V_P", 14, 0.3) +ZPF_V_P = bind_add_param("V_P", 14, 0.02) --[[ // @Param: ZPF_V_I @@ -207,7 +207,7 @@ ZPF_V_P = bind_add_param("V_P", 14, 0.3) // @Description: I gain for the speed PID controller velocity component // @Range: 0 1 --]] -ZPF_V_I = bind_add_param("V_I", 15, 0.3) +ZPF_V_I = bind_add_param("V_I", 15, 0.02) --[[ // @Param: ZPF_V_D @@ -215,7 +215,7 @@ ZPF_V_I = bind_add_param("V_I", 15, 0.3) // @Description: D gain for the speed PID controller velocity component // @Range: 0 1 --]] -ZPF_V_D = bind_add_param("V_D", 16, 0.05) +ZPF_V_D = bind_add_param("V_D", 16, 0.015) --[[ // @Param: ZPF_LkAHD @@ -247,12 +247,12 @@ ZPF_SIM_TELF_FN = bind_add_param("SIM_TELF_FN", 19, 302) // @Description: This is the serial/channel where mavlink messages will go to the target vehicle // @Range: 0 9 --]] -ZPF_SR_CH = bind_add_param("SR_CH", 20, 0) +ZPF_SR_CH = bind_add_param("SR_CH", 20, -1) --[[ // @Param: ZPF_SR_INT // @DisplayName: Plane Follow Stream Rate Interval - // @Description: This is the stream rate that the chase plane will request from the lead plane for GLOBAL_POSITION_INT and ATTITUDE telemetry + // @Description: This is the stream rate that the chase plane will request from the lead plane for GLOBAL_POSITION_INT and ATTITUDE telemetry. Set to -1 to dieable. // @Range: 25 500 // @Units: ms --]] @@ -273,7 +273,7 @@ local use_wide_turns = ZPF_WIDE_TURNS:get() or 1 local distance_fudge = ZPF_DIST_FUDGE:get() or 0.92 -local target_serial_channel = ZPF_SR_CH:get() or 0 +local target_serial_channel = ZPF_SR_CH:get() or -1 local simulate_telemetry_failed = false @@ -299,15 +299,15 @@ local function constrain(v, vmin, vmax) end local speedpid = require("speedpid") -local pid_controller_distance = speedpid.speed_controller(ZPF_D_P:get() or 0.3, - ZPF_D_I:get() or 0.3, - ZPF_D_D:get() or 0.05, - 5.0, airspeed_min - airspeed_max, airspeed_max - airspeed_min) +local pid_controller_distance = speedpid.speed_controller(ZPF_D_P:get() or 0.01, + ZPF_D_I:get() or 0.01, + ZPF_D_D:get() or 0.005, + 0.5, airspeed_min - airspeed_max, airspeed_max - airspeed_min) -local pid_controller_velocity = speedpid.speed_controller(ZPF_V_P:get() or 0.3, - ZPF_V_I:get() or 0.3, - ZPF_V_D:get() or 0.05, - 5.0, airspeed_min, airspeed_max) +local pid_controller_velocity = speedpid.speed_controller(ZPF_V_P:get() or 0.01, + ZPF_V_I:get() or 0.01, + ZPF_V_D:get() or 0.005, + 2.0, airspeed_min, airspeed_max) local mavlink_attitude = require("mavlink_attitude") @@ -700,7 +700,7 @@ local function update() -- if the target vehicle is starting to roll we need to pre-empt a turn is coming -- this is fairly simplistic and could probably be improved - -- got the code from Mission Planner - this is how it calculates the turn radius in c# + -- got the code from Mission Planner - this is how MP calculates the turn radius in c# --[[ public float radius { @@ -721,7 +721,9 @@ local function update() if target_attitude ~= nil then if (now - (target_attitude.timestamp_ms * 0.001) < LOST_TARGET_TIMEOUT) and math.abs(target_attitude.roll) > 0.1 or math.abs(target_attitude.rollspeed) > 1 then - local turn_radius = vehicle_airspeed * vehicle_airspeed / (9.80665 * math.tan(target_attitude.roll + target_attitude.rollspeed)) + -- the roll and rollspeed are delayed values from the target plane, try to extrapolate them to at least "now" + 1 second + local rollspeed_impact = target_attitude.rollspeed * (target_attitude.delay_ms + 1000) + local turn_radius = vehicle_airspeed * vehicle_airspeed / (9.80665 * math.tan(target_attitude.roll + rollspeed_impact)) turning = true -- predict the roll in 1s from now and use that based on rollspeed @@ -809,11 +811,20 @@ local function update() yaw = foll_ofs_y}) mechanism = 1 -- position/location - for logging end - -- dv = interim delta velocity based on the pid controller using projected_distance as the error (we want distance == 0) - local dv = pid_controller_distance.update(target_airspeed - vehicle_airspeed, projected_distance) - local airspeed_new = pid_controller_velocity.update(vehicle_airspeed, dv) + -- dv = interim delta velocity based on the pid controller using projected_distance per loop as the error (we want distance == 0) + local dv_error = projected_distance * REFRESH_RATE + local airspeed_new = vehicle_airspeed + local dv = 0.0 + if dv_error ~= nil then + if projected_distance < 0 then + dv_error = dv_error * 1.75 -- attempt to slow down faster if we are overshooting + end + dv = pid_controller_distance.update(target_airspeed - vehicle_airspeed, dv_error) + airspeed_new = pid_controller_velocity.update(vehicle_airspeed, dv) + -- gcs:send_text(MAV_SEVERITY.INFO, string.format("Err: %.1f DstP: %.0f Dst: %.0f dv: %.1f AspO: %.1f", dv_error, projected_distance, xy_dist, dv, airspeed_new)) - set_vehicle_speed({speed = constrain(airspeed_new, airspeed_min, airspeed_max)}) + set_vehicle_speed({speed = constrain(airspeed_new, airspeed_min, airspeed_max)}) + end local log_too_close = 0 local log_too_close_follow_up = 0 @@ -827,12 +838,13 @@ local function update() if overshot then log_overshot = 1 end - logger:write("ZPF1",'Dst,DstP,DstE,AspT,Asp,AspO,Mech,Cls,ClsF,OSht','ffffffBBBB','mmmnnn----','----------', + logger:write("ZPF1",'Dst,DstP,DstE,AspT,Asp,AspD,AspO,Mech,Cls,ClsF,OSht','fffffffBBBB','mmmnnnn----','-----------', xy_dist, projected_distance, - distance_error, + dv_error, target_airspeed, vehicle_airspeed, + dv, airspeed_new, mechanism, log_too_close, log_too_close_follow_up, log_overshot ) @@ -864,7 +876,7 @@ local function protected_wrapper() return protected_wrapper, 1000 * REFRESH_RATE end -local function delayed_start() +local function delayed_startup() gcs:send_text(MAV_SEVERITY.INFO, string.format("%s %s script loaded", SCRIPT_NAME, SCRIPT_VERSION) ) return protected_wrapper() end @@ -872,9 +884,9 @@ end -- start running update loop - waiting 20s for the AP to initialize if FWVersion:type() == 3 then if arming:is_armed() then - return delayed_start, 1000 + return delayed_startup, 1000 else - return delayed_start, 20000 + return delayed_startup, 20000 end else gcs:send_text(MAV_SEVERITY.ERROR, string.format("%s: must run on Plane", SCRIPT_NAME_SHORT)) diff --git a/libraries/AP_Scripting/modules/mavlink_command_int.lua b/libraries/AP_Scripting/modules/mavlink_command_int.lua index 44b2557fbc971c..12253ae2df9186 100644 --- a/libraries/AP_Scripting/modules/mavlink_command_int.lua +++ b/libraries/AP_Scripting/modules/mavlink_command_int.lua @@ -21,7 +21,7 @@ local MAVLink_command_int = {} -MAVLink_command_int.SCRIPT_VERSION = "4.7.0-002" +MAVLink_command_int.SCRIPT_VERSION = "4.7.0-003" MAVLink_command_int.SCRIPT_NAME = "MAVLink Command Int" MAVLink_command_int.SCRIPT_NAME_SHORT = "MAVCMDINT" @@ -123,6 +123,9 @@ function MAVLink_command_int.request_message_interval(channel, target) gcs:send_text(MAVLink_command_int.MAV_SEVERITY.ERROR, MAVLink_command_int.SCRIPT_NAME_SHORT .. ": request_message_interval no target") return end + if channel < 0 then -- -1 means "don't request" + return + end local message = { command = MAVLink_command_int.MAV_CMD_INT.CMD_SET_MESSAGE_INTERVAL, @@ -146,7 +149,7 @@ function MAVLink_command_int.request_message_interval(channel, target) return false end end - gcs:send_text(MAVLink_command_int.MAV_SEVERITY.INFO, MAVLink_command_int.SCRIPT_NAME_SHORT .. " MAVLink sent to: " .. target.sysid .. " at " .. target.interval_ms) + gcs:send_text(MAVLink_command_int.MAV_SEVERITY.INFO, MAVLink_command_int.SCRIPT_NAME_SHORT .. " interval sent to: " .. target.sysid .. " at " .. target.interval_ms) return true end From d53d0288ef74069f99432036a39e2bef2f9e3e76 Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Tue, 25 Feb 2025 08:37:11 -0700 Subject: [PATCH 14/14] AP_Scripting: changes to support scripted plane follow --- .../dual_plane2/scripts/plane_follow.lua | 892 ++++++++++++++++++ .../AP_Scripting/applets/plane_follow.lua | 5 +- .../AP_Scripting/modules/mavlink_attitude.lua | 20 + 3 files changed, 914 insertions(+), 3 deletions(-) create mode 100644 ArduPlane/dual_plane2/scripts/plane_follow.lua diff --git a/ArduPlane/dual_plane2/scripts/plane_follow.lua b/ArduPlane/dual_plane2/scripts/plane_follow.lua new file mode 100644 index 00000000000000..f260254356224a --- /dev/null +++ b/ArduPlane/dual_plane2/scripts/plane_follow.lua @@ -0,0 +1,892 @@ +--[[ + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + + Follow in Plane + Support follow "mode" in plane. This will actually use GUIDED mode with + a scripting switch to allow guided to track the vehicle id in FOLL_SYSID + Uses the AP_Follow library so all of the existing FOLL_* parameters are used + as documented for Copter, + add 3 more for this script + ZPF_EXIT_MODE - the mode to switch to when follow is turned of using the switch + ZPF_FAIL_MODE - the mode to switch to if the target is lost + ZPF_TIMEOUT - number of seconds to try to reaquire the target after losing it before failing + ZPF_OVRSHT_DEG - if the target is more than this many degrees left or right, assume an overshoot + ZPR_TURN_DEG - if the target is more than this many degrees left or right, assume it's turning +--]] + +SCRIPT_VERSION = "4.7.0-052" +SCRIPT_NAME = "Plane Follow" +SCRIPT_NAME_SHORT = "PFollow" + +-- FOLL_ALT_TYPE and Mavlink FRAME use different values +ALT_FRAME = { GLOBAL = 0, RELATIVE = 1, TERRAIN = 3} + +MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} +MAV_FRAME = {GLOBAL = 0, GLOBAL_RELATIVE_ALT = 3, GLOBAL_TERRAIN_ALT = 10} +MAV_CMD_INT = { ATTITUDE = 30, GLOBAL_POSITION_INT = 33, REQUEST_DATA_STREAM = 66, + DO_SET_MODE = 176, DO_CHANGE_SPEED = 178, DO_REPOSITION = 192, + CMD_SET_MESSAGE_INTERVAL = 511, CMD_REQUEST_MESSAGE = 512, + GUIDED_CHANGE_SPEED = 43000, GUIDED_CHANGE_ALTITUDE = 43001, GUIDED_CHANGE_HEADING = 43002 } +MAV_SPEED_TYPE = { AIRSPEED = 0, GROUNDSPEED = 1, CLIMB_SPEED = 2, DESCENT_SPEED = 3 } +MAV_HEADING_TYPE = { COG = 0, HEADING = 1, DEFAULT = 2} -- COG = Course over Ground, i.e. where you want to go, HEADING = which way the vehicle points + +MAV_DATA_STREAM = { MAV_DATA_STREAM_ALL=0, MAV_DATA_STREAM_RAW_SENSORS=1, MAV_DATA_STREAM_EXTENDED_STATUS=2, MAV_DATA_STREAM_RC_CHANNELS=3, + MAV_DATA_STREAM_RAW_CONTROLLER=4, MAV_DATA_STREAM_POSITION=6, MAV_DATA_STREAM_EXTRA1=10, MAV_DATA_STREAM_EXTRA2=11 } + +FLIGHT_MODE = {AUTO=10, RTL=11, LOITER=12, GUIDED=15, QHOVER=18, QLOITER=19, QRTL=21} + +local ahrs_eas2tas = ahrs:get_EAS2TAS() +local windspeed_vector = ahrs:wind_estimate() + +local now = millis():tofloat() * 0.001 +local now_target_heading = now +local now_telemetry_request = now +local now_follow_lost = now +local follow_enabled = false +local too_close_follow_up = 0 +local save_target_heading1 = -400.0 +local save_target_heading2 = -400.0 +local save_target_altitude +local tight_turn = false + +local PARAM_TABLE_KEY = 120 +local PARAM_TABLE_PREFIX = "ZPF_" + +-- add a parameter and bind it to a variable +local function bind_add_param(name, idx, default_value) + assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), string.format('could not add param %s', name)) + return Parameter(PARAM_TABLE_PREFIX .. name) +end +-- setup follow mode specific parameters +assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 25), 'could not add param table') + +-- add a parameter and bind it to a variable +--local function bind_add_param2(name, idx, default_value) +-- assert(param:add_param(PARAM_TABLE_KEY2, idx, name, default_value), string.format('could not add param %s', name)) +-- return Parameter(PARAM_TABLE_PREFIX2 .. name) +--end +-- setup follow mode specific parameters- second tranche +-- assert(param:add_table(PARAM_TABLE_KEY2, PARAM_TABLE_PREFIX2, 10), 'could not add param table') + +-- This uses the exisitng FOLL_* parameters and just adds a couple specific to this script +-- but because most of the logic is already in AP_Follow (called by binding to follow:) there +-- is no need to access them in the scriot + +-- we need these existing FOLL_ parametrs +FOLL_ALT_TYPE = Parameter('FOLL_ALT_TYPE') +FOLL_SYSID = Parameter('FOLL_SYSID') +FOLL_OFS_Y = Parameter('FOLL_OFS_Y') +local foll_sysid = FOLL_SYSID:get() or -1 +local foll_ofs_y = FOLL_OFS_Y:get() or 0.0 +local foll_alt_type = FOLL_ALT_TYPE:get() or ALT_FRAME.GLOBAL + +-- Add these ZPF_ parameters specifically for this script +--[[ + // @Param: ZPF_FAIL_MODE + // @DisplayName: Plane Follow lost target mode + // @Description: Mode to switch to if the target is lost (no signal or > FOLL_DIST_MAX). + // @User: Standard +--]] +ZPF_FAIL_MODE = bind_add_param('FAIL_MODE', 1, FLIGHT_MODE.RTL) + +--[[ + // @Param: ZPF_EXIT_MODE + // @DisplayName: Plane Follow exit mode + // @Description: Mode to switch to when follow mode is exited normally + // @User: Standard +--]] +ZPF_EXIT_MODE = bind_add_param('EXIT_MODE', 2, FLIGHT_MODE.LOITER) + +--[[ + // @Param: ZPF_ACT_FN + // @DisplayName: Plane Follow Scripting ActivationFunction + // @Description: Setting an RC channel's _OPTION to this value will use it for Plane Follow enable/disable + // @Range: 300 307 +--]] +ZPF_ACT_FN = bind_add_param("ACT_FN", 3, 301) + +--[[ + // @Param: ZPF_TIMEOUT + // @DisplayName: Plane Follow Telemetry Timeout + // @Description: How long to try reaquire a target if telemetry from the lead vehicle is lost. + // @Range: 0 30 + // @Units: s +--]] +ZPF_TIMEOUT = bind_add_param("TIMEOUT", 4, 10) + +--[[ + // @Param: ZPF_OVRSHT_DEG + // @DisplayName: Plane Follow Scripting Overshoot Angle + // @Description: If the target is greater than this many degrees left or right, assume an overshoot + // @Range: 0 180 + // @Units: deg +--]] +ZPF_OVRSHT_DEG = bind_add_param("OVRSHT_DEG", 5, 75) + +--[[ + // @Param: ZPF_TURN_DEG + // @DisplayName: Plane Follow Scripting Turn Angle + // @Description: If the target is greater than this many degrees left or right, assume it's turning + // @Range: 0 180 + // @Units: deg +--]] +ZPF_TURN_DEG = bind_add_param("TURN_DEG", 6, 15) + +--[[ + // @Param: ZPF_DIST_CLOSE + // @DisplayName: Plane Follow Scripting Close Distance + // @Description: When closer than this distance assume we track by heading + // @Range: 0 100 + // @Units: m +--]] +ZPF_DIST_CLOSE = bind_add_param("DIST_CLOSE", 7, 50) + +--[[ + // @Param: ZPF_WIDE_TURNS + // @DisplayName: Plane Follow Scripting Wide Turns + // @Description: Use wide turns when following a turning target. Alternative is "cutting the corner" + // @Range: 0 1 +--]] +ZPF_WIDE_TURNS = bind_add_param("WIDE_TURNS", 8, 1) + +--[[ + // @Param: ZPF_ALT + // @DisplayName: Plane Follow Scripting Altitude Override + // @Description: When non zero, this altitude value (in FOLL_ALT_TYPE frame) overrides the value sent by the target vehicle + // @Range: 0 1000 + // @Units: m +--]] +ZPF_ALT_OVR = bind_add_param("ALT_OVR", 9, 0) + +--[[ + // @Param: ZPF_D_P + // @DisplayName: Plane Follow Scripting distance P gain + // @Description: P gain for the speed PID controller distance component + // @Range: 0 1 +--]] +ZPF_D_P = bind_add_param("D_P", 11, 0.02) + +--[[ + // @Param: ZPF_D_I + // @DisplayName: Plane Follow Scripting distance I gain + // @Description: I gain for the speed PID distance component + // @Range: 0 1 +--]] +ZPF_D_I = bind_add_param("D_I", 12, 0.02) + +--[[ + // @Param: ZPF_D_D + // @DisplayName: Plane Follow Scripting distance D gain + // @Description: D gain for the speed PID controller distance component + // @Range: 0 1 +--]] +ZPF_D_D = bind_add_param("D_D", 13, 0.015) + +--[[ + // @Param: ZPF_V_P + // @DisplayName: Plane Follow Scripting speed P gain + // @Description: P gain for the speed PID controller velocity component + // @Range: 0 1 +--]] +ZPF_V_P = bind_add_param("V_P", 14, 0.02) + +--[[ + // @Param: ZPF_V_I + // @DisplayName: Plane Follow Scripting speed I gain + // @Description: I gain for the speed PID controller velocity component + // @Range: 0 1 +--]] +ZPF_V_I = bind_add_param("V_I", 15, 0.02) + +--[[ + // @Param: ZPF_V_D + // @DisplayName: Plane Follow Scripting speed D gain + // @Description: D gain for the speed PID controller velocity component + // @Range: 0 1 +--]] +ZPF_V_D = bind_add_param("V_D", 16, 0.015) + +--[[ + // @Param: ZPF_LkAHD + // @DisplayName: Plane Follow Lookahead seconds + // @Description: Time to "lookahead" when calculating distance errors + // @Units: s +--]] +ZPF_LKAHD = bind_add_param("LKAHD", 17, 5) + +--[[ + // @Param: ZPF_DIST_FUDGE + // @DisplayName: Plane Follow distance fudge factor + // @Description: THe distance returned by the AP_FOLLOW library might be off by about this factor of airspeed + // @Units: s +--]] +ZPF_DIST_FUDGE = bind_add_param("DIST_FUDGE", 18, 0.92) + +--[[ + // @Param: ZPF_SIM_TELF_FN + // @DisplayName: Plane Follow Simulate Telemetry fail function + // @Description: Set this switch to simulate losing telemetry from the other vehicle + // @Range: 300 307 +--]] +ZPF_SIM_TELF_FN = bind_add_param("SIM_TELF_FN", 19, 302) + +--[[ + // @Param: ZPF_SR_CH + // @DisplayName: Plane Follow Stream Rate Serial Channel + // @Description: This is the serial/channel where mavlink messages will go to the target vehicle + // @Range: 0 9 +--]] +ZPF_SR_CH = bind_add_param("SR_CH", 20, -1) + +--[[ + // @Param: ZPF_SR_INT + // @DisplayName: Plane Follow Stream Rate Interval + // @Description: This is the stream rate that the chase plane will request from the lead plane for GLOBAL_POSITION_INT and ATTITUDE telemetry. Set to -1 to dieable. + // @Range: 25 500 + // @Units: ms +--]] +ZPF_SR_INT = bind_add_param("SR_INT", 21, 50) + +REFRESH_RATE = 0.05 -- in seconds, so 20Hz +LOST_TARGET_TIMEOUT = (ZPF_TIMEOUT:get() or 10) / REFRESH_RATE +OVERSHOOT_ANGLE = ZPF_OVRSHT_DEG:get() or 75.0 +TURNING_ANGLE = ZPF_TURN_DEG:get() or 20.0 +DISTANCE_LOOKAHEAD_SECONDS = ZPF_LKAHD:get() or 5.0 + +local lost_target_countdown = LOST_TARGET_TIMEOUT + +local fail_mode = ZPF_FAIL_MODE:get() or FLIGHT_MODE.QRTL +local exit_mode = ZPF_EXIT_MODE:get() or FLIGHT_MODE.LOITER + +local use_wide_turns = ZPF_WIDE_TURNS:get() or 1 + +local distance_fudge = ZPF_DIST_FUDGE:get() or 0.92 + +local target_serial_channel = ZPF_SR_CH:get() or -1 + +local simulate_telemetry_failed = false + +AIRSPEED_MIN = Parameter('AIRSPEED_MIN') +AIRSPEED_MAX = Parameter('AIRSPEED_MAX') +AIRSPEED_CRUISE = Parameter('AIRSPEED_CRUISE') +WP_LOITER_RAD = Parameter('WP_LOITER_RAD') +WINDSPEED_MAX = Parameter('AHRS_WIND_MAX') + +local airspeed_max = AIRSPEED_MAX:get() or 25.0 +local airspeed_min = AIRSPEED_MIN:get() or 12.0 +local airspeed_cruise = AIRSPEED_CRUISE:get() or 18.0 +local windspeed_max = WINDSPEED_MAX:get() or 100.0 + +local function constrain(v, vmin, vmax) + if v < vmin then + v = vmin + end + if v > vmax then + v = vmax + end + return v +end + +local speedpid = require("speedpid") +local pid_controller_distance = speedpid.speed_controller(ZPF_D_P:get() or 0.01, + ZPF_D_I:get() or 0.01, + ZPF_D_D:get() or 0.005, + 0.5, airspeed_min - airspeed_max, airspeed_max - airspeed_min) + +local pid_controller_velocity = speedpid.speed_controller(ZPF_V_P:get() or 0.01, + ZPF_V_I:get() or 0.01, + ZPF_V_D:get() or 0.005, + 2.0, airspeed_min, airspeed_max) + + +local mavlink_attitude = require("mavlink_attitude") +local mavlink_attitude_receiver = mavlink_attitude.mavlink_attitude_receiver() + +local function follow_frame_to_mavlink(follow_frame) + local mavlink_frame = MAV_FRAME.GLOBAL; + if (follow_frame == ALT_FRAME.TERRAIN) then + mavlink_frame = MAV_FRAME.GLOBAL_TERRAIN_ALT + end + if (follow_frame == ALT_FRAME.RELATIVE) then + mavlink_frame = MAV_FRAME.GLOBAL_RELATIVE_ALT + end + return mavlink_frame +end + +local mavlink_command_int = require("mavlink_command_int") + +-- set_vehicle_target_altitude() Parameters +-- target.alt = new target altitude in meters +-- target.frame = Altitude frame MAV_FRAME, it's very important to get this right! +-- target.alt = altitude in meters to acheive +-- target.speed = z speed of change to altitude (1000.0 = max) +local function set_vehicle_target_altitude(target) + local speed = target.speed or 1000.0 -- default to maximum z acceleration + if target.alt == nil then + gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": set_vehicle_target_altitude no altiude") + return + end + -- GUIDED_CHANGE_ALTITUDE takes altitude in meters + if not gcs:run_command_int(MAV_CMD_INT.GUIDED_CHANGE_ALTITUDE, { + frame = follow_frame_to_mavlink(target.frame), + p3 = speed, + z = target.alt }) then + gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": MAVLink CHANGE_ALTITUDE returned false") + end +end + +-- set_vehicle_heading() Parameters +-- heading.heading_type = MAV_HEADING_TYPE (defaults to HEADING) +-- heading.heading = the target heading in degrees +-- heading.accel = rate/acceleration to acheive the heading 0 = max +local function set_vehicle_heading(heading) + local heading_type = heading.type or MAV_HEADING_TYPE.HEADING + local heading_heading = heading.heading or 0 + local heading_accel = heading.accel or 10.0 + + if heading_heading == nil or heading_heading <= -400 or heading_heading > 360 then + gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": set_vehicle_heading no heading") + return + end + + if not gcs:run_command_int(MAV_CMD_INT.GUIDED_CHANGE_HEADING, { frame = MAV_FRAME.GLOBAL, + p1 = heading_type, + p2 = heading_heading, + p3 = heading_accel }) then + gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": MAVLink GUIDED_CHANGE_HEADING failed") + end +end + +-- set_vehicle_speed() Parameters +-- speed.speed - new target speed +-- speed.type - speed type MAV_SPEED_TYPE +-- speed.throttle - new target throttle (used if speed = 0) +-- speed.slew - specified slew rate to hit the target speed, rather than jumping to it immediately 0 = maximum acceleration +local function set_vehicle_speed(speed) + local new_speed = speed.speed or 0.0 + local speed_type = speed.type or MAV_SPEED_TYPE.AIRSPEED + local throttle = speed.throttle or 0.0 + local slew = speed.slew or 0.0 + local mode = vehicle:get_mode() + + if speed_type == MAV_SPEED_TYPE.AIRSPEED and mode == FLIGHT_MODE.GUIDED then + if not gcs:run_command_int(MAV_CMD_INT.GUIDED_CHANGE_SPEED, { frame = MAV_FRAME.GLOBAL, + p1 = speed_type, + p2 = new_speed, + p3 = slew }) then + gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": MAVLink GUIDED_CHANGE_SPEED failed") + end + else + if not gcs:run_command_int(MAV_CMD_INT.DO_CHANGE_SPEED, { frame = MAV_FRAME.GLOBAL, + p1 = speed_type, + p2 = new_speed, + p3 = throttle }) then + gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": MAVLink DO_CHANGE_SPEED failed") + end + end +end + +-- set_vehicle_target_location() Parameters +-- target.groundspeed (-1 for ignore) +-- target.radius (defaults to 2m) +-- target.yaw - not really yaw - it's the loiter direction 1 = CCW, -1 = CW NaN = default +-- target.lat - latitude in decimal degrees +-- target.lng - longitude in decimal degrees +-- target.alt - target alitude in meters +local function set_vehicle_target_location(target) + local radius = target.radius or 2.0 + local yaw = target.yaw or 1 + -- If we are on the right side of the vehicle make sure any loitering is CCW (moves away from the other plane) + -- yaw > 0 - CCW = turn to the right of the target point + -- yaw < 0 - Clockwise = turn to the left of the target point + -- if following direct we turn on the "outside" + + -- if we were in HEADING mode, need to switch out of it so that REPOSITION will work + -- Note that MAVLink DO_REPOSITION requires altitude in meters + set_vehicle_heading({type = MAV_HEADING_TYPE.DEFAULT}) + if not gcs:run_command_int(MAV_CMD_INT.DO_REPOSITION, { frame = follow_frame_to_mavlink(target.frame), + p1 = target.groundspeed or -1, + p2 = 1, + p3 = radius, + p4 = yaw, + x = target.lat, + y = target.lng, + z = target.alt }) then -- altitude in m + gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": MAVLink DO_REPOSITION returned false") + end +end + +--[[ + return true if we are in a state where follow can apply +--]] +local reported_target = true +local lost_target_now = now +local function follow_active() + local mode = vehicle:get_mode() + + if mode == FLIGHT_MODE.GUIDED then + if follow_enabled then + if follow:have_target() then + reported_target = true + lost_target_now = now + else + if reported_target then -- i.e. if we previously reported a target but lost it + if (now - lost_target_now) > 5 then + gcs:send_text(MAV_SEVERITY.WARNING, SCRIPT_NAME_SHORT .. ": lost prior target: " .. follow:get_target_sysid()) + lost_target_now = now + end + end + reported_target = false + end + end + else + reported_target = false + end + + return reported_target +end + +--[[ + check for user activating follow using an RC switch set HIGH +--]] +local last_follow_active_state = rc:get_aux_cached(ZPF_ACT_FN:get()) +local last_tel_fail_state = rc:get_aux_cached(ZPF_SIM_TELF_FN:get()) + +local function follow_check() + if ZPF_ACT_FN == nil then + return + end + local foll_act_fn = ZPF_ACT_FN:get() + if foll_act_fn == nil then + return + end + local active_state = rc:get_aux_cached(foll_act_fn) + if (active_state ~= last_follow_active_state) then + if( active_state == 0) then + if follow_enabled then + -- Follow disabled - return to EXIT mode + vehicle:set_mode(exit_mode) + follow_enabled = false + gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. ": disabled") + end + elseif (active_state == 2) then + if not (arming:is_armed()) then + gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. ": must be armed") + end + -- Follow enabled - switch to guided mode + vehicle:set_mode(FLIGHT_MODE.GUIDED) + follow_enabled = true + lost_target_countdown = LOST_TARGET_TIMEOUT + --speed_controller_pid.reset() + pid_controller_distance.reset() + pid_controller_velocity.reset() + gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. ": enabled") + end + -- Don't know what to do with the 3rd switch position right now. + last_follow_active_state = active_state + end + local sim_tel_fail = ZPF_SIM_TELF_FN:get() + local tel_fail_state = rc:get_aux_cached(sim_tel_fail) + if tel_fail_state ~= last_tel_fail_state then + if tel_fail_state == 0 then + simulate_telemetry_failed = false + else + simulate_telemetry_failed = true + end + last_tel_fail_state = tel_fail_state + end +end + +local function wrap_360(angle) + local res = math.fmod(angle, 360.0) + if res < 0 then + res = res + 360.0 + end + return res +end + +local function wrap_180(angle) + local res = wrap_360(angle) + if res > 180 then + res = res - 360 + end + return res +end + +local function calculate_airspeed_from_groundspeed(velocity_vector) + --[[ + This is the code from AHRS.cpp + Vector3f true_airspeed_vec = nav_vel - wind_vel; + + This is the c++ code from AP_AHRS_DCM.cpp and also from AP_AHRS.cpp + float true_airspeed = airspeed_ret * get_EAS2TAS(); + true_airspeed = constrain_float(true_airspeed, + gnd_speed - _wind_max, + gnd_speed + _wind_max); + airspeed_ret = true_airspeed / get_EAS2TAS( + --]] + + local airspeed_vector = velocity_vector - windspeed_vector + local airspeed = airspeed_vector:length() + airspeed = airspeed * ahrs_eas2tas + airspeed = constrain(airspeed, airspeed - windspeed_max, airspeed + windspeed_max) + airspeed = airspeed / ahrs_eas2tas + + return airspeed +end + +-- main update function +local function update() + now = millis():tofloat() * 0.001 + ahrs_eas2tas = ahrs:get_EAS2TAS() + windspeed_vector = ahrs:wind_estimate() + + follow_check() + if not follow_active() then + return + end + + -- set the target frame as per user set parameter - this is fundamental to this working correctly + local close_distance = ZPF_DIST_CLOSE:get() or airspeed_cruise * 2.0 + local long_distance = close_distance * 4.0 + local altitude_override = ZPF_ALT_OVR:get() or 0 + + LOST_TARGET_TIMEOUT = (ZPF_TIMEOUT:get() or 10) / REFRESH_RATE + OVERSHOOT_ANGLE = ZPF_OVRSHT_DEG:get() or 75.0 + TURNING_ANGLE = ZPF_TURN_DEG:get() or 20.0 + foll_ofs_y = FOLL_OFS_Y:get() or 0.0 + foll_alt_type = FOLL_ALT_TYPE:get() or ALT_FRAME.GLOBAL + use_wide_turns = ZPF_WIDE_TURNS:get() or 1 + distance_fudge = ZPF_DIST_FUDGE:get() or 0.92 + target_serial_channel = ZPF_SR_CH:get() or 0 + + -- Need to request that the follow vehicle sends telemetry at a reasonable rate + -- we send a new request every 10 seconds, just to make sure the message gets through + if (now - now_telemetry_request) > 10 then + local stream_interval = ZPF_SR_INT:get() or 50 + -- we'd like to get GLOGALOOSITION_INT and ATTITUDE messages from the target vehicle at 20Hz = every 50ms + mavlink_command_int.request_message_interval(target_serial_channel, {sysid = foll_sysid, message_id = MAV_CMD_INT.ATTITUDE, interval_ms = stream_interval}) + mavlink_command_int.request_message_interval(target_serial_channel, {sysid = foll_sysid, message_id = MAV_CMD_INT.GLO, interval_ms = stream_interval}) + now_telemetry_request = now + end + + --[[ + get the current navigation target. + --]] + local target_location -- = Location() of the target + local target_location_offset -- Location of the target with FOLL_OFS_* offsets applied + local target_velocity -- = Vector3f() -- current velocity of lead vehicle + local target_velocity_offset -- Vector3f -- velocity to the offset target_location_offset + local target_distance -- = Vector3f() -- vector to lead vehicle + local target_distance_offset -- vector to the target taking offsets into account + local xy_dist -- distance to target with offsets in meters + local target_heading -- heading of the target vehicle + + local current_location = ahrs:get_location() + if current_location == nil then + return + end + ---@cast current_location -nil + local current_altitude = current_location:alt() * 0.01 + + local vehicle_airspeed = ahrs:airspeed_estimate() + local current_target = vehicle:get_target_location() + + -- because of the methods available on AP_Follow, need to call these multiple methods get_target_dist_and_vel_ned() MUST BE FIRST + -- to get target_location, target_velocity, target distance and target + -- and yes target_offsets (hopefully the same value) is returned by both methods + -- even worse - both internally call get_target_location_and_Velocity, but making a single method + -- in AP_Follow is probably a high flash cost, so we just take the runtime hit + --[[ + target_distance, target_distance_offsets, target_velocity = follow:get_target_dist_and_vel_ned() -- THIS HAS TO BE FIRST + 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() + --]] + + target_distance, target_distance_offset, + target_velocity, target_velocity_offset, + target_location, target_location_offset, + xy_dist = follow:get_target_info() + target_heading = follow:get_target_heading_deg() or -400 + + -- if we lose the target wait for LOST_TARGET_TIMEOUT seconds to try to reaquire it + if target_location == nil or target_location_offset == nil or + target_velocity == nil or target_velocity_offset == nil or + target_distance_offset == nil or current_target == nil or target_distance == nil or xy_dist == nil or + simulate_telemetry_failed then + lost_target_countdown = lost_target_countdown - 1 + if lost_target_countdown <= 0 then + follow_enabled = false + vehicle:set_mode(fail_mode) + gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. string.format(": follow: %.0f FAILED", FOLL_SYSID:get())) + return + end + + -- maintain the current heading until we re-establish telemetry from the target vehicle + -- gcs:send_text(MAV_SEVERITY.WARNING, SCRIPT_NAME_SHORT .. ": follow: lost " .. (now_follow_lost - now) .. " seconds ") + if (now - now_follow_lost) > 2 then + gcs:send_text(MAV_SEVERITY.WARNING, SCRIPT_NAME_SHORT .. string.format(": follow: lost %.0f set hdg: %.0f", FOLL_SYSID:get(), save_target_heading1)) + now_follow_lost = now + set_vehicle_heading({heading = save_target_heading1}) + set_vehicle_target_altitude({alt = save_target_altitude, frame = foll_alt_type}) -- pass altitude in meters (location has it in cm) + end + return + else + -- have a good target so reset the countdown + lost_target_countdown = LOST_TARGET_TIMEOUT + now_follow_lost = now + end + + -- target_velocity from MAVLink (via AP_Follow) is groundspeed, need to convert to airspeed, + -- we can only assume the windspeed for the target is the same as the chase plane + local target_airspeed = calculate_airspeed_from_groundspeed(target_velocity_offset) + + local vehicle_heading = math.abs(wrap_360(math.deg(ahrs:get_yaw()))) + local heading_to_target_offset = math.deg(current_location:get_bearing(target_location_offset)) + + -- offset_angle is the difference between the current heading of the follow vehicle and the target_location (with offsets) + local offset_angle = wrap_180(vehicle_heading - heading_to_target_offset) + + -- rotate the target_distance_offsets in NED to the same direction has the follow vehicle, we use this below + local target_distance_rotated = target_distance_offset:copy() + target_distance_rotated:rotate_xy(math.rad(vehicle_heading)) + + -- default the desired heading to the target heading (adjusted for projected turns) - we might change this below + local airspeed_difference = vehicle_airspeed - target_airspeed + + -- distance seem to be out by about 0.92s at approximately current airspeed just eyeballing it. + xy_dist = math.abs(xy_dist) - vehicle_airspeed * distance_fudge + -- xy_dist will always be a positive value. To get -v to represent overshoot, use the offset_angle + -- to decide if the target is behind + if (math.abs(xy_dist) < long_distance) and (math.abs(offset_angle) > OVERSHOOT_ANGLE) then + xy_dist = -xy_dist + end + + -- projected_distance is how far off the target we expect to be if we maintain the current airspeed for DISTANCE_LOOKAHEAD_SECONDS + local projected_distance = xy_dist - airspeed_difference * DISTANCE_LOOKAHEAD_SECONDS + + -- target angle is the difference between the heading of the target and what its heading was 2 seconds ago + local target_angle = 0.0 + if (target_heading ~= nil and target_heading > -400) then + -- want the greatest angle of we might have turned + local angle_diff1 = wrap_180(math.abs(save_target_heading1 - target_heading)) + local angle_diff2 = wrap_180(math.abs(save_target_heading2 - target_heading)) + if angle_diff2 > angle_diff1 then + target_angle = angle_diff2 + else + target_angle = angle_diff1 + end + -- remember the target heading from 2 seconds ago, so we can tell if it is turning or not + if (now - now_target_heading) > 1 then + save_target_altitude = current_altitude + save_target_heading2 = save_target_heading1 + save_target_heading1 = target_heading + now_target_heading = now + end + end + + -- if the target vehicle is starting to roll we need to pre-empt a turn is coming + -- this is fairly simplistic and could probably be improved + -- got the code from Mission Planner - this is how MP calculates the turn radius in c# + --[[ + public float radius + { + get + { + if (_groundspeed <= 1) return 0; + return (float)toDistDisplayUnit(_groundspeed * _groundspeed / (9.80665 * Math.Tan(roll * MathHelper.deg2rad))); + } + } + --]] + local turning = (math.abs(projected_distance) < long_distance and math.abs(target_angle) > TURNING_ANGLE) + local turn_starting = false + local target_attitude = mavlink_attitude_receiver.get_attitude(foll_sysid) + local pre_roll_target_heading = target_heading + local desired_heading = target_heading + local angle_adjustment + tight_turn = false + if target_attitude ~= nil then + if (now - (target_attitude.timestamp_ms * 0.001) < LOST_TARGET_TIMEOUT) and + math.abs(target_attitude.roll) > 0.1 or math.abs(target_attitude.rollspeed) > 1 then + -- the roll and rollspeed are delayed values from the target plane, try to extrapolate them to at least "now" + 1 second + local rollspeed_impact = target_attitude.rollspeed * (target_attitude.delay_ms + 1000) + local turn_radius = vehicle_airspeed * vehicle_airspeed / (9.80665 * math.tan(target_attitude.roll + rollspeed_impact)) + + turning = true + -- predict the roll in 1s from now and use that based on rollspeed + -- need some more maths to convert a roll angle into a turn angle - from Mission Planner: + -- turn_radius = vehicle_airspeed * vehicle_airspeed / (9.80665 * math.tan(target_attitude.roll + target_attitude.rollspeed)) + local tangent_angle = wrap_360(math.deg(math.pi/2.0 - vehicle_airspeed / turn_radius)) + + angle_adjustment = tangent_angle * 0.6 + -- if the roll direction is the same as the y offset, then we are turning on the "inside" (a tight turn) + if (target_attitude.roll < 0 and foll_ofs_y < 0) or + (target_attitude.roll > 0 and foll_ofs_y > 0) then + tight_turn = true + end + + -- if the roll direction is the same as the rollspeed then we are heading into a turn, otherwise we are finishing a turn + if foll_ofs_y == 0 or + (target_attitude.roll < 0 and target_attitude.rollspeed < 0) or + (target_attitude.roll > 0 and target_attitude.rollspeed > 0) then + turn_starting = true + target_angle = wrap_360(target_angle - angle_adjustment) + desired_heading = wrap_360(target_heading - angle_adjustment) + -- push the target heading back because it hasn't figured out we are turning yet + save_target_heading1 = save_target_heading2 + end + end + end + + -- don't count it as close if the heading is diverging (i.e. the target plane has overshot the target so extremely that it's pointing in the wrong direction) + local too_close = (projected_distance > 0) and (math.abs(projected_distance) < close_distance) and (offset_angle < OVERSHOOT_ANGLE) + + -- we've overshot if + -- the distance to the target location is not too_close but will be hit in DISTANCE_LOOKAHEAD_SECONDS (projected_distance) + -- or the distance to the target location is already negative AND the target is very close OR + -- the angle to the target plane is effectively backwards + local overshot = not too_close and ( + (projected_distance < 0 or xy_dist < 0) and + (math.abs(xy_dist or 0.0) < close_distance) + or offset_angle > OVERSHOOT_ANGLE + ) + + if overshot or too_close or (too_close_follow_up > 0) then + if too_close_follow_up > 0 then + too_close = true + too_close_follow_up = too_close_follow_up - 1 + else + too_close_follow_up = 10 + end + else + too_close_follow_up = 0 + end + + local target_altitude = 0.0 + local frame_type_log = foll_alt_type + + if altitude_override ~= 0 then + target_altitude = altitude_override + frame_type_log = -1 + elseif target_location_offset ~= nil then + -- change the incoming altitude frame from the target_vehicle to the frame this vehicle wants + target_location_offset:change_alt_frame(foll_alt_type) + target_altitude = target_location_offset:alt() * 0.01 + end + + local mechanism = 0 -- for logging 1: position/location 2:heading + local normalized_distance = math.abs(projected_distance) + local close = (normalized_distance < close_distance) + local too_wide = (math.abs(target_distance_rotated:y()) > (close_distance/5) and not turning) + + -- xy_dist < 3.0 is a special case because mode_guided will try to loiter around the target location if within 2m + -- target_heading - vehicle_heading catches the circumstance where the target vehicle is heaidng in completely the opposite direction + if math.abs(xy_dist or 0.0) < 3.0 or + ((turning and ((tight_turn and turn_starting) or use_wide_turns or foll_ofs_y == 0)) or -- turning + ((close or overshot) and not too_wide) -- we are very close to the target + --math.abs(target_heading - vehicle_heading) > 135) -- the target is going the other way + ) then + set_vehicle_heading({heading = desired_heading}) + set_vehicle_target_altitude({alt = target_altitude, frame = foll_alt_type}) -- pass altitude in meters (location has it in cm) + mechanism = 2 -- heading - for logging + elseif target_location_offset ~= nil then + set_vehicle_target_location({lat = target_location_offset:lat(), + lng = target_location_offset:lng(), + alt = target_altitude, + frame = foll_alt_type, + yaw = foll_ofs_y}) + mechanism = 1 -- position/location - for logging + end + -- dv = interim delta velocity based on the pid controller using projected_distance per loop as the error (we want distance == 0) + local dv_error = projected_distance * REFRESH_RATE + local airspeed_new = vehicle_airspeed + local dv = 0.0 + if dv_error ~= nil then + if projected_distance < 0 then + dv_error = dv_error * 1.75 -- attempt to slow down faster if we are overshooting + end + dv = pid_controller_distance.update(target_airspeed - vehicle_airspeed, dv_error) + airspeed_new = pid_controller_velocity.update(vehicle_airspeed, dv) + -- gcs:send_text(MAV_SEVERITY.INFO, string.format("Err: %.1f DstP: %.0f Dst: %.0f dv: %.1f AspO: %.1f", dv_error, projected_distance, xy_dist, dv, airspeed_new)) + + set_vehicle_speed({speed = constrain(airspeed_new, airspeed_min, airspeed_max)}) + end + + local log_too_close = 0 + local log_too_close_follow_up = 0 + local log_overshot = 0 + if too_close then + log_too_close = 1 + end + if too_close_follow_up then + log_too_close_follow_up = 1 + end + if overshot then + log_overshot = 1 + end + logger:write("ZPF1",'Dst,DstP,DstE,AspT,Asp,AspD,AspO,Mech,Cls,ClsF,OSht','fffffffBBBB','mmmnnnn----','-----------', + xy_dist, + projected_distance, + dv_error, + target_airspeed, + vehicle_airspeed, + dv, + airspeed_new, + mechanism, log_too_close, log_too_close_follow_up, log_overshot + ) + logger:write("ZPF2",'AngT,AngO,Alt,AltT,AltFrm,HdgT,Hdg,HdgP,HdgO','ffffbffff','ddmm-dddd','---------', + target_angle, + offset_angle, + current_altitude, + target_altitude, + frame_type_log, + target_heading, + vehicle_heading, + pre_roll_target_heading, + desired_heading + ) +end + +-- wrapper around update(). This calls update() at 1/REFRESH_RATE Hz +-- and if update faults then an error is displayed, but the script is not +-- stopped +local function protected_wrapper() + local success, err = pcall(update) + + if not success then + gcs:send_text(MAV_SEVERITY.ALERT, SCRIPT_NAME_SHORT .. "Internal Error: " .. err) + -- when we fault we run the update function again after 1s, slowing it + -- down a bit so we don't flood the console with errors + return protected_wrapper, 1000 + end + return protected_wrapper, 1000 * REFRESH_RATE +end + +local function delayed_startup() + gcs:send_text(MAV_SEVERITY.INFO, string.format("%s %s script loaded", SCRIPT_NAME, SCRIPT_VERSION) ) + return protected_wrapper() +end + +-- start running update loop - waiting 20s for the AP to initialize +if FWVersion:type() == 3 then + if arming:is_armed() then + return delayed_startup, 1000 + else + return delayed_startup, 20000 + end +else + gcs:send_text(MAV_SEVERITY.ERROR, string.format("%s: must run on Plane", SCRIPT_NAME_SHORT)) +end diff --git a/libraries/AP_Scripting/applets/plane_follow.lua b/libraries/AP_Scripting/applets/plane_follow.lua index 3f16a9973fb2de..f260254356224a 100644 --- a/libraries/AP_Scripting/applets/plane_follow.lua +++ b/libraries/AP_Scripting/applets/plane_follow.lua @@ -25,7 +25,7 @@ ZPR_TURN_DEG - if the target is more than this many degrees left or right, assume it's turning --]] -SCRIPT_VERSION = "4.7.0-051" +SCRIPT_VERSION = "4.7.0-052" SCRIPT_NAME = "Plane Follow" SCRIPT_NAME_SHORT = "PFollow" @@ -98,7 +98,7 @@ local foll_alt_type = FOLL_ALT_TYPE:get() or ALT_FRAME.GLOBAL // @Description: Mode to switch to if the target is lost (no signal or > FOLL_DIST_MAX). // @User: Standard --]] -ZPF_FAIL_MODE = bind_add_param('FAIL_MODE', 1, FLIGHT_MODE.LOITER) +ZPF_FAIL_MODE = bind_add_param('FAIL_MODE', 1, FLIGHT_MODE.RTL) --[[ // @Param: ZPF_EXIT_MODE @@ -764,7 +764,6 @@ local function update() or offset_angle > OVERSHOOT_ANGLE ) - local distance_error = constrain(math.abs(projected_distance) / (close_distance * DISTANCE_LOOKAHEAD_SECONDS), 0.1, 1.0) if overshot or too_close or (too_close_follow_up > 0) then if too_close_follow_up > 0 then too_close = true diff --git a/libraries/AP_Scripting/modules/mavlink_attitude.lua b/libraries/AP_Scripting/modules/mavlink_attitude.lua index 03fcefdbb95979..e80b95acdb8685 100644 --- a/libraries/AP_Scripting/modules/mavlink_attitude.lua +++ b/libraries/AP_Scripting/modules/mavlink_attitude.lua @@ -139,6 +139,10 @@ function MAVLinkAttitude.mavlink_attitude_receiver() local sysid = parsed_msg.sysid local attitude = {} attitude.timestamp_ms = jitter_correction.correct_offboard_timestamp_msec(parsed_msg.time_boot_ms, timestamp_ms:toint()) + if attitude.timestamp_ms == nil then -- not sure why this can happen but it can, so need to let the caller know to not use the results + return nil + end + attitude.delay_ms = millis():tofloat() - attitude.timestamp_ms -- the latency/delay from when the message was sent attitude.roll = parsed_msg.roll attitude.pitch = parsed_msg.pitch attitude.yaw = parsed_msg.yaw @@ -146,6 +150,22 @@ function MAVLinkAttitude.mavlink_attitude_receiver() attitude.pitchspeed = parsed_msg.pitchspeed attitude.yawspeed = parsed_msg.yawspeed if sysid == target_sysid then + -- Log ATI = Attitude In + -- Time = Timestamp received in ms + -- TimeJC = Timestamp received with Jitter Correction + -- Delay = Delay between message sent and received in ms + logger:write("ZATI",'Time,TimeJC,Delay,roll,ptch,yaw,rollspd,ptchspd,yawspd','iiiffffff','---rrrEEE','---------', + parsed_msg.time_boot_ms, + attitude.timestamp_ms, + attitude.delay_ms, + attitude.roll, + attitude.pitch, + attitude.yaw, + attitude.rollspeed, + attitude.pitchspeed, + attitude.yawspeed + ) + return attitude end end