AP_Scripting: added mission_spiral.lua example

useful for creating very large missions with a distinctive pattern

libraries/AP_Scripting/examples/mission_spiral.lua

@@ -0,0 +1,141 @@
+--[[
+   create a mission of SCR_USER1 waypoints in a spiral pattern, for
+   testing large mission transfer
+
+   Runs on any change to SCR_USER1
+--]]
+
+local SCR_USER1 = Parameter("SCR_USER1") -- number of WPs
+local SCR_USER2 = Parameter("SCR_USER2") -- radius
+local SCR_USER3 = Parameter("SCR_USER3") -- center lat
+local SCR_USER4 = Parameter("SCR_USER4") -- center lon
+local SCR_USER5 = Parameter("SCR_USER5") -- altitude
+local SCR_USER6 = Parameter("SCR_USER6") -- number of WPs for spiral compare
+
+local last_SCR_USER1 = SCR_USER1:get()
+local last_SCR_USER6 = SCR_USER6:get()
+
+local NAV_WAYPOINT = 16
+local FRAME_GLOBAL = 3
+
+local ANGLE_INCREMENT = 5.0
+local MIN_RADIUS = 10.0
+
+--[[
+   create a waypoint
+--]]
+function create_WP(i, center, radius, angle)
+   local item = mavlink_mission_item_int_t()
+   local loc = center:copy()
+   loc:offset_bearing(radius, angle)
+
+   item:seq(i)
+   item:frame(FRAME_GLOBAL)
+   item:command(NAV_WAYPOINT)
+   item:param1(0)
+   item:param2(0)
+   item:param3(0)
+   item:param4(0)
+   item:x(loc:lat())
+   item:y(loc:lng())
+   item:z(100+loc:alt()*0.01)
+   return item
+end
+
+--[[
+   create a waypoint based on index
+--]]
+function create_WP_idx(center, idx)
+   local radius = SCR_USER2:get()
+   if radius < MIN_RADIUS then
+      radius = MIN_RADIUS
+   end
+   return create_WP(idx, center, radius+idx, idx*ANGLE_INCREMENT)
+end
+
+--[[
+   get center Location
+--]]
+function get_center()
+   -- if USER3 and USER4 set then use those, allowing autotest to control exact position
+   if SCR_USER3:get() ~= 0 and SCR_USER4:get() ~= 0 then
+      local loc = Location()
+      loc:lat(math.floor(SCR_USER3:get()*1.0e7))
+      loc:lng(math.floor(SCR_USER4:get()*1.0e7))
+      loc:alt(math.floor(SCR_USER5:get()*100))
+      return loc
+   end
+   return ahrs:get_location()
+end
+
+--[[
+   create an N point spiral mission
+--]]
+function create_spiral(N)
+   local center = get_center()
+   if not center then
+      gcs:send_text(0, "Error: Need AHRS location for spiral center")
+      return
+   end
+   mission:clear()
+
+   gcs:send_text(0, string.format("Creating spiral of size %u", N))
+
+   for i=0, N-1 do
+      local item = create_WP_idx(center, i)
+      if not mission:set_item(i, item) then
+         gcs:send_text(0, string.format("Failed to create WP %u", i))
+         N = i
+         break
+      end
+   end
+   gcs:send_text(0, string.format("Created spiral of size %u", N))
+end
+
+--[[
+   compare current mission with spiral, used in autotest
+--]]
+function compare_spiral(N)
+   local center = get_center()
+   if not center then
+      gcs:send_text(0, "Error: Need AHRS location for spiral center")
+      return
+   end
+   gcs:send_text(0, string.format("Comparing spiral of size %u", N))
+
+   -- start from index 1 as home can change
+   for i=1, N-1 do
+      local item1 = create_WP_idx(center, i)
+      local item2 = mission:get_item(i)
+      if not item2 then
+         gcs:send_text(0, string.format("Failed to fetch WP %u", i))
+         return
+      end
+      if item1:x() ~= item2:x() or item1:y() ~= item2:y() or item1:z() ~= item2:z() then
+         gcs:send_text(0, string.format("Compared fail %u (%.8f %.8f %.2f) (%.8f %.8f %.2f)", i,
+                                        item1:x()*1.0e-7,
+                                        item1:y()*1.0e-7,
+                                        item1:z(),
+                                        item2:x()*1.0e-7,
+                                        item2:y()*1.0e-7,
+                                        item2:z()))
+         return
+      end
+   end
+   gcs:send_text(0, string.format("Compared spiral of size %u OK", N))
+end
+
+function update()
+   if SCR_USER1:get() ~= last_SCR_USER1 and SCR_USER1:get() > 0 then
+      last_SCR_USER1 = SCR_USER1:get();
+      create_spiral(last_SCR_USER1);
+   end
+   if SCR_USER6:get() ~= last_SCR_USER6 and SCR_USER6:get() > 0 then
+      last_SCR_USER6 = SCR_USER6:get();
+      compare_spiral(last_SCR_USER6);
+   end
+   return update, 1000
+end
+
+gcs:send_text(0, "Loaded spiral mission creator")
+return update, 1000