Rover: use new angle wrap code

APMrover2/navigation.pde

@@ -42,32 +42,18 @@ static void calc_bearing_error()
 {    
     static butter10hz1_6 butter;
 
-	bearing_error_cd = wrap_180(nav_bearing - ahrs.yaw_sensor);
+	bearing_error_cd = wrap_180_cd(nav_bearing - ahrs.yaw_sensor);
     bearing_error_cd = butter.filter(bearing_error_cd);
 }
 
-static long wrap_360(long error)
-{
-	if (error > 36000)	error -= 36000;
-	if (error < 0)		error += 36000;
-	return error;
-}
-
-static long wrap_180(long error)
-{
-	if (error > 18000)	error -= 36000;
-	if (error < -18000)	error += 36000;
-	return error;
-}
-
 static void update_crosstrack(void)
 {
 	// Crosstrack Error
 	// ----------------
-	if (abs(wrap_180(target_bearing - crosstrack_bearing)) < 4500) {	 // If we are too far off or too close we don't do track following
+	if (abs(wrap_180_cd(target_bearing - crosstrack_bearing)) < 4500) {	 // If we are too far off or too close we don't do track following
 		crosstrack_error = sinf(radians((target_bearing - crosstrack_bearing) / (float)100)) * wp_distance;	 // Meters we are off track line
 		nav_bearing += constrain(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get());
-		nav_bearing = wrap_360(nav_bearing);
+		nav_bearing = wrap_360_cd(nav_bearing);
 	}
 }
 

APMrover2/test.pde

@@ -517,7 +517,7 @@ test_mag(uint8_t argc, const Menu::arg *argv)
                 if (compass.healthy) {
                     Vector3f maggy = compass.get_offsets();
                     cliSerial->printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d,\tXYZoff: %6.2f, %6.2f, %6.2f\n"),
-                                    (wrap_360(ToDeg(heading) * 100)) /100,
+                                    (wrap_360_cd(ToDeg(heading) * 100)) /100,
                                     (int)compass.mag_x,
                                     (int)compass.mag_y,
                                     (int)compass.mag_z,