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

Rover: added support for finish line waypoints 

this ensures waypoints complete, even with bad yaw from mag
interference
This commit is contained in:
Andrew Tridgell 2012-11-18 16:07:50 +11:00
parent 07a7a1acd8
commit 283fb7e606
3 changed files with 42 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
APMrover2/APMrover2.pde
View File

@ -419,11 +419,7 @@ static bool GPS_light;
// This approximation makes calculations integer and it's easy to read
static const float t7 = 10000000.0;
// We use atan2 and other trig techniques to calaculate angles
// We need to scale the longitude up to make these calcs work
// to account for decreasing distance between lines of longitude away from the equator
static float scaleLongUp = 1;
// Sometimes we need to remove the scaling for distance calcs
static float scaleLongDown = 1;
// A counter used to count down valid gps fixes to allow the gps estimate to settle
// before recording our home position (and executing a ground start if we booted with an air start)
static byte ground_start_count = 5;

19
APMrover2/commands.pde
View File

@ -110,15 +110,21 @@ static void set_next_WP(struct Location *wp)
// ---------------------
next_WP = *wp;
// are we already past the waypoint? This happens when we jump
// waypoints, and it can cause us to skip a waypoint. If we are
// past the waypoint when we start on a leg, then use the current
// location as the previous waypoint, to prevent immediately
// considering the waypoint complete
if (location_passed_point(current_loc, prev_WP, next_WP)) {
gcs_send_text_P(SEVERITY_LOW, PSTR("Resetting prev_WP"));
prev_WP = current_loc;
}
// zero out our loiter vals to watch for missed waypoints
loiter_delta = 0;
loiter_sum = 0;
loiter_total = 0;
// this is used to offset the shrinking longitude as we go towards the poles
float rads = (fabs((float)next_WP.lat)/t7) * 0.0174532925;
scaleLongDown = cos(rads);
scaleLongUp = 1.0f/cos(rads);
// this is handy for the groundstation
wp_totalDistance = get_distance(&current_loc, &next_WP);
wp_distance = wp_totalDistance;
@ -144,11 +150,6 @@ static void set_guided_WP(void)
// ---------------------
next_WP = guided_WP;
// this is used to offset the shrinking longitude as we go towards the poles
float rads = (abs(next_WP.lat)/t7) * 0.0174532925;
scaleLongDown = cos(rads);
scaleLongUp = 1.0f/cos(rads);
// this is handy for the groundstation
wp_totalDistance = get_distance(&current_loc, &next_WP);
wp_distance = wp_totalDistance;

29
APMrover2/commands_logic.pde
View File

@ -257,27 +257,36 @@ static bool verify_nav_wp()
hold_course = -1;
update_crosstrack();
if(g.auto_wp_radius)
{
if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) {
gcs_send_text_fmt(PSTR("Reached Waypoint #%i dist %um"),
(unsigned)nav_command_index,
(unsigned)get_distance(&current_loc, &next_WP));
return true;
}
if(g.auto_wp_radius) {
calc_turn_radius(); // JLN update - auto-adap the wp_radius Vs the gspeed and max roll angle
if ((wp_distance > 0) && (wp_distance <= wp_radius)) {
gcs_send_text_fmt(PSTR("Reached Waypoint #%i"),nav_command_index);
return true;
}
} else {
if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) {
gcs_send_text_fmt(PSTR("Reached Waypoint #%i"),nav_command_index);
return true;
}
}
// add in a more complex case
// Doug to do
if(loiter_sum > 300){
// have we circled around the waypoint?
if (loiter_sum > 300) {
gcs_send_text_P(SEVERITY_MEDIUM,PSTR("Missed WP"));
return true;
}
// have we flown past the waypoint?
if (location_passed_point(current_loc, prev_WP, next_WP)) {
gcs_send_text_fmt(PSTR("Passed Waypoint #%i dist %um"),
(unsigned)nav_command_index,
(unsigned)get_distance(&current_loc, &next_WP));
return true;
}
return false;
}