diff --git a/Pilot/AndroidManifest.xml b/Pilot/AndroidManifest.xml
index 22edb27..200cabf 100755
--- a/Pilot/AndroidManifest.xml
+++ b/Pilot/AndroidManifest.xml
@@ -86,7 +86,7 @@
 	<uses-feature android:name="android.hardware.camera.autofocus" />
 	
 	<uses-sdk
-		android:minSdkVersion="8"
-		android:targetSdkVersion="8"/>
+		android:minSdkVersion="10"
+		android:targetSdkVersion="10"/>
 
 </manifest> diff --git a/Pilot/default.properties b/Pilot/default.properties
index 51e933a..033cddc 100755
--- a/Pilot/default.properties
+++ b/Pilot/default.properties
@@ -10,4 +10,4 @@
 # Indicates whether an apk should be generated for each density.
 split.density=false
 # Project target.
-target=android-8
+target=android-10
diff --git a/at/abraxas/amarino/AmarinoService.java b/at/abraxas/amarino/AmarinoService.java
index 2be4a10..461d3ef 100644
--- a/at/abraxas/amarino/AmarinoService.java
+++ b/at/abraxas/amarino/AmarinoService.java
@@ -306,9 +306,15 @@
 		localDevice.init(this, new ReadyListener() {
 			@Override
 			public void ready() {
-				RemoteDevice device = localDevice.getRemoteForAddr(address);
-				localDevice.destroy();
-				new ConnectThread(device).start();
+				try {
+					RemoteDevice device = localDevice.getRemoteForAddr(address);
+					localDevice.destroy();
+					new ConnectThread(device).start();
+				}
+				catch (NullPointerException e) {
+					Log.e(TAG, "Fuck external libraries.");
+					e.printStackTrace();
+				}
 			}
 		});
 		
diff --git a/org/haldean/chopper/pilot/Angler.java b/org/haldean/chopper/pilot/Angler.java
new file mode 100644
index 0000000..866efa2
--- /dev/null
+++ b/org/haldean/chopper/pilot/Angler.java
@@ -0,0 +1,95 @@
+package org.haldean.chopper.pilot;
+
+import android.util.Log;
+
+public class Angler implements Constants {
+	/** Maximum permissible target velocity, in m/s; larger vectors will be resized */
+	public static final double MAX_VEL = 2.0;
+	
+	/** The maximum angle, in degrees, guidance will permit the chopper to have */
+	public static final double MAX_ANGLE = 10;
+	
+	public static final String TAG = new String("chopper.Angler");
+	
+	private Navigation mNav;
+	private ChopperStatus mStatus;
+	private double[] mNavTarget;
+	private double[] mRelativeTarget;
+	private double[] mCurrent;
+	
+	public Angler(ChopperStatus status, Navigation nav) {
+		mNav = nav;
+		mStatus = status;
+		mCurrent = new double[4];
+		mNavTarget = new double[4];
+		mRelativeTarget = new double[4];
+		
+	}
+	
+	public void getAngleTarget(double[] target) {
+		if ((target == null) || (target.length < 4)) {
+			return;
+		}
+		mNav.evalNextVector(mNavTarget);
+		
+		double mAzimuth = mStatus.getReadingField(AZIMUTH);		
+
+		// Calculate target relative velocity.
+		double theta = mAzimuth * Math.PI / 180.0;
+		mRelativeTarget[0] = mNavTarget[0] * Math.cos(theta) - mNavTarget[1] * Math.sin(theta);
+		mRelativeTarget[1] = mNavTarget[0] * Math.sin(theta) + mNavTarget[1] * Math.cos(theta);
+		mRelativeTarget[2] = mNavTarget[2];
+		mRelativeTarget[3] = mNavTarget[3];
+			
+		//Calculate recorded velocity; reduce, if necessary, to MAXVEL
+		double myVel = 0;
+		for (int i = 0; i < 3; i++) {
+			myVel += Math.pow(mRelativeTarget[i], 2);
+		}
+		myVel = Math.sqrt(myVel);
+		if (myVel > MAX_VEL) {
+			Log.v(TAG, "guid, Reducing requested velocity");
+			double adjustment = MAX_VEL / myVel;
+			for (int i = 0; i < 3; i++) {
+				mRelativeTarget[i] *= adjustment;
+			}
+		}
+		
+		// Calculate current relative velocity.
+		double mGpsBearing = mStatus.getGpsField(BEARING);
+		double phi = (mGpsBearing - mAzimuth) * Math.PI / 180.0;
+		
+		double mGpsSpeed = mStatus.getGpsField(SPEED);
+		mCurrent[0] = mGpsSpeed * Math.sin(phi);
+		mCurrent[1] = mGpsSpeed * Math.cos(phi);
+		mCurrent[2] = mStatus.getGpsField(dALT);
+		mCurrent[3] = mAzimuth;
+		
+		if (mCurrent[0] < mRelativeTarget[0]) {
+			target[0] += 1.0;
+		} else {
+			target[0] -= 1.0;
+		}
+		
+		if (mCurrent[1] < mRelativeTarget[1]) {
+			target[1] += 1.0;
+		} else {
+			target[1] -= 1.0;
+		}
+		
+		target[0] = restrainedTarget(target[0]);
+		target[1] = restrainedTarget(target[1]);
+		target[2] = mRelativeTarget[2];
+		target[3] = mRelativeTarget[3];
+	}
+	
+	private static double restrainedTarget(double requested) {
+		if (requested < -MAX_ANGLE) {
+			return -MAX_ANGLE;
+		}
+		if (requested > MAX_ANGLE) {
+			return MAX_ANGLE;
+		}
+		return requested;
+	}
+}
diff --git a/org/haldean/chopper/pilot/ChopperMain.java b/org/haldean/chopper/pilot/ChopperMain.java
index 211fab1..1a2636c 100644
--- a/org/haldean/chopper/pilot/ChopperMain.java
+++ b/org/haldean/chopper/pilot/ChopperMain.java
@@ -16,13 +16,14 @@
 	/** Tag for logging */
 	public static final String TAG = "chopper.ChopperMain";
 	
-	private boolean telemetry = false;
+	private boolean telemetry = true;
 	
 	/** Prevent duplicate launching of threads if app loses focus **/
 	private static boolean mFirstRun = true;
 	
 	/** Need a permanent reference to this, to destroy it. **/
 	private Guidance guid;
+	private ChopperStatus status;
 	
 	/**
 	 * Holds the wakelock; needed to keep the camera preview rendering on
@@ -79,7 +80,7 @@
         previewHolder.setType(SurfaceHolder.SURFACE_TYPE_PUSH_BUFFERS);
         
         Comm comm = new Comm(true);
-        ChopperStatus status = new ChopperStatus(getApplicationContext());
+        status = new ChopperStatus(getApplicationContext());
         StatusReporter reporter = new StatusReporter(status);
         MakePicture pic = null;
         if (telemetry) {
@@ -95,6 +96,7 @@
         }
         comm.registerReceiver(NAV, nav);
         comm.registerReceiver(CSYS, nav);
+        comm.registerReceiver(CSYS, guid);
         comm.registerReceiver(GUID, guid);
         comm.registerReceiver(GUID, nav);
         
@@ -117,7 +119,6 @@
 	        if (telemetry) {
 	        	new Thread(pic).start();
 	        }
-	        new Thread(nav).start();
 	        new Thread(guid).start();
         }
         catch (Exception e) {
@@ -135,5 +136,7 @@
 		super.onDestroy();
 		if (guid != null)
 			guid.onDestroy();
+		if (status != null)
+			status.close();
 	} 
 }
diff --git a/org/haldean/chopper/pilot/ChopperStatus.java b/org/haldean/chopper/pilot/ChopperStatus.java
index 834d5c2..d3d5133 100644
--- a/org/haldean/chopper/pilot/ChopperStatus.java
+++ b/org/haldean/chopper/pilot/ChopperStatus.java
@@ -1,12 +1,12 @@
 package org.haldean.chopper.pilot;
 
+import java.io.BufferedWriter;
+import java.io.File;
+import java.io.FileWriter;
+import java.io.IOException;
 import java.util.LinkedList;
 import java.util.ListIterator;
-import java.util.concurrent.ExecutorService;
-import java.util.concurrent.Executors;
-import java.util.concurrent.TimeUnit;
 import java.util.concurrent.atomic.AtomicInteger;
-import java.util.concurrent.locks.ReentrantLock;
 
 import android.content.BroadcastReceiver;
 import android.content.Context;
@@ -22,6 +22,7 @@
 import android.location.LocationProvider;
 import android.os.BatteryManager;
 import android.os.Bundle;
+import android.os.Environment;
 import android.os.Looper;
 import android.util.Log;
 
@@ -46,6 +47,9 @@
 	/** Point of "officially" low battery */
 	public final static int LOW_BATT = 30;
 	
+	public static final String logfilename = "fastest.txt";
+	public static BufferedWriter logwriter;
+	
 	/** Tag for logging */
 	public static final String TAG = "chopper.ChopperStatus";
 	
@@ -57,9 +61,6 @@
 	 * Value of '0' means maximum accuracy. */
 	private static final long GPS_MIN_TIME = 0;
 	
-	/** Number of threads in the mutator pool */
-	private static final int sNumMutatorThreads = 5;
-	
 	/** Used to obtain location manager. */
 	private Context mContext;
 	
@@ -73,15 +74,15 @@
 	private float mGpsAccuracy; //accuracy of said reading
 	
 	/** Lock for gps extras */
-	private ReentrantLock mGpsExtrasLock;
+	private Object mGpsExtrasLock;
 	
 	/** Locks for fields in gps[]. */
-	private ReentrantLock[] mGpsLock;
-	
-	/** Number of satellites used to obtain last GPS reading.  This field is only accessed by one thread, so no lock is needed. */
+	private Object[] mGpsLock;
+	
+	/** Number of satellites used to obtain last GPS reading. */
 	private int mGpsNumSats; //number of satellites used to collect last reading
 	
-	/** Timestamp of last GPS reading.  This field is only accessed by one thread, so no lock is needed. */
+	/** Timestamp of last GPS reading. */
 	private long mGpsTimeStamp; //timestamp of the reading
 	
 	/** Stores the location object last returned by the GPS. */
@@ -90,30 +91,25 @@
 	/** Max battery level. */
 	private final AtomicInteger mMaxBatt = new AtomicInteger(100);
 
-	/** Lock for motorspeed[]. */
-	private ReentrantLock mMotorLock;
-	
 	/** Stores the speeds last submitted to the motors. */
 	private double[] mMotorSpeed = new double[4];
 	
-	/** Lock for mMotorPower[] */
-	private ReentrantLock mMotorPowerLock;
-	
 	/** Stores power levels for the motors. */
 	private double[] mMotorPower = new double[4];
 	
-	/** Thread pool for mutator methods */
-	private ExecutorService mMutatorPool;
-	
 	/** Holds data from various sensors, like gyroscope, acceleration and magnetic flux. */
 	private double[] mReading = new double[SENSORS]; //last known data for a given sensor
 
 	/** Locks for fields in reading[]. */
-	private ReentrantLock[] mReadingLock;
+	private Object[] mReadingLock;
 	
 	/** List of registered receivers */
 	private LinkedList<Receivable> mRec;
 	
+	private float[] orientation = new float[3];
+	private float[] rotationMatrix = new float[9];
+	private float[] flux = new float[3];
+
 	/**
 	 * Initializes the locks, registers application context for runtime use.
 	 * @param mycontext Application context
@@ -123,22 +119,27 @@
 		mRec = new LinkedList<Receivable>();
 		
 		//Initialize the data locks
-		mReadingLock = new ReentrantLock[SENSORS];
+		mReadingLock = new Object[SENSORS];
 		
 		for (int i = 0; i < SENSORS; i++) {
-			mReadingLock[i] = new ReentrantLock();
+			mReadingLock[i] = new Object();
 		}
 		
-		mGpsLock = new ReentrantLock[GPS_FIELDS];
+		mGpsLock = new Object[GPS_FIELDS];
 		for (int i = 0; i < GPS_FIELDS; i++) {
-			mGpsLock[i] = new ReentrantLock();
-		}
-		
-		mMotorLock = new ReentrantLock();
-		mGpsExtrasLock = new ReentrantLock();
-		mMotorPowerLock = new ReentrantLock();
-		
-		mMutatorPool = Executors.newFixedThreadPool(sNumMutatorThreads);
+			mGpsLock[i] = new Object();
+		}
+		mGpsExtrasLock = new Object();
+	}
+	
+	public void close() {
+		if (logwriter != null) {
+			try {
+				logwriter.close();
+			} catch (IOException e) {
+				Log.e(TAG, "Canno close logfile");
+			}
+		}
 	}
 	
 	/**
@@ -152,23 +153,13 @@
 	/**
 	 * Gets GPS "extras" data, through a string of the form accuracy:numberOfSatellitesInLatFix:timeStampOfLastFix
 	 * @return The data string
-	 * @throws IllegalAccessException If the lock is currently in use.
-	 */
-	public String getGpsExtrasNow() throws IllegalAccessException {
+	 */
+	public String getGpsExtras() {
 		String gpsData = "";
-		try {
-			if (mGpsExtrasLock.tryLock(0, TimeUnit.SECONDS)) {
-				gpsData += mGpsAccuracy + 
-				":" + mGpsNumSats +
-				":" + mGpsTimeStamp;
-				mGpsExtrasLock.unlock();
-			}
-			else {
-				throw new InterruptedException();
-			}
-		}
-		catch (InterruptedException e) {
-			throw new IllegalAccessException();
+		synchronized (mGpsExtrasLock) {
+			gpsData += mGpsAccuracy + 
+			":" + mGpsNumSats +
+			":" + mGpsTimeStamp;
 		}
 		return gpsData;
 	}
@@ -180,57 +171,16 @@
 	 */
 	public double getGpsField(int whichField) {
 		double myValue;
-		mGpsLock[whichField].lock();
-		myValue = mGps[whichField];
-		mGpsLock[whichField].unlock();
+		synchronized (mGpsLock[whichField]) {
+			myValue = mGps[whichField];
+		}
 		return myValue;
 	}
 	
-	/**
-	 * Returns the value stored at the specified GPS index, if its lock is available.  Otherwise, throws an exception.
-	 * @param whichField The index of the desired GPS data.
-	 * @return The desired GPS data.
-	 * @throws IllegalAccessException If the lock for the desired data is unavailable.
-	 */
-	public double getGpsFieldNow(int whichField) throws IllegalAccessException {
-		double myValue;
-		try {
-			if (mGpsLock[whichField].tryLock(0, TimeUnit.SECONDS)) {
-				myValue = mGps[whichField];
-				mGpsLock[whichField].unlock();
-			}
-			else {
-				throw new InterruptedException();
-			}
-		}
-		catch (InterruptedException e) {
-			Log.w(TAG, "GPS Field " + whichField + " is locked.");
-			throw new IllegalAccessException();
-		}
-		return myValue;
-	}
-	
-	/**
-	 * Returns the value stored at the specified GPS index, if its lock is available.  Otherwise, returns the supplied default value.
-	 * @param whichField The index of the desired GPS data.
-	 * @param expectedValue The default to return, should its lock be unavailable.
-	 * @return Either the GPS data or the supplied default, depending on whether or not its lock is available.
-	 */
-	public double getGpsFieldNow(int whichField, double expectedValue) {
-		double myValue;
-		try {
-			if (mGpsLock[whichField].tryLock(0, TimeUnit.SECONDS)) {
-				myValue = mGps[whichField];
-				mGpsLock[whichField].unlock();
-			}
-			else {
-				throw new InterruptedException();
-			}
-		}
-		catch (InterruptedException e) {
-			myValue = expectedValue;
-		}
-		return myValue;
+	public long getGpsTimeStamp() {
+		synchronized(mGpsExtrasLock) {
+			return mGpsTimeStamp;
+		}
 	}
 	
 	/**
@@ -249,51 +199,27 @@
 	
 	/**
 	 * Obtains the current motor speeds.
-	 * @return A copy of the array containing the motor speeds.
-	 * @throws IllegalAccessException If the lock is unavailable.
-	 */
-	public double[] getMotorFieldsNow() throws IllegalAccessException {
-		double[] myValues = new double[4];
-		try {
-			if (mMotorLock.tryLock(0, TimeUnit.SECONDS)) {
-				for (int i = 0; i < 4; i++) {
-					myValues[i] = mMotorSpeed[i];
-				}
-				mMotorLock.unlock();
-			}
-			else {
-				throw new InterruptedException();
-			}
-		}
-		catch (InterruptedException e) {
-			Log.w(TAG, "motorspeed is locked.");
-			throw new IllegalAccessException();
-		}
-		return myValues;
+	 * @param myValues A copy of the array containing the motor speeds.  Must have length >= 4.
+	 */
+	public void getMotorFields (double[] myValues){
+		if (myValues.length < 4) {
+			throw new IllegalArgumentException();
+		}
+		synchronized (mMotorSpeed) {
+			System.arraycopy(mMotorSpeed, 0, myValues, 0, 4);
+		}
 	}
 	
 	/** Obtains the current motor power levels.
-	 * @return A copy of the array containing the motor power levels.
-	 * @throws IllegalAccessException If the lock is unavailable.
-	 */
-	public double[] getMotorPowerFieldsNow() throws IllegalAccessException {
-		double[] myValues = new double[4];
-		try {
-			if (mMotorPowerLock.tryLock(0, TimeUnit.SECONDS)) {
-				for (int i = 0; i < 4; i++) {
-					myValues[i] = mMotorPower[i];
-				}
-				mMotorPowerLock.unlock();
-			}
-			else {
-				throw new InterruptedException();
-			}
-		}
-		catch (InterruptedException e) {
-			Log.w(TAG, "motorpowers is locked.");
-			throw new IllegalAccessException();
-		}
-		return myValues;
+	 * @param myValues A copy of the array containing the motor power levels.  Must have length >= 4.
+	 */
+	public void getMotorPowerFields(double[] myValues) {
+		if (myValues.length < 4) {
+			throw new IllegalArgumentException();
+		}
+		synchronized (mMotorPower) {
+			System.arraycopy(mMotorPower, 0, myValues, 0, 4);
+		}
 	}
 	
 	/**
@@ -304,7 +230,14 @@
 		//System.out.println("ChopperStatus run() thread ID " + getId());
 		Looper.prepare();
 		Thread.currentThread().setName("ChopperStatus");
-		
+		File root = Environment.getExternalStorageDirectory();
+		if (root == null) Log.e(TAG, "No root directory found");
+		try {
+			logwriter = new BufferedWriter(new FileWriter(root + "/chopper/" + logfilename, false));
+		} catch (IOException e) {
+			Log.e(TAG, "No ChopperStatus logfile");
+			e.printStackTrace();
+		}
         /* Register to receive battery status updates */
         BroadcastReceiver batteryInfo = new BroadcastReceiver() {
 			public void onReceive(Context context, Intent intent) {
@@ -322,16 +255,18 @@
         };
 	
         /* Gets a sensor manager */
-		SensorManager sensors = (SensorManager) mContext.getSystemService(Context.SENSOR_SERVICE);
+		final SensorManager sensors = (SensorManager) mContext.getSystemService(Context.SENSOR_SERVICE);
+		/* Registers this class as a sensor listener for every necessary sensor. */
+		//sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_ACCELEROMETER), SensorManager.SENSOR_DELAY_FASTEST);
+		//sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_LIGHT), SensorManager.SENSOR_DELAY_NORMAL);
+		sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD), SensorManager.SENSOR_DELAY_FASTEST);				
 		
-		/* Registers this class as a sensor listener for every necessary sensor. */
-		sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_ACCELEROMETER), SensorManager.SENSOR_DELAY_NORMAL);
-		//sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_LIGHT), SensorManager.SENSOR_DELAY_NORMAL);
-		//sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD), SensorManager.SENSOR_DELAY_NORMAL);
-		sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_ORIENTATION), SensorManager.SENSOR_DELAY_FASTEST);
+		//sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_GYROSCOPE), SensorManager.SENSOR_DELAY_FASTEST);
 		//sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_PRESSURE), SensorManager.SENSOR_DELAY_NORMAL);
 		//sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_PROXIMITY), SensorManager.SENSOR_DELAY_NORMAL);
-		sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_TEMPERATURE), SensorManager.SENSOR_DELAY_NORMAL);
+		//sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_TEMPERATURE), SensorManager.SENSOR_DELAY_NORMAL);
+		sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_GRAVITY), SensorManager.SENSOR_DELAY_FASTEST);				
+		
 
 		/* Initialize GPS reading: */
 		LocationManager LocMan = (LocationManager) mContext.getSystemService(Context.LOCATION_SERVICE);
@@ -342,58 +277,15 @@
 		Looper.loop();
 	}
 	
-	public double getReadingField(int whichField) {
-		double myValue;
-		mReadingLock[whichField].lock();
-		myValue = mReading[whichField];
-		mReadingLock[whichField].unlock();
-		return myValue;
-	}
-	
 	/**
 	 * Returns the reading specified by the supplied index, if its lock is available.  Otherwise, throws an exception.
 	 * @param whichField The index of the desired reading.
 	 * @return The desired reading.
-	 * @throws IllegalAccessException If the lock for the desired reading is unavailable.
-	 */
-	public double getReadingFieldNow(int whichField) throws IllegalAccessException {
+	 */
+	public double getReadingField(int whichField) {
 		double myValue;
-		try {
-			if (mReadingLock[whichField].tryLock(0, TimeUnit.SECONDS)) {
-				myValue = mReading[whichField];
-				mReadingLock[whichField].unlock();
-			}
-			else {
-				throw new InterruptedException();
-			}
-		}
-		catch (InterruptedException e) {
-			Log.w(TAG, "Reading field " + whichField + " is locked.");
-			throw new IllegalAccessException();
-		}
-		return myValue;
-	}
-	
-	/**
-	 * Returns the reading specified by the supplied index, if its lock is available.  Otherwise, returns the supplied default value.
-	 * @param whichField The index of the desired reading.
-	 * @param expectedValue The default to return, should its lock be unavailable.
-	 * @return Either the reading or the supplied default, depending on whether or not its lock is available.
-	 */
-	public double getReadingFieldNow(int whichField, double expectedValue) {
-		double myValue;
-		try {
-			if (mReadingLock[whichField].tryLock(0, TimeUnit.SECONDS)) {
-				myValue = mReading[whichField];
-				mReadingLock[whichField].unlock();
-			}
-			else {
-				throw new InterruptedException();
-			}
-		}
-		catch (InterruptedException e) {
-			Log.e(TAG, "Reading field " + whichField + " is locked.");
-			myValue = expectedValue;
+		synchronized (mReadingLock[whichField]) {
+			myValue = mReading[whichField];
 		}
 		return myValue;
 	}
@@ -421,9 +313,10 @@
 			/* Vertical velocity does not update until vertical position does; prevents false conclusions that vertical velocity == 0 */
 			double oldAlt = getGpsField(ALTITUDE);
 			if (newalt != oldAlt) {
-				mGpsExtrasLock.lock();
-				long timeElapsed = mGpsTimeStamp - loc.getTime();
-				mGpsExtrasLock.unlock();
+				long timeElapsed;
+				synchronized (mGpsExtrasLock) {
+					timeElapsed = mGpsTimeStamp - loc.getTime();
+				}
 				double newdalt = ((newalt - oldAlt) / (double) timeElapsed) * 1000.0;
 				setGpsField(dALT, newdalt);
 				Log.i(TAG, "new dalt: " + newdalt);
@@ -434,12 +327,12 @@
 			setGpsField(LAT, loc.getLatitude());			
 			setGpsField(SPEED, loc.getSpeed());
 			
-			mGpsExtrasLock.lock();
-			mGpsAccuracy = loc.getAccuracy();
-			mGpsTimeStamp = loc.getTime();
-			if (loc.getExtras() != null)
-				mGpsNumSats = loc.getExtras().getInt("satellites");
-			mGpsExtrasLock.unlock();
+			synchronized (mGpsExtrasLock) {
+				mGpsAccuracy = loc.getAccuracy();
+				mGpsTimeStamp = loc.getTime();
+				if (loc.getExtras() != null)
+					mGpsNumSats = loc.getExtras().getInt("satellites");
+			}
 			if (mLastLoc != null) {
 				synchronized (mLastLoc) {
 					mLastLoc = loc;
@@ -474,8 +367,9 @@
 	 * @param event Object containing new data--sensor type, accuracy, timestamp and value.
 	 */
 	public void onSensorChanged(SensorEvent event) {
+		long time = System.currentTimeMillis();
 		int type = event.sensor.getType();
-		switch (type) {
+		switch (type) {			
 			case Sensor.TYPE_ACCELEROMETER:
 				setReadingField(X_ACCEL, event.values[0]);
 				setReadingField(Y_ACCEL, event.values[1]);
@@ -485,15 +379,11 @@
 				setReadingField(LIGHT, event.values[0]);
 				break;
 			case Sensor.TYPE_MAGNETIC_FIELD:
+				System.arraycopy(event.values, 0, flux, 0, 3);
 				setReadingField(X_FLUX, event.values[0]);
 				setReadingField(Y_FLUX, event.values[1]);
 				setReadingField(Z_FLUX, event.values[2]);
 				break;
-			case Sensor.TYPE_ORIENTATION:
-				setReadingField(AZIMUTH, event.values[0]);
-				setReadingField(PITCH, event.values[1]);
-				setReadingField(ROLL, event.values[2]);
-				break;
 			case Sensor.TYPE_PRESSURE:
 				setReadingField(PRESSURE, event.values[0]);
 				break;
@@ -502,6 +392,35 @@
 				break;
 			case Sensor.TYPE_TEMPERATURE:
 				setReadingField(TEMPERATURE, event.values[0]);
+				break;
+			case Sensor.TYPE_GRAVITY:
+				//Log.v(TAG, "grav time: " + (time - grav_time));
+				String timestring = Long.toString(time);
+				try {
+					if (logwriter != null) {
+						logwriter.write(timestring + "\n");
+						logwriter.flush();
+					}
+				} catch (IOException e) {
+					// Do nothing.
+				}
+				/*Log.v(TAG, "");
+				Log.v(TAG, "" + event.values[0]);
+				Log.v(TAG, "" + event.values[1]);
+				Log.v(TAG, "" + event.values[2]);
+				Log.v(TAG, ""); */
+				SensorManager.getRotationMatrix(rotationMatrix,
+				  							    null,
+												event.values,
+												flux);
+				/*for (int i = 0; i < 9; i++) {
+					Log.v(TAG, "" + rotationMatrix[i]);
+				}*/
+				SensorManager.getOrientation(rotationMatrix, orientation);
+				//Log.v(TAG, "" + orientation[i]);
+				setReadingField(AZIMUTH, orientation[0] * 180.0 / Math.PI);
+				setReadingField(PITCH, orientation[1] * 180.0 / Math.PI);
+				setReadingField(ROLL, orientation[2] * -180.0 / Math.PI);
 				break;
 		}
 	}
@@ -542,45 +461,29 @@
 	 * Sets the motor speed data to the supplied array.
 	 * @param mySpeeds The data to which the motor speeds should be set.
 	 */
-	protected void setMotorFields(double[] mySpeeds) {
+	protected void setMotorFields(final double[] mySpeeds) {
 		if (mySpeeds.length < 4)
 			return;
-		final double[] speeds = mySpeeds.clone();
-		mMutatorPool.submit(new Runnable() {
-			public void run() {
-				//Log.v(TAG, "Changing motorspeeds:");
-				
-				mMotorLock.lock();
-				for (int i = 0; i < 4; i++) {
-					mMotorSpeed[i] = speeds[i];
-				}
-				/*Log.v(TAG, "vector " + mMotorSpeed[0] + ", "
-									 + mMotorSpeed[1] + ", "
-									 + mMotorSpeed[2] + ", "
-									 + mMotorSpeed[3]);*/
-				mMotorLock.unlock();
-				//Log.v(TAG, "Done changing motorspeeds");
-			}
-		});
+		synchronized (mMotorSpeed) {
+			System.arraycopy(mySpeeds, 0, mMotorSpeed, 0, 4);
+			/*Log.v(TAG, "vector " + mMotorSpeed[0] + ", "
+								 + mMotorSpeed[1] + ", "
+								 + mMotorSpeed[2] + ", "
+								 + mMotorSpeed[3]);*/
+		}
+		//Log.v(TAG, "Done changing motorspeeds");
 	}
 	
 	/**
 	 * Sets the motor power levels to the supplied array.
 	 * @param myPowers The data to which the motor power levels should be set.
 	 */
-	protected void setMotorPowerFields(double[] myPowers) {
+	protected void setMotorPowerFields(final double[] myPowers) {
 		if (myPowers.length < 4)
 			return;
-		final double[] powers = myPowers.clone();
-		mMutatorPool.submit(new Runnable() {
-			public void run() {
-				mMotorPowerLock.lock();
-				for (int i = 0; i < 4; i++) {
-					mMotorPower[i] = powers[i];
-				}
-				mMotorPowerLock.unlock();	
-			}
-		});
+		synchronized (mMotorPower) {
+			System.arraycopy(myPowers, 0, mMotorPower, 0, 4);
+		}
 	}
 	
 	/**
@@ -589,13 +492,9 @@
 	 * @param value The GPS data.
 	 */
 	private void setGpsField(final int whichField, final double value) {
-		mMutatorPool.submit(new Runnable() {
-			public void run() {
-				mGpsLock[whichField].lock();
-				mGps[whichField] = value;
-				mGpsLock[whichField].unlock();
-			}
-		});
+		synchronized (mGpsLock[whichField]) {
+			mGps[whichField] = value;
+		}
 	}
 	
 	
@@ -605,13 +504,9 @@
 	 * @param value The reading.
 	 */
 	private void setReadingField(final int whichField, final double value) {
-		mMutatorPool.submit(new Runnable() {
-			public void run() {
-				mReadingLock[whichField].lock();
-				mReading[whichField] = value;
-				mReadingLock[whichField].unlock();
-			}
-		});
+		synchronized (mReadingLock[whichField]) {
+			mReading[whichField] = value;
+		}
 	}
 	
 	/** Updates all registered receivers with the specified String */
diff --git a/org/haldean/chopper/pilot/Guidance.java b/org/haldean/chopper/pilot/Guidance.java
index eb23733..bc5dded 100644
--- a/org/haldean/chopper/pilot/Guidance.java
+++ b/org/haldean/chopper/pilot/Guidance.java
@@ -4,6 +4,7 @@
 import java.io.IOException;
 import java.util.LinkedList;
 import java.util.ListIterator;
+import java.util.concurrent.atomic.AtomicInteger;
 
 import android.os.Handler;
 import android.os.Looper;
@@ -25,7 +26,7 @@
  *      PID:
  *          SET:&lt;pid_loop_number&gt;:&lt;pid_parameter_index&gt;:&lt;pid_parameter_value&gt;
  *          GET
- *      AUTOMATIC
+ *      AUTOPILOT
  *      VECTOR:&lt;north_motor_speed&gt;:&lt;south_motor_speed&gt;:&lt;east_motor_speed&gt;:&lt;west_motor_speed&gt;
  * 		LOCALVEC
  * 		ABSVEC
@@ -37,39 +38,26 @@
 public class Guidance implements Runnable, Constants, Receivable {
 	
 	/** How many times per second the PID loop will run */
-	public static final int PIDREPS = 10;
-	
-	/** Maximum permissible target velocity, in m/s; larger vectors will be resized */
-	public static final double MAX_VEL = 2.0;
-	
-	/** The maximum angle, in degrees, guidance will permit the chopper to have */
-	public static final double MAX_ANGLE = 10;
+	public static final int PIDREPS = 40;
 	
 	/** The maximum change in motor speed permitted at one time.  Must be positive. */
-	public static final double MAX_DMOTOR = .01;
+	public static final double MAX_DMOTOR = .05;
 	
 	/** The maximum change in motor speed permitted at one time if the chopper is stabilizing.  Must be positive. */
-	public static final double MAX_DSTABLE = .05;
+	public static final double MAX_DSTABLE = .1;
 	
 	/** Tag for logging */
 	public static final String TAG = new String("chopper.Guidance");
 	
-	/** Used when a really big number is needed, still small enough to prevent overflow. */
-	private static final double sReallyBig = 2.0;
-	
 	/** Handles messages for the thread */
 	private Handler mHandler;
 	
-	/** Stores whether or not a motor-eval loop should add itself to the queue again. **/
-	private boolean mReviseMotorSpeeds = true;
+	private Angler mAngler;
 	
 	/** Stores orientation data persistently, as expected values in case lock is not immediately available*/
 	private double mAzimuth;
 	private double mPitchDeg;
 	private double mRollDeg;
-	private double mGpsBearing;
-	private double mGpsSpeed;
-	private double mGpsDalt;
 	
 	/** Log file name **/
 	public static final String logname = "/sdcard/chopper/guidlog.txt";
@@ -86,12 +74,6 @@
 	 * remain persistent from iteration to iteration. 
 	 */
 	
-	/** Stores desired velocity */
-	private double[] mTarget = new double[4];
-	
-	/** Stores the current velocity (relative to the chopper) */
-	private double[] mCurrent = new double[4];
-	
 	/** Stores current PID error */
 	private double[][] mErrors = new double[4][3];
 	
@@ -102,11 +84,14 @@
 	/** Timestamp of last PID evaluation */
 	private long mLastUpdate = 0;
 	
-	/** Sum of errors * tuning parameter for a given PID loop */
-	private double[] mTorques = new double[4];
-	
-	/** Stores motor speeds temporarily */
-	private double[] mTempMotorSpeed = new double[4];
+	private double[] mControlVars = new double[4];
+	private double[] mAngleTarget = new double[4];
+	private long mLastGpsTimestamp;
+	private AtomicInteger mGuidanceMode = new AtomicInteger();
+	
+	private static final int DIRECT = 0;
+	private static final int MANUAL = 1;
+	private static final int AUTOPILOT = 2;
 	
 	/** Tuning parameters */
 	private double[][] mGain = new double[4][3];
@@ -114,21 +99,12 @@
 	/** Motor speed */
 	private double[] mMotorSpeed = new double[4]; //ORDER: North, South, East, West
 	
-	/** If set to true, disregards lateral velocity commands
-	 * Currently unused, though later may implement as extra safety protocol
-	 * in the event of difficulty maintaining altitude */
-	private boolean mHorizontalDrift = false; //if true, does not consider dx, dy or azimuth error; makes for maximally efficient altitude control
-	
 	/** List of registered receivers */
 	private LinkedList<Receivable> mRec;
 	
 	/** Handles to other chopper components */
 	private ChopperStatus mStatus;
-	private Navigation mNav;
 	private BluetoothOutput mBt;
-	
-	/** Controls whether N/S and E/W commands refer to absolute vectors or local **/
-	private boolean mAbsVec = true;
 	
 	/** Flag for writing motor speeds to output file **/
 	public final static boolean mEnableLogging = true;
@@ -143,31 +119,14 @@
 			throw new NullPointerException();
 		}
 		mStatus = status;
-		mNav = nav;
 		mRec = new LinkedList<Receivable>();
 		mBt = bT;
+		mAngler = new Angler(status, nav);
 		
 		//Temporary: need real tuning values at some point. Crap.
-		for (int i = 0; i < 2; i++)
-			for (int j = 0; j < 3; j++)
-				mGain[i][j] = .0003;
-				//mGain[i][j] = .05;
-		
-		for (int j = 0; j < 3; j++) {
-		//	mGain[3][j] = .0005;
-			mGain[2][j] = .0025;
-			mGain[3][j] = 0;
-		}
-		
-		mGain[0][0] = .001;
-		mGain[0][1] = .0003;
-		mGain[0][2] = 0; 
-		mGain[1][0] = .001;
-		mGain[1][1] = .0003;
-		mGain[1][2] = 0;
-		mGain[2][0] = .006;
-		mGain[2][1] = .006;
-		mGain[2][2] = 0;
+		for (int i = 0; i < 3; i++)
+			mGain[i][0] = .1;
+		mGain[3][0] = 0;
 				
 		try {
 			if (mEnableLogging)
@@ -246,7 +205,7 @@
 			}
 		};
 		//mHandler.sendEmptyMessage(EVAL_MOTOR_SPEED);
-		receiveMessage("GUID:VECTOR:0:0:0:0", null);
+		receiveMessage("DIRECT:0:0:0:0", null);
 		Looper.loop();
 	}
 	
@@ -258,6 +217,12 @@
 	public void receiveMessage(String msg, Receivable source) {
 		//Log.d(TAG, "Receiving message " + msg);
 		String[] parts = msg.split(":");
+		if (parts[0].equals("CSYS")) {
+			if (parts[1].equals("NOCONN")) {
+				Log.v(TAG, "NoConn in guidance");
+				receiveMessage("GUID:AUTOPILOT", source);
+			}
+		}
 		if (parts[0].equals("GUID")) {
 			if (parts[1].equals("PID")) {
 				if (parts[2].equals("SET")) {
@@ -273,22 +238,16 @@
 					getPids.sendToTarget();
 				}
 			}
-			if (parts[1].equals("AUTOMATIC")) {
+			if (parts[1].equals("AUTOPILOT")) {
+				Log.v(TAG, "AUTOPILOT mode");
+				mGuidanceMode.set(AUTOPILOT);
 				mHandler.removeMessages(NEW_GUID_VECTOR);
-				mReviseMotorSpeeds = true;
 				if (!mHandler.hasMessages(EVAL_MOTOR_SPEED))
 					mHandler.sendEmptyMessage(EVAL_MOTOR_SPEED);
 			}
-			if (parts[1].equals("LOCALVEC")) {
-				Log.v(TAG, "Setting mabsvec to false");
-				mAbsVec = false;
-			}
-			if (parts[1].equals("ABSVEC")) {
-				Log.v(TAG, "Setting mabsvec to true");
-				mAbsVec = true;
-			}
-			if (parts[1].equals("VECTOR")) {
-				mReviseMotorSpeeds = false;
+			if (parts[1].equals("DIRECT")) {
+				Log.v(TAG, "direct mode");
+				mGuidanceMode.set(DIRECT);
 				mHandler.removeMessages(EVAL_MOTOR_SPEED);
 				Double[] myVector = new Double[4];
 				for (int i = 0; i < 4; i++) {
@@ -297,6 +256,29 @@
 				Message newValue = Message.obtain(mHandler, NEW_GUID_VECTOR, myVector);
 				newValue.sendToTarget();
 			}
+			if (parts[1].equals("MANUAL")) {
+				//autoPilot(false);
+				String log = "manual mode: ";				
+				if (parts.length > 2) {
+					double[] newTarget = new double[4];
+					for (int i = 0; i < 4; i++) {
+						newTarget[i] = new Double(parts[i + 2]);
+						log += newTarget[i] + ": ";
+					}
+					//Log.v(TAG, log);
+					newTarget[0] *= Angler.MAX_ANGLE;
+					newTarget[1] *= Angler.MAX_ANGLE;
+					newTarget[2] += 1.0;
+					//newTarget[2] *= 2.0;
+					synchronized (mAngleTarget) {
+						System.arraycopy(newTarget, 0, mAngleTarget, 0, 4);
+					}
+				}
+				mGuidanceMode.set(MANUAL);
+				mHandler.removeMessages(NEW_GUID_VECTOR);
+				if (!mHandler.hasMessages(EVAL_MOTOR_SPEED))
+					mHandler.sendEmptyMessage(EVAL_MOTOR_SPEED);
+			}
 		}
 	}
 	
@@ -313,137 +295,42 @@
 	/** Core of the class; calculates new motor speeds based on status */
 	private void reviseMotorSpeed() {
 		mHandler.removeMessages(EVAL_MOTOR_SPEED);
-		//Log.v(TAG, "START MOTOR REVISION");
 		long starttime = System.currentTimeMillis();
-		
-		//Copying motor values to temporary array for working purposes.
+		updateAngleTarget();
+		
+		//Retrieve current orientation.		
+		
+		mAzimuth = mStatus.getReadingField(AZIMUTH);		
+		mPitchDeg = mStatus.getReadingField(PITCH);
+		mRollDeg = -mStatus.getReadingField(ROLL);
+		
+		double[] errors = new double[4];
+		synchronized (mAngleTarget) {
+			errors[0] = mAngleTarget[0] - mRollDeg;
+			errors[1] = mAngleTarget[1] - mPitchDeg;
+			errors[2] = mAngleTarget[2] - mStatus.getGpsField(dALT);
+			errors[3] = mAngleTarget[3] - mAzimuth;
+		}
+		
+		String errs = "errors: ";
 		for (int i = 0; i < 4; i++) {
-			mTempMotorSpeed[i] = mMotorSpeed[i];
-		}
-		
-		boolean mStabilizing = false; //initializing value
-		//Retrieve current orientation.		
-		
-		mAzimuth = mStatus.getReadingFieldNow(AZIMUTH, mAzimuth);		
-		mPitchDeg = -mStatus.getReadingFieldNow(PITCH, -mPitchDeg);
-		mRollDeg = mStatus.getReadingFieldNow(ROLL, mRollDeg);
-		
-		double pitchrad = mPitchDeg * Math.PI / 180.0;
-		double rollrad = mRollDeg * Math.PI / 180.0;
-		
-		double gradient = Math.sqrt(
-				Math.pow(Math.tan(rollrad), 2) +
-				Math.pow(Math.tan(pitchrad), 2)
-				);
-		double ascentrad = Math.atan(gradient);
-		double mAscentDeg = ascentrad * 180.0 / Math.PI;
-		//if orientation is out-of-bounds,
-		if ((mAscentDeg > MAX_ANGLE) | (mPitchDeg > 90.0) | (mPitchDeg < -90.0)) {
-			mStabilizing = true;
-			//set target velocity to some big number in the direction of maximum ascent
-			double gradangle = Math.atan2(
-						Math.tan(rollrad) ,
-						Math.tan(pitchrad)
-						);
-			mTarget[0] = sReallyBig * Math.sin(gradangle);
-			mTarget[1] = sReallyBig * Math.cos(gradangle);
-			
-			//Make sure the velocity vector components point in the right directions.
-			mTarget[0] *= Math.signum(mTarget[0]) * Math.signum(mRollDeg);
-			mTarget[1] *= Math.signum(mTarget[1]) * Math.signum(mPitchDeg);
-			mTarget[2] = 0;
-			mTarget[3] = mAzimuth;
-			//System.out.println(target[0] + ", " + target[1]);
-		}
-		else {
-			//Retrieve target velocity from nav,
-			//Transform absolute target velocity to relative target velocity
-			double theta = mAzimuth * Math.PI / 180.0;
-			if (mNav != null) {
-				try {
-					double[] absTarget = mNav.getTarget();
-					if (mAbsVec) {
-						//Log.v(TAG, "Abs vectors");
-						mTarget[0] = absTarget[0] * Math.cos(theta) - absTarget[1] * Math.sin(theta);
-						mTarget[1] = absTarget[0] * Math.sin(theta) + absTarget[1] * Math.cos(theta);
-						mTarget[2] = absTarget[2];
-						mTarget[3] = absTarget[3];
-					}
-					else {
-						//Log.v(TAG, "Local vectors");
-						String targ = "";
-						for (int i = 0; i < 4; i++) {
-							mTarget[i] = absTarget[i];
-							targ = targ + " " + mTarget[i];
-						}
-						//Log.v(TAG, "guid, nav targ: " + targ);
-					}
-					
-					//Calculate recorded velocity; reduce, if necessary, to MAXVEL
-					double myVel = 0;
-					for (int i = 0; i < 3; i++) {
-						myVel += Math.pow(mTarget[i], 2);
-					}
-					myVel = Math.sqrt(myVel);
-					if (myVel > MAX_VEL) {
-						Log.v(TAG, "guid, Reducing requested velocity");
-						double adjustment = MAX_VEL / myVel;
-						for (int i = 0; i < 3; i++) {
-							mTarget[i] *= adjustment;
-						}
-					}
-				}
-				catch (IllegalAccessException e) {
-					Log.w(TAG, "Nav Target lock not available.");
-				}
-			}
-			//Log.v(TAG, "Relative target: " + mTarget[0] + ", " + mTarget[1] + ", " + mTarget[2] + ", " + mTarget[3]);
-		}
-		
-		
-		
-		long thistime = System.currentTimeMillis();
-		
-		//Retrieve current absolute velocity.  For now, only from GPS data; later, maybe write a kalman filter to use accelerometer data as well. 
-		//Transform current velocity from absolute to relative
-		
-		if (mStabilizing) {
-			mCurrent[0] = 0;
-			mCurrent[1] = 0;
-			mCurrent[2] = 0;
-			mCurrent[3] = mAzimuth;
-		}
-		else {
-			mGpsBearing = mStatus.getGpsFieldNow(BEARING, mGpsBearing);
-			double theta = (mGpsBearing - mAzimuth) * Math.PI / 180.0;
-			
-			mGpsSpeed = mStatus.getGpsFieldNow(SPEED, mGpsSpeed);
-			mCurrent[0] = mGpsSpeed * Math.sin(theta);
-			mCurrent[1] = mGpsSpeed * Math.cos(theta);
-			
-			mGpsDalt = mStatus.getGpsFieldNow(dALT, mGpsDalt);
-			mCurrent[2] = mGpsDalt;
-			mCurrent[3] = mAzimuth;
-			
-			String targ = "guid, curr vec: ";
-			for (int i = 0; i < 4; i++) {
-				targ = targ + " " + mCurrent[i];
-			}
-			//Log.v(TAG, targ);
-		}
+			errs += errors[i] + ": ";
+		}
+		//Log.v(TAG, errs);
+		//For azimuth, multiple possibilities exist for error, each equally valid; but only the error nearest zero makes practical sense.
+		if (errors[3] > 180.0)
+			errors[3] -= 360.0;
+		if (errors[3] < -180.0)
+			errors[3] += 360.0;
+		
 		
 		for (int i = 0; i < 4; i++) {
 			//Calculate proportional errors
-			double err = mTarget[i] - mCurrent[i];
-			if (i == 3) { //For azimuth, multiple possibilities exist for error, each equally valid; but only the error nearest zero makes practical sense.
-				if (err > 180.0)
-					err -= 360.0;
-				if (err < -180.0)
-					err += 360.0;
-			}
+			double err = errors[i];//mTarget[i] - mCurrent[i];
+			
 
 			//Calculate derivative errors.
-			mErrors[i][2] = (err - mErrors[i][0]) * 1000.0 / (thistime - mLastUpdate);
+			mErrors[i][2] = (err - mErrors[i][0]) * 1000.0 / (starttime - mLastUpdate);
 			
 			
 			//Mark proportional error
@@ -456,117 +343,31 @@
 			mErrors[i][1] += err;
 			mIntegralIndex = ++mIntegralIndex % PIDREPS;
 			
-			double dmotor = 0;
-			
 			//Calculate changes in output
 			for (int j = 0; j < 3; j++) {
-			/*	if (i == 1) {
-					Log.v(TAG, "guid, dy error " + j + " is " + mErrors[i][j]);
-					Log.v(TAG, "guid, dy gain " + j + " is " + mGain[i][j]);
-				}*/
-				dmotor += mErrors[i][j] * mGain[i][j];
-			}
-			/*if (i == 1)
-				Log.v(TAG, "guid, dmotor 1 is " + dmotor);
-			if (i == 2)
-				Log.v(TAG, "guid, dmotor 2 is " + dmotor);*/
-			double phi = 0;
-			switch (i) {
-			case 0: //X velocity
-				if (!mStabilizing) {
-					phi = Math.sin(rollrad);
-					phi = Math.abs(phi);
-					if (phi == 0)
-						dmotor = 2 * dmotor; 
-					else
-						dmotor = dmotor / phi;
-				}
-				break;
-			case 1: //Y velocity
-				if (!mStabilizing) {
-					phi = Math.sin(pitchrad);
-					phi = Math.abs(phi);
-					if (phi == 0)
-						dmotor = 2 * dmotor;
-					else
-						dmotor = dmotor / phi;
-					Log.v(TAG, "guid, phi 1 is " + phi);
-					Log.v(TAG, "guid, dmotor 1 is " + dmotor);
-				}
-				break;
-			case 2: //Z velocity
-				phi = Math.cos(ascentrad);
-				phi = Math.abs(phi);
-				if (phi == 0)
-					dmotor = 0; //Don't bother with altitude control, gives more efficiency to torque[0, 1] for stabilization
-				else
-					dmotor = dmotor / phi;
-				//Log.v(TAG, "guid, phi 2 is " + phi);
-				break;
-			case 3: //Azimuth
-				break;
-			}
-			mTorques[i] = dmotor;
-			//Log.v(TAG, "Torque " + i + " " + dmotor);
-		}
-		mLastUpdate = thistime;
-		
-		/*String targ = "";
-		for (int i = 0; i < 4; i++)
-			targ = targ + " " + mTorques[i];
-		Log.v(TAG, "guid, torques is " + targ);
-		*/
-		if ((!mHorizontalDrift) || (mStabilizing)) { //if horizontal drift is on, motor speeds give full efficiency to altitude control
-		//but if the chopper is stabilizing, under no circumstances ignore torques 0, 1
-			//changes torques to motor values
-			mTempMotorSpeed[0] -= mTorques[1] / 2.0;
-			mTempMotorSpeed[1] += mTorques[1] / 2.0;
-			
-			//Log.v(TAG, "Temp 1 " + mTempMotorSpeed[2] + "\nTemp 2 " + mTempMotorSpeed[3]);
-			mTempMotorSpeed[2] -= mTorques[0] / 2.0;
-			mTempMotorSpeed[3] += mTorques[0] / 2.0;
-			
-			
-			
-			double spintorque = mTorques[3] / 4.0;
-			mTempMotorSpeed[0] += spintorque;
-			mTempMotorSpeed[1] += spintorque;
-			mTempMotorSpeed[2] -= spintorque;
-			mTempMotorSpeed[3] -= spintorque;
-		}
-		
-		double dalttorque = mTorques[2] / 4.0;
+				mControlVars[i] += mErrors[i][j] * mGain[i][j];
+			}
+		}
+		if (mGuidanceMode.get() == MANUAL) {
+			synchronized (mAngleTarget) {
+				mControlVars[2] = mAngleTarget[2];
+			}
+		}
+		mLastUpdate = starttime;
+		
+		// Constrain control vars:
+		mControlVars[0] = constrainValue(mControlVars[0], -1, 1);
+		mControlVars[1] = constrainValue(mControlVars[1], -1, 1);
+		mControlVars[2] = constrainValue(mControlVars[2], 0, 4);
+		mControlVars[3] = constrainValue(mControlVars[3], -2, 2);
+		
+		/*String vars = "Control vars: ";
 		for (int i = 0; i < 4; i++) {
-			mTempMotorSpeed[i] += dalttorque;
-		}
-		
-		//Bounds Check--values must be between zero and one.
-		for (int i = 0; i < 4; i++) {
-			if (mTempMotorSpeed[i] < 0)
-				mTempMotorSpeed[i] = 0;
-			else if (mTempMotorSpeed[i] > 1)
-				mTempMotorSpeed[i] = 1;
-			double diff = mTempMotorSpeed[i] - mMotorSpeed[i];
-			//if (i==1)
-				//Log.v(TAG, "guid, diff is " + diff);
-			if (mStabilizing) {
-				if (diff > 0)
-					mMotorSpeed[i] += Math.min(diff, MAX_DSTABLE);
-				else if (diff < 0)
-					mMotorSpeed[i] += Math.max(diff, -MAX_DSTABLE);
-			}
-			else {				
-				if (diff > 0) {
-					mMotorSpeed[i] += Math.min(diff, MAX_DMOTOR);
-				}
-				else if (diff < 0) {
-					mMotorSpeed[i] += Math.max(diff, -MAX_DMOTOR);
-				}
-			}
-			mTempMotorSpeed[i] = mMotorSpeed[i];
-			//if (i == 1)
-			//	Log.v(TAG, "guid, ms1 is " + mMotorSpeed[i]);
-		}	
+			vars += mControlVars[i] + ": ";
+		}
+		Log.v(TAG, vars);*/
+		
+		controlVarsToMotorSpeeds();
 		
 		//Send motor values to motors here:
 		updateMotors();
@@ -574,7 +375,9 @@
 		//Log.v(TAG, "motors: " + mMotorSpeed[0] + ", " + mMotorSpeed[1] + ", " + mMotorSpeed[2] + ", " + mMotorSpeed[3]);
 		//Sleep a while
 		long timetonext = (1000 / PIDREPS) - (System.currentTimeMillis() - starttime);
-		if (mReviseMotorSpeeds) {
+		Log.v(TAG, "time to next: " + timetonext);
+		int currentMode = mGuidanceMode.get();
+		if ((currentMode == MANUAL) || (currentMode == AUTOPILOT)) {
 			if (timetonext > 0)
 				mHandler.sendEmptyMessageDelayed(EVAL_MOTOR_SPEED, timetonext);
 			else {
@@ -582,6 +385,52 @@
 				mHandler.sendEmptyMessage(EVAL_MOTOR_SPEED);
 			}
 		}
+	}
+	
+	private void controlVarsToMotorSpeeds() {
+		double pitchrad = mPitchDeg * Math.PI / 180.0;
+		double rollrad = mRollDeg * Math.PI / 180.0;
+		double gradient = Math.sqrt(
+				Math.pow(Math.tan(rollrad), 2) +
+				Math.pow(Math.tan(pitchrad), 2)
+				);
+		double ascentRad = Math.atan(gradient);
+		
+		double cosGrad = Math.cos(ascentRad);
+		
+		double x = mControlVars[0];
+		double y = mControlVars[1];
+		double z = mControlVars[2] / cosGrad;
+		double t = mControlVars[3];
+		
+		mMotorSpeed[0] = Math.sqrt(constrainValue(t - 2*y + z, 0, 1));
+		mMotorSpeed[1] = Math.sqrt(constrainValue(t + 2*y + z, 0, 1));
+		mMotorSpeed[2] = Math.sqrt(constrainValue(-t - 2*x + z, 0, 1));
+		mMotorSpeed[3] = Math.sqrt(constrainValue(-t + 2*x + z, 0 ,1));
+	}
+	
+	private void updateAngleTarget() {
+		if (mGuidanceMode.get() != AUTOPILOT) {
+			return;
+		}
+		long currentGpsTimeStamp = mStatus.getGpsTimeStamp();
+		if (mLastGpsTimestamp == currentGpsTimeStamp) {
+			return;
+		}
+		mLastGpsTimestamp = currentGpsTimeStamp;
+		synchronized (mAngleTarget) {
+			mAngler.getAngleTarget(mAngleTarget);
+		}
+	}
+	
+	private static double constrainValue(double requested, double min, double max) {
+		if (requested > max) {
+			return max;
+		}
+		if (requested < min) {
+			return min;
+		}
+		return requested;
 	}
 	
 	/**
@@ -598,7 +447,7 @@
 			}
 		}
 		catch (IOException e) {
-			Log.e(TAG, "Cannot write to logfile");
+			//Log.e(TAG, "Cannot write to logfile");
 		}
 		//Pass motor values to motor controller!
 		Message msg = Message.obtain(mBt.mHandler, SEND_MOTOR_SPEEDS, mMotorSpeed);
diff --git a/org/haldean/chopper/pilot/Navigation.java b/org/haldean/chopper/pilot/Navigation.java
index 6116286..b497b64 100644
--- a/org/haldean/chopper/pilot/Navigation.java
+++ b/org/haldean/chopper/pilot/Navigation.java
@@ -3,16 +3,11 @@
 import java.util.LinkedList;
 import java.util.ListIterator;
 import java.util.Vector;
-import java.util.concurrent.atomic.AtomicBoolean;
 import java.util.concurrent.atomic.AtomicInteger;
-import java.util.concurrent.locks.ReentrantLock;
 
 import org.haldean.chopper.nav.NavData;
 import org.haldean.chopper.nav.NavList;
 
-import android.os.Handler;
-import android.os.Looper;
-import android.os.Message;
 import android.util.Log;
 
 /**
@@ -39,7 +34,7 @@
  * 
  * @author Benjamin Bardin
  */
-public class Navigation implements Runnable, Constants, Receivable {
+public class Navigation implements Constants, Receivable {
 	
 	/** Tag for logging */
 	public static final String TAG = "chopper.Navigation";
@@ -53,15 +48,9 @@
 	/** Velocity to achieve.  Must be locked on each read/write. */
 	private double[] mTarget = new double[4];
 	
-	/** Lock for target[] */
-	private ReentrantLock mTargetLock;
-	
 	/** Used to calculate next nav vector; prevents the thread
 	 * from holding a lock on mTarget itself for too long */
 	private double[] mTempTarget = new double[4];
-	
-	/** True if autopilot is engaged */
-	private final AtomicBoolean mAutoPilot = new AtomicBoolean(false);
 	
 	/** Chopper's navigation status */
 	private final AtomicInteger mNavStatus = new AtomicInteger(NAV_STATUSES - 1);
@@ -77,9 +66,6 @@
 	/** Handle for other chopper components */
 	private ChopperStatus mStatus;
 	
-	/** Handles messages */
-	private Handler mHandler;
-	
 	/** Registered receivers */
 	private LinkedList<Receivable> mRec;
 	
@@ -103,29 +89,30 @@
 		mTravelPlans.add(mLowPower);
 		mTravelPlans.add(mFlightPath);
 		mTravelPlans.add(mOnMyOwn);
-		
-		mTargetLock = new ReentrantLock();
 
 		mStatus = status;
-	}
-	
-	/**
-	 * Changes autopilot status (on or off)
-	 * @param onoff The new autopilot status
-	 */
-	public void autoPilot(boolean onoff) {
-		mAutoPilot.set(onoff);
-		mHandler.removeMessages(EVAL_NAV);
-		if (mAutoPilot.get()) {
-			mHandler.sendEmptyMessage(EVAL_NAV);
-			Log.i(TAG, "Autopilot engaged");
-		}
-		else
-			Log.i(TAG, "Autopilot disengaged");
-	}
-	
-	/** Evaluates a new navigation vector, based on current status and the relevant NavTask */
-	private void evalNextVector() {
+		
+		//FOR TESTING ONLY:
+		/*String taskList = "{ " + 
+			"{ DEST!targ1!300!-74.012345!40.74!10!100!232 { DEST!targ2!300!-77.07950!38.97300!100!250!233 " +
+				" DEST!targ3!587!-117.15!32.72!10!600!234 } } }";*/
+		String taskList = "{ VEL!name1!1!0!0!0!180000!-10 VEL!name1!-1!0!0!0!10000!-10 VEL!name1!0!1!0!0!10000!-10 VEL!name1!0!-1!0!0!10000!-10 VEL!name1!0!0!1!0!10000!-10 VEL!name1!0!0!-1!0!10000!-10 -4}";
+		setTask(BASIC_AUTO, taskList);
+		setTask(NO_CONN, "{ VEL!No_Conn!0!0!-1!0!" + THREE_HRS_IN_MS + "!-1!-5 -6}");
+		setTask(LOW_POWER, "{ VEL!Low_Power!0!0!-1!0!"+ THREE_HRS_IN_MS + "!-1!-7 -8}");
+		//mNavStatus.set(BASIC_AUTO);
+		
+		
+		//autoPilot(true);
+	}
+	
+	
+	
+	/** 
+	 * Evaluates a new navigation vector, based on current status and the relevant NavTask.
+	 * @param newNavTarget If supplied and has length >= 4, writes the new target here.  May be null.
+	 */
+	public void evalNextVector(double[] newNavTarget) {
 
 		/*Determine what the current task should be.  Copies to a local variable in case
 		 * 'status'	changes during execution of the method */
@@ -140,79 +127,14 @@
 		}
 		mTask.getVelocity(myList, mTempTarget);
 		setTarget(mTempTarget);
-		
-		
-		
-		long interval = mTask.getInterval(myList);
-		Log.v(TAG, "Nav Interval is " + interval);
+		if (newNavTarget != null) {
+			getTarget(newNavTarget);
+		}
+
+		//long interval = mTask.getInterval(myList);
+		//Log.v(TAG, "Nav Interval is " + interval);
 		//Send the current NavList to the server, in case any tasks have been completed
 		updateReceivers("NAV:AUTOTASK:" + thisStatus + ":" + myList.toString());
-		
-		mHandler.removeMessages(EVAL_NAV);
-		if (mAutoPilot.get()) {
-			if (interval > 0) {
-				mHandler.sendEmptyMessageDelayed(EVAL_NAV, interval);
-			}
-			else {
-				mHandler.sendEmptyMessage(EVAL_NAV);
-			}
-		}
-	}
-	
-	/**
-	 * Starts the navigation thread
-	 */
-	public void run() {
-		Looper.prepare();
-		Thread.currentThread().setName("Navigation");
-		mHandler = new Handler() {
-			public void handleMessage(Message msg)
-            {
-                switch (msg.what) {
-                case EVAL_NAV:
-                	if (mAutoPilot.get())
-                		evalNextVector();
-                	break;
-                }
-            }
-		};
-
-		//FOR TESTING ONLY:
-		/*String taskList = "{ " + 
-			"{ DEST!targ1!300!-74.012345!40.74!10!100!232 { DEST!targ2!300!-77.07950!38.97300!100!250!233 " +
-				" DEST!targ3!587!-117.15!32.72!10!600!234 } } }";*/
-		String taskList = "{ VEL!name1!1!0!0!0!10000!-10 VEL!name1!-1!0!0!0!10000!-10 VEL!name1!0!1!0!0!10000!-10 VEL!name1!0!-1!0!0!10000!-10 VEL!name1!0!0!1!0!10000!-10 VEL!name1!0!0!-1!0!10000!-10 -4}";
-		setTask(BASIC_AUTO, taskList);
-		setTask(NO_CONN, "{ VEL!No_Conn!0!0!-1!0!" + THREE_HRS_IN_MS + "!-1!-5 -6}");
-		setTask(LOW_POWER, "{ VEL!Low_Power!0!0!-1!0!"+ THREE_HRS_IN_MS + "!-1!-7 -8}");
-		//mNavStatus.set(BASIC_AUTO);
-		
-		
-		//autoPilot(true);
-		
-		Looper.loop();
-	}
-	
-	/**
-	 * Obtains current navigation target vector.  If the data is
-	 * locked, immediately throws an exception.
-	 *
-	 * @return A new array containing the navigation target vector.
-	 * @throws IllegalAccessException If the data is currently locked.
-	 */
-	public double[] getTarget() throws IllegalAccessException {
-		double[] myTarget = new double[4];
-		if (mTargetLock.tryLock()) {
-			for (int i = 0; i < 4; i++) {
-				myTarget[i] = mTarget[i];
-			}
-			mTargetLock.unlock();
-		}
-		else {
-			Log.w(TAG, "Target vector is locked.");
-			throw new IllegalAccessException();
-		}
-		return mTarget;
 	}
 	
 	/**
@@ -223,17 +145,11 @@
 	 * @param expectedValues The array in which to write the
 	 * vector--must be at least of length 4.
 	 */
-	public void getTarget(double[] expectedValues) {
-		if (expectedValues.length < 4)
+	public void getTarget(double[] navTarget) {
+		if (navTarget.length < 4)
 			return;
-		if (mTargetLock.tryLock()) {
-			for (int i = 0; i < 4; i++) {
-				expectedValues[i] = mTarget[i];
-			}
-			mTargetLock.unlock();
-		}
-		else {
-			Log.w(TAG, "Target vector is locked.");
+		synchronized (mTarget) {
+			System.arraycopy(mTarget, 0, navTarget, 0, 4);
 		}
 	}
 	
@@ -256,10 +172,8 @@
 		for (int i = 0; i < 3; i++) {
 			mTempTarget[i] = 0;
 		}
-		mTempTarget[3] = mStatus.getReadingFieldNow(AZIMUTH, mTempTarget[3]);
+		mTempTarget[3] = mStatus.getReadingField(AZIMUTH);
 		setTarget(mTempTarget);
-		mHandler.removeMessages(EVAL_NAV);
-		mHandler.sendEmptyMessageDelayed(EVAL_NAV, HOVER_PAUSE);
 	}
 	
 	/**
@@ -267,33 +181,17 @@
 	 * @param source The source of the message, if a reply is needed.  May be null.
 	 */
 	public void receiveMessage(String msg, Receivable source) {
-		Log.d(TAG, "Receiving " + msg);
 		String[] parts = msg.split(":");
-		if (parts[0].equals("GUID")) {
-			if (parts[1].equals("VECTOR")) {
-				autoPilot(false);
-			}
-		}
+		
 		if (parts[0].equals("NAV")) {
 			if (parts[1].equals("SET")) {
 				Log.v(TAG, "Updating Nav Status");				
-				if (parts[2].equals("MANUAL")) {
-					autoPilot(false);
-					if (parts.length > 3) {
-						updateReceivers("GUID:LOCALVEC");
-						updateReceivers("GUID:AUTOMATIC");
-						double[] newTarget = new double[4];
-						for (int i = 0; i < 4; i++) {
-							newTarget[i] = new Double(parts[i + 3]);
-						}
-						setTarget(newTarget);
-					}
-				}
+				
 				if (parts[2].equals("AUTOPILOT")) {
 					mNavStatus.set(BASIC_AUTO);
 					updateReceivers("GUID:ABSVEC");
 					updateReceivers("GUID:AUTOMATIC");
-					autoPilot(true);
+					//autoPilot(true);
 				}
 				if (parts[2].equals("AUTOTASK")) {
 					Integer taskList = new Integer(parts[3]);
@@ -317,15 +215,12 @@
 		}
 		if (parts[0].equals("CSYS")) {
 			if (parts[1].equals("NOCONN")) {
-				Log.d(TAG, "no conn in Nav");
 				updateStatus(NO_CONN);
-				autoPilot(true);
-				updateReceivers("GUID:AUTOMATIC");
+				//autoPilot(true);
 			}
 			if (parts[1].equals("LOWPOWER")) {
 				updateStatus(LOW_POWER);
-				autoPilot(true);
-				updateReceivers("GUID:AUTOMATIC");
+				//autoPilot(true);
 			}
 		}
 	}
@@ -344,24 +239,18 @@
 	 * Sets the navigation target vector to the values contained by the supplied array (length must be at least 4).
 	 * @param newTarget The new target vector.
 	 */
-	protected void setTarget(double[] newTarget) {
+	private void setTarget(double[] newTarget) {
 		if (newTarget.length < 4) {
 			return;
 		}
-		final double[] myCopy = newTarget.clone();
-		new Thread() {
-			public void run() {
+		synchronized (mTarget) {
 			String newNav = "New_Nav_Vector: ";
-			mTargetLock.lock();
-			for (int i = 0; i < 4; i++) {
-				mTarget[i] = myCopy[i];
-				newNav += myCopy[i] + ":";
-			}
-			mTargetLock.unlock();
-			Log.i(TAG, newNav);
+			for (int i = 0; i < 4; i++) {				
+				mTarget[i] = newTarget[i];
+				newNav += newTarget[i] + ":";
+			}
 			updateReceivers(newNav);
-			}
-		}.start();
+		}
 	}
 	
 	/** 
@@ -384,8 +273,6 @@
 			Log.i(TAG, "Nav set index " + whichPlan + " to task " + myList);
 			//Confirm change to server:
 			updateReceivers("NAV:AUTOTASK:" + whichPlan + ":" + myList.toString());
-			if (mAutoPilot.get())
-				evalNextVector();
 		}
 		else {
 			Log.e(TAG, "Nav received invalid task!");
diff --git a/org/haldean/chopper/pilot/StatusReporter.java b/org/haldean/chopper/pilot/StatusReporter.java
index fca9c05..276a609 100644
--- a/org/haldean/chopper/pilot/StatusReporter.java
+++ b/org/haldean/chopper/pilot/StatusReporter.java
@@ -6,7 +6,6 @@
 import android.os.Handler;
 import android.os.Looper;
 import android.os.Message;
-import android.util.Log;
 
 /**
  * Sends regular chopper status reports to all registered receivers. <P>
@@ -61,26 +60,17 @@
 		LinkedList<String> infoList = new LinkedList<String>();
 		
 		/* Lock data, send it, unlock.  If the lock is unavailable (unlikely), skip this datapiece for this iteration */
-		try {
-			double myAzimuth = mStatus.getReadingFieldNow(AZIMUTH);
-			double myPitch = mStatus.getReadingFieldNow(PITCH);
-			double myRoll = mStatus.getReadingFieldNow(ROLL);
-			infoList.add("ORIENT:" + myAzimuth + ":" + myPitch + ":" + myRoll);
-		}
-		catch (IllegalAccessException e) {
-			Log.w(TAG, "Orientation Report Unavailable");
-		}
-		
-		try {
-			double myXaccel = mStatus.getReadingFieldNow(X_ACCEL);
-			double myYaccel = mStatus.getReadingFieldNow(Y_ACCEL);
-			double myZaccel = mStatus.getReadingFieldNow(Z_ACCEL);
-			infoList.add("ACCEL:" + myXaccel + ":" + myYaccel + ":" + myZaccel);
-		}
-		catch (IllegalAccessException e) {
-			Log.w(TAG, "Acceleration Report Unavailable");
-		}
-
+		double myAzimuth = mStatus.getReadingField(AZIMUTH);
+		double myPitch = mStatus.getReadingField(PITCH);
+		double myRoll = mStatus.getReadingField(ROLL);
+		infoList.add("ORIENT:" + myAzimuth + ":" + myPitch + ":" + myRoll);
+		
+		
+		double myXaccel = mStatus.getReadingField(X_ACCEL);
+		double myYaccel = mStatus.getReadingField(Y_ACCEL);
+		double myZaccel = mStatus.getReadingField(Z_ACCEL);
+		infoList.add("ACCEL:" + myXaccel + ":" + myYaccel + ":" + myZaccel);
+			
 		/*try {
 			double myXflux = mStatus.getReadingFieldNow(X_FLUX);
 			double myYflux = mStatus.getReadingFieldNow(Y_FLUX);
@@ -91,16 +81,13 @@
 			Log.w(TAG, "Flux Report Unavailable");
 		}*/
 		
-		try {
-			double[] mySpeeds = mStatus.getMotorFieldsNow();
-			infoList.add("MOTORSPEED:" + mySpeeds[0] +
-					":" + mySpeeds[1] +
-					":" + mySpeeds[2] +
-					":" + mySpeeds[3]);
-		}
-		catch (IllegalAccessException e) {
-			Log.w(TAG, "MotorSpeeds Report Unavailable");
-		}
+		double[] mySpeeds = new double[4];
+		mStatus.getMotorFields(mySpeeds);
+		infoList.add("MOTORSPEED:" + mySpeeds[0] +
+				":" + mySpeeds[1] +
+				":" + mySpeeds[2] +
+				":" + mySpeeds[3]);
+	
 		/*
 		try {
 			double[] myPowers = mStatus.getMotorFieldsNow();
@@ -136,30 +123,18 @@
 			Log.w(TAG, "Pressure Report Unavailable");
 		}
 		*/
-		try {
-			double myTemp = mStatus.getReadingFieldNow(TEMPERATURE);
-			infoList.add("TEMPERATURE:" + myTemp);
-		}
-		catch (IllegalAccessException e) {
-			Log.w(TAG, "Temperature Report Unavailable");
-		}
-		
+		infoList.add("TEMPERATURE:" + mStatus.getReadingField(TEMPERATURE));
 		infoList.add("BATTERY:" + mStatus.getBatteryLevel());
 		
 		/* Send GPS data */
 		String gpsData = new String("GPS");
-		try {
-			for (int i = 0; i < GPS_FIELDS; i++) {
-				double myValue = mStatus.getGpsFieldNow(i);
-				gpsData += ":" + myValue;
-			}
-			
-			gpsData += ":" + mStatus.getGpsExtrasNow();			
-			infoList.add(gpsData);
-		}
-		catch (IllegalAccessException e) {
-			Log.w(TAG, "GPS Report Unavailable");
-		}
+
+		for (int i = 0; i < GPS_FIELDS; i++) {
+			double myValue = mStatus.getGpsField(i);
+			gpsData += ":" + myValue;
+		}
+		gpsData += ":" + mStatus.getGpsExtras();			
+		infoList.add(gpsData);
 		
 		return infoList;
 	}
diff --git a/org/haldean/chopper/server/EnsignCrusher.java b/org/haldean/chopper/server/EnsignCrusher.java
index 354e1a6..b055e5a 100644
--- a/org/haldean/chopper/server/EnsignCrusher.java
+++ b/org/haldean/chopper/server/EnsignCrusher.java
@@ -16,8 +16,9 @@
  *  @author William Brown
  */
 public class EnsignCrusher {
-    private static String navGoToManual = "NAV:SET:MANUAL";
-    private static String navGoToAutomatic = "NAV:SET:AUTOPILOT";
+	private static String navGoToDirect = "GUID:DIRECT";  // Direct control of motor speeds.
+    private static String navGoToManual = "GUID:MANUAL";  // Manual control of chopper target angles.
+    private static String navGoToAutomatic = "GUID:AUTO";  // Full automatic navigation.
     private static double bearing = 0;
 
     /* In meters per second (we hope.) */
@@ -99,7 +100,7 @@
      *  the motor closest to North.
      */
     public static void setMotorSpeeds(double[] speeds) {
-	String taskString = "GUID:VECTOR";
+	String taskString = navGoToDirect;
 	for (int i=0; i<4; i++) {
 	    taskString += ":" + speeds[i];
 	}
diff --git a/org/haldean/chopper/server/PadController.java b/org/haldean/chopper/server/PadController.java
index 2550771..8b8d1b9 100644
--- a/org/haldean/chopper/server/PadController.java
+++ b/org/haldean/chopper/server/PadController.java
@@ -90,7 +90,7 @@
 		    buttons[BUTTON_X] = c;
 		else if (id == Component.Identifier.Button.Y)
 		    buttons[BUTTON_Y] = c;
-		else if (id == Component.Identifier.Button.BACK)
+		else if ((id == Component.Identifier.Button.BACK) || (id == Component.Identifier.Button.SELECT))
 		    buttons[BUTTON_BACK] = c;
 		else if (id == Component.Identifier.Button.LEFT_THUMB)
 		    buttons[BUTTON_L] = c;
@@ -120,7 +120,7 @@
 		    axes[AXIS_R_TRIGGER] = c;
 		else if (id == Component.Identifier.Axis.POV)
 		    axes[D_PAD] = c;
-	    }
+	    } 
 	}
     }