From d7fa0eec3238a4037e6496f3761f62bc6abadac6 Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Tue, 19 Aug 2025 10:34:07 -0600 Subject: [PATCH 1/3] Tools: plane_follow.lua applet --- Tools/autotest/arduplane.py | 38 +++++++++++++++++++++++++++++++++++++ 1 file changed, 38 insertions(+) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 898d3eac4b337..dcc5abdc638f0 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -7733,6 +7733,43 @@ def ScriptedArmingChecksAppletRally(self): self.set_parameter("RALLY_LIMIT_KM", 7) self.wait_text("clear: Rally too far", check_context=True) + def PlaneFollowAppletSanity(self): + '''PLane Follow Sanity Check, not a detailed test''' + self.start_subtest("Plane Follow Script Load and Start") + + self.install_applet_script_context("plane_follow.lua") + self.install_script_module(self.script_modules_source_path("pid.lua"), "pid.lua") + self.install_script_module(self.script_modules_source_path("mavlink_attitude.lua"), "mavlink_attitude.lua") + self.install_mavlink_module() + + self.set_parameters({ + "SCR_ENABLE": 1, + "SIM_SPEEDUP": 20, # need to give some cycles to lua + "RC7_OPTION": 301, + }) + + self.context_collect("STATUSTEXT") + + self.reboot_sitl() + + self.wait_text("Plane Follow .* script loaded", timeout=30, regex=True, check_context=True) + + self.wait_ready_to_arm() + self.set_rc(7, 2000) + self.wait_text("PFollow: must be armed", check_context=True) + self.set_rc(7, 1000) + self.arm_vehicle() + self.set_rc(7, 2000) + self.wait_text("PFollow: enabled", check_context=True) + self.set_rc(7, 1000) + self.wait_text("PFollow: disabled", check_context=True) + self.disarm_vehicle() + + self.reboot_sitl() + # remove the installed modules. + self.remove_installed_script_module("pid.lua") + self.remove_installed_script_module("mavlink_attitude.lua") + def tests(self): '''return list of all tests''' ret = [] @@ -7908,6 +7945,7 @@ def tests1b(self): self.ScriptedArmingChecksApplet, self.ScriptedArmingChecksAppletEStop, self.ScriptedArmingChecksAppletRally, + self.PlaneFollowAppletSanity, ] def tests1c(self): From 7df6d2c9a927ba457e4896024591964bbe7f05a4 Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Tue, 19 Aug 2025 10:34:07 -0600 Subject: [PATCH 2/3] AP_Scripting: plane_follow.lua applet --- .../AP_Scripting/applets/plane_follow.lua | 1017 +++++++++++++++++ .../AP_Scripting/applets/plane_follow.md | 148 +++ libraries/AP_Scripting/docs/docs.lua | 30 +- .../generator/description/bindings.desc | 2 + .../AP_Scripting/modules/mavlink_attitude.lua | 189 +++ libraries/AP_Scripting/modules/pid.lua | 211 ++++ 6 files changed, 1587 insertions(+), 10 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/pid.lua diff --git a/libraries/AP_Scripting/applets/plane_follow.lua b/libraries/AP_Scripting/applets/plane_follow.lua new file mode 100644 index 0000000000000..499593de8ec68 --- /dev/null +++ b/libraries/AP_Scripting/applets/plane_follow.lua @@ -0,0 +1,1017 @@ +--[[ + + 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 + FOLLP_EXIT_MODE - the mode to switch to when follow is turned of using the switch + FOLLP_FAIL_MODE - the mode to switch to if the target is lost + FOLLP_TIMEOUT - number of seconds to try to reaquire the target after losing it before failing + FOLLP_OVRSHT_DEG - if the target is more than this many degrees left or right, assume an overshoot + FOLLP_TURN_DEG - if the target is more than this many degrees left or right, assume it's turning +--]] + +SCRIPT_VERSION = "4.7.0-073" +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_ms = millis() +local now_target_heading_ms = now_ms +local now_follow_lost_ms = now_ms +local now_heading_ms = now_ms +local too_close_follow_up = 0 +local save_target_heading1 = -400.0 +local save_target_heading2 = -400.0 +local tight_turn = false + +local PARAM_TABLE_KEY = 123 +local PARAM_TABLE_PREFIX = "FOLLP_" + +-- 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, 30), '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() +local foll_ofs_y = FOLL_OFS_Y:get() +local foll_alt_type = FOLL_ALT_TYPE:get() + +-- Add these FOLLP_ parameters specifically for this script +--[[ + // @Param: FOLLP_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 +--]] +FOLLP_FAIL_MODE = bind_add_param('FAIL_MODE', 1, FLIGHT_MODE.RTL) + +--[[ + // @Param: FOLLP_EXIT_MODE + // @DisplayName: Plane Follow exit mode + // @Description: Mode to switch to when follow mode is exited normally + // @User: Standard +--]] +FOLLP_EXIT_MODE = bind_add_param('EXIT_MODE', 2, FLIGHT_MODE.LOITER) + +--[[ + // @Param: FOLLP_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 +--]] +FOLLP_ACT_FN = bind_add_param("ACT_FN", 3, 301) + +--[[ + // @Param: FOLLP_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 +--]] +FOLLP_TIMEOUT = bind_add_param("TIMEOUT", 4, 10) + +--[[ + // @Param: FOLLP_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 +--]] +FOLLP_OVRSHT_DEG = bind_add_param("OVRSHT_DEG", 5, 75) + +--[[ + // @Param: FOLLP_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 +--]] +FOLLP_TURN_DEG = bind_add_param("TURN_DEG", 6, 15) + +--[[ + // @Param: FOLLP_DIST_CLOSE + // @DisplayName: Plane Follow Scripting Close Distance + // @Description: When closer than this distance assume we track by heading + // @Range: 0 100 + // @Units: m +--]] +FOLLP_DIST_CLOSE = bind_add_param("DIST_CLOSE", 7, 75) + +--[[ + // @Param: FOLLP_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 +--]] +FOLLP_WIDE_TURNS = bind_add_param("WIDE_TURNS", 8, 1) + +--[[ + // @Param: FOLLP_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 +--]] +FOLLP_ALT_OVR = bind_add_param("ALT_OVR", 9, 0) + +--[[ + // @Param: FOLLP_D_P + // @DisplayName: Plane Follow Scripting distance P gain + // @Description: P gain for the speed PID controller distance component + // @Range: 0 1 +--]] +FOLLP_D_P = bind_add_param("D_P", 10, 0.00025) + +--[[ + // @Param: FOLLP_D_I + // @DisplayName: Plane Follow Scripting distance I gain + // @Description: I gain for the speed PID distance component + // @Range: 0 1 +--]] +FOLLP_D_I = bind_add_param("D_I", 11, 0.00025) + +--[[ + // @Param: FOLLP_D_D + // @DisplayName: Plane Follow Scripting distance D gain + // @Description: D gain for the speed PID controller distance component + // @Range: 0 1 +--]] +FOLLP_D_D = bind_add_param("D_D", 12, 0.00013) + +--[[ + // @Param: FOLLP_V_P + // @DisplayName: Plane Follow Scripting speed P gain + // @Description: P gain for the speed PID controller velocity component + // @Range: 0 1 +--]] +FOLLP_V_P = bind_add_param("V_P", 13, 0.00025) + +--[[ + // @Param: FOLLP_V_I + // @DisplayName: Plane Follow Scripting speed I gain + // @Description: I gain for the speed PID controller velocity component + // @Range: 0 1 +--]] +FOLLP_V_I = bind_add_param("V_I", 14, 0.00025) + +--[[ + // @Param: FOLLP_V_D + // @DisplayName: Plane Follow Scripting speed D gain + // @Description: D gain for the speed PID controller velocity component + // @Range: 0 1 +--]] +FOLLP_V_D = bind_add_param("V_D", 15, 0.0005) + +--[[ + // @Param: FOLLP_LkAHD + // @DisplayName: Plane Follow Lookahead seconds + // @Description: Time to "lookahead" when calculating distance errors + // @Units: s +--]] +FOLLP_LKAHD = bind_add_param("LKAHD", 16, 3) + +--[[ + // @Param: FOLLP_SIM_TLF_FN + // @DisplayName: Plane Follow Simulate Telemetry fail function + // @Description: Set this switch to simulate losing telemetry from the other vehicle + // @Range: 300 307 +--]] +FOLLP_SIM_TLF_FN = bind_add_param("SIM_TlF_FN", 17, 302) + +--[[ + // @Param: FOLLP_XT_P + // @DisplayName: Plane Follow crosstrack PID controller P term + // @Description: P term for the crosstrack/heading PID controller + // @Range: 0 1 +--]] +FOLLP_XT_P = bind_add_param("XT_P", 20, 0.8) + +--[[ + // @Param: FOLLP_XT_I + // @DisplayName: Plane Follow crosstrack PID controller I term + // @Description: I term for the crosstrack/heading PID controller + // @Range: 0 1 +--]] +FOLLP_XT_I = bind_add_param("XT_I", 21, 0.01) + +--[[ + // @Param: FOLLP_XT_D + // @DisplayName: Plane Follow crosstrack PID controller D term + // @Description: D term for the crosstrack/heading PID controller + // @Range: 0 1 +--]] +FOLLP_XT_D = bind_add_param("XT_D", 22, 0.5) + +--[[ + // @Param: FOLLP_XT_MAX + // @DisplayName: Plane Follow crosstrack PID controller maximum correction + // @Description: maximum correction retured by the crosstrack/heading PID controller + // @Range: 0 1 + // @Units: deg +--]] +FOLLP_XT_MAX = bind_add_param("XT_MAX", 23, 45) + +--[[ + // @Param: FOLLP_XT_I_MAX + // @DisplayName: Plane Follow crosstrack PID controller maximum integral + // @Description: maximum I applied the crosstrack/heading PID controller + // @Range: 0 100 + // @Units: ms +--]] +FOLLP_XT_I_MAX = bind_add_param("XT_I_MAX", 24, 100) + +--[[ + // @Param: FOLLP_REFRESH + // @DisplayName: Plane Follow refresh rate + // @Description: refresh rate for Plane Follow updates + // @Range: 0 0.2 + // @Units: s +--]] +FOLLP_REFRESH = bind_add_param("REFRESH", 25, 0.05) + +local refresh_rate = FOLLP_REFRESH:get() +LOST_TARGET_TIMEOUT = FOLLP_TIMEOUT:get() / refresh_rate +OVERSHOOT_ANGLE = FOLLP_OVRSHT_DEG:get() +TURNING_ANGLE = FOLLP_TURN_DEG:get() +DISTANCE_LOOKAHEAD_SECONDS = FOLLP_LKAHD:get() + +local lost_target_countdown = LOST_TARGET_TIMEOUT + +local fail_mode = FOLLP_FAIL_MODE:get() +local exit_mode = FOLLP_EXIT_MODE:get() + +local use_wide_turns = FOLLP_WIDE_TURNS:get() + +--local simulate_telemetry_failed = false + +AIRSPEED_MIN = Parameter('AIRSPEED_MIN') +AIRSPEED_MAX = Parameter('AIRSPEED_MAX') +WP_LOITER_RAD = Parameter('WP_LOITER_RAD') +WINDSPEED_MAX = Parameter('AHRS_WIND_MAX') + +local airspeed_max = AIRSPEED_MAX:get() +local airspeed_min = AIRSPEED_MIN:get() +local windspeed_max = WINDSPEED_MAX:get() + +------------------------------------------------------------------------------- +--- Utility Functions +------------------------------------------------------------------------------- + +---@diagnostic disable-next-line:lowercase-global +function constrain(v, vmin, vmax) + if v < vmin then + v = vmin + end + if v > vmax then + v = vmax + end + return v +end + +---@diagnostic disable-next-line:lowercase-global +function wrap_360(angle) + local res = math.fmod(angle, 360.0) + if res < 0 then + res = res + 360.0 + end + return res +end + +---@diagnostic disable-next-line:lowercase-global +function wrap_180(angle) + local res = wrap_360(angle) + if res > 180 then + res = res - 360 + end + return res +end + + +------------------------------------------------------------------------------- +--- PID Controllers +------------------------------------------------------------------------------- + +local pid = require("pid") +local pid_controller_distance = pid.speed_controller(FOLLP_D_P:get(), + FOLLP_D_I:get(), + FOLLP_D_D:get(), + 0.5, airspeed_min - airspeed_max, airspeed_max - airspeed_min) + +local pid_controller_velocity = pid.speed_controller(FOLLP_V_P:get(), + FOLLP_V_I:get(), + FOLLP_V_D:get(), + 0.5, airspeed_min, airspeed_max) + +-- We need a PID controller to manage cross track errors +-- start of crosstrackpid {} class definition +local crosstrackpid = {} +crosstrackpid.__index = crosstrackpid + +function crosstrackpid.new(kp, ki, kd, max_correction, integral_limit) + local self = {} + setmetatable(self, { __index = crosstrackpid }) + + self.kp = kp or 0.8 + self.ki = ki or 0.01 + self.kd = kd or 0.5 + self.max_correction = max_correction -- degrees + self.integral_limit = integral_limit -- m·s + + self.integral = 0 + self.last_error = 0 + + return self +end + +-- reset integrator to an initial value +function crosstrackpid.reset(self) + if self == nil then + self = setmetatable({}, { __index = crosstrackpid }) + end + self.integral = 0 + self.last_error = 0 +end + +function crosstrackpid:compute(desired_track_heading, cross_track_error, dt) + -- Derivative + local error_rate = (cross_track_error - self.last_error) / dt + + -- Integral with clamp + self.integral = self.integral + cross_track_error * dt + if self.integral > self.integral_limit then self.integral = self.integral_limit end + if self.integral < -self.integral_limit then self.integral = -self.integral_limit end + + -- PID correction (heading offset in degrees) + local correction = self.kp * cross_track_error + + self.kd * error_rate + + self.ki * self.integral + + -- Clamp heading correction + if correction > self.max_correction then correction = self.max_correction end + if correction < -self.max_correction then correction = -self.max_correction end + + -- Apply correction (subtract because +error = left of track) + local corrected_heading = desired_track_heading - correction + corrected_heading = (corrected_heading + 360) % 360 + + -- Save state + self.last_error = cross_track_error + + return corrected_heading +end +-- end of crosstrackpid {} class definition + +-- Instantiate the crosstrack/heading PID controller (outside update loop) +local xt_pid = crosstrackpid.new(FOLLP_XT_P:get(), + FOLLP_XT_I:get(), + FOLLP_XT_D:get(), + FOLLP_XT_MAX:get(), + FOLLP_XT_I_MAX:get()) + +------------------------------------------------------------------------------- +--- MAVLink utility objects and functions +------------------------------------------------------------------------------- + +local mavlink_attitude = require("mavlink_attitude") +local mavlink_attitude_receiver = mavlink_attitude.mavlink_attitude_receiver() + +---@diagnostic disable-next-line:lowercase-global +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) +---@diagnostic disable-next-line:lowercase-global +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 altitude") + 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 +---@diagnostic disable-next-line:lowercase-global +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 +---@diagnostic disable-next-line:lowercase-global +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 + +------------------------------------------------------------------------------- +--- Allow for simulation of telemetry fail for testing +------------------------------------------------------------------------------- + +--[[ + -- checks for user simulating telemetry fail using FOLLP_SIM_TLF_FN + - enables (HIGH)/disables (LOW) +--]] +local simulate_failure = { + telemetry = false, +} +(function () + local last_tel_fail_state = rc:get_aux_cached(FOLLP_SIM_TLF_FN:get()) + function simulate_failure.check() + local sim_tel_fail = FOLLP_SIM_TLF_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 + simulate_failure.telemetry = false + else + --simulate_telemetry_failed = true + simulate_failure.telemetry = true + end + last_tel_fail_state = tel_fail_state + end + end +end) () + +------------------------------------------------------------------------------- +--- FOLLOW mode managmenet +------------------------------------------------------------------------------- +local follow_mode = { + enabled = false, +} +(function () + local reported_target = true + local lost_target_now_ms = now_ms + --[[ + follow_active() - return true if we are in a state where follow can apply + --]] + function follow_mode.active() + local mode = vehicle:get_mode() + + if not follow_mode.enabled then + return false + end + + if mode ~= FLIGHT_MODE.GUIDED then + reported_target = false + return reported_target + end + if follow:have_target() then + reported_target = true + lost_target_now_ms = now_ms + else + if reported_target then -- i.e. if we previously reported a target but lost it + if (now_ms - lost_target_now_ms) > 5000 then + gcs:send_text(MAV_SEVERITY.WARNING, SCRIPT_NAME_SHORT .. ": lost prior target: " .. follow:get_target_sysid()) + lost_target_now_ms = now_ms + end + end + reported_target = false + gcs:send_text(MAV_SEVERITY.WARNING, SCRIPT_NAME_SHORT .. ": no target: " .. follow:get_target_sysid()) + end + + return reported_target + end + function follow_mode.enable() + if follow_mode.enabled then + return + end + if not arming:is_armed() then + gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": must be armed") + return + end + -- Follow enabled - switch to guided mode but only if armed + vehicle:set_mode(FLIGHT_MODE.GUIDED) + follow_mode.enabled = true + lost_target_countdown = LOST_TARGET_TIMEOUT + pid_controller_distance.reset() + pid_controller_velocity.reset() + xt_pid.reset() + gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. ": enabled") + end + function follow_mode.disable() + if not follow_mode.enabled then + return + end + -- Follow switched to disabled - return to EXIT mode - but not if disarmed + if not arming:is_armed() then + vehicle:set_mode(exit_mode) + end + follow_mode.enabled = false + gcs:send_text(MAV_SEVERITY.INFO, SCRIPT_NAME_SHORT .. ": disabled") + end + + --[[ + follow_check() - check for user activating follow using an RC switch + - set HIGH and if so + - switches to GUIDED + - enables follow + - resets the PID controllers + -- set LOW and if so + - exits from GUIDED to the FOLLP_EXIT_MODE + - disables follow + --]] + local last_follow_active_state = rc:get_aux_cached(FOLLP_ACT_FN:get()) + function follow_mode.check() + if FOLLP_ACT_FN == nil then + return + end + local foll_act_fn = FOLLP_ACT_FN:get() + local active_state = rc:get_aux_cached(foll_act_fn) + if (active_state == last_follow_active_state) then + return + end + if( active_state == 0) then + follow_mode.disable() + elseif (active_state == 2) then + follow_mode.enable() + end + last_follow_active_state = active_state + end +end) () + +-- convert a groundspeed to airspeed using windspeed and EAS2TAS (from AHRS) +---@diagnostic disable-next-line:lowercase-global +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 + +-- ChatGPT code to calculate the distance to the target along_track and cross_track +---@diagnostic disable-next-line:lowercase-global +function calculate_track_distance(P_f, P_l) + -- Get the follower's ground-relative velocity + local V_f = ahrs:get_velocity_NED() + if V_f == nil or (V_f:x() == 0 and V_f:y() == 0) then + -- No valid velocity, return zeros + return 0, 0 + end + + -- Create horizontal velocity vector + local D_f = Vector3f() + D_f:x(V_f:x()) + D_f:y(V_f:y()) + D_f:z(0) -- need to do a dot product with 3d vector R below + + -- Normalize in-place + D_f:normalize() + + -- Get relative position vector + local R = P_f:get_distance_NED(P_l) + + -- Along-track distance: projection of R onto D_f + local along_track_distance = R:dot(D_f) + + -- Cross-track distance: projection onto perpendicular to D_f + local perp_D_f = Vector3f() + perp_D_f:x(-D_f:y()) + perp_D_f:y(D_f:x()) + perp_D_f:z(0) -- need to do a dot product with 3d vector R below + perp_D_f:normalize() + + local cross_track_distance = R:dot(perp_D_f) + + -- -ve cross_track_distance to align to the sign of FOLL_OFS_Y where +ve is to the right + return along_track_distance, -cross_track_distance +end + +---@diagnostic disable-next-line:lowercase-global +function calculate_target_angle(heading) + local new_target_angle = 0.0 + if (heading ~= nil and heading > -400) then + -- want the greatest angle of we might have turned + local angle_diff1 = wrap_180(math.abs(save_target_heading1 - heading)) + local angle_diff2 = wrap_180(math.abs(save_target_heading2 - heading)) + if angle_diff2 > angle_diff1 then + new_target_angle = angle_diff2 + else + new_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_ms - now_target_heading_ms) > 1000 then + save_target_heading2 = save_target_heading1 + save_target_heading1 = heading + now_target_heading_ms = now_ms + end + end + return new_target_angle +end + +-- main update function +function Update() + now_ms = millis() + ahrs_eas2tas = ahrs:get_EAS2TAS() + windspeed_vector = ahrs:wind_estimate() + + simulate_failure.check() + follow_mode.check() + if not follow_mode.active() then + return + end + + -- set the target frame as per user set parameter - this is fundamental to this working correctly + local close_distance = FOLLP_DIST_CLOSE:get() + local long_distance = close_distance * 4.0 + local altitude_override = FOLLP_ALT_OVR:get() + + LOST_TARGET_TIMEOUT = FOLLP_TIMEOUT:get() / refresh_rate + OVERSHOOT_ANGLE = FOLLP_OVRSHT_DEG:get() + TURNING_ANGLE = FOLLP_TURN_DEG:get() + foll_ofs_y = FOLL_OFS_Y:get() + foll_alt_type = FOLL_ALT_TYPE:get() + use_wide_turns = FOLLP_WIDE_TURNS:get() + + --[[ + 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_heading -- heading of the target vehicle + + local current_location = ahrs:get_location() + if current_location == nil then + return + end + local current_altitude = current_location:alt() * 0.01 + + local vehicle_airspeed = ahrs:airspeed_estimate() + local current_target = vehicle:get_target_location() + + target_location, target_velocity = follow:get_target_location_and_velocity() + target_location_offset, target_velocity_offset = follow:get_target_location_and_velocity_ofs() + + target_heading = follow:get_target_heading_deg() or -400 + + -- if we lose the target wait for LOST_TARGET_TIMEOUT seconds to try to re-aquire it + if target_location == nil or target_location_offset == nil or + target_velocity == nil or target_velocity_offset == nil or current_target == nil or + simulate_failure.telemetry then + lost_target_countdown = lost_target_countdown - 1 + if lost_target_countdown <= 0 then + follow_mode.enabled = false + vehicle:set_mode(fail_mode) + gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. string.format(": follow: %.0f FAILED", foll_sysid)) + return + end + + -- maintain the current heading for 2 seconds until we re-establish telemetry from the target vehicle + if (now_ms - now_follow_lost_ms) > 2000 then + gcs:send_text(MAV_SEVERITY.WARNING, SCRIPT_NAME_SHORT .. string.format(": follow: lost %.0f set hdg: %.0f", foll_sysid, save_target_heading1)) + now_follow_lost_ms = now_ms + set_vehicle_heading({heading = save_target_heading1}) + end + return + else + -- have a good target so reset the countdown + lost_target_countdown = LOST_TARGET_TIMEOUT + now_follow_lost_ms = now_ms + end + + -- calculate the target_distance from target distance offset so we don't need to call get_target_dist_and_vel_NED + local new_target_distance = current_location:get_distance_NED(target_location_offset) + local along_track_distance, cross_track_distance = calculate_track_distance(current_location, target_location_offset) + + -- 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) + + local vehicle_heading = math.abs(wrap_360(math.deg(ahrs:get_yaw_rad()))) + 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 heading to 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() + local target_distance_rotated = new_target_distance: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 + + -- 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 = along_track_distance - 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 = calculate_target_angle(target_heading) + + -- 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 turning_airspeed_ratio = 1.0 + tight_turn = false + if target_attitude ~= nil and target_airspeed ~= nil and target_airspeed > airspeed_min then + if ((now_ms - target_attitude.timestamp_ms) < 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 target_turn_radius = vehicle_airspeed * vehicle_airspeed / (9.80665 * math.tan(target_attitude.roll + rollspeed_impact)) + + turning = true + + -- 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 + + -- calculate the path/distance around the turn for the lead and follow vehicle so we can slow down or speed up + -- depending on which is flying the shorter path + local turn_radius = target_turn_radius + foll_ofs_y + if tight_turn then + turn_radius = target_turn_radius - foll_ofs_y + end + -- theta = l/r - i.e. the angle of the arc is the length of the arc divided by the radius + local theta = target_airspeed / target_turn_radius + -- now calculate what my airspeed would need to be to match the target, given I'm flying an arc with a different radius + local my_airspeed = theta * turn_radius + if my_airspeed > 0 and my_airspeed > airspeed_min and target_airspeed > 0 then + turning_airspeed_ratio = constrain(my_airspeed / target_airspeed, 0.0, 2.0) + 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 along_track_distance < 0) and + (math.abs(along_track_distance) < 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 close = (math.abs(projected_distance) < close_distance) + local too_wide = (math.abs(target_distance_rotated:y()) > (close_distance/4) and not turning) + local desired_heading = target_heading + + -- target_heading - vehicle_heading catches the circumstance where the target vehicle is heading in completely the opposite direction + if (math.abs(along_track_distance) < airspeed_max * 0.75 or (math.abs(cross_track_distance) < airspeed_max * 0.25)) 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 + ) then + -- set the desired heading to the targt heading + mechanism = 2 -- heading - for logging + elseif target_location_offset ~= nil then + -- override the heading to point directly to the target location with offsets. + desired_heading = heading_to_target_offset + mechanism = 1 -- position/location - for logging + end + + -- The desired heading needs a PID controller, especially when it gets close. + desired_heading = xt_pid:compute(desired_heading, cross_track_distance, (now_ms - now_heading_ms):tofloat() * 0.001) + + -- dv = interim delta velocity based on the pid controller using projected_distance per loop as the error (we want distance == 0) + local dv_error = along_track_distance * refresh_rate * 2.0 + if dv_error < 0 then + dv_error = dv_error * 5.0 -- fudge factor to deal with it being harder to slow down from overshoot + end + -- re-project the distance error based on the turning angle + -- if (turning and (tight_turn and turn_starting)) and math.abs(offset_angle) > TURNING_ANGLE and turning_airspeed_ratio > 0 then + if turning_airspeed_ratio > 0 and turning_airspeed_ratio < 2.0 then + dv_error = dv_error * turning_airspeed_ratio + end + local airspeed_new = vehicle_airspeed + local airspeed_error = (target_airspeed - vehicle_airspeed) + local dv = 0.0 + if dv_error ~= nil then + dv = pid_controller_distance.update(airspeed_error, dv_error) + airspeed_new = pid_controller_velocity.update(vehicle_airspeed, dv) + end + + -- Finally after all the calculations - send the target heading, altitude and airspeed to AP + 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) + set_vehicle_speed({speed = constrain(airspeed_new, airspeed_min, airspeed_max)}) + + -- now log everything + 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("PF1",'Dst,DstP,DstE,AspT,Asp,AspD,AspE,AspO,TrnR,Mech,Cls,CF,OS','fffffffffBBBB','mmmnnnnnn----','-------------', + along_track_distance, + projected_distance, + dv_error, + target_airspeed, + vehicle_airspeed, + dv, + airspeed_error, + airspeed_new, + turning_airspeed_ratio, + mechanism, log_too_close, log_too_close_follow_up, log_overshot + ) + logger:write("PF2",'AngT,AngO,Alt,AltT,AltFrm,HdgT,Hdg,HdgP,HdgO,XTD','ffffbfffff','ddmm-ddddm','----------', + target_angle, + offset_angle, + current_altitude, + target_altitude, + frame_type_log, + target_heading, + vehicle_heading, + pre_roll_target_heading, + desired_heading, + cross_track_distance + ) +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 +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_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 25s for the AP to initialize if not armed +if FWVersion:type() == 3 then + if arming:is_armed() then + return Delayed_Startup() + else + return Delayed_Startup, 25000 + 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 new file mode 100644 index 0000000000000..7e186259ae2c4 --- /dev/null +++ b/libraries/AP_Scripting/applets/plane_follow.md @@ -0,0 +1,148 @@ +# 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. + +## FOLLP_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. + +## FOLLP_EXIT_MODE + +The flight mode the plane will switch to if it exits following. + +## FOLLP_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. + +## FOLLP_TIMEOUT + +If the target is lost, this is the timeout to wait to re-aquire the target before +triggering FOLLP_FAIL_MODE + +## FOLLP_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. + +## FOLLP_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. + +## FOLLP_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. + +## FOLLP_ALT_OVR + +The follow logic can have the follow vehicle track the altitude of the target, but setting a value +in FOLLP_ALT_OVR allows the follow vehicle to follow at a fixed altitude regardless of the altitude +of the target. The FOLLP_ALT_OVR is in meters in FOLL_ALT_TYPE frame. + +## FOLLP_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. + +## FOLLP_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. + +## FOLLP_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. + +## FOLLP_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. + +## FOLLP_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. + +## FOLLP_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. + +## FOLLP_LKAHD + +Time to "lookahead" when calculating distance errors. + + +# Operation +Enable Lua scripting by setting `SCR_ENABLE = 1` on the FOLLOW plane. + +Install the plane_follow.lua script in the `APM/scripts` directory on the flight +controller's microSD card on the FOLLOW plane. s + +Install the speedpid.lua, mavlink_command_int.lua, mavlink_attitude.lua files +in the `APM/scripts/modules` directory on the SD card on the FOLLOW plane. + +Install the mavlink_msgs.lua files +in the `APM/scripts/modules/mavlink` 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. + +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. + +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/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index 2fe2f3ab4cbe5..95230406ea2ae 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -3907,28 +3907,38 @@ function precland:healthy() end -- desc follow = {} --- desc +-- get the SYSID_THISMAV of the target +---@return uint32_t_ud +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 with offsets added, and velocity (in NED) +---@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 +-- get distance vector to target (in meters) and target's velocity all in NED frame +---@return Vector3f_ud|nil -- distance NED +---@return Vector3f_ud|nil -- distance NED with offsets +---@return Vector3f_ud|nil -- velocity NED +function follow:get_target_dist_and_vel_NED_m() end + -- desc scripting = {} diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index 56a763a35f843..b7d0d4d0ba6a5 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -805,10 +805,12 @@ include AP_Follow/AP_Follow.h singleton AP_Follow depends AP_FOLLOW_ENABLED && (APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI||APM_BUILD_TYPE(APM_BUILD_Rover)) singleton AP_Follow rename follow singleton AP_Follow method have_target boolean +singleton AP_Follow method get_target_sysid uint32_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_dist_and_vel_NED_m boolean Vector3f'Null Vector3f'Null Vector3f'Null 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..1717bd7859317 --- /dev/null +++ b/libraries/AP_Scripting/modules/mavlink_attitude.lua @@ -0,0 +1,189 @@ +--[[ + + 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.7.0-009" +MAVLinkAttitude.SCRIPT_NAME = "MAVLink Attitude" +MAVLinkAttitude.SCRIPT_NAME_SHORT = "MAVATT" + +ATTITUDE_MESSAGE = "ATTITUDE" + +MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} + +--[[ + 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 = {} + local last_timestamp = millis():tofloat() + ATTITUDE_map.id = 30 + ATTITUDE_map.fields = { + { "time_boot_ms", ". + + PIDcontroller + A simple "PID" 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("speedpid.lua") + 3. create an instance - you may need to tune the gains + local speed_controller = speedpid.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 PIDcontroller = {} + +PIDcontroller.SCRIPT_VERSION = "4.7.0-001" +PIDcontroller.SCRIPT_NAME = "PID Controller" +PIDcontroller.SCRIPT_NAME_SHORT = "PID" + +PIDcontroller.MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} + +-- constrain a value between limits +function PIDcontroller.constrain(v, vmin, vmax) + if v < vmin then + v = vmin + end + if v > vmax then + v = vmax + end + return v +end + +function PIDcontroller.is_zero(x) + return math.abs(x) < 1.1920928955078125e-7 +end + +function PIDcontroller.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_us = 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 + + 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 (PIDcontroller.is_zero(cutoff_freq)) then + return 1.0 + end + if (PIDcontroller.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_us = micros() + if not _last_t_us then + _last_t_us = now_us + end + local dt = (now_us - _last_t_us):tofloat() * 0.000001 + _last_t_us = now_us + 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 = PIDcontroller.constrain(_I, -_iMax, iMax) + end + local I = _I + local ret = target + P + I + D + _target = target + _current = current + _P = P + + ret = PIDcontroller.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 PIDcontroller.speed_controller(kP_param, kI_param, kD_param, iMax, sMin, sMax) + local self = {} + local speedpid = PIDcontroller.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(PIDcontroller.MAV_SEVERITY.INFO, string.format("%s %s module loaded", PIDcontroller.SCRIPT_NAME, PIDcontroller.SCRIPT_VERSION) ) + +return PIDcontroller From d4bcc6fc56c1bed84b50eac2eeeb6e4bce678cbf Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Tue, 19 Aug 2025 10:34:07 -0600 Subject: [PATCH 3/3] AP_Follow: plane_follow.lua applet --- libraries/AP_Follow/AP_Follow.cpp | 77 ++++++++++++++++++++----------- libraries/AP_Follow/AP_Follow.h | 9 ++-- 2 files changed, 54 insertions(+), 32 deletions(-) diff --git a/libraries/AP_Follow/AP_Follow.cpp b/libraries/AP_Follow/AP_Follow.cpp index 1cd1573364e17..4b72303e1578d 100644 --- a/libraries/AP_Follow/AP_Follow.cpp +++ b/libraries/AP_Follow/AP_Follow.cpp @@ -45,21 +45,19 @@ extern const AP_HAL::HAL& hal; // Constants and Definitions //============================================================================== -#define AP_FOLLOW_TIMEOUT_MS 3000 // position estimate timeout +#define AP_FOLLOW_TIMEOUT 3 // position estimate timeout in seconds #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_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_DIST_MAX_DEFAULT 0 + #define AP_FOLLOW_ALT_TYPE_DEFAULT static_cast(Location::AltFrame::ABSOLUTE) + #define AP_FOLLOW_DIST_MAX_DEFAULT 0 // zero = ignored #else - #define AP_FOLLOW_ALT_TYPE_DEFAULT AP_FOLLOW_ALTITUDE_TYPE_RELATIVE + #define AP_FOLLOW_ALT_TYPE_DEFAULT static_cast(Location::AltFrame::ABOVE_HOME) #define AP_FOLLOW_DIST_MAX_DEFAULT 100 #endif @@ -93,9 +91,9 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = { // @Param: _DIST_MAX // @DisplayName: Follow distance maximum - // @Description: Follow distance maximum. If exceeded, the follow estimate will be considered invalid. + // @Description: Follow distance maximum. If exceeded, the follow estimate will be considered invalid. If zero there is no maximum. // @Units: m - // @Range: 1 1000 + // @Range: 0 1000 // @User: Standard AP_GROUPINFO("_DIST_MAX", 5, AP_Follow, _dist_max_m, AP_FOLLOW_DIST_MAX_DEFAULT), @@ -152,7 +150,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, 3:terrain // @User: Standard AP_GROUPINFO("_ALT_TYPE", 10, AP_Follow, _alt_type, AP_FOLLOW_ALT_TYPE_DEFAULT), #endif @@ -212,6 +210,12 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = { // @User: Advanced AP_GROUPINFO("_JERK_H", 17, AP_Follow, _jerk_max_h_degsss, 360.0), + // @Param: _TIMEOUT + // @DisplayName: Follow timeout + // @Description: Follow position update from lead - timeout after x seconds + // @User: Standard + // @Units: s + AP_GROUPINFO("_TIMEOUT", 18, AP_Follow, _timeout, AP_FOLLOW_TIMEOUT), AP_GROUPEND }; @@ -408,7 +412,7 @@ bool AP_Follow::get_heading_heading_rate_rad(float &heading_rad, float &heading_ return true; } -// Retrieves the target's estimated global location and velocity +// Retrieves the target's estimated global location and estimated velocity bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ned) { WITH_SEMAPHORE(_follow_sem); @@ -422,10 +426,12 @@ bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ne } vel_ned = _estimate_vel_ned_ms; - return true; + // The frame requested by FOLL_ALT_TYPE may not be the frame of location returned by ahrs. + // Make sure we give the caller the frame they have asked for. + return loc.change_alt_frame(_alt_type); } -// Retrieves the target's estimated global location and velocity, including configured offsets, for LUA bindings. +// Retrieves the target's estimated global location including configured offsets, and estimated velocity, for LUA bindings. bool AP_Follow::get_target_location_and_velocity_ofs(Location &loc, Vector3f &vel_ned) { WITH_SEMAPHORE(_follow_sem); @@ -547,7 +553,7 @@ bool AP_Follow::should_handle_message(const mavlink_message_t &msg) const // Checks whether the current estimate should be reset based on position and velocity errors. bool AP_Follow::estimate_error_too_large() const { - const float timeout_sec = AP_FOLLOW_TIMEOUT_MS * 0.001f; + const float timeout_sec = _timeout; // Calculate position thresholds based on maximum acceleration then deceleration for the timeout duration const float pos_thresh_horiz_m = _accel_max_ne_mss.get() * sq(timeout_sec * 0.5); @@ -615,15 +621,28 @@ bool AP_Follow::handle_global_position_int_message(const mavlink_message_t &msg) _target_location.lat = packet.lat; _target_location.lng = packet.lon; - // set target altitude based on configured altitude type - if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) { - // use relative altitude above home - _target_location.set_alt_cm(packet.relative_alt / 10, Location::AltFrame::ABOVE_HOME); - } else { - // use absolute altitude - _target_location.set_alt_cm(packet.alt / 10, Location::AltFrame::ABSOLUTE); + switch((Location::AltFrame)_alt_type) { + case Location::AltFrame::ABSOLUTE: + _target_location.set_alt_cm(packet.alt * 0.1, Location::AltFrame::ABSOLUTE); + break; + case Location::AltFrame::ABOVE_HOME: + _target_location.set_alt_cm(packet.relative_alt * 0.1, Location::AltFrame::ABOVE_HOME); + break; +#if APM_BUILD_TYPE(APM_BUILD_ArduPlane) || APM_BUILD_TYPE(APM_BUILD_ArduCopter) + case Location::AltFrame::ABOVE_TERRAIN: + /// Altitude comes in as AMSL + _target_location.set_alt_cm(packet.alt * 0.1, Location::AltFrame::ABSOLUTE); + // convert the incoming altitude to terrain altitude, but fail if there is no terrain data available + if (!_target_location.change_alt_frame(Location::AltFrame::ABOVE_TERRAIN)) { + return false; + }; + break; +#endif + default: + // don't process the packet if the _alt_type is invalid + return false; } - + // convert global location to local NED frame position if (!_target_location.get_vector_from_origin_NEU(_target_pos_ned_m)) { return false; @@ -885,12 +904,13 @@ void AP_Follow::Log_Write_FOLL() // @Field: VelD: Target velocity, Down (m/s) // @Field: LatE: Vehicle estimated latitude (degrees * 1E7) // @Field: LonE: Vehicle estimated longitude (degrees * 1E7) - // @Field: AltE: Vehicle estimated absolute altitude (centimeters) + // @Field: AltE: Vehicle estimated altitude (centimeters) + // @Field: FrmE: Vehicle estimated altitude Frame AP::logger().WriteStreaming("FOLL", - "TimeUS,Lat,Lon,Alt,VelN,VelE,VelD,LatE,LonE,AltE", // labels - "sDUmnnnDUm", // units - "F--B000--B", // mults - "QLLifffLLi", // fmt + "TimeUS,Lat,Lon,Alt,VelN,VelE,VelD,LatE,LonE,AltE,FrmE", // labels + "sDUmnnnDUm-", // units + "F--B000--B-", // mults + "QLLifffLLib", // fmt AP_HAL::micros64(), _target_location.lat, _target_location.lng, @@ -900,7 +920,8 @@ void AP_Follow::Log_Write_FOLL() (double)_target_vel_ned_ms.z, loc_estimate.lat, loc_estimate.lng, - loc_estimate.alt + loc_estimate.alt, + loc_estimate.get_alt_frame() ); } #endif // HAL_LOGGING_ENABLED @@ -918,7 +939,7 @@ 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 * 1000.0f))) { return false; } return true; diff --git a/libraries/AP_Follow/AP_Follow.h b/libraries/AP_Follow/AP_Follow.h index a4bbad8dac50c..24ee0be15c36f 100644 --- a/libraries/AP_Follow/AP_Follow.h +++ b/libraries/AP_Follow/AP_Follow.h @@ -99,7 +99,7 @@ class AP_Follow // Retrieves the estimated global location and velocity of the target bool get_target_location_and_velocity(Location &loc, Vector3f &vel_ned); - // Retrieves the estimated global location and velocity of the target, including configured positional offsets (for LUA bindings). + // Retrieves the estimated global location including configured positional offsets and velocity of the target, (for LUA bindings). bool get_target_location_and_velocity_ofs(Location &loc, Vector3f &vel_ned); // Retrieves the estimated target heading in degrees (0° = North, 90° = East) for LUA bindings. @@ -115,8 +115,8 @@ class AP_Follow // Accessor Methods //========================================================================== - // get target sysid - uint8_t get_target_sysid() const { return _sysid.get(); } + // get target sysid as a 32 bit number to allow for future expansion of MAV_SYSID + uint32_t get_target_sysid() const { return (uint32_t)_sysid.get(); } // get position controller. this controller is not used within this library but it is convenient to hold it here const AC_P& get_pos_p() const { return _p_pos; } @@ -199,9 +199,10 @@ class AP_Follow AP_Int8 _offset_type; // Offset frame type: 0 = NED, 1 = relative to lead vehicle heading AP_Vector3f _offset_m; // Offset from lead vehicle (meters), in NED or FRD frame depending on _offset_type AP_Int8 _yaw_behave; // Yaw behavior mode (see YawBehave enum) - AP_Int8 _alt_type; // Altitude reference: 0 = absolute, 1 = relative to home + AP_Enum _alt_type; // altitude source for follow mode AC_P _p_pos; // Position error P-controller for optional altitude following AP_Int16 _options; // Bitmask of follow behavior options (e.g., mount follow, etc.) + AP_Float _timeout; // position estimate timeout after x milliseconds AP_Float _accel_max_ne_mss; // Max horizontal acceleration for kinematic shaping (m/s²) AP_Float _jerk_max_ne_msss; // Max horizontal jerk for kinematic shaping (m/s³)