 #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();
+	if(!isRadioOn(driveInput)){
+
+
+		if(ttime < millis()){
+			ttime = millis()+200;
+			observeHeading();
+			readAccelerometer();
+
+			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();
+			}
+
+			reportLocation();
 		}
-		readAccelerometer();
-		reportLocation();
+
+
+	} else {
+
+		if(ttime < millis()){
+			ttime = millis()+200;
+			observeHeading();
+			readAccelerometer();
+			reportLocation();
+		}
+
+		int tmp = getRadio(steerInput);
+		steer.write( tmp );
+		backSteer.write( 180-tmp );
+		drive.write( getRadio(driveInput) );
+
 	}
 }
 

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

void loop() {

#include "DroneUtils.h"
#include "GreatCircle.h"

//#define DEBUG
#define LEARN_COMPASS_ERROR
#define UBLOX_GPS_FIX

const double FILTER_PARAM = .992; //closer to 1 is slower "learning"
const double POINT_RADIUS = .01; //in miles, margin for error in rover location
const int BLUE = 25;
const int YLLW = 26;
