diff --git a/libraries/AC_WPNav/AC_Circle.cpp b/libraries/AC_WPNav/AC_Circle.cpp index 51b29d651b..14e74d2e7d 100644 --- a/libraries/AC_WPNav/AC_Circle.cpp +++ b/libraries/AC_WPNav/AC_Circle.cpp @@ -150,6 +150,13 @@ void AC_Circle::set_radius(float radius_cm) calc_velocities(false); } +/// returns true if update has been run recently +/// used by vehicle code to determine if get_yaw() is valid +bool AC_Circle::is_active() const +{ + return (AP_HAL::millis() - _last_update_ms < 200); +} + /// update - update circle controller bool AC_Circle::update() { @@ -219,6 +226,9 @@ bool AC_Circle::update() // update position controller _pos_control.update_xy_controller(); + // set update time + _last_update_ms = AP_HAL::millis(); + return true; } diff --git a/libraries/AC_WPNav/AC_Circle.h b/libraries/AC_WPNav/AC_Circle.h index 177417e173..63143128b0 100644 --- a/libraries/AC_WPNav/AC_Circle.h +++ b/libraries/AC_WPNav/AC_Circle.h @@ -68,6 +68,10 @@ public: float get_pitch() const { return _pos_control.get_pitch(); } float get_yaw() const { return _yaw; } + /// returns true if update has been run recently + /// used by vehicle code to determine if get_yaw() is valid + bool is_active() const; + // get_closest_point_on_circle - returns closest point on the circle // circle's center should already have been set // closest point on the circle will be placed in result @@ -136,6 +140,7 @@ private: float _angular_vel; // angular velocity in radians/sec float _angular_vel_max; // maximum velocity in radians/sec float _angular_accel; // angular acceleration in radians/sec/sec + uint32_t _last_update_ms; // system time of last update // terrain following variables bool _terrain_alt; // true if _center.z is alt-above-terrain, false if alt-above-ekf-origin