 	if(manager.numWaypoints() <= 0) return;
 
 	distance = calcDistance(manager.getTargetWaypoint(), location);
-	pathHeading = calcHeading(location, manager.getTargetWaypoint());
-	//if there is a waypoint path, find the line to drive from;
-	//either going to waypoint != to the first
-	//or looping is on
+	if(backWaypoint.radLongitude() == 0 || distance*5280.l < 25){
+		pathHeading = calcHeading(location, manager.getTargetWaypoint());
+	} else {
+		double fullDist = calcDistance(backWaypoint, manager.getTargetWaypoint());
+		double AB = calcHeading(backWaypoint, manager.getTargetWaypoint());
+		double AL = calcHeading(backWaypoint, location);
+		double d  = cos(AB-AL)*calcDistance(backWaypoint, location);
+		double D  = d + (fullDist-d)*lineGravity;
+		Point target = extrapPosition(backWaypoint, AB, D);
+		pathHeading = calcHeading(location, target);
+	}
 }
 
 void readAccelerometer(){

const int REVSPEED        = 75;
const int STOP_TIME       = 1500;//time to coast to stop
const int DANGER_TIME     = STOP_TIME+800; //time to backup after last sighting
const uint16_t warn[]     = {850, 1050, 2150, 1050, 850};
const double POINT_RADIUS = .001; //in miles, margin for error in rover location
const double PING_CALIB   = 1500.l;
const double pAngle[5]    = { 79.27l, 36.83l, 0.l, -36.83l, -79.27l};
const double dPlsb        = 4.f/float(0xffff); //dps Per leastSigBit for gyro

HardwareSerial *CommSerial = &Serial;
NMEA			nmea(Serial1);
CommManager		manager(CommSerial);
Point			location(0,0);
HLA				lowFilter (600000, 0);//10 minutes
HLA				highFilter(    10, 0);//10 milliseconds
Servo			servo[3]; //drive, steer, backSteer

		if(distance > POINT_RADIUS) return;

		if(manager.getTargetIndex() < manager.numWaypoints()-1){
			backWaypoint = manager.getTargetWaypoint();
			manager.advanceTargetIndex();
		}
		else if (manager.loopWaypoints()){
			backWaypoint = manager.getTargetWaypoint();
			manager.setTargetIndex(0);
		}
		else{
