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

Copter: Obey loiter_ccw flag for loiter turns

This commit is contained in:
rishabsingh3003 2023-02-15 03:47:31 +05:30 committed by Andrew Tridgell
parent 5cb77d9f81
commit 7a8f8f7907
2 changed files with 12 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/mode.h
View File

@ -469,7 +469,7 @@ public:
void takeoff_start(const Location& dest_loc);
bool wp_start(const Location& dest_loc);
void land_start();
void circle_movetoedge_start(const Location &circle_center, float radius_m);
void circle_movetoedge_start(const Location &circle_center, float radius_m, bool ccw_turn);
void circle_start();
void nav_guided_start();

14
ArduCopter/mode_auto.cpp
View File

@ -433,7 +433,7 @@ void ModeAuto::land_start()
// 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 performed all required GPS_ok checks
void ModeAuto::circle_movetoedge_start(const Location &circle_center, float radius_m)
void ModeAuto::circle_movetoedge_start(const Location &circle_center, float radius_m, bool ccw_turn)
{
// set circle center
copter.circle_nav->set_center(circle_center);
@ -443,6 +443,11 @@ void ModeAuto::circle_movetoedge_start(const Location &circle_center, float radi
copter.circle_nav->set_radius_cm(radius_m * 100.0f);
}
// set circle direction by using rate
float current_rate = copter.circle_nav->get_rate();
current_rate = ccw_turn ? -fabsf(current_rate) : fabsf(current_rate);
copter.circle_nav->set_rate(current_rate);
// check our distance from edge of circle
Vector3f circle_edge_neu;
copter.circle_nav->get_closest_point_on_circle(circle_edge_neu);
@ -487,7 +492,7 @@ void ModeAuto::circle_movetoedge_start(const Location &circle_center, float radi
void ModeAuto::circle_start()
{
// initialise circle controller
copter.circle_nav->init(copter.circle_nav->get_center(), copter.circle_nav->center_is_terrain_alt());
copter.circle_nav->init(copter.circle_nav->get_center(), copter.circle_nav->center_is_terrain_alt(), copter.circle_nav->get_rate());
if (auto_yaw.mode() != AutoYaw::Mode::ROI) {
auto_yaw.set_mode(AutoYaw::Mode::CIRCLE);
@ -1631,8 +1636,11 @@ void ModeAuto::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
circle_movetoedge_start(circle_center, circle_radius_m);
circle_movetoedge_start(circle_center, circle_radius_m, circle_direction_ccw);
}
// do_loiter_time - initiate loitering at a point for a given time period