diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 12f2704ec7f17..527f8dd68c285 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -932,48 +932,25 @@ 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 - float new_target_alt = packet.z * 100; - float new_target_alt_rel = packet.z * 100 + plane.home.alt; - - // only global/relative/terrain frames are supported - switch(packet.frame) { - 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. - return MAV_RESULT_ACCEPTED; - } - plane.guided_state.target_alt = new_target_alt_rel; - 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. - return MAV_RESULT_ACCEPTED; - } - plane.guided_state.target_alt = new_target_alt; - break; - } - default: - // MAV_RESULT_DENIED means Command is invalid (is supported but has invalid parameters). - return MAV_RESULT_DENIED; + Location::AltFrame new_target_alt_frame; + if (!mavlink_coordinate_frame_to_location_alt_frame((MAV_FRAME)packet.frame, new_target_alt_frame)) { + return MAV_RESULT_DENIED; } + // keep a copy of what came in via MAVLink - this is needed for logging, but not for anything else + plane.guided_state.target_mav_frame = packet.frame; - plane.guided_state.target_alt_frame = packet.frame; - plane.guided_state.last_target_alt = plane.current_loc.alt; // FIXME: Reference frame is not corrected for here + const int32_t new_target_alt_cm = packet.z * 100; + plane.guided_state.target_location.set_alt_cm(new_target_alt_cm, new_target_alt_frame); plane.guided_state.target_alt_time_ms = AP_HAL::millis(); + // param3 contains the desired vertical velocity (not acceleration) if (is_zero(packet.param3)) { - // the user wanted /maximum acceleration, pick a large value as close enough - plane.guided_state.target_alt_accel = 1000.0; + // the user wanted /maximum altitude change rate, pick a large value as close enough + plane.guided_state.target_alt_rate = 1000.0; } else { - plane.guided_state.target_alt_accel = fabsf(packet.param3); + plane.guided_state.target_alt_rate = fabsf(packet.param3); } - // assign an acceleration direction - if (plane.guided_state.target_alt < plane.current_loc.alt) { - plane.guided_state.target_alt_accel *= -1.0f; - } return MAV_RESULT_ACCEPTED; } diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index b06bb99cb655f..a258269779a39 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -128,10 +128,11 @@ struct PACKED log_OFG_Guided { float target_airspeed_cm; float target_airspeed_accel; float target_alt; - float target_alt_accel; - uint8_t target_alt_frame; + float target_alt_rate; + uint8_t target_mav_frame; // received MavLink frame float target_heading; float target_heading_limit; + uint8_t target_alt_frame; // internal AltFrame }; // Write a OFG Guided packet. @@ -142,11 +143,12 @@ void Plane::Log_Write_OFG_Guided() time_us : AP_HAL::micros64(), target_airspeed_cm : (float)guided_state.target_airspeed_cm*(float)0.01, 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 : guided_state.target_location.alt * 0.01, + target_alt_rate : guided_state.target_alt_rate, + target_mav_frame : guided_state.target_mav_frame, target_heading : guided_state.target_heading, - target_heading_limit : guided_state.target_heading_accel_limit + target_heading_limit : guided_state.target_heading_accel_limit, + target_alt_frame : static_cast(guided_state.target_location.get_alt_frame()), }; logger.WriteBlock(&pkt, sizeof(pkt)); } @@ -274,7 +276,7 @@ void Plane::Log_Write_Guided(void) logger.Write_PID(LOG_PIDG_MSG, g2.guidedHeading.get_pid_info()); } - if ( is_positive(guided_state.target_alt) || is_positive(guided_state.target_airspeed_cm) ) { + if ( guided_state.target_location.alt != -1 || is_positive(guided_state.target_airspeed_cm) ) { Log_Write_OFG_Guided(); } #endif // AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED @@ -490,12 +492,13 @@ const struct LogStructure Plane::log_structure[] = { // @Field: Arsp: target airspeed cm // @Field: ArspA: target airspeed accel // @Field: Alt: target alt -// @Field: AltA: target alt accel -// @Field: AltF: target alt frame +// @Field: AltA: target alt velocity (rate of change) +// @Field: AltF: target alt frame (MAVLink) // @Field: Hdg: target heading // @Field: HdgA: target heading lim +// @Field: AltL: target alt frame (Location) { LOG_OFG_MSG, sizeof(log_OFG_Guided), - "OFG", "QffffBff", "TimeUS,Arsp,ArspA,Alt,AltA,AltF,Hdg,HdgA", "s-------", "F-------" , true }, + "OFG", "QffffBffB", "TimeUS,Arsp,ArspA,Alt,AltA,AltF,Hdg,HdgA,AltL", "snnmo-d--", "F--------" , true }, #endif }; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 030131abc308f..70ef5fe7a9348 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -577,11 +577,10 @@ class Plane : public AP_Vehicle { uint32_t target_airspeed_time_ms; // altitude adjustments - float target_alt = -1; // don't default to zero here, as zero is a valid alt. - uint32_t last_target_alt = 0; - float target_alt_accel; + Location target_location; + float target_alt_rate; uint32_t target_alt_time_ms = 0; - uint8_t target_alt_frame = 0; + uint8_t target_mav_frame = -1; // 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.cpp b/ArduPlane/mode.cpp index f442bd830cf84..ade2abb2a317f 100644 --- a/ArduPlane/mode.cpp +++ b/ArduPlane/mode.cpp @@ -58,9 +58,8 @@ bool Mode::enter() plane.guided_state.target_heading = -4; // radians here are in range -3.14 to 3.14, so a default value needs to be outside that range plane.guided_state.target_heading_type = GUIDED_HEADING_NONE; plane.guided_state.target_airspeed_cm = -1; // same as above, although an airspeed of -1 is rare on plane. - plane.guided_state.target_alt = -1; // same as above, although a target alt of -1 is rare on plane. plane.guided_state.target_alt_time_ms = 0; - plane.guided_state.last_target_alt = 0; + plane.guided_state.target_location.set_alt_cm(-1, Location::AltFrame::ABSOLUTE); #endif #if AP_CAMERA_ENABLED diff --git a/ArduPlane/mode_guided.cpp b/ArduPlane/mode_guided.cpp index 4fef88ee3c73f..a97bedcd7dffb 100644 --- a/ArduPlane/mode_guided.cpp +++ b/ArduPlane/mode_guided.cpp @@ -129,24 +129,29 @@ void ModeGuided::set_radius_and_direction(const float radius, const bool directi void ModeGuided::update_target_altitude() { #if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED - if (((plane.guided_state.target_alt_time_ms != 0) || plane.guided_state.target_alt > -0.001 )) { // target_alt now defaults to -1, and _time_ms defaults to zero. + // target altitude can be negative (e.g. flying below home altitude from the top of a mountain) + if (((plane.guided_state.target_alt_time_ms != 0) || plane.guided_state.target_location.alt != -1 )) { // target_alt now defaults to -1, and _time_ms defaults to zero. // offboard altitude demanded uint32_t now = AP_HAL::millis(); float delta = 1e-3f * (now - plane.guided_state.target_alt_time_ms); plane.guided_state.target_alt_time_ms = now; // determine delta accurately as a float - float delta_amt_f = delta * plane.guided_state.target_alt_accel; + float delta_amt_f = delta * plane.guided_state.target_alt_rate; // then scale x100 to match last_target_alt and convert to a signed int32_t as it may be negative 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, - if (is_positive(plane.guided_state.target_alt_accel)) { - temp.alt = MIN(plane.guided_state.target_alt, temp.alt); - } else { - temp.alt = MAX(plane.guided_state.target_alt, temp.alt); + // To calculate the required velocity (up or down), we need to target and current altitudes in the target frame + const Location::AltFrame target_frame = plane.guided_state.target_location.get_alt_frame(); + int32_t target_alt_previous_cm; + if (plane.current_loc.initialised() && plane.guided_state.target_location.initialised() && + plane.current_loc.get_alt_cm(target_frame, target_alt_previous_cm)) { + // create a new interim target location that that takes current_location and moves delta_amt_i in the right direction + int32_t temp_alt_cm = constrain_int32(plane.guided_state.target_location.alt, target_alt_previous_cm - delta_amt_i, target_alt_previous_cm + delta_amt_i); + Location temp_location = plane.guided_state.target_location; + temp_location.set_alt_cm(temp_alt_cm, target_frame); + + // incrementally step the altitude towards the target + plane.set_target_altitude_location(temp_location); } - plane.guided_state.last_target_alt = temp.alt; - plane.set_target_altitude_location(temp); } else #endif // AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED { diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index ea6f0a1e3827e..11f14e3db6066 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -5533,6 +5533,7 @@ def RunMissionScript(self): def MAV_CMD_GUIDED_CHANGE_ALTITUDE(self): '''test handling of MAV_CMD_GUIDED_CHANGE_ALTITUDE''' + self.start_subtest("set home relative altitude") self.takeoff(30, relative=True) self.change_mode('GUIDED') for alt in 50, 70: @@ -5544,6 +5545,7 @@ def MAV_CMD_GUIDED_CHANGE_ALTITUDE(self): self.wait_altitude(alt-1, alt+1, timeout=30, relative=True) # test for #24535 + self.start_subtest("switch to loiter and resume guided maintains home relative altitude") self.change_mode('LOITER') self.delay_sim_time(5) self.change_mode('GUIDED') @@ -5554,6 +5556,55 @@ def MAV_CMD_GUIDED_CHANGE_ALTITUDE(self): timeout=30, relative=True, ) + # test that this works if switching between RELATIVE (HOME) and GLOBAL(AMSL) + self.start_subtest("set global/AMSL altitude, switch to loiter and resume guided") + alt = 625 + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_GUIDED_CHANGE_ALTITUDE, + p7=alt, + frame=mavutil.mavlink.MAV_FRAME_GLOBAL, + ) + self.wait_altitude(alt-3, alt+3, timeout=30, relative=False) + self.change_mode('LOITER') + self.delay_sim_time(5) + self.change_mode('GUIDED') + self.wait_altitude( + alt-5, # NOTE: reuse of alt from above CHANGE_ALTITUDE + alt+5, + minimum_duration=10, + timeout=30, + relative=False, + ) + # test that this works if switching between RELATIVE (HOME) and terrain + self.start_subtest("set terrain altitude, switch to loiter and resume guided") + self.change_mode('GUIDED') + alt = 100 + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_GUIDED_CHANGE_ALTITUDE, + p7=alt, + frame=mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT, + ) + self.wait_altitude( + alt-5, # NOTE: reuse of alt from abovE + alt+5, # use a 5m buffer as the plane needs to go up and down a bit to maintain terrain distance + minimum_duration=10, + timeout=30, + relative=False, + altitude_source="TERRAIN_REPORT.current_height" + ) + self.change_mode('LOITER') + self.delay_sim_time(5) + self.change_mode('GUIDED') + self.wait_altitude( + alt-5, # NOTE: reuse of alt from abovE + alt+5, # use a 5m buffer as the plane needs to go up and down a bit to maintain terrain distance + minimum_duration=10, + timeout=30, + relative=False, + altitude_source="TERRAIN_REPORT.current_height" + ) + + self.delay_sim_time(5) self.fly_home_land_and_disarm() def _MAV_CMD_PREFLIGHT_CALIBRATION(self, command): diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index c70d7a3bddbf0..c3a82045e7510 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -7176,6 +7176,10 @@ def get_altitude(self, relative=False, timeout=30, altitude_source=None): altitude_source = "GLOBAL_POSITION_INT.relative_alt" else: altitude_source = "GLOBAL_POSITION_INT.alt" + if altitude_source == "TERRAIN_REPORT.current_height": + terrain = self.assert_receive_message('TERRAIN_REPORT') + return terrain.current_height + (msg, field) = altitude_source.split('.') msg = self.poll_message(msg, quiet=True) divisor = 1000.0 # mm is pretty common in mavlink 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..86f6ded9f329b --- /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