From 6a74be104f6a4dabf700862974762bdfb14151f1 Mon Sep 17 00:00:00 2001 From: Yuri Date: Thu, 30 Jun 2022 21:47:45 -0500 Subject: [PATCH] AP_Scripting: add set_desired_speed binding --- libraries/AP_Scripting/docs/docs.lua | 5 + .../examples/rover-TerrainDetector.lua | 209 ++++++++++++++++++ .../generator/description/bindings.desc | 1 + 3 files changed, 215 insertions(+) create mode 100644 libraries/AP_Scripting/examples/rover-TerrainDetector.lua diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index 1baf400744..f56df7641a 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -1433,6 +1433,11 @@ function vehicle:set_velocity_match(param1) end ---@return boolean function vehicle:nav_scripting_enable(param1) end +-- desc sets autopilot nav speed (Copter and Rover) +---@param param1 number +---@return boolean +function vehicle:set_desired_speed(param1) end + -- desc ---@param param1 number ---@param param2 number diff --git a/libraries/AP_Scripting/examples/rover-TerrainDetector.lua b/libraries/AP_Scripting/examples/rover-TerrainDetector.lua new file mode 100644 index 0000000000..7ba8646821 --- /dev/null +++ b/libraries/AP_Scripting/examples/rover-TerrainDetector.lua @@ -0,0 +1,209 @@ +--[[---------------------------------------------------------------------------- + +TerrainDetector ArduPilot Lua script + +Uses gyro and accelerometer data to detect rough terrain and slow the +vehicle's speed accordingly during auto missions. + +Provides a set of custom tuning parameters: +ROUGH_SPEED : (m/s) rough terrain desired speed, set to -1 to disable detection +ROUGH_TIMEOUT_MS: (ms) min time to remain slowed down for rough terrain +ROUGH_GZ_MAX : (G) slow down on Gz impulses greater than this value +ROUGH_RATE_MAX : (deg/s) slow down on rate transients greater than this value + +These gain values are used for smooth terrain detection. +Smaller values (less than 1.0) are more restrictive about resuming normal speed. +ROUGH_GZ_GAIN : Gz impulse gain +ROUGH_RATE_GAIN : gyro transient rate gain + +CAUTION: This script is capable of engaging and disengaging autonomous control +of a vehicle. Use this script AT YOUR OWN RISK. + +-- Yuri -- Jul 2022 + +LICENSE - GNU GPLv3 https://www.gnu.org/licenses/gpl-3.0.en.html + +Concept first presented during this live stream: +https://www.youtube.com/watch?v=UdXGXjigxAo&t=7155s +Credit to @ktrussell for the idea and discussion! +------------------------------------------------------------------------------]] + +local SCRIPT_NAME = 'TerrainDetector' +local RUN_INTERVAL_MS = 25 -- needs to be pretty fast for good detection +local SBY_INTERVAL_MS = 500 -- slower interval when detection is disabled +local PARAM_TABLE_KEY = 117 -- unique index value between 0 and 200 + +-- GCS messages +local VERBOSE_MODE = 1 -- 0 to suppress all GCS messages, + -- 1 for normal status messages + -- 2 for additional debug messages +local MSG_NORMAL = 1 +local MSG_DEBUG = 2 + +-- MAVLink values +local MAV_SEVERITY_WARNING = 4 +local MAV_SEVERITY_INFO = 6 + +-- mathematical/physical constants +local G = -9.81 -- m/s/s + +-- create custom parameter set +local function add_params(key, prefix, tbl) + assert(param:add_table(key, prefix, #tbl), string.format('Could not add %s param table.', prefix)) + for num = 1, #tbl do + assert(param:add_param(key, num, tbl[num][1], tbl[num][2]), string.format('Could not add %s%s.', prefix, tbl[num][1])) + end +end + +add_params(PARAM_TABLE_KEY, 'ROUGH_', { + -- { name, default value }, + { 'SPEED', 0.7 }, + { 'GZ_MAX', 1.33 }, + { 'RATE_MAX', 28 }, + { 'GZ_GAIN', 0.9 }, + { 'RATE_GAIN', 0.8 }, + { 'TIMEOUT_MS', 7500 } + }) + +-- wrapper for gcs:send_text() +local function gcs_msg(msg_type, severity, txt) + if type(msg_type) == 'string' then + -- allow just a string to be passed for simple/routine messages + txt = msg_type + msg_type = MSG_NORMAL + severity = MAV_SEVERITY_INFO + end + if type(severity) == 'string' then + -- allow just severity and string to be passed for normal messages + txt = severity + severity = msg_type + msg_type = MSG_NORMAL + end + if msg_type <= VERBOSE_MODE then + gcs:send_text(severity, string.format('%s: %s', SCRIPT_NAME, txt)) + end +end + +local last_g_z = 0 -- to calculate impulse Gz +local timeout_ms = 0 + +-- debug values +local max_g_z = 0 +local max_gyro_rate = 0 + +local wp_speed_normal = nil +local wp_speed_rough = nil +local impulse_gz_threshold = nil +local gyro_rate_threshold = nil +local rough_terrain_timeout_ms = nil +local impulse_gain = nil +local gyro_gain = nil + +function standby() + wp_speed_rough = param:get('ROUGH_SPEED') + if wp_speed_rough < 0 then return standby, SBY_INTERVAL_MS end + + if mission:state() == mission.MISSION_RUNNING then + -- only poll remaining parameters at start of mission + wp_speed_normal = param:get('WP_SPEED') + impulse_gz_threshold = param:get('ROUGH_GZ_MAX') + gyro_rate_threshold = param:get('ROUGH_RATE_MAX') + rough_terrain_timeout_ms = param:get('ROUGH_TIMEOUT_MS') + impulse_gain = param:get('ROUGH_GZ_GAIN') + gyro_gain = param:get('ROUGH_RATE_GAIN') + -- if ROUGH_SPEED param not set or invalid, use half of WP_SPEED + if wp_speed_rough == 0 or wp_speed_rough > wp_speed_normal then + wp_speed_rough = wp_speed_normal / 2 + gcs_msg(MAV_SEVERITY_WARNING, 'ROUGH_SPEED invalid, using half WP_SPEED') + end + + last_g_z = 0 + return do_normal_speed, RUN_INTERVAL_MS + end + return standby, SBY_INTERVAL_MS +end + +function initiate_rough_speed() + gcs_msg(MSG_NORMAL, MAV_SEVERITY_WARNING, 'Slowing for rough terrain') + vehicle:set_desired_speed(wp_speed_rough) + last_g_z = 0 + timeout_ms = millis() + rough_terrain_timeout_ms + max_g_z = 0 + max_gyro_rate = 0 + return do_rough_speed, RUN_INTERVAL_MS +end + +function initiate_normal_speed() + gcs_msg(MSG_NORMAL, MAV_SEVERITY_WARNING, 'Resuming normal speed') + vehicle:set_desired_speed(wp_speed_normal) + last_g_z = 0 + return do_normal_speed, RUN_INTERVAL_MS +end + +function do_normal_speed() + if mission:state() ~= mission.MISSION_RUNNING then + return standby, SBY_INTERVAL_MS + end + + local g_z = ahrs:get_accel():z() / G -- convert to G from m/s/s + local gyro = ahrs:get_gyro() + -- sample max of roll/pitch rates + local gyro_rate = math.deg(math.max(math.abs(gyro:x()), math.abs(gyro:y()))) + local impulse_g_z = math.abs(g_z - last_g_z) -- sample delta is the measured impulse + last_g_z = g_z + + if impulse_g_z > impulse_gz_threshold then -- slow down + gcs_msg(MSG_DEBUG, MAV_SEVERITY_INFO, string.format('Impulse - %.2f Gz', impulse_g_z)) + return initiate_rough_speed, RUN_INTERVAL_MS + end + + if gyro_rate > gyro_rate_threshold then -- slow down + gcs_msg(MSG_DEBUG, MAV_SEVERITY_INFO, string.format('Transient - %.2f deg/s', gyro_rate)) + return initiate_rough_speed, RUN_INTERVAL_MS + end + + return do_normal_speed, RUN_INTERVAL_MS +end + +function do_rough_speed() + if mission:state() ~= mission.MISSION_RUNNING then + return standby, SBY_INTERVAL_MS + end + + local g_z = ahrs:get_accel():z() / G -- convert to G from m/s/s + local gyro = ahrs:get_gyro() + -- sample max of roll/pitch rates + local gyro_rate = math.deg(math.max(math.abs(gyro:x()), math.abs(gyro:y()))) + local impulse_g_z = math.abs(g_z - last_g_z) -- sample delta is the measured impulse + last_g_z = g_z + + max_g_z = math.max(g_z, max_g_z) + max_gyro_rate = math.max(gyro_rate, max_gyro_rate) + + local now = millis() + + if impulse_g_z > impulse_gz_threshold * impulse_gain then -- stay slow + gcs_msg(MSG_DEBUG, MAV_SEVERITY_INFO, string.format('Timer reset - %.2fGz', impulse_g_z)) + max_g_z = 0 + timeout_ms = now + rough_terrain_timeout_ms + return do_rough_speed, RUN_INTERVAL_MS + end + + if gyro_rate > gyro_rate_threshold * gyro_gain then -- stay slow + gcs_msg(MSG_DEBUG, MAV_SEVERITY_INFO, string.format('Timer reset - %.2f deg/s', gyro_rate)) + max_gyro_rate = 0 + timeout_ms = now + rough_terrain_timeout_ms + return do_rough_speed, RUN_INTERVAL_MS + end + + if now > timeout_ms then -- no longer in rough terrain + gcs_msg(MSG_DEBUG, MAV_SEVERITY_INFO, string.format('Max - %.2fGz ... %.2f deg/s', max_g_z, max_gyro_rate)) + return initiate_normal_speed, RUN_INTERVAL_MS + end + + return do_rough_speed, RUN_INTERVAL_MS +end + +gcs_msg('Script active') + +return standby, SBY_INTERVAL_MS diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index 5753a32142..839fa20c8a 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -234,6 +234,7 @@ singleton AP_Vehicle method nav_script_time boolean uint16_t'Null uint8_t'Null f singleton AP_Vehicle method nav_script_time_done void uint16_t'skip_check singleton AP_Vehicle method set_target_throttle_rate_rpy void float -100 100 float'skip_check float'skip_check float'skip_check singleton AP_Vehicle method set_desired_turn_rate_and_speed boolean float'skip_check float'skip_check +singleton AP_Vehicle method set_desired_speed boolean float'skip_check singleton AP_Vehicle method nav_scripting_enable boolean uint8_t 0 UINT8_MAX singleton AP_Vehicle method set_velocity_match boolean Vector2f singleton AP_Vehicle method has_ekf_failsafed boolean