Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Sub: Obey loiter_ccw flag for loiter turns

This commit is contained in:
rishabsingh3003 2023-02-15 03:47:46 +05:30 committed by Andrew Tridgell
parent 7a8f8f7907
commit 34702ed986
3 changed files with 13 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduSub/Sub.h
View File

@ -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();

6
ArduSub/commands_logic.cpp
View File

@ -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

9
ArduSub/control_auto.cpp
View File

@ -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