diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 8ffa3f82972e8..855b9a62413a1 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -1083,6 +1083,12 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_packet(const mavlink_command_in plane.set_mode(plane.mode_rtl, ModeReason::GCS_COMMAND); return MAV_RESULT_ACCEPTED; +#if AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED + case MAV_CMD_SET_HAGL: + plane.handle_external_hagl(packet); + return MAV_RESULT_ACCEPTED; +#endif + default: return GCS_MAVLINK::handle_command_int_packet(packet, msg); } diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 489276ded4592..b1af208397ff3 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -210,6 +210,20 @@ class Plane : public AP_Vehicle { AP_FixedWing::Rangefinder_State rangefinder_state; #endif +#if AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED + struct { + // allow for external height above ground estimate + float hagl; + uint32_t last_update_ms; + uint32_t timeout_ms; + } external_hagl; + bool get_external_HAGL(float &height_agl); + void handle_external_hagl(const mavlink_command_int_t &packet); +#endif // AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED + + float get_landing_height(bool &using_rangefinder); + + #if AP_RPM_ENABLED AP_RPM rpm_sensor; #endif diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index b20c1ffeb0336..8b7970e6315d1 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -115,6 +115,14 @@ int32_t Plane::get_RTL_altitude_cm() const */ float Plane::relative_ground_altitude(bool use_rangefinder_if_available, bool use_terrain_if_available) { +#if AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED + float height_AGL; + // use external HAGL if available + if (get_external_HAGL(height_AGL)) { + return height_AGL; + } +#endif // AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED + #if AP_RANGEFINDER_ENABLED if (use_rangefinder_if_available && rangefinder_state.in_range) { return rangefinder_state.height_estimate; @@ -810,3 +818,67 @@ bool Plane::terrain_enabled_in_mode(Mode::Number num) const return false; } #endif + +#if AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED +/* + handle a MAV_CMD_SET_HAGL request. The accuracy is ignored + */ +void Plane::handle_external_hagl(const mavlink_command_int_t &packet) +{ + auto &hagl = plane.external_hagl; + hagl.hagl = packet.param1; + hagl.last_update_ms = AP_HAL::millis(); + hagl.timeout_ms = uint32_t(packet.param3 * 1000); +} + +/* + get HAGL from external source if current + */ +bool Plane::get_external_HAGL(float &height_agl) +{ + auto &hagl = plane.external_hagl; + if (hagl.last_update_ms != 0) { + const uint32_t now_ms = AP_HAL::millis(); + if (now_ms - hagl.last_update_ms <= hagl.timeout_ms) { + height_agl = hagl.hagl; + return true; + } + hagl.last_update_ms = 0; + } + return false; +} +#endif // AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED + +/* + get height for landing. Set using_rangefinder to true if a rangefinder + or external HAGL source is active + */ +float Plane::get_landing_height(bool &rangefinder_active) +{ + float height; + +#if AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED + // if external HAGL is active use that + if (get_external_HAGL(height)) { + // ensure no terrain correction is applied - that is the job + // of the external system if it is wanted + auto_state.terrain_correction = 0; + + // an external HAGL is considered to be a type of rangefinder + rangefinder_active = true; + return height; + } +#endif + + // get basic height above target + height = height_above_target(); + rangefinder_active = false; + +#if AP_RANGEFINDER_ENABLED + // possibly correct with rangefinder + height -= rangefinder_correction(); + rangefinder_active = g.rangefinder_landing && rangefinder_state.in_range; +#endif + + return height; +} diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 47a588b861673..4766cea5c1fba 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -255,23 +255,16 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd) // Ret } else { // use rangefinder to correct if possible -#if AP_RANGEFINDER_ENABLED - float height = height_above_target() - rangefinder_correction(); -#else - float height = height_above_target(); -#endif + bool rangefinder_active = false; + float height = plane.get_landing_height(rangefinder_active); + // for flare calculations we don't want to use the terrain // correction as otherwise we will flare early on rising // ground height -= auto_state.terrain_correction; return landing.verify_land(prev_WP_loc, next_WP_loc, current_loc, height, auto_state.sink_rate, auto_state.wp_proportion, auto_state.last_flying_ms, arming.is_armed(), is_flying(), -#if AP_RANGEFINDER_ENABLED - g.rangefinder_landing && rangefinder_state.in_range -#else - false -#endif - ); + rangefinder_active); } case MAV_CMD_NAV_LOITER_UNLIM: diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index a5a112a220d8f..09d65181dd963 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -344,6 +344,7 @@ def __init__(self, Feature('MAVLink', 'MAV_MSG_SERIAL_CONTROL', 'AP_MAVLINK_MSG_SERIAL_CONTROL_ENABLED', 'Enable handling of Serial Control mavlink messages', 0, None), # noqa Feature('MAVLink', 'MAVLINK_MSG_MISSION_REQUEST', 'AP_MAVLINK_MSG_MISSION_REQUEST_ENABLED', 'Enable handling of MISSION_REQUEST mavlink messages', 0, None), # noqa Feature('MAVLink', 'AP_MAVLINK_FTP_ENABLED', 'AP_MAVLINK_FTP_ENABLED', 'Enable MAVLink FTP Protocol', 0, None), # noqa + Feature('MAVLink', 'MAV_CMD_SEY_HAGL', 'AP_MAVLINK_MAV_CMD_SEY_HAGL_ENABLED', 'Enable MAVLink HAGL command', 0, None), # noqa Feature('Developer', 'KILL_IMU', 'AP_INERTIALSENSOR_KILL_IMU_ENABLED', 'Allow IMUs to be disabled at runtime', 0, None), Feature('Developer', 'CRASHCATCHER', 'AP_CRASHDUMP_ENABLED', 'Enable CrashCatcher', 0, None), diff --git a/Tools/scripts/extract_features.py b/Tools/scripts/extract_features.py index 63a83863f05ec..6b3238f1f4e9d 100755 --- a/Tools/scripts/extract_features.py +++ b/Tools/scripts/extract_features.py @@ -238,6 +238,7 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"): ('AP_MAVLINK_MSG_SERIAL_CONTROL_ENABLED', 'GCS_MAVLINK::handle_serial_control'), ('AP_MAVLINK_MSG_MISSION_REQUEST_ENABLED', 'GCS_MAVLINK::handle_mission_request\b'), ('AP_MAVLINK_FTP_ENABLED', 'GCS_MAVLINK::ftp_worker'), + ('AP_MAVLINK_MAV_CMD_SEY_HAGL_ENABLED', 'Plane::get_external_HAGL'), ('AP_DRONECAN_HIMARK_SERVO_SUPPORT', 'AP_DroneCAN::SRV_send_himark'), ('AP_DRONECAN_HOBBYWING_ESC_SUPPORT', 'AP_DroneCAN::hobbywing_ESC_update'), diff --git a/libraries/AP_Scripting/examples/land_hagl.lua b/libraries/AP_Scripting/examples/land_hagl.lua new file mode 100644 index 0000000000000..3bdbc36329aea --- /dev/null +++ b/libraries/AP_Scripting/examples/land_hagl.lua @@ -0,0 +1,69 @@ +--[[ + demonstrate proving HAGL to plane code for landing +--]] + +local MAV_CMD_SET_HAGL = 43005 + +local ROTATION_PITCH_90 = 24 +local ROTATION_PITCH_270 = 25 + +-- for normal landing use PITCH_270, for inverted use PITCH_90 +local RANGEFINDER_ORIENT = ROTATION_PITCH_270 + +local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} + +local RNG_STATUS = { NotConnected = 0, NoData = 1, OutOfRangeLow = 2, OutOfRangeHigh = 3, Good = 4 } + + +--[[ + create a NaN value +--]] +local function NaN() + return 0/0 +end + +local last_active = false + +--[[ + send HAGL data +--]] +local function send_HAGL() + local status = rangefinder:status_orient(RANGEFINDER_ORIENT) + if status ~= RNG_STATUS.Good then + last_active = false + return + end + local rangefinder_dist = rangefinder:distance_cm_orient(RANGEFINDER_ORIENT)*0.01 + local correction = math.cos(ahrs:get_roll())*math.cos(ahrs:get_pitch()) + local rangefinder_corrected = rangefinder_dist * correction + if RANGEFINDER_ORIENT == ROTATION_PITCH_90 then + rangefinder_corrected = -rangefinder_corrected + end + if rangefinder_corrected < 0 then + last_active = false + return + end + + -- tell plane the height above ground level + local timeout_s = 0.2 + local accuracy = NaN() + gcs:run_command_int(MAV_CMD_SET_HAGL, { p1 = rangefinder_corrected, p2 = accuracy, p3=timeout_s }) + + if not last_active then + last_active = true + gcs:send_text(MAV_SEVERITY.INFO, string.format("HAGL Active %.1f", rangefinder_corrected)) + end + + -- log it + logger:write("HAGL", "RDist,HAGL", "ff", rangefinder_dist, rangefinder_corrected) +end + +local function update() + send_HAGL() + return update, 50 +end + +gcs:send_text(MAV_SEVERITY.INFO, "Loaded land_hagl") + +return update, 1000 + diff --git a/libraries/GCS_MAVLink/GCS_config.h b/libraries/GCS_MAVLink/GCS_config.h index e5d78fb8601e0..b26c727d35c4a 100644 --- a/libraries/GCS_MAVLink/GCS_config.h +++ b/libraries/GCS_MAVLink/GCS_config.h @@ -127,3 +127,7 @@ #ifndef AP_MAVLINK_MSG_HIGHRES_IMU_ENABLED #define AP_MAVLINK_MSG_HIGHRES_IMU_ENABLED (BOARD_FLASH_SIZE > 1024) && AP_INERTIALSENSOR_ENABLED #endif + +#ifndef AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED +#define AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED (BOARD_FLASH_SIZE > 1024) +#endif diff --git a/modules/mavlink b/modules/mavlink index 2825252d6d13e..603e3c8e0c603 160000 --- a/modules/mavlink +++ b/modules/mavlink @@ -1 +1 @@ -Subproject commit 2825252d6d13eb347520f028b3c580ab29bd78e5 +Subproject commit 603e3c8e0c6031992da4206239681d13df5915be