From a663c80fbd071a28a5929939302ddf5e05f41271 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 | 229 ++++++++++++++++++ 1 file changed, 229 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..0f1bc69091f806 --- /dev/null +++ b/libraries/AP_Scripting/examples/plane_guided_terrain.lua @@ -0,0 +1,229 @@ +--[[ + + 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-003" + +REFRESH_RATE = 0.2 -- in seconds, so 5Hz + +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} + +local PARAM_TABLE_KEY = 101 +local PARAM_TABLE_PREFIX = "ZGT_" + +local now = millis():tofloat() * 0.001 +local now_location = now + +-- 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} + + +local function follow_frame_to_mavlink(follow_frame) + local mavlink_frame = MAV_FRAME.GLOBAL + --gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. string.format(" set frame : %d", follow_frame)) + 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 home_location = ahrs:get_home() + 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 + --[[ + gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. string.format(": set heading alt: %.1f", target.alt) ) + if home_location ~= nil then + gcs:send_text(MAV_SEVERITY.ERROR, SCRIPT_NAME_SHORT .. string.format(" set location : alt home relative: %.1f", (target.alt - (home_location:alt() * 0.01)))) + end + --]] + 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 function update() + + vehicle_mode = vehicle:get_mode() + + terrain_enable = TERRAIN_ENABLE:get() + terrain_follow = TERRAIN_FOLLOW:get() + + if 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() + gcs:send_text(MAV_SEVERITY.NOTICE, string.format("%s: CHANGE frame was %f to %f", SCRIPT_NAME_SHORT, + target_location_frame, + ALT_FRAME.ABOVE_TERRAIN + )) + if target_location_frame ~= ALT_FRAME.ABOVE_TERRAIN then + local old_target_altitude = target_location:alt() * 0.01 -- important this is before change_alt_frame() + local new_target_location = target_location:copy() + if new_target_location:change_alt_frame(ALT_FRAME.ABOVE_TERRAIN) then + local new_target_altitude = new_target_location:alt()/100 + + gcs:send_text(MAV_SEVERITY.NOTICE, string.format("%s: change_frame alt was %f now %f", SCRIPT_NAME_SHORT, + target_location:alt()/100, + new_target_altitude + )) + + + -- 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) + --[[ + gcs:send_text(MAV_SEVERITY.INFO, string.format("%s: adj alt home %.0f diff %.0f old %.0f", SCRIPT_NAME_SHORT, + home_amsl, + above_home, + new_target_altitude + )) + --]] + local location = ahrs:get_location() + local current_altitude = location:alt() * 0.01 + if location ~= nil then + if location:get_alt_frame() ~= ALT_FRAME.ABOVE_TERRAIN then + location:change_alt_frame(ALT_FRAME.ABOVE_TERRAIN) + end + -- @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 + zgt_mode = ZGT_MODE:get() + 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 + end + + gcs:send_text(MAV_SEVERITY.INFO, string.format("%s: cur alt %.0f diff %.0f new %.0f", SCRIPT_NAME_SHORT, + current_altitude, + above_home, + new_target_altitude + )) + + else + gcs:send_text(MAV_SEVERITY.ERROR, string.format("%s: FAILED to get HOME terrain height", SCRIPT_NAME_SHORT)) + end + + if set_vehicle_target_altitude({alt = new_target_altitude, frame = ALT_FRAME.ABOVE_TERRAIN}) then -- pass altitude in meters (location has it in cm) + gcs:send_text(MAV_SEVERITY.NOTICE, string.format("%s: Reset to alt %.0fm above terrain", SCRIPT_NAME_SHORT, + new_target_altitude + )) + 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 + else + gcs:send_text(MAV_SEVERITY.ERROR, string.format("%s: altitude not available", SCRIPT_NAME_SHORT)) + end + end + end + end + + 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 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", SCRIPT_NAME_SHORT)) +end \ No newline at end of file