diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 6e6991fd54b01..6c63aab413f2e 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -1309,7 +1309,14 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @User: Standard AP_GROUPINFO("RNGFND_LND_ORNT", 36, ParametersG2, rangefinder_land_orient, ROTATION_PITCH_270), #endif - + + // @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 9945869250b4b..93d70624b86e0 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -573,6 +573,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 7879defdd794d..eae01f64be18e 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 a97bedcd7dffb..f0afd409edafb 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); diff --git a/libraries/AP_Follow/AP_Follow.cpp b/libraries/AP_Follow/AP_Follow.cpp index 8792efd248898..795c10e4d1825 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,41 @@ 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)) { + 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 48c43fc9ff470..6730525428032 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 diff --git a/libraries/AP_Scripting/applets/plane_follow.lua b/libraries/AP_Scripting/applets/plane_follow.lua new file mode 100644 index 0000000000000..6444f87a1a335 --- /dev/null +++ b/libraries/AP_Scripting/applets/plane_follow.lua @@ -0,0 +1,946 @@ +--[[ + + 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-047" +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, + 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 + +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 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_" + +-- 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, 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 +-- 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.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 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.3) + +--[[ + // @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.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 +--]] +ZPF_D_D = bind_add_param("D_D", 13, 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 +--]] +ZPF_V_P = bind_add_param("V_P", 14, 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 +--]] +ZPF_V_I = bind_add_param("V_I", 15, 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 +--]] +ZPF_V_D = bind_add_param("V_D", 16, 0.05) + +--[[ + // @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) + +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 = ZPF_DIST_FUDGE:get() or 0.92 + +DISTANCE_LOOKAHEAD_SECONDS = ZPF_LKAHD:get() or 5.0 + +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.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(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 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 COMMAND_INT_map = {} +COMMAND_INT_map.id = 75 +COMMAND_INT_map.fields = { + { "param1", " 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 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 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 + + -- 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 + 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 + + --[[ + 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 -- note that this is not logged, needs fixing + -- 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 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 * 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)) + + 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 0000000000000..f83909b039e39 --- /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 4e15c6d33d033..10c921de2afae 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 @@ -3830,28 +3829,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 3772ca51c0754..06f5d5cc3765e 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -751,10 +751,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 0000000000000..a563698835131 --- /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