Rover: added support for finish line waypoints

this ensures waypoints complete, even with bad yaw from mag
interference

APMrover2/APMrover2.pde

@@ -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;

APMrover2/commands.pde

@@ -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;

APMrover2/commands_logic.pde

@@ -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;
 }