diff --git a/libraries/AC_PrecLand/AC_PrecLand.cpp b/libraries/AC_PrecLand/AC_PrecLand.cpp index a2fb9d9ad2..1f5eb04f94 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.cpp +++ b/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) { diff --git a/libraries/AC_PrecLand/AC_PrecLand.h b/libraries/AC_PrecLand/AC_PrecLand.h index 056206696c..3dbac8a6ae 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.h +++ b/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