AC_PrecLand: Add features to check if target is moving and figure out its velocity

libraries/AC_PrecLand/AC_PrecLand.cpp

@@ -162,6 +162,13 @@ const AP_Param::GroupInfo AC_PrecLand::var_info[] = {
     // @Units: m
     AP_GROUPINFO("ALT_MAX", 16, AC_PrecLand, _sensor_max_alt, 8),
 
+    // @Param: MOVING
+    // @DisplayName: Precision Landing Target stationary/moving
+    // @Description: Precision Landing Target stationary/moving
+    // @Values: 0:Stationary, 1:Moving
+    // @User: Advanced
+    AP_GROUPINFO("MOVING", 17, AC_PrecLand, _moving, 0),
+
     AP_GROUPEND
 };
 
@@ -412,6 +419,31 @@ bool AC_PrecLand::get_target_velocity_relative_cms(Vector2f& ret)
     return true;
 }
 
+// get the absolute velocity of the vehicle
+void AC_PrecLand::get_target_velocity_cms(const Vector2f& vehicle_velocity_cms, Vector2f& target_vel_cms)
+{
+    if (!_moving) {
+        // the target should not be moving
+        target_vel_cms.zero();
+        return;
+    }
+    if ((EstimatorType)_estimator_type.get() == EstimatorType::RAW_SENSOR) {
+        // We do not predict the velocity of the target in this case
+        // assume velocity to be zero
+        target_vel_cms.zero();
+        return;
+    }
+    Vector2f target_vel_rel_cms;
+    if (!get_target_velocity_relative_cms(target_vel_rel_cms)) {
+        // Don't know where the target is
+        // assume velocity to be zero
+        target_vel_cms.zero();
+        return;
+    }
+    // return the absolute velocity
+    target_vel_cms  = target_vel_rel_cms + vehicle_velocity_cms;
+}
+
 // handle_msg - Process a LANDING_TARGET mavlink message
 void AC_PrecLand::handle_msg(const mavlink_landing_target_t &packet, uint32_t timestamp_ms)
 {

libraries/AC_PrecLand/AC_PrecLand.h

@@ -72,6 +72,9 @@ public:
     // returns target velocity relative to vehicle
     bool get_target_velocity_relative_cms(Vector2f& ret);
 
+    // get the absolute velocity of the vehicle
+    void get_target_velocity_cms(const Vector2f& vehicle_velocity_cms, Vector2f& target_vel_cms);
+
     // returns true when the landing target has been detected
     bool target_acquired();
 
@@ -165,6 +168,7 @@ private:
     AP_Int8                     _retry_behave;      // Action to do when trying a landing retry
     AP_Float                    _sensor_min_alt;     // PrecLand minimum height required for detecting target
     AP_Float                    _sensor_max_alt;     // PrecLand maximum height the sensor can detect target
+    AP_Int8                     _moving;             // True if the landing target is moving (non-stationary)
 
     uint32_t                    _last_update_ms;    // system time in millisecond when update was last called
     bool                        _target_acquired;   // true if target has been seen recently after estimator is initialized