mirror of https://github.com/ArduPilot/ardupilot
AP_Scripting: add rover-MinFixType and rover-SaveTurns examples
This commit is contained in:
Yuri
Randy Mackay
parent
eefc41fe30
commit
f155c1b027
|
|
@ -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()
|
||||
|
|
@ -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()
|
||||
Loading…
Reference in New Issue