Update MissionRotation.lua

- for each loaded mission it also shows the number of events it contains
- cosmetic
This commit is contained in:
Marco Robustini 2024-12-06 18:35:08 +01:00 committed by GitHub
parent 7c8f1f5373
commit d707b7675a
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194

View File

@ -3,28 +3,29 @@
-- Reset the mission0.txt if AUX is high for more than 3 seconds -- Reset the mission0.txt if AUX is high for more than 3 seconds
-- Prevents mission change if the vehicle is in AUTO mode -- Prevents mission change if the vehicle is in AUTO mode
-- Prevents the script from loading if the vehicle is in AUTO -- Prevents the script from loading if the vehicle is in AUTO
-- For each loaded mission it also shows the number of events it contains
local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7}
local rc_switch = rc:find_channel_for_option(24) local rc_switch = rc:find_channel_for_option(24)
if not rc_switch then if not rc_switch then
gcs:send_text(MAV_SEVERITY.ERROR, "Mission Reset switch not assigned") gcs:send_text(MAV_SEVERITY.ERROR, "Mission Reset switch not right assigned")
return return
end end
local vehicle_type
local vehicle_fw_type = FWVersion:type() local vehicle_fw_type = FWVersion:type()
local vehicle_type if vehicle_fw_type == 3 then
if vehicle_fw_type == 1 then
vehicle_type = "Rover"
elseif vehicle_fw_type == 2 then
vehicle_type = "Copter"
elseif vehicle_fw_type == 3 then
vehicle_type = "Plane" vehicle_type = "Plane"
elseif vehicle_fw_type == 2 then
vehicle_type = "Rover"
elseif vehicle_fw_type == 4 then elseif vehicle_fw_type == 4 then
vehicle_type = "Tracker" vehicle_type = "Copter"
elseif vehicle_fw_type == 5 then elseif vehicle_fw_type == 5 then
vehicle_type = "Submarine" vehicle_type = "Submarine"
elseif vehicle_fw_type == 10 then
vehicle_type = "Tracker"
else else
gcs:send_text(MAV_SEVERITY.ERROR, "Unrecognized vehicle type!") gcs:send_text(MAV_SEVERITY.ERROR, "Unrecognized vehicle type!")
return return
@ -41,8 +42,18 @@ elseif vehicle_type == "Rover" then
mode_auto = 10 mode_auto = 10
elseif vehicle_type == "Submarine" then elseif vehicle_type == "Submarine" then
mode_auto = 10 mode_auto = 10
elseif vehicle_type == "Helicopter" then
mode_auto = 3
elseif vehicle_type == "Tracker" then elseif vehicle_type == "Tracker" then
mode_auto = 10 mode_auto = 10
elseif vehicle_type == "Sailboat" then
mode_auto = 3
elseif vehicle_type == "Car" then
mode_auto = 10
elseif vehicle_type == "Boat" then
mode_auto = 3
elseif vehicle_type == "VTOL" then
mode_auto = 10
else else
return return
end end
@ -74,39 +85,45 @@ local function read_mission(file_name)
local item = mavlink_mission_item_int_t() local item = mavlink_mission_item_int_t()
local index = 0 local index = 0
local last_event = nil
while true do while true do
local line = file:read('l')
if not line then break end
if string.match(line, '^%s*$') == nil then
last_event = line
end
local data = {} local data = {}
for i = 1, 12 do for number in string.gmatch(line, "[^%s]+") do
data[i] = file:read('n') table.insert(data, tonumber(number))
if data[i] == nil then end
if i == 1 then if #data >= 12 then
file:close() item:seq(data[1])
return true item:frame(data[3])
else item:command(data[4])
mission:clear() item:param1(data[5])
error('Failed to read file: premature end of data') item:param2(data[6])
end item:param3(data[7])
item:param4(data[8])
item:x(data[9] * 10^7)
item:y(data[10] * 10^7)
item:z(data[11])
if not mission:set_item(index, item) then
mission:clear()
error(string.format('Failed to set mission item %i', index))
end end
index = index + 1
end end
item:seq(data[1])
item:frame(data[3])
item:command(data[4])
item:param1(data[5])
item:param2(data[6])
item:param3(data[7])
item:param4(data[8])
item:x(data[9] * 10^7)
item:y(data[10] * 10^7)
item:z(data[11])
if not mission:set_item(index, item) then
mission:clear()
error(string.format('Failed to set mission item %i', index))
end
index = index + 1
end end
file:close()
if last_event then
local first_number = tonumber(string.match(last_event, "^%d+"))
gcs:send_text(MAV_SEVERITY.WARNING, "Loaded " .. file_name .. " with " .. tostring(first_number) .. " events")
else
gcs:send_text(MAV_SEVERITY.WARNING, "Loaded " .. file_name .. " with 0 events")
end
return true
end end
if not read_mission("mission0.txt") then if not read_mission("mission0.txt") then
@ -128,7 +145,6 @@ local function load_next_mission()
if file then if file then
file:close() file:close()
if read_mission(file_name) then if read_mission(file_name) then
gcs:send_text(MAV_SEVERITY.WARNING, "Loaded mission: " .. file_name)
return return
end end
end end
@ -163,5 +179,4 @@ function update()
end end
gcs:send_text(MAV_SEVERITY.NOTICE, "Mission Rotation loaded") gcs:send_text(MAV_SEVERITY.NOTICE, "Mission Rotation loaded")
gcs:send_text(MAV_SEVERITY.NOTICE, "Loaded default mission: mission0.txt")
return update, 1000 return update, 1000