diff --git a/libraries/AP_Scripting/examples/plane_guided_follow.lua b/libraries/AP_Scripting/examples/plane_guided_follow.lua new file mode 100644 index 0000000000..6223b39d93 --- /dev/null +++ b/libraries/AP_Scripting/examples/plane_guided_follow.lua @@ -0,0 +1,149 @@ +-- support follow in GUIDED mode in plane + +local PARAM_TABLE_KEY = 11 +local PARAM_TABLE_PREFIX = "GFOLL_" + +local MODE_MANUAL = 0 +local MODE_GUIDED = 15 + +local ALT_FRAME_ABSOLUTE = 0 + +-- bind a parameter to a variable +function bind_param(name) + local p = Parameter() + assert(p:init(name), string.format('could not find %s parameter', name)) + return p +end + +-- add a parameter and bind it to a variable +function bind_add_param(name, idx, default_value) + assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), string.format('could not add param %s', name)) + return bind_param(PARAM_TABLE_PREFIX .. name) +end + +-- setup SHIP specific parameters +assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 3), 'could not add param table') +GFOLL_ENABLE = bind_add_param('ENABLE', 1, 0) + +-- current target +local target_pos = Location() +local current_pos = Location() +local target_velocity = Vector3f() +local target_heading = 0.0 + +-- other state +local vehicle_mode = MODE_MANUAL +local have_target = false + +-- check key parameters +function check_parameters() + --[[ + parameter values which are auto-set on startup + --]] + local key_params = { + FOLL_ENABLE = 1, + FOLL_OFS_TYPE = 1, + FOLL_ALT_TYPE = 0, + } + + for p, v in pairs(key_params) do + local current = param:get(p) + assert(current, string.format("Parameter %s not found", p)) + if math.abs(v-current) > 0.001 then + param:set_and_save(p, v) + gcs:send_text(0,string.format("Parameter %s set to %.2f was %.2f", p, v, current)) + end + end +end + +function wrap_360(angle) + local res = math.fmod(angle, 360.0) + if res < 0 then + res = res + 360.0 + end + return res +end + +function wrap_180(angle) + local res = wrap_360(angle) + if res > 180 then + res = res - 360 + end + return res +end + +-- update target state +function update_target() + if not follow:have_target() then + if have_target then + gcs:send_text(0,"Lost beacon") + end + have_target = false + return + end + if not have_target then + gcs:send_text(0,"Have beacon") + end + have_target = true + + target_pos, target_velocity = follow:get_target_location_and_velocity_ofs() + target_heading = follow:get_target_heading_deg() +end + +-- main update function +function update() + if GFOLL_ENABLE:get() < 1 then + return + end + + update_target() + if not have_target then + return + end + + current_pos = ahrs:get_position() + if not current_pos then + return + end + current_pos:change_alt_frame(ALT_FRAME_ABSOLUTE) + + if vehicle:get_mode() ~= MODE_GUIDED then + return + end + + local next_WP = vehicle:get_target_location() + if not next_WP then + -- not in a flight mode with a target location + return + end + + -- update the target position from the follow library, which includes the offsets + target_pos:change_alt_frame(ALT_FRAME_ABSOLUTE) + vehicle:update_target_location(next_WP, target_pos) +end + +function loop() + update() + -- run at 20Hz + return loop, 50 +end + +check_parameters() + +-- wrapper around update(). This calls update() at 20Hz, +-- and if update faults then an error is displayed, but the script is not +-- stopped +function protected_wrapper() + local success, err = pcall(update) + if not success then + gcs:send_text(0, "Internal Error: " .. err) + -- when we fault we run the update function again after 1s, slowing it + -- down a bit so we don't flood the console with errors + return protected_wrapper, 1000 + end + return protected_wrapper, 50 +end + +-- start running update loop +return protected_wrapper() +