AP_Scripting: add rover-MinFixType and rover-SaveTurns examples

3 years ago · f155c1b027
2 changed files with 348 additions and 0 deletions
--- a/libraries/AP_Scripting/examples/rover-MinFixType.lua
+++ b/libraries/AP_Scripting/examples/rover-MinFixType.lua
@ -0,0 +1,187 @@
				@@ -0,0 +1,187 @@
+--[[----------------------------------------------------------------------------
+
+MinFixType ArduPilot Lua script
+
+Checks for mission running and commands a hold/pause if the GPS fix type is less
+than a threshold value.
+
+CAUTION: This script is capable of engaging and disengaging autonomous control
+of a vehicle.  Use this script AT YOUR OWN RISK.
+
+-- Yuri -- Aug 2021, revised Apr 2022
+
+LICENSE - GNU GPLv3 https://www.gnu.org/licenses/gpl-3.0.en.html
+------------------------------------------------------------------------------]]
+
+local SCRIPT_NAME = 'MinFixType'
+
+--------  MAVLINK/AUTOPILOT 'CONSTANTS'  --------
+local ROVER_MODE_MANUAL       =  0
+local ROVER_MODE_HOLD         =  4
+local ROVER_MODE_AUTO         = 10
+local MAV_SEVERITY_WARNING    =  4
+local MAV_SEVERITY_INFO       =  6
+
+--------  USER EDITABLE GLOBALS  --------
+local GPS_INSTANCE = 0                 -- GPS to monitor (moving base, most likely)
+local MIN_FIX_TYPE = 6                 -- see table below
+local MSN_PAUSE_MODE     = ROVER_MODE_MANUAL  -- mode to command when GPS fix is inadequate
+local THR_SAFEGUARD_MODE = ROVER_MODE_HOLD    -- mode to command if mission paused with non-zero throttle
+local BAD_FIX_TIMEOUT  = 1600          -- how long a bad fix type must be present before pausing the mission
+local GOOD_FIX_TIMEOUT =  600          -- how long a good fix type must be present before resuming the mission
+local RUN_INTERVAL_MS  =  200          -- (ms) how often to run this script (50-250 should work fine)
+local VERBOSE_MODE     =    2  -- 0 to suppress all GCS messages,
+                               -- 1 for normal status messages
+                               -- 2 for additional debug messages
+local MSG_NORMAL = 1
+local MSG_DEBUG  = 2
+
+local FIX_TYPES = {
+    [0] = 'No GPS',  -- Lua arrays are 1 based unless you specify discrete indices like this
+    [1] = 'No Fix',
+    [2] = '2D Fix',
+    [3] = '3D Fix',
+    [4] = 'DGPS Fix',
+    [5] = 'RTK Float',
+    [6] = 'RTK Fixed',
+    [7] = 'Static Fixed',
+    [8] = 'PPP, 3D'}
+
+local MODE_THRESHOLDS = {1231, 1361, 1491, 1621, 1750, 2050}
+
+local MODE_CH  = param:get('MODE_CH')
+local THR_CH   = param:get('RCMAP_THROTTLE')
+local THR_TRIM = param:get(string.format('RC%d_TRIM', THR_CH))
+local THR_DZ   = param:get(string.format('RC%d_DZ', THR_CH))
+
+-- 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 msg_type <= VERBOSE_MODE then
+        gcs:send_text(severity, string.format('%s: %s', SCRIPT_NAME, txt))
+    end
+end
+
+-- return RC transmitter selected mode
+local function get_user_mode()
+    local pwm = rc:get_pwm(MODE_CH)
+    local mode_num = 6
+    for i, threshold in pairs(MODE_THRESHOLDS) do
+        if (pwm < threshold) then
+            mode_num = i
+            break
+        end
+    end
+    return tonumber(param:get('MODE' .. mode_num))
+end
+
+local function get_pause_mode()
+    if math.abs(rc:get_pwm(THR_CH) - THR_TRIM) > THR_DZ then
+        return THR_SAFEGUARD_MODE
+    end
+    return MSN_PAUSE_MODE
+end
+
+function resume_mission()
+    if get_user_mode() ~= ROVER_MODE_AUTO then
+        gcs_msg(MSG_NORMAL, MAV_SEVERITY_WARNING, 'Pause Canceled - Mode Change')
+        return standby, RUN_INTERVAL_MS
+    end
+    if not arming:is_armed() then
+        gcs_msg(MSG_NORMAL, MAV_SEVERITY_WARNING, 'Pause Canceled - Disarmed')
+        return standby, RUN_INTERVAL_MS
+    end
+
+    if gps:status(GPS_INSTANCE) < MIN_FIX_TYPE then
+        return mission_paused, RUN_INTERVAL_MS
+    end
+
+    vehicle:set_mode(ROVER_MODE_AUTO)
+    gcs_msg('Mission Resumed')
+    return monitor, RUN_INTERVAL_MS
+end
+
+function mission_paused()
+    if get_user_mode() ~= ROVER_MODE_AUTO then
+        gcs_msg(MSG_NORMAL, MAV_SEVERITY_WARNING, 'Pause Canceled - Mode Change')
+        return standby, RUN_INTERVAL_MS
+    end
+    if not arming:is_armed() then
+        gcs_msg(MSG_NORMAL, MAV_SEVERITY_WARNING, 'Pause Canceled - Disarmed')
+        return standby, RUN_INTERVAL_MS
+    end
+
+    local fix_type = gps:status(GPS_INSTANCE)
+    if fix_type >= MIN_FIX_TYPE then
+        return resume_mission, GOOD_FIX_TIMEOUT
+    end
+
+    if vehicle:get_mode() == THR_SAFEGUARD_MODE then
+        local mode = get_pause_mode()
+        if mode == MSN_PAUSE_MODE then
+            gcs_msg(MSG_NORMAL, MAV_SEVERITY_WARNING, 'Throttle neutral')
+            vehicle:set_mode(mode)
+        end
+        return mission_paused, RUN_INTERVAL_MS
+    end
+
+    -- for edge cases where a non-RC command to resume was issued but fix type is still unsuitable
+    if vehicle:get_mode() ~= MSN_PAUSE_MODE then
+        gcs_msg(MSG_NORMAL, MAV_SEVERITY_WARNING, 'Cannot resume - ' .. FIX_TYPES[fix_type])
+        vehicle:set_mode(get_pause_mode())
+    end
+    return mission_paused, RUN_INTERVAL_MS
+end
+
+function transition_to_pause()
+    if mission:state() ~= mission.MISSION_RUNNING then
+        return standby, RUN_INTERVAL_MS
+    end
+
+    local fix_type = gps:status(GPS_INSTANCE)
+
+    if fix_type >= MIN_FIX_TYPE then
+        return monitor, RUN_INTERVAL_MS
+    end
+
+    local mode = get_pause_mode()
+    if mode == THR_SAFEGUARD_MODE then
+        gcs_msg(MSG_NORMAL, MAV_SEVERITY_WARNING, 'Throttle not neutral!')
+    end
+
+    vehicle:set_mode(mode)
+    gcs_msg(MSG_NORMAL, MAV_SEVERITY_WARNING, 'Mission Paused - ' .. FIX_TYPES[fix_type])
+
+    return mission_paused, RUN_INTERVAL_MS
+end
+
+function monitor()  -- monitor for reduced GPS fix state during auto mission
+    if mission:state() ~= mission.MISSION_RUNNING then
+        return standby, RUN_INTERVAL_MS
+    end
+
+    local fix_type = gps:status(GPS_INSTANCE)
+
+    if fix_type < MIN_FIX_TYPE then
+        gcs_msg(MSG_DEBUG, MAV_SEVERITY_WARNING, 'GPS ' .. (GPS_INSTANCE + 1) .. ' - ' .. FIX_TYPES[fix_type])
+        return transition_to_pause, BAD_FIX_TIMEOUT
+    end
+
+    return monitor, RUN_INTERVAL_MS
+end
+
+function standby()  -- wait here until an auto mission is active
+    if mission:state() == mission.MISSION_RUNNING then
+        return monitor, RUN_INTERVAL_MS
+    end
+    return standby, RUN_INTERVAL_MS
+end
+
+gcs_msg('Script active')
+
+return standby()
--- a/libraries/AP_Scripting/examples/rover-SaveTurns.lua
+++ b/libraries/AP_Scripting/examples/rover-SaveTurns.lua
@ -0,0 +1,161 @@
				@@ -0,0 +1,161 @@
+--[[----------------------------------------------------------------------------
+
+SaveTurns ArduPilot Lua script
+
+Saves the locations of vehicle turns as waypoints in the current nav mission.
+
+CAUTION: This script is capable of engaging and disengaging autonomous control
+of a vehicle.  Use this script AT YOUR OWN RISK.
+
+-- Yuri -- Nov 2021, revised Apr 2022
+
+LICENSE - GNU GPLv3 https://www.gnu.org/licenses/gpl-3.0.en.html
+------------------------------------------------------------------------------]]
+
+local SCRIPT_NAME = 'SaveTurns'
+
+--------  USER EDITABLE GLOBALS  --------
+local RC_OPTION       = 300  -- RC option number for switched control (300, 301, etc)
+local MIN_DIST        = 2.0  -- (m)   min distance between waypoints
+local HDG_DELTA       = 8.0  -- (deg) save wp after heading change of this magnitude
+local RUN_INTERVAL_MS = 200  -- (ms)  how often to run this script
+local VERBOSE_MODE    =   2  -- 0 to suppress all GCS messages,
+                             -- 1 for normal status messages
+                             -- 2 for additional GPS/debug messages
+
+--------  MAVLINK/AUTOPILOT 'CONSTANTS'  --------
+local ROVER_MODE_AUTO = 10
+local STANDBY         =  0
+local SAVE_WPS        =  1
+local CLEAR_WPS       =  2
+local WAYPOINT        = 16   -- waypoint command
+local MAV_SEVERITY_WARNING = 4
+local MAV_SEVERITY_INFO    = 6
+local MSG_NORMAL           = 1
+local MSG_DEBUG            = 2
+
+local RC_CHAN = rc:find_channel_for_option(RC_OPTION)
+local last_wp = Location()
+local last_yaw = 999.0
+
+-- 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 msg_type <= VERBOSE_MODE then
+        gcs:send_text(severity, string.format('%s: %s', SCRIPT_NAME, txt))
+    end
+end
+
+local function yaw_diff(yaw1, yaw2)
+    --https://stackoverflow.com/questions/5024375/getting-the-difference-between-two-headings
+    return math.abs((yaw2 - yaw1 + 540.0) % 360.0 - 180.0)
+end
+
+local function new_mission()
+    local home = ahrs:get_home()
+    local item = mavlink_mission_item_int_t()
+
+    mission:clear()
+
+    item:command(WAYPOINT)
+    item:x(home:lat())
+    item:y(home:lng())
+    item:z(home:alt())
+
+    if not mission:set_item(0, item) then
+        gcs_msg(MSG_NORMAL, MAV_SEVERITY_WARNING, 'Failed to create new mission')
+        return false
+    end
+
+    return true
+end
+
+local function save_wp(position, index)
+    local item = mavlink_mission_item_int_t()
+
+    if (not position) then
+		gcs_msg(MSG_NORMAL, MAV_SEVERITY_WARNING, string.format('WP %d - invalid position', index))
+        return false
+    end
+
+    item:command(WAYPOINT)
+    item:x(position:lat())
+    item:y(position:lng())
+    item:z(0)
+
+    if not mission:set_item(index, item) then
+        gcs_msg(MSG_NORMAL, MAV_SEVERITY_WARNING, string.format('WP %d - failed to save', index))
+        return false
+    end
+
+    return true
+end
+
+function collect_breadcrumbs()
+    if vehicle:get_mode() == ROVER_MODE_AUTO then
+        gcs_msg('AUTO mode - saving canceled')
+        return standby, RUN_INTERVAL_MS
+    end
+    local sw_pos = RC_CHAN:get_aux_switch_pos()
+    if sw_pos ~= SAVE_WPS then return standby, RUN_INTERVAL_MS end
+
+    if not ahrs:healthy() then return collect_breadcrumbs, RUN_INTERVAL_MS end
+
+    local cur_pos = ahrs:get_location()
+    local cur_yaw = ahrs:get_yaw() * 180.0 / math.pi
+
+    if cur_pos:get_distance(last_wp) < MIN_DIST then
+        last_yaw = cur_yaw
+        return collect_breadcrumbs, RUN_INTERVAL_MS
+    end
+
+    if yaw_diff(cur_yaw, last_yaw) > HDG_DELTA then
+        local idx = mission:num_commands()
+        if save_wp(cur_pos, idx) then
+            gcs_msg(string.format('WP %d -  saved', idx))
+            last_wp = cur_pos
+            last_yaw = cur_yaw
+        end
+    end
+
+    return collect_breadcrumbs, RUN_INTERVAL_MS
+end
+
+function await_switch_change()
+    local sw_pos = RC_CHAN:get_aux_switch_pos()
+    if sw_pos == STANDBY then return standby, RUN_INTERVAL_MS end
+    if sw_pos == SAVE_WPS then return collect_breadcrumbs, RUN_INTERVAL_MS end
+    return await_switch_change, RUN_INTERVAL_MS
+end
+
+function standby()
+    if vehicle:get_mode() == ROVER_MODE_AUTO then return standby, RUN_INTERVAL_MS end
+    local sw_pos = RC_CHAN:get_aux_switch_pos()
+    if sw_pos == SAVE_WPS then
+        last_wp = Location()
+        last_yaw = 999.0
+        return collect_breadcrumbs, RUN_INTERVAL_MS
+    end
+    if sw_pos == CLEAR_WPS then
+        if new_mission() then
+            gcs_msg(MSG_NORMAL, MAV_SEVERITY_WARNING, 'Mission cleared')
+        end
+        return await_switch_change, RUN_INTERVAL_MS
+    end
+    return standby, RUN_INTERVAL_MS
+end
+
+function initialize()
+    if not ahrs:healthy() then return initialize, RUN_INTERVAL_MS end
+    gcs_msg('Ready')
+    return standby, RUN_INTERVAL_MS
+end
+
+gcs_msg('Awaiting AHRS initialization...')
+
+return initialize()