Plane: added MAV_CMD_SET_HAGL support

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);
     }

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

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;
+}

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:

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),

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'),

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
+

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