From 367250bb84fa666ec4886cdaa546da4e4324cf73 Mon Sep 17 00:00:00 2001 From: timtuxworth Date: Tue, 20 Aug 2024 17:21:21 -0600 Subject: [PATCH 1/3] ArduPlane: allow guided altitude in terrain frame --- ArduPlane/GCS_Mavlink.cpp | 45 ++++++++++++++++++++++++++++++--------- ArduPlane/Log.cpp | 2 +- ArduPlane/Plane.h | 3 ++- ArduPlane/mode_guided.cpp | 12 +++++++++++ 4 files changed, 50 insertions(+), 12 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 887599d6b985d..71c8e02e2b375 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -929,34 +929,57 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands(const mavl return MAV_RESULT_DENIED; } - // the requested alt data might be relative or absolute + // the requested alt data might be relative or absolute or terrain relative float new_target_alt = packet.z * 100; - float new_target_alt_rel = packet.z * 100 + plane.home.alt; + float old_target_alt; // only global/relative/terrain frames are supported switch(packet.frame) { + case MAV_FRAME_GLOBAL_TERRAIN_ALT: { + if (plane.guided_state.target_alt_frame == Location::AltFrame::ABOVE_TERRAIN && + plane.current_loc.get_alt_m(Location::AltFrame::ABOVE_TERRAIN, old_target_alt) && + is_equal(old_target_alt, new_target_alt) ) { // compare two floats as near-enough + // no need to process any new packet/s with the same ALT/frame any further, if we are already doing it. + return MAV_RESULT_ACCEPTED; + } + plane.guided_state.target_alt = new_target_alt; + plane.guided_state.target_alt_frame = Location::AltFrame::ABOVE_TERRAIN; + break; + } case MAV_FRAME_GLOBAL_RELATIVE_ALT: { - if (is_equal(plane.guided_state.target_alt,new_target_alt_rel) ) { // compare two floats as near-enough - // no need to process any new packet/s with the same ALT any further, if we are already doing it. + if (plane.guided_state.target_alt_frame == Location::AltFrame::ABOVE_HOME && + plane.current_loc.get_alt_m(Location::AltFrame::ABOVE_HOME, old_target_alt) && + is_equal(old_target_alt, new_target_alt) ) { // compare two floats as near-enough + // no need to process any new packet/s with the same ALT/frame any further, if we are already doing it. return MAV_RESULT_ACCEPTED; } - plane.guided_state.target_alt = new_target_alt_rel; + // float new_target_alt_rel = packet.z * 100 + plane.home.alt; + plane.guided_state.target_alt = new_target_alt + plane.home.alt; + plane.guided_state.target_alt_frame = Location::AltFrame::ABOVE_HOME; break; } case MAV_FRAME_GLOBAL: { - if (is_equal(plane.guided_state.target_alt,new_target_alt) ) { // compare two floats as near-enough - // no need to process any new packet/s with the same ALT any further, if we are already doing it. + if (plane.guided_state.target_alt_frame == Location::AltFrame::ABSOLUTE && + plane.current_loc.get_alt_m(Location::AltFrame::ABSOLUTE, old_target_alt) && + is_equal(old_target_alt, new_target_alt) ) { // compare two floats as near-enough + // no need to process any new packet/s with the same ALT/frame any further, if we are already doing it. return MAV_RESULT_ACCEPTED; } plane.guided_state.target_alt = new_target_alt; + plane.guided_state.target_alt_frame = Location::AltFrame::ABSOLUTE; break; } default: // MAV_RESULT_DENIED means Command is invalid (is supported but has invalid parameters). return MAV_RESULT_DENIED; } + // So far we have only changed the interim altitude. Need to change the next_WP_loc altitude + // So that navigation will work correctly. + plane.next_WP_loc.set_alt_cm(new_target_alt, plane.guided_state.target_alt_frame); + + //plane.guided_state.target_alt_frame = packet.frame; MAVLINK frame <> Location:AltFrame - plane.guided_state.target_alt_frame = packet.frame; + plane.guided_state.last_target_alt_frame = plane.current_loc.get_alt_frame(); plane.guided_state.last_target_alt = plane.current_loc.alt; // FIXME: Reference frame is not corrected for here plane.guided_state.target_alt_time_ms = AP_HAL::millis(); @@ -967,8 +990,10 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands(const mavl plane.guided_state.target_alt_accel = fabsf(packet.param3); } - // assign an acceleration direction - if (plane.guided_state.target_alt < plane.current_loc.alt) { + // assign an acceleration direction comparing altitudes in the requested frame + float current_alt; + if (plane.current_loc.get_alt_m(plane.guided_state.target_alt_frame, current_alt) && + plane.guided_state.target_alt < current_alt) { plane.guided_state.target_alt_accel *= -1.0f; } return MAV_RESULT_ACCEPTED; diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index c9fdd0783b767..3562bb56a0ac4 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -144,7 +144,7 @@ void Plane::Log_Write_OFG_Guided() target_airspeed_accel : guided_state.target_airspeed_accel, target_alt : guided_state.target_alt, target_alt_accel : guided_state.target_alt_accel, - target_alt_frame : guided_state.target_alt_frame, + target_alt_frame : static_cast(guided_state.target_alt_frame), target_heading : guided_state.target_heading, target_heading_limit : guided_state.target_heading_accel_limit }; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index c86b9b92cbbbe..3ed4f13b21afe 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -563,9 +563,10 @@ class Plane : public AP_Vehicle { // altitude adjustments float target_alt = -1; // don't default to zero here, as zero is a valid alt. uint32_t last_target_alt = 0; + Location::AltFrame last_target_alt_frame = Location::AltFrame::ABSOLUTE; float target_alt_accel; uint32_t target_alt_time_ms = 0; - uint8_t target_alt_frame = 0; + Location::AltFrame target_alt_frame = Location::AltFrame::ABSOLUTE; // heading track float target_heading = -4; // don't default to zero or -1 here, as both are valid headings in radians diff --git a/ArduPlane/mode_guided.cpp b/ArduPlane/mode_guided.cpp index 4fef88ee3c73f..cca3359fe587e 100644 --- a/ArduPlane/mode_guided.cpp +++ b/ArduPlane/mode_guided.cpp @@ -140,6 +140,18 @@ void ModeGuided::update_target_altitude() int32_t delta_amt_i = (int32_t)(100.0 * delta_amt_f); Location temp {}; temp.alt = plane.guided_state.last_target_alt + delta_amt_i; // ...to avoid floats here, + switch(plane.guided_state.target_alt_frame) { + case Location::AltFrame::ABOVE_HOME: + temp.relative_alt = true; + break; + case Location::AltFrame::ABOVE_TERRAIN: + temp.relative_alt = true; + temp.terrain_alt = true; + break; + default: + break; + } + if (is_positive(plane.guided_state.target_alt_accel)) { temp.alt = MIN(plane.guided_state.target_alt, temp.alt); } else { From 9ec20e18887e0765572c83aa7ab331bc94ee5047 Mon Sep 17 00:00:00 2001 From: timtuxworth Date: Tue, 20 Aug 2024 17:23:34 -0600 Subject: [PATCH 2/3] AP_Scripting: allow guided altitude in terrain frame AP_Scripting: allow guided altitude in terrain frame --- .../examples/plane_guided_terrain.lua | 185 ++++++++++++++++++ 1 file changed, 185 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 0000000000000..951386bbfb98f --- /dev/null +++ b/libraries/AP_Scripting/examples/plane_guided_terrain.lua @@ -0,0 +1,185 @@ +--[[ + + example script to show interrupting a mission and then + reseting the crosstracking to the correct line when + returning to the mission after the interruption + + this functionality is only available in Plane +--]] + +SCRIPT_NAME = "OverheadIntel Guided Terrain" +SCRIPT_NAME_SHORT = "TerrGuided" +SCRIPT_VERSION = "4.6.0-002" + +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 +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 + 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() + 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 + + -- 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 + 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 + 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 + vehicle_mode_previous = vehicle_mode + 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 From 58bb462c73edc3070b657198e8aea5317a8977b3 Mon Sep 17 00:00:00 2001 From: timtuxworth Date: Thu, 22 Aug 2024 11:17:16 -0600 Subject: [PATCH 3/3] AP_Common: allow guided altitude in terrain frame