Copter: Loiter_Turns moves to edge of circle if location provided

This commit is contained in:
Randy Mackay 2014-04-16 16:23:24 +09:00
parent f6676be400
commit e6d2692eab
3 changed files with 79 additions and 13 deletions

View File

@ -19,6 +19,7 @@ static void do_roi(const AP_Mission::Mission_Command& cmd);
static void do_parachute(const AP_Mission::Mission_Command& cmd);
#endif
static bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);
static bool verify_circle(const AP_Mission::Mission_Command& cmd);
static bool verify_spline_wp(const AP_Mission::Mission_Command& cmd);
static void auto_spline_start(const Vector3f& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Vector3f& next_spline_destination);
@ -192,7 +193,7 @@ static bool verify_command(const AP_Mission::Mission_Command& cmd)
break;
case MAV_CMD_NAV_LOITER_TURNS:
return verify_circle();
return verify_circle(cmd);
break;
case MAV_CMD_NAV_LOITER_TIME:
@ -360,24 +361,35 @@ static void do_circle(const AP_Mission::Mission_Command& cmd)
{
Vector3f curr_pos = inertial_nav.get_position();
Vector3f circle_center = pv_location_to_vector(cmd.content.location);
bool move_to_edge_required = false;
// set target altitude if not provided
if (circle_center.z == 0) {
if (cmd.content.location.alt == 0) {
circle_center.z = curr_pos.z;
} else {
move_to_edge_required = true;
}
// set lat/lon position if not provided
// To-Do: use stopping point instead of current location
// To-Do: use previous command's destination if it was a straight line or spline waypoint command
if (cmd.content.location.lat == 0 && cmd.content.location.lng == 0) {
circle_center.x = curr_pos.x;
circle_center.y = curr_pos.y;
} else {
move_to_edge_required = true;
}
// start auto_circle
auto_circle_start(circle_center);
// set circle controller's center
circle_nav.set_center(circle_center);
// record number of desired rotations from mission command
circle_desired_rotations = cmd.p1;
// check if we need to move to edge of circle
if (move_to_edge_required) {
// move to edge of circle (verify_circle) will ensure we begin circling once we reach the edge
auto_circle_movetoedge_start();
} else {
// start circling
auto_circle_start();
}
}
// do_loiter_time - initiate loitering at a point for a given time period
@ -568,10 +580,33 @@ static bool verify_loiter_time()
}
// verify_circle - check if we have circled the point enough
static bool verify_circle()
static bool verify_circle(const AP_Mission::Mission_Command& cmd)
{
// have we rotated around the center enough times?
return fabsf(circle_nav.get_angle_total()/(2*M_PI)) >= circle_desired_rotations;
// check if we've reached the edge
if (auto_mode == Auto_CircleMoveToEdge) {
if (wp_nav.reached_wp_destination()) {
Vector3f curr_pos = inertial_nav.get_position();
Vector3f circle_center = pv_location_to_vector(cmd.content.location);
// set target altitude if not provided
if (circle_center.z == 0) {
circle_center.z = curr_pos.z;
}
// set lat/lon position if not provided
if (cmd.content.location.lat == 0 && cmd.content.location.lng == 0) {
circle_center.x = curr_pos.x;
circle_center.y = curr_pos.y;
}
// start circling
auto_circle_start();
}
return false;
}
// check if we have completed circling
return fabsf(circle_nav.get_angle_total()/(2*M_PI)) >= cmd.p1;
}
// externs to remove compiler warning
@ -633,6 +668,7 @@ static void do_change_alt(const AP_Mission::Mission_Command& cmd)
case Auto_RTL:
// ignore altitude
break;
case Auto_CircleMoveToEdge:
case Auto_Circle:
// move circle altitude up to target (we will need to store this target in circle class)
break;

View File

@ -47,6 +47,7 @@ static void auto_run()
break;
case Auto_WP:
case Auto_CircleMoveToEdge:
auto_wp_run();
break;
@ -304,13 +305,41 @@ void auto_rtl_run()
rtl_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
static void auto_circle_movetoedge_start()
{
// check our distance from edge of circle
Vector3f circle_edge;
circle_nav.get_closest_point_on_circle(circle_edge);
// set the state to move to the edge of the circle
auto_mode = Auto_CircleMoveToEdge;
// initialise wpnav to move to edge of circle
wp_nav.set_wp_destination(circle_edge);
// if we are outside the circle, point at the edge, otherwise hold yaw
const Vector3f &curr_pos = inertial_nav.get_position();
const Vector3f &circle_center = circle_nav.get_center();
float dist_to_center = pythagorous2(circle_center.x - curr_pos.x, circle_center.y - curr_pos.y);
if (dist_to_center > circle_nav.get_radius() && dist_to_center > 500) {
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
} else {
// vehicle is within circle so hold yaw to avoid spinning as we move to edge of circle
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
}
// auto_circle_start - initialises controller to fly a circle in AUTO flight mode
static void auto_circle_start(const Vector3f& center)
static void auto_circle_start()
{
auto_mode = Auto_Circle;
// set circle center
circle_nav.set_center(center);
// initialise circle controller
// center was set in do_circle so initialise with current center
circle_nav.init(circle_nav.get_center());
}
// auto_circle_run - circle in AUTO flight mode

View File

@ -175,6 +175,7 @@ enum AutoMode {
Auto_WP,
Auto_Land,
Auto_RTL,
Auto_CircleMoveToEdge,
Auto_Circle,
Auto_Spline
};