-
Notifications
You must be signed in to change notification settings - Fork 17.3k
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
AP_Scripting: example for use Location::AltFrame for guided_state.tar…
…get_alt_frame
- Loading branch information
1 parent
1d4fa24
commit f7e74b6
Showing
1 changed file
with
234 additions
and
0 deletions.
There are no files selected for viewing
234 changes: 234 additions & 0 deletions
234
libraries/AP_Scripting/examples/plane_guided_terrain.lua
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 |