Plane follow lua applet

Tools/autotest/arduplane.py

@@ -7733,6 +7733,43 @@ def ScriptedArmingChecksAppletRally(self):
         self.set_parameter("RALLY_LIMIT_KM", 7)
         self.wait_text("clear: Rally too far", check_context=True)
 
+    def PlaneFollowAppletSanity(self):
+        '''PLane Follow Sanity Check, not a detailed test'''
+        self.start_subtest("Plane Follow Script Load and Start")
+
+        self.install_applet_script_context("plane_follow.lua")
+        self.install_script_module(self.script_modules_source_path("pid.lua"), "pid.lua")
+        self.install_script_module(self.script_modules_source_path("mavlink_attitude.lua"), "mavlink_attitude.lua")
+        self.install_mavlink_module()
+
+        self.set_parameters({
+            "SCR_ENABLE": 1,
+            "SIM_SPEEDUP": 20, # need to give some cycles to lua
+            "RC7_OPTION": 301,
+        })
+
+        self.context_collect("STATUSTEXT")
+
+        self.reboot_sitl()
+
+        self.wait_text("Plane Follow .* script loaded", timeout=30, regex=True, check_context=True)
+
+        self.wait_ready_to_arm()
+        self.set_rc(7, 2000)
+        self.wait_text("PFollow: must be armed", check_context=True)
+        self.set_rc(7, 1000)
+        self.arm_vehicle()
+        self.set_rc(7, 2000)
+        self.wait_text("PFollow: enabled", check_context=True)
+        self.set_rc(7, 1000)
+        self.wait_text("PFollow: disabled", check_context=True)
+        self.disarm_vehicle()
+
+        self.reboot_sitl()
+        # remove the installed modules.
+        self.remove_installed_script_module("pid.lua")
+        self.remove_installed_script_module("mavlink_attitude.lua")
+
     def tests(self):
         '''return list of all tests'''
         ret = []
@@ -7908,6 +7945,7 @@ def tests1b(self):
             self.ScriptedArmingChecksApplet,
             self.ScriptedArmingChecksAppletEStop,
             self.ScriptedArmingChecksAppletRally,
+            self.PlaneFollowAppletSanity,
         ]
 
     def tests1c(self):

libraries/AP_Follow/AP_Follow.cpp

@@ -45,21 +45,19 @@ extern const AP_HAL::HAL& hal;
 // Constants and Definitions
 //==============================================================================
 
-#define AP_FOLLOW_TIMEOUT_MS    3000    // position estimate timeout
+#define AP_FOLLOW_TIMEOUT          3     // position estimate timeout in seconds
 #define AP_FOLLOW_SYSID_TIMEOUT_MS 10000 // forget sysid we are following if we have not heard from them in 10 seconds
 
 #define AP_FOLLOW_OFFSET_TYPE_NED       0   // offsets are in north-east-down frame
 #define AP_FOLLOW_OFFSET_TYPE_RELATIVE  1   // offsets are relative to lead vehicle's heading
 
-#define AP_FOLLOW_ALTITUDE_TYPE_RELATIVE  1 // relative altitude is used by default   
-
 #define AP_FOLLOW_POS_P_DEFAULT 0.1f    // position error gain default
 
 #if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
- #define AP_FOLLOW_ALT_TYPE_DEFAULT 0
- #define AP_FOLLOW_DIST_MAX_DEFAULT 0
+ #define AP_FOLLOW_ALT_TYPE_DEFAULT static_cast<float>(Location::AltFrame::ABSOLUTE)
+ #define AP_FOLLOW_DIST_MAX_DEFAULT 0    // zero = ignored
 #else
- #define AP_FOLLOW_ALT_TYPE_DEFAULT AP_FOLLOW_ALTITUDE_TYPE_RELATIVE
+ #define AP_FOLLOW_ALT_TYPE_DEFAULT static_cast<float>(Location::AltFrame::ABOVE_HOME)
  #define AP_FOLLOW_DIST_MAX_DEFAULT 100
 #endif
 
@@ -93,9 +91,9 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
 
     // @Param: _DIST_MAX
     // @DisplayName: Follow distance maximum
-    // @Description: Follow distance maximum. If exceeded, the follow estimate will be considered invalid.
+    // @Description: Follow distance maximum. If exceeded, the follow estimate will be considered invalid. If zero there is no maximum.
     // @Units: m
-    // @Range: 1 1000
+    // @Range: 0 1000
     // @User: Standard
     AP_GROUPINFO("_DIST_MAX", 5, AP_Follow, _dist_max_m, AP_FOLLOW_DIST_MAX_DEFAULT),
 
@@ -152,7 +150,7 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
     // @Param: _ALT_TYPE
     // @DisplayName: Follow altitude type
     // @Description: Follow altitude type
-    // @Values: 0:absolute, 1:relative
+    // @Values: 0:absolute, 1:relative, 3:terrain
     // @User: Standard
     AP_GROUPINFO("_ALT_TYPE", 10, AP_Follow, _alt_type, AP_FOLLOW_ALT_TYPE_DEFAULT),
 #endif
@@ -212,6 +210,12 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("_JERK_H", 17, AP_Follow, _jerk_max_h_degsss, 360.0),
 
+    // @Param: _TIMEOUT
+    // @DisplayName: Follow timeout
+    // @Description: Follow position update from lead - timeout after x seconds
+    // @User: Standard
+    // @Units: s
+    AP_GROUPINFO("_TIMEOUT", 18, AP_Follow, _timeout, AP_FOLLOW_TIMEOUT),
 
     AP_GROUPEND
 };
@@ -408,7 +412,7 @@ bool AP_Follow::get_heading_heading_rate_rad(float &heading_rad, float &heading_
     return true;
 }
 
-// Retrieves the target's estimated global location and velocity
+// Retrieves the target's estimated global location and estimated velocity
 bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ned)
 {
     WITH_SEMAPHORE(_follow_sem);
@@ -422,10 +426,12 @@ bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ne
     }
     vel_ned = _estimate_vel_ned_ms;
 
-    return true;
+    // The frame requested by FOLL_ALT_TYPE may not be the frame of location returned by ahrs. 
+    // Make sure we give the caller the frame they have asked for.
+    return loc.change_alt_frame(_alt_type);
 }
 
-// Retrieves the target's estimated global location and velocity, including configured offsets, for LUA bindings.
+// Retrieves the target's estimated global location including configured offsets, and estimated velocity,  for LUA bindings.
 bool AP_Follow::get_target_location_and_velocity_ofs(Location &loc, Vector3f &vel_ned)
 {
     WITH_SEMAPHORE(_follow_sem);
@@ -547,7 +553,7 @@ bool AP_Follow::should_handle_message(const mavlink_message_t &msg) const
 // Checks whether the current estimate should be reset based on position and velocity errors.
 bool AP_Follow::estimate_error_too_large() const
 {
-    const float timeout_sec = AP_FOLLOW_TIMEOUT_MS * 0.001f;
+    const float timeout_sec = _timeout;
 
     // Calculate position thresholds based on maximum acceleration then deceleration for the timeout duration
     const float pos_thresh_horiz_m = _accel_max_ne_mss.get() * sq(timeout_sec * 0.5);
@@ -615,15 +621,28 @@ bool AP_Follow::handle_global_position_int_message(const mavlink_message_t &msg)
     _target_location.lat = packet.lat;
     _target_location.lng = packet.lon;
 
-    // set target altitude based on configured altitude type
-    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
-        // use relative altitude above home
-        _target_location.set_alt_cm(packet.relative_alt / 10, Location::AltFrame::ABOVE_HOME);
-    } else {
-        // use absolute altitude
-        _target_location.set_alt_cm(packet.alt / 10, Location::AltFrame::ABSOLUTE);
+    switch((Location::AltFrame)_alt_type) {
+        case Location::AltFrame::ABSOLUTE:
+            _target_location.set_alt_cm(packet.alt * 0.1, Location::AltFrame::ABSOLUTE);
+            break;
+        case Location::AltFrame::ABOVE_HOME:
+            _target_location.set_alt_cm(packet.relative_alt * 0.1, Location::AltFrame::ABOVE_HOME);
+            break;
+#if APM_BUILD_TYPE(APM_BUILD_ArduPlane) || APM_BUILD_TYPE(APM_BUILD_ArduCopter)
+        case Location::AltFrame::ABOVE_TERRAIN:
+            /// Altitude comes in as AMSL
+            _target_location.set_alt_cm(packet.alt * 0.1, Location::AltFrame::ABSOLUTE);
+            // convert the incoming altitude to terrain altitude, but fail if there is no terrain data available
+            if (!_target_location.change_alt_frame(Location::AltFrame::ABOVE_TERRAIN)) {
+                return false;
+            };
+            break;
+#endif
+        default:
+            // don't process the packet if the _alt_type is invalid
+            return false;
     }
-
+    
     // convert global location to local NED frame position
     if (!_target_location.get_vector_from_origin_NEU(_target_pos_ned_m)) {
         return false;
@@ -885,12 +904,13 @@ void AP_Follow::Log_Write_FOLL()
     // @Field: VelD: Target velocity, Down (m/s)
     // @Field: LatE: Vehicle estimated latitude (degrees * 1E7)
     // @Field: LonE: Vehicle estimated longitude (degrees * 1E7)
-    // @Field: AltE: Vehicle estimated absolute altitude (centimeters)
+    // @Field: AltE: Vehicle estimated altitude (centimeters)
+    // @Field: FrmE: Vehicle estimated altitude Frame
     AP::logger().WriteStreaming("FOLL",
-                                "TimeUS,Lat,Lon,Alt,VelN,VelE,VelD,LatE,LonE,AltE",  // labels
-                                "sDUmnnnDUm",    // units
-                                "F--B000--B",    // mults
-                                "QLLifffLLi",    // fmt
+                                "TimeUS,Lat,Lon,Alt,VelN,VelE,VelD,LatE,LonE,AltE,FrmE",  // labels
+                                "sDUmnnnDUm-",    // units
+                                "F--B000--B-",    // mults
+                                "QLLifffLLib",    // fmt
                                 AP_HAL::micros64(),
                                 _target_location.lat,
                                 _target_location.lng,
@@ -900,7 +920,8 @@ void AP_Follow::Log_Write_FOLL()
                                 (double)_target_vel_ned_ms.z,
                                 loc_estimate.lat,
                                 loc_estimate.lng,
-                                loc_estimate.alt
+                                loc_estimate.alt,
+                                loc_estimate.get_alt_frame()
                                 );
 }
 #endif  // HAL_LOGGING_ENABLED
@@ -918,7 +939,7 @@ bool AP_Follow::have_target(void) const
     }
 
     // check for timeout
-    if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
+    if ((_last_location_update_ms == 0) || ((AP_HAL::millis() - _last_location_update_ms) > (uint32_t)(_timeout * 1000.0f))) {
         return false;
     }
     return true;

libraries/AP_Follow/AP_Follow.h

@@ -99,7 +99,7 @@ class AP_Follow
     // Retrieves the estimated global location and velocity of the target
     bool get_target_location_and_velocity(Location &loc, Vector3f &vel_ned);
 
-    // Retrieves the estimated global location and velocity of the target, including configured positional offsets (for LUA bindings).
+    // Retrieves the estimated global location including configured positional offsets and velocity of the target,  (for LUA bindings).
     bool get_target_location_and_velocity_ofs(Location &loc, Vector3f &vel_ned);
 
     // Retrieves the estimated target heading in degrees (0° = North, 90° = East) for LUA bindings.
@@ -115,8 +115,8 @@ class AP_Follow
     // Accessor Methods
     //==========================================================================
 
-    // get target sysid
-    uint8_t get_target_sysid() const { return _sysid.get(); }
+    // get target sysid as a 32 bit number to allow for future expansion of MAV_SYSID
+    uint32_t get_target_sysid() const { return (uint32_t)_sysid.get(); }
 
     // get position controller.  this controller is not used within this library but it is convenient to hold it here
     const AC_P& get_pos_p() const { return _p_pos; }
@@ -199,9 +199,10 @@ class AP_Follow
     AP_Int8     _offset_type;       // Offset frame type: 0 = NED, 1 = relative to lead vehicle heading
     AP_Vector3f _offset_m;          // Offset from lead vehicle (meters), in NED or FRD frame depending on _offset_type
     AP_Int8     _yaw_behave;        // Yaw behavior mode (see YawBehave enum)
-    AP_Int8     _alt_type;          // Altitude reference: 0 = absolute, 1 = relative to home
+    AP_Enum<Location::AltFrame>    _alt_type;          // altitude source for follow mode
     AC_P        _p_pos;             // Position error P-controller for optional altitude following
     AP_Int16    _options;           // Bitmask of follow behavior options (e.g., mount follow, etc.)
+    AP_Float    _timeout;           // position estimate timeout after x milliseconds
 
     AP_Float    _accel_max_ne_mss;  // Max horizontal acceleration for kinematic shaping (m/s²)
     AP_Float    _jerk_max_ne_msss;  // Max horizontal jerk for kinematic shaping (m/s³)