diff --git a/Pilot/bin/classes.dex b/Pilot/bin/classes.dex index 9c91099..ef293db 100644 Binary files a/Pilot/bin/classes.dex and b/Pilot/bin/classes.dex differ diff --git a/Pilot/bin/resources.ap_ b/Pilot/bin/resources.ap_ index 4078294..9381bdc 100644 Binary files a/Pilot/bin/resources.ap_ and b/Pilot/bin/resources.ap_ differ diff --git a/org/haldean/chopper/pilot/ChopperMain.java b/org/haldean/chopper/pilot/ChopperMain.java index e681fb9..7704589 100644 --- a/org/haldean/chopper/pilot/ChopperMain.java +++ b/org/haldean/chopper/pilot/ChopperMain.java @@ -69,6 +69,7 @@ reporter.registerReceiver(comm); pic.registerReceiver(comm); + guid.registerReceiver(comm); new PersistentThread(comm).start(); status.getPersistentThreadInstance().start(); diff --git a/org/haldean/chopper/pilot/ChopperStatus.java b/org/haldean/chopper/pilot/ChopperStatus.java index f3618c9..24791f4 100644 --- a/org/haldean/chopper/pilot/ChopperStatus.java +++ b/org/haldean/chopper/pilot/ChopperStatus.java @@ -105,7 +105,7 @@ private ReentrantLock[] mReadingLock; /** List of registered receivers */ - private LinkedList mRec = new LinkedList(); + private LinkedList mRec; /** Hides Runnability, ensures singleton-ness */ private Runnable mRunner; @@ -117,6 +117,7 @@ */ public ChopperStatus(Context mycontext) { mContext = mycontext; + mRec = new LinkedList(); //Initialize the data locks mReadingLock = new ReentrantLock[SENSORS]; diff --git a/org/haldean/chopper/pilot/Comm.java b/org/haldean/chopper/pilot/Comm.java index e25b2e3..750dfe5 100644 --- a/org/haldean/chopper/pilot/Comm.java +++ b/org/haldean/chopper/pilot/Comm.java @@ -280,9 +280,11 @@ */ public void sendMessage(final String message) { if (mTextOut == null) { //connection not yet initialized + //Log.d(TAG, "Message not sent, socket not available."); return; } else { + // Log.v(TAG, "Sending msg: " + message); mPool.submit(new Runnable() { public void run() { try { diff --git a/org/haldean/chopper/pilot/Guidance.java b/org/haldean/chopper/pilot/Guidance.java index b0f23c9..5065db3 100644 --- a/org/haldean/chopper/pilot/Guidance.java +++ b/org/haldean/chopper/pilot/Guidance.java @@ -1,4 +1,7 @@ package org.haldean.chopper.pilot; + +import java.util.LinkedList; +import java.util.ListIterator; import android.os.Handler; import android.os.Looper; @@ -7,6 +10,11 @@ /** * Determines motor speeds, based on chopper's status and desired velocity vector. + * + * May send the following messages to registered Receivables:
+ *
+ * GUID:ERROR:<loop_1_error>:<loop_2_error>:<loop_3_error>:<loop_4_error>
+ * 
* * May receive the following messages from Chopper components: *
@@ -22,7 +30,7 @@
 public class Guidance implements Constants, Receivable {
 	
 	/** How many times per second the PID loop will run */
-	public int PIDREPS = 20;
+	public int PIDREPS = 1;
 	
 	/** Maximum permissible target velocity, in m/s; larger vectors will be resized */
 	public static final double MAX_VEL = 2.0;
@@ -31,7 +39,10 @@
 	public static final double MAX_ANGLE = 20;
 	
 	/** The maximum change in motor speed permitted at one time.  Must be positive. */
-	public static final double MAX_DMOTOR = .05F;
+	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 = .1;
 	
 	/** Tag for logging */
 	public static final String TAG = new String("chopper.Guidance");
@@ -92,6 +103,8 @@
 	 * 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
 	
+	private LinkedList mRec;
+	
 	/** Handles to other chopper components */
 	private ChopperStatus mStatus;
 	private Navigation mNav;
@@ -111,10 +124,19 @@
 		}
 		mStatus = status;
 		mNav = nav;
-		//Temporary: need real tuning values at some point:
+		mRec = new LinkedList();
+		
+		//Temporary: need real tuning values at some point. Crap.
 		for (int i = 0; i < 4; i++)
 			for (int j = 0; j < 3; j++)
 				mGain[i][j] = .05;
+	}
+	
+	private String getErrorString() {
+		return "GUID:ERROR:" + mErrors[0][0]
+		               + ":" + mErrors[1][0]
+		               + ":" + mErrors[2][0]
+		               + ":" + mErrors[3][0];
 	}
 	
 	/**
@@ -139,6 +161,7 @@
 							switch (msg.what) {
 							case EVAL_MOTOR_SPEED:
 								reviseMotorSpeed();
+								updateReceivers(getErrorString());
 								break;
 							case NEW_PID_VALUE:
 								mGain[msg.arg1][msg.arg2] = (Double)msg.obj;
@@ -193,6 +216,16 @@
 				Message newValue = Message.obtain(mHandler, NEW_GUID_VECTOR, myVector);
 				newValue.sendToTarget();
 			}
+		}
+	}
+	
+	/**
+	 * Registers a receiver to receive Guidance updates.
+	 * @param rec
+	 */
+	public void registerReceiver(Receivable rec) {
+		synchronized (mRec) {
+			mRec.add(rec);
 		}
 	}
 	
@@ -264,7 +297,7 @@
 					Log.w(TAG, "Nav Target lock not available.");
 				}
 			}
-			Log.v(TAG, "Relative target: " + mTarget[0] + ", " + mTarget[1] + ", " + mTarget[2]);
+			Log.v(TAG, "Relative target: " + mTarget[0] + ", " + mTarget[1] + ", " + mTarget[2] + ", " + mTarget[3]);
 		}
 		
 		
@@ -346,9 +379,7 @@
 			case 3: //Azimuth
 				break;
 			}				
-			mTorques[i] = dmotor;
-			//Log.v(TAG, "phi: " + phi);
-			//Log.v(TAG, "dmotor: " + dmotor);	
+			mTorques[i] = dmotor;	
 		}
 		mLastUpdate = thistime;
 		
@@ -382,11 +413,18 @@
 			else if (mTempMotorSpeed[i] > 1)
 				mTempMotorSpeed[i] = 1;
 			double diff = mTempMotorSpeed[i] - mMotorSpeed[i];
-			if (diff > 0)
-				mMotorSpeed[i] += Math.min(diff, MAX_DMOTOR);
-			else if (diff < 0)
-				mMotorSpeed[i] += Math.max(diff, -MAX_DMOTOR);
-			
+			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);
+			}
 			//Linearizes system with regard to prop thrust, not prop RPMs
 			mMotorSpeed[i] = Math.sqrt(mMotorSpeed[i]);
 			
@@ -413,5 +451,18 @@
 		
 		//Pass motor values to motor controller!
 	}
+	
+	/**
+	 * Updates all receivers
+	 * @param str The message to send.
+	 */
+	private void updateReceivers(String str) {
+		synchronized (mRec) {
+			ListIterator myList = mRec.listIterator();
+			while (myList.hasNext()) {
+				//myList.next().receiveMessage(str, this);
+			}
+		}
+	}
 }
 
diff --git a/org/haldean/chopper/pilot/MakePicture.java b/org/haldean/chopper/pilot/MakePicture.java
index 4d77c9a..4433924 100644
--- a/org/haldean/chopper/pilot/MakePicture.java
+++ b/org/haldean/chopper/pilot/MakePicture.java
@@ -86,13 +86,14 @@
 	private static PersistentThread myThread;
 	
 	/** Registered receivers */
-	private LinkedList mRec = new LinkedList();
+	private LinkedList mRec;
 	
 	/**
 	 * Constructs the thread, stores the surface for preview rendering.
 	 * @param sh The SurfaceHolder to which the preview will be rendered
 	 */
 	public MakePicture(SurfaceHolder sh) {
+		mRec = new LinkedList();
 		mPreviewHolder = sh;
 	}
 	
diff --git a/org/haldean/chopper/pilot/Navigation.java b/org/haldean/chopper/pilot/Navigation.java
index f9c564c..5331b6a 100644
--- a/org/haldean/chopper/pilot/Navigation.java
+++ b/org/haldean/chopper/pilot/Navigation.java
@@ -82,7 +82,7 @@
 	private static PersistentThread sThread;
 	
 	/** Registered receivers */
-	private LinkedList mRec = new LinkedList();
+	private LinkedList mRec;
 	
 	/**
 	 * Constructs a navigation object, initializes NavLists
@@ -92,7 +92,7 @@
 		if (status == null) {
 			throw new NullPointerException();
 		}
-		
+		mRec = new LinkedList();
 		mLowPower = new NavList();
 		mFlightPath = new NavList();
 		mOnMyOwn = new NavList();
@@ -144,14 +144,14 @@
 			                		evalNextVector();
 			                	break;
 			                }
-			            } 
+			            }
 					};
 
 					//FOR TESTING ONLY:
 					/*String taskList = "{ { VEL!0!10!0!0!300 VEL!5!10!5!10!180 } " + 
 						"{ DEST!300!-74.012345!40.74!10!100 { DEST!300!-77.07950!38.97300!100!250 " +
 							" DEST!587!-117.15!32.72!10!600 } } }";*/
-					String taskList = "{VEL!0!0!0!0!600}";
+					String taskList = "{ VEL!0!0!0!180!600 }";
 					setTask(BASIC_AUTO, taskList);
 					updateStatus(BASIC_AUTO);
 					autoPilot(true);
diff --git a/org/haldean/chopper/pilot/StatusReporter.java b/org/haldean/chopper/pilot/StatusReporter.java
index c9f2ace..53f7b01 100644
--- a/org/haldean/chopper/pilot/StatusReporter.java
+++ b/org/haldean/chopper/pilot/StatusReporter.java
@@ -39,7 +39,7 @@
 	private ChopperStatus mStatus;
 	
 	/** List of registered receivers */
-	private LinkedList mRec = new LinkedList();;
+	private LinkedList mRec;
 	
 	/** Handles task scheduling */
 	private Handler mHandler;
@@ -49,6 +49,7 @@
 	 * @param status The ChopperStatus from which to compile status reports
 	 */
 	public StatusReporter(ChopperStatus status) {
+		mRec = new LinkedList();
 		mStatus = status;
 	}