Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 18:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: loiter turns radius from command's p1 field

Browse Source
This commit is contained in:
Randy Mackay 2014-06-11 16:18:35 +09:00 committed by Andrew Tridgell
parent 9b81e46e61
commit e0f4a570c6
1 changed files with 7 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ArduCopter/commands_logic.pde
View File

@ -373,6 +373,7 @@ static void do_circle(const AP_Mission::Mission_Command& cmd)
{ {
Vector3f curr_pos = inertial_nav.get_position(); Vector3f curr_pos = inertial_nav.get_position();
Vector3f circle_center = pv_location_to_vector(cmd.content.location); Vector3f circle_center = pv_location_to_vector(cmd.content.location);
uint8_t circle_radius_m = HIGHBYTE(cmd.p1); // circle radius held in high byte of p1
bool move_to_edge_required = false; bool move_to_edge_required = false;
// set target altitude if not provided // set target altitude if not provided
@ -394,6 +395,11 @@ static void do_circle(const AP_Mission::Mission_Command& cmd)
// set circle controller's center // set circle controller's center
circle_nav.set_center(circle_center); circle_nav.set_center(circle_center);
// set circle radius
if (circle_radius_m != 0) {
circle_nav.set_radius((float)circle_radius_m * 100.0f);
}
// check if we need to move to edge of circle // check if we need to move to edge of circle
if (move_to_edge_required) { if (move_to_edge_required) {
// 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
@ -618,7 +624,7 @@ static bool verify_circle(const AP_Mission::Mission_Command& cmd)
} }
// check if we have completed circling // check if we have completed circling
return fabsf(circle_nav.get_angle_total()/(2*M_PI)) >= cmd.p1; return fabsf(circle_nav.get_angle_total()/(2*M_PI)) >= (float)LOWBYTE(cmd.p1);
} }
// externs to remove compiler warning // externs to remove compiler warning