From 34702ed986fa99ea42bdf0da7dc891cb172febdd Mon Sep 17 00:00:00 2001 From: rishabsingh3003 Date: Wed, 15 Feb 2023 03:47:46 +0530 Subject: [PATCH] Sub: Obey loiter_ccw flag for loiter turns --- ArduSub/Sub.h | 2 +- ArduSub/commands_logic.cpp | 6 +++++- ArduSub/control_auto.cpp | 9 +++++++-- 3 files changed, 13 insertions(+), 4 deletions(-) diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 747b59baed..95205f84b2 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -426,7 +426,7 @@ private: void auto_wp_start(const Vector3f& destination); void auto_wp_start(const Location& dest_loc); void auto_wp_run(); - void auto_circle_movetoedge_start(const Location &circle_center, float radius_m); + void auto_circle_movetoedge_start(const Location &circle_center, float radius_m, bool ccw_turn); void auto_circle_start(); void auto_circle_run(); void auto_nav_guided_start(); diff --git a/ArduSub/commands_logic.cpp b/ArduSub/commands_logic.cpp index bfeb57bc5f..51c5e7c00e 100644 --- a/ArduSub/commands_logic.cpp +++ b/ArduSub/commands_logic.cpp @@ -358,8 +358,12 @@ void Sub::do_circle(const AP_Mission::Mission_Command& cmd) circle_radius_m *= 10; } + + // true if circle should be ccw + const bool circle_direction_ccw = cmd.content.location.loiter_ccw; + // move to edge of circle (verify_circle) will ensure we begin circling once we reach the edge - auto_circle_movetoedge_start(circle_center, circle_radius_m); + auto_circle_movetoedge_start(circle_center, circle_radius_m, circle_direction_ccw); } // do_loiter_time - initiate loitering at a point for a given time period diff --git a/ArduSub/control_auto.cpp b/ArduSub/control_auto.cpp index 4860cbc15c..4631e8dcef 100644 --- a/ArduSub/control_auto.cpp +++ b/ArduSub/control_auto.cpp @@ -173,7 +173,7 @@ void Sub::auto_wp_run() // auto_circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location // we assume the caller has set the circle's circle with circle_nav.set_center() // we assume the caller has performed all required GPS_ok checks -void Sub::auto_circle_movetoedge_start(const Location &circle_center, float radius_m) +void Sub::auto_circle_movetoedge_start(const Location &circle_center, float radius_m, bool ccw_turn) { // set circle center circle_nav.set_center(circle_center); @@ -183,6 +183,11 @@ void Sub::auto_circle_movetoedge_start(const Location &circle_center, float radi circle_nav.set_radius_cm(radius_m * 100.0f); } + // set circle direction by using rate + float current_rate = circle_nav.get_rate(); + current_rate = ccw_turn ? -fabsf(current_rate) : fabsf(current_rate); + circle_nav.set_rate(current_rate); + // check our distance from edge of circle Vector3f circle_edge_neu; circle_nav.get_closest_point_on_circle(circle_edge_neu); @@ -225,7 +230,7 @@ void Sub::auto_circle_start() auto_mode = Auto_Circle; // initialise circle controller - circle_nav.init(circle_nav.get_center(), circle_nav.center_is_terrain_alt()); + circle_nav.init(circle_nav.get_center(), circle_nav.center_is_terrain_alt(), circle_nav.get_rate()); } // auto_circle_run - circle in AUTO flight mode