Skip to content

Commit

Permalink
AP_Scripting: make min speed configurable in rover-quicktune
Browse files Browse the repository at this point in the history
  • Loading branch information
stephendade committed Dec 20, 2024
1 parent ac2ef4c commit f1aa3a3
Show file tree
Hide file tree
Showing 2 changed files with 20 additions and 4 deletions.
17 changes: 13 additions & 4 deletions libraries/AP_Scripting/applets/rover-quicktune.lua
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,7 @@ local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTI

local PARAM_TABLE_KEY = 15
local PARAM_TABLE_PREFIX = "RTUN_"
local PARAM_TABLE_SIZE = 11
local PARAM_TABLE_SIZE = 12

-- bind a parameter to a variable
function bind_param(name)
Expand Down Expand Up @@ -143,6 +143,16 @@ local RTUN_AUTO_SAVE = bind_add_param('AUTO_SAVE', 10, 5)
--]]
local RTUN_RC_FUNC = bind_add_param('RC_FUNC', 11, 300)

--[[
// @Param: RTUN_SPEED_MIN
// @DisplayName: Rover Quicktune minimum speed for tuning
// @Description: The mimimum speed in m/s required for tuning to start
// @Units: m/s
// @Range: 0.1 0.5
// @User: Standard
--]]
local SPEED_FF_SPEED_MIN = bind_add_param('SPEED_MIN', 12, 0.5)

-- other vehicle parameters used by this script
local INS_GYRO_FILTER = bind_param("INS_GYRO_FILTER")
local GCS_PID_MASK = bind_param("GCS_PID_MASK")
Expand All @@ -159,7 +169,6 @@ local FLTD_MUL = 0.5 -- ATC_STR_RAT_FLTD set to 0.5 * INS_GYRO_FI
local FLTT_MUL = 0.5 -- ATC_STR_RAT_FLTT set to 0.5 * INS_GYRO_FILTER
local STR_RAT_FF_TURNRATE_MIN = math.rad(10) -- steering rate feedforward min vehicle turn rate (in radians/sec)
local STR_RAT_FF_STEERING_MIN = 0.10 -- steering rate feedforward min steering output (in the range 0 to 1)
local SPEED_FF_SPEED_MIN = 0.5 -- speed feedforward minimum vehicle speed (in m/s)
local SPEED_FF_THROTTLE_MIN = 0.20 -- speed feedforward requires throttle output (in the range 0 to 1)

-- get time in seconds since boot
Expand Down Expand Up @@ -458,7 +467,7 @@ function update_speed_ff(ff_pname)

-- check throttle and speed
local throttle_ok = throttle_out >= SPEED_FF_THROTTLE_MIN
local speed_ok = speed > SPEED_FF_SPEED_MIN
local speed_ok = speed > SPEED_FF_SPEED_MIN:get()
if (throttle_ok and speed_ok) then
ff_throttle_sum = ff_throttle_sum + throttle_out
ff_speed_sum = ff_speed_sum + speed
Expand All @@ -471,7 +480,7 @@ function update_speed_ff(ff_pname)
if not throttle_ok then
gcs:send_text(MAV_SEVERITY.WARNING, string.format("RTun: increase throttle (%d < %d)", math.floor(throttle_out * 100), math.floor(SPEED_FF_THROTTLE_MIN * 100)))
elseif not speed_ok then
gcs:send_text(MAV_SEVERITY.WARNING, string.format("RTun: increase speed (%3.1f < %3.1f)", speed, SPEED_FF_SPEED_MIN))
gcs:send_text(MAV_SEVERITY.WARNING, string.format("RTun: increase speed (%3.1f < %3.1f)", speed, SPEED_FF_SPEED_MIN:get()))
end
end
end
Expand Down
7 changes: 7 additions & 0 deletions libraries/AP_Scripting/applets/rover-quicktune.md
Original file line number Diff line number Diff line change
Expand Up @@ -60,6 +60,8 @@ If you move the switch to the low position at any time in the tune before gains

If the pilot gives steering or throttle input during tuning then tuning is paused for 4 seconds. Tuning restarts once the pilot returns to the input to the neutral position.

If the vehicle (small vehicles in particular) is not able to turn correctly to enter or track the circle, the ``PSC_VEl`` parameters may be to be increased. For example ``PSC_VEL_FF`` to 1 and ``PSC_VEL_P`` to 8.

# Parameters

The script has the following parameters to configure its behaviour
Expand Down Expand Up @@ -120,3 +122,8 @@ completes unless the pilot move the RC switch low to revert the tune.
Setting this to a non-zero value allows you to use quicktune with a 2-position
switch, with the switch settings as low and mid positions. A zero
value disables auto-save and you need to have a 3 position switch.

## RTUN_SPEED_MIN

The minimum speed at which tuning will occur. The vehicle must be able to
run in Circle mode at this speed or greater.

0 comments on commit f1aa3a3

Please sign in to comment.