diff --git a/libraries/AP_Scripting/examples/mission_spiral.lua b/libraries/AP_Scripting/examples/mission_spiral.lua new file mode 100644 index 0000000000..eab6a20426 --- /dev/null +++ b/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