Yuri
3 years ago
committed by
Randy Mackay
2 changed files with 348 additions and 0 deletions
@ -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