From ae5d3103f41efb14be07c77906f4e6b73852ce3d Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Thu, 6 Aug 2020 17:06:30 +0300 Subject: [PATCH] fw l1 controller: added a flag to indicate if navigation has updated - can be used to check if l1 controller ran during a cycle Signed-off-by: RomanBapst --- src/lib/l1/ECL_L1_Pos_Controller.cpp | 8 ++++++++ src/lib/l1/ECL_L1_Pos_Controller.hpp | 7 +++++++ 2 files changed, 15 insertions(+) diff --git a/src/lib/l1/ECL_L1_Pos_Controller.cpp b/src/lib/l1/ECL_L1_Pos_Controller.cpp index f711bb15a2..5174f9d3f1 100644 --- a/src/lib/l1/ECL_L1_Pos_Controller.cpp +++ b/src/lib/l1/ECL_L1_Pos_Controller.cpp @@ -76,6 +76,8 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const Vector2d &vector_A, const Vector2d &vector_B, const Vector2d &vector_curr_position, const Vector2f &ground_speed_vector) { + _has_guidance_updated = true; + /* this follows the logic presented in [1] */ float eta = 0.0f; @@ -211,6 +213,8 @@ void ECL_L1_Pos_Controller::navigate_loiter(const Vector2d &vector_A, const Vector2d &vector_curr_position, float radius, int8_t loiter_direction, const Vector2f &ground_speed_vector) { + _has_guidance_updated = true; + /* the complete guidance logic in this section was proposed by [2] */ /* calculate the gains for the PD loop (circle tracking) */ @@ -312,6 +316,8 @@ ECL_L1_Pos_Controller::navigate_loiter(const Vector2d &vector_A, const Vector2d void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading, const Vector2f &ground_speed_vector) { + _has_guidance_updated = true; + /* the complete guidance logic in this section was proposed by [2] */ /* @@ -348,6 +354,8 @@ void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float cur void ECL_L1_Pos_Controller::navigate_level_flight(float current_heading) { + _has_guidance_updated = true; + /* the logic in this section is trivial, but originally proposed by [2] */ /* reset all heading / error measures resulting in zero roll */ diff --git a/src/lib/l1/ECL_L1_Pos_Controller.hpp b/src/lib/l1/ECL_L1_Pos_Controller.hpp index 4c56239cc0..a2d4d085e3 100644 --- a/src/lib/l1/ECL_L1_Pos_Controller.hpp +++ b/src/lib/l1/ECL_L1_Pos_Controller.hpp @@ -203,6 +203,10 @@ public: */ void set_dt(float dt) { _dt = dt;} + void reset_has_guidance_updated() { _has_guidance_updated = false; } + + bool has_guidance_updated() { return _has_guidance_updated; } + private: float _lateral_accel{0.0f}; ///< Lateral acceleration setpoint in m/s^2 @@ -224,6 +228,9 @@ private: float _roll_slew_rate{0.0f}; ///< roll angle setpoint slew rate limit in rad/s float _dt{0}; ///< control loop time in seconds + bool _has_guidance_updated = + false; ///< this flag is set to true by any of the guidance methods. This flag has to be manually reset using has_guidance_updated_reset() + /** * Convert a 2D vector from WGS84 to planar coordinates. *