 		#endif
 	}
 #ifndef DEBUG
-	trueHeading = CompassHeading+gpsFoundShift;
+	trueHeading = compassHeading+gpsFoundShift;
 #endif
 }
 

#ifndef DEBUG
	updateGPS();
#endif
	if(ttime < millis()){
		ttime = millis()+200;
		observeHeading();
		if(manager.numWaypoints() >= 1 && !nmea.getWarning()){
			distance = calcDistance(manager.getTargetWaypoint(), location);
			if(distance > POINT_RADIUS)
				navigate(manager.getTargetWaypoint());
			else if(manager.getTargetIndex() < manager.numWaypoints()-1)
				manager.advanceTargetIndex();
			else if (manager.loopWaypoints())
				manager.setTargetIndex(0);
			else
				stop();
		}
		readAccelerometer();
		reportLocation();
	}
}


	#ifdef UBLOX_GPS_FIX
		Serial1.write(GPS_SETUP, sizeof(GPS_SETUP)); //setup 3DR LEA6 GPS module
	#endif

	getRadio(driveInput);
}

void loop() {
