From f7e74b6489b810718b14fee8bba8f56a7360358f Mon Sep 17 00:00:00 2001 From: timtuxworth Date: Mon, 9 Sep 2024 17:33:05 -0600 Subject: [PATCH] AP_Scripting: example for use Location::AltFrame for guided_state.target_alt_frame --- .../examples/plane_guided_terrain.lua | 234 ++++++++++++++++++ 1 file changed, 234 insertions(+) create mode 100644 libraries/AP_Scripting/examples/plane_guided_terrain.lua diff --git a/libraries/AP_Scripting/examples/plane_guided_terrain.lua b/libraries/AP_Scripting/examples/plane_guided_terrain.lua new file mode 100644 index 00000000000000..86f6ded9f329b9 --- /dev/null +++ b/libraries/AP_Scripting/examples/plane_guided_terrain.lua @@ -0,0 +1,234 @@ +--[[ + + example script to show reseting guided mode to terrain height. + It's intended to be used when selecting "fly to altitude" in a ground station + for example by right clicking on the map in QGC. + + This is actually a workaround to QGC and Mission Planner not having a + way to set guided altitude above terrain. + + Depending on ZGP_MODE + When the GCS requests a guided altitude X above home + 1: reset to current terrain height ignore X + 2: reset to X above terrain + 3: reset to current alt + X + + this functionality is only available in Plane and + requires TERRAIN_ENABLE = 1 and TERRAIN_FOLLOW =1 +--]] + +SCRIPT_NAME = "OverheadIntel Guided Terrain" +SCRIPT_NAME_SHORT = "TerrGuided" +SCRIPT_VERSION = "4.6.0-005" + +REFRESH_RATE = 0.2 -- in seconds, so 5Hz +ALTITUDE_MIN = 50 +ALTITUDE_MAX = 120 + +MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} +MAV_FRAME = { GLOBAL = 0, GLOBAL_RELATIVE_ALT = 3} +ALT_FRAME = { ABSOLUTE = 0, ABOVE_HOME = 1, ABOVE_ORIGIN = 2, ABOVE_TERRAIN = 3 } +FLIGHT_MODE = {AUTO=10, RTL=11, LOITER=12, GUIDED=15, QHOVER=18, QLOITER=19, QRTL=21} + +PARAM_TABLE_KEY = 101 +PARAM_TABLE_PREFIX = "ZGT_" + +local now = millis():tofloat() * 0.001 + +-- bind a parameter to a variable +function bind_param(name) + return Parameter(name) +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), SCRIPT_NAME_SHORT .. string.format('could not add param %s', name)) + return bind_param(PARAM_TABLE_PREFIX .. name) +end + +-- setup follow mode specific parameters +assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 10), SCRIPT_NAME_SHORT .. 'could not add param table: ' .. PARAM_TABLE_PREFIX) + +--[[ + // @Param: ZGT_MODE + // @DisplayName: Guided Terrain Mode + // @Description: When the GCS requests a guided altitude X above home 1: reset to current terrain height ignore X 2: reset to X above terrain 3: reset to current alt + X + // @Range: 1,2,3 +--]] +ZGT_MODE = bind_add_param("MODE", 1, 1) +TERRAIN_ENABLE = bind_param("TERRAIN_ENABLE") +TERRAIN_FOLLOW = bind_param("TERRAIN_FOLLOW") + +local zgt_mode = ZGT_MODE:get() +local terrain_enable = TERRAIN_ENABLE:get() +local terrain_follow = TERRAIN_FOLLOW:get() + +MAV_CMD_INT = { DO_SET_MODE = 176, DO_CHANGE_SPEED = 178, DO_REPOSITION = 192, + GUIDED_CHANGE_SPEED = 43000, GUIDED_CHANGE_ALTITUDE = 43001, GUIDED_CHANGE_HEADING = 43002 } +MAV_FRAME = { GLOBAL = 0, GLOBAL_RELATIVE_ALT = 3, GLOBAL_TERRAIN_ALT = 10} + +-- constrain a value between limits +local function constrain(v, vmin, vmax) + if v < vmin then + v = vmin + end + if v > vmax then + v = vmax + end + return v +end + +local function follow_frame_to_mavlink(follow_frame) + local mavlink_frame = MAV_FRAME.GLOBAL + if (follow_frame == ALT_FRAME.ABOVE_TERRAIN) then + mavlink_frame = MAV_FRAME.GLOBAL_TERRAIN_ALT + elseif (follow_frame == ALT_FRAME.ABOVE_HOME) then + mavlink_frame = MAV_FRAME.GLOBAL_RELATIVE_ALT + end + return mavlink_frame + end + +local now_altitude = millis():tofloat() * 0.001 +-- target.alt = new target altitude in meters +-- set_vehicle_target_altitude() Parameters +-- target.frame = Altitude frame MAV_FRAME, it's very important to get this right! +-- target.alt = altitude in meters to acheive +-- target.accel = z acceleration to altitude (1000.0 = max) +local function set_vehicle_target_altitude(target) + local acceleration = target.accel or 1000.0 -- default to maximum z acceleration + if math.floor(now) ~= math.floor(now_altitude) then + now_altitude = millis():tofloat() * 0.001 + end + if target.alt == nil then + gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. ": set_vehicle_target_altitude no altiude") + return + end + -- GUIDED_CHANGE_ALTITUDE takes altitude in meters + local mavlink_result = gcs:run_command_int(MAV_CMD_INT.GUIDED_CHANGE_ALTITUDE, { + frame = follow_frame_to_mavlink(target.frame), + p3 = acceleration, + z = target.alt }) + if mavlink_result > 0 then + gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. string.format(": MAVLink GUIDED_CHANGE_ALTITUDE returned %d", mavlink_result)) + return false + else + return true + end +end + +local vehicle_mode = vehicle:get_mode() +local save_target_altitude = -1 +local save_old_target_altitude = -1 +local save_zgt_mode = -1 +local save_vehicle_mode = -1 + +local function update() + + vehicle_mode = vehicle:get_mode() + + terrain_enable = TERRAIN_ENABLE:get() + terrain_follow = TERRAIN_FOLLOW:get() + zgt_mode = ZGT_MODE:get() + if zgt_mode ~= save_zgt_mode then + -- user changed modes, reset everything + save_target_altitude = -1 + save_old_target_altitude = -1 + end + save_zgt_mode = zgt_mode + + -- We should only reset the altitude if newly switched to guided mode + if save_vehicle_mode ~= vehicle_mode and vehicle_mode == FLIGHT_MODE.GUIDED and terrain_enable == 1 and + ((terrain_follow & 1) == 1 or (terrain_follow & (1 << 6)) == 64) then + local target_location = vehicle:get_target_location() + if target_location ~= nil then + local target_location_frame = target_location:get_alt_frame() + -- need to convert the target_location to ABOVE_TERRAIN so we have apples and apples + local new_target_location = target_location:copy() + local new_target_altitude = save_target_altitude + local old_target_altitude = new_target_location:alt()/100 + + if save_old_target_altitude ~= old_target_altitude then + if target_location_frame ~= ALT_FRAME.ABOVE_TERRAIN then + if new_target_location:change_alt_frame(ALT_FRAME.ABOVE_TERRAIN) then + old_target_altitude = new_target_location:alt()/100 + end + end + -- adjust target_location for home altitude + local home_location = ahrs:get_home() + if home_location ~= nil then + local home_amsl = terrain:height_amsl(home_location, true) + local above_home = (target_location:alt() * 0.01 - home_amsl) + local location = ahrs:get_location() + if location ~= nil then + if location:get_alt_frame() ~= ALT_FRAME.ABOVE_TERRAIN then + location:change_alt_frame(ALT_FRAME.ABOVE_TERRAIN) + end + local current_altitude = location:alt() * 0.01 + -- @Description: When the GCS requests a guided altitude X above home + -- 1: reset to current terrain height ignore X + -- 2: reset to X above terrain + -- 3: reset to current alt + X + if zgt_mode == 1 then + new_target_altitude = current_altitude + elseif zgt_mode == 2 then + new_target_altitude = above_home + elseif zgt_mode == 3 then + new_target_altitude = current_altitude + above_home + end + new_target_altitude = constrain(new_target_altitude, ALTITUDE_MIN, ALTITUDE_MAX) + end + else + gcs:send_text(MAV_SEVERITY.ERROR, string.format("%s: FAILED to get HOME terrain height", SCRIPT_NAME_SHORT)) + end + + if new_target_altitude > 0 and + set_vehicle_target_altitude({alt = new_target_altitude, frame = ALT_FRAME.ABOVE_TERRAIN}) then -- pass altitude in meters (location has it in cm) + if new_target_altitude ~= save_target_altitude then + gcs:send_text(MAV_SEVERITY.NOTICE, string.format("%s: Reset to alt %.0fm above terrain", SCRIPT_NAME_SHORT, + new_target_altitude + )) + save_target_altitude = new_target_altitude + end + else + gcs:send_text(MAV_SEVERITY.ERROR, string.format("%s: FAILED to set altitude ABOVE_TERRAIN ait %.0f", SCRIPT_NAME_SHORT, + new_target_altitude + )) + end + end + save_old_target_altitude = old_target_altitude + else + gcs:send_text(MAV_SEVERITY.ERROR, string.format("%s: altitude not available", SCRIPT_NAME_SHORT)) + end + else + -- we switched out of guided, so forget what we thought we knew + save_target_altitude = -1 + save_old_target_altitude = -1 + end + save_vehicle_mode = vehicle_mode + + return update, 1000 * REFRESH_RATE +end + +-- wrapper around update(). This calls update() at 1/REFRESHRATE Hz, +-- and if update faults then an error is displayed, but the script is not +-- stopped +local function protected_wrapper() + local success, err = pcall(update) + if not success then + gcs:send_text(0, SCRIPT_NAME_SHORT .. ": 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, 1000 * REFRESH_RATE +end + +gcs:send_text(MAV_SEVERITY.NOTICE, string.format("%s %s script loaded", SCRIPT_NAME, SCRIPT_VERSION) ) + +-- start running update loop +if FWVersion:type() == 3 and terrain_enable then + gcs:send_text(MAV_SEVERITY.NOTICE, string.format("%s %s script loaded", SCRIPT_NAME, SCRIPT_VERSION) ) + return protected_wrapper() +else + gcs:send_text(MAV_SEVERITY.NOTICE,string.format("%s: Must run on Plane with terrain follow", SCRIPT_NAME_SHORT)) +end