Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: Obey loiter_ccw flag for loiter turns

Browse Source
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); void takeoff_start(const Location& dest_loc);
bool wp_start(const Location& dest_loc); bool wp_start(const Location& dest_loc);
void land_start(); 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 circle_start();
void nav_guided_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 // 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 // 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 // set circle center
copter.circle_nav->set_center(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); 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 // check our distance from edge of circle
Vector3f circle_edge_neu; Vector3f circle_edge_neu;
copter.circle_nav->get_closest_point_on_circle(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() void ModeAuto::circle_start()
{ {
// initialise circle controller // 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) { if (auto_yaw.mode() != AutoYaw::Mode::ROI) {
auto_yaw.set_mode(AutoYaw::Mode::CIRCLE); 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; 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 // 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 // do_loiter_time - initiate loitering at a point for a given time period