git.haldean.org droidcopter / b748482
POSSIBLY FLAWED COMMIT Ben 11 years ago
9 changed file(s) with 76 addition(s) and 19 deletion(s). Raw diff Collapse all Expand all
Binary diff not shown
Binary diff not shown
6868
6969 reporter.registerReceiver(comm);
7070 pic.registerReceiver(comm);
71 guid.registerReceiver(comm);
7172
7273 new PersistentThread(comm).start();
7374 status.getPersistentThreadInstance().start();
104104 private ReentrantLock[] mReadingLock;
105105
106106 /** List of registered receivers */
107 private LinkedList<Receivable> mRec = new LinkedList<Receivable>();
107 private LinkedList<Receivable> mRec;
108108
109109 /** Hides Runnability, ensures singleton-ness */
110110 private Runnable mRunner;
116116 */
117117 public ChopperStatus(Context mycontext) {
118118 mContext = mycontext;
119 mRec = new LinkedList<Receivable>();
119120
120121 //Initialize the data locks
121122 mReadingLock = new ReentrantLock[SENSORS];
279279 */
280280 public void sendMessage(final String message) {
281281 if (mTextOut == null) { //connection not yet initialized
282 //Log.d(TAG, "Message not sent, socket not available.");
282283 return;
283284 }
284285 else {
286 // Log.v(TAG, "Sending msg: " + message);
285287 mPool.submit(new Runnable() {
286288 public void run() {
287289 try {
00 package org.haldean.chopper.pilot;
1
2 import java.util.LinkedList;
3 import java.util.ListIterator;
14
25 import android.os.Handler;
36 import android.os.Looper;
69
710 /**
811 * Determines motor speeds, based on chopper's status and desired velocity vector.
12 *
13 * May send the following messages to registered Receivables:<br>
14 * <pre>
15 * GUID:ERROR:&lt;loop_1_error&gt;:&lt;loop_2_error&gt;:&lt;loop_3_error&gt;:&lt;loop_4_error&gt;
16 * </pre>
917 *
1018 * May receive the following messages from Chopper components:
1119 * <pre>
2129 public class Guidance implements Constants, Receivable {
2230
2331 /** How many times per second the PID loop will run */
24 public int PIDREPS = 20;
32 public int PIDREPS = 1;
2533
2634 /** Maximum permissible target velocity, in m/s; larger vectors will be resized */
2735 public static final double MAX_VEL = 2.0;
3038 public static final double MAX_ANGLE = 20;
3139
3240 /** The maximum change in motor speed permitted at one time. Must be positive. */
33 public static final double MAX_DMOTOR = .05F;
41 public static final double MAX_DMOTOR = .05;
42
43 /** The maximum change in motor speed permitted at one time if the chopper is stabilizing. Must be positive. */
44 public static final double MAX_DSTABLE = .1;
3445
3546 /** Tag for logging */
3647 public static final String TAG = new String("chopper.Guidance");
91102 * in the event of difficulty maintaining altitude */
92103 private boolean mHorizontalDrift = false; //if true, does not consider dx, dy or azimuth error; makes for maximally efficient altitude control
93104
105 private LinkedList<Receivable> mRec;
106
94107 /** Handles to other chopper components */
95108 private ChopperStatus mStatus;
96109 private Navigation mNav;
110123 }
111124 mStatus = status;
112125 mNav = nav;
113 //Temporary: need real tuning values at some point:
126 mRec = new LinkedList<Receivable>();
127
128 //Temporary: need real tuning values at some point. Crap.
114129 for (int i = 0; i < 4; i++)
115130 for (int j = 0; j < 3; j++)
116131 mGain[i][j] = .05;
132 }
133
134 private String getErrorString() {
135 return "GUID:ERROR:" + mErrors[0][0]
136 + ":" + mErrors[1][0]
137 + ":" + mErrors[2][0]
138 + ":" + mErrors[3][0];
117139 }
118140
119141 /**
138160 switch (msg.what) {
139161 case EVAL_MOTOR_SPEED:
140162 reviseMotorSpeed();
163 updateReceivers(getErrorString());
141164 break;
142165 case NEW_PID_VALUE:
143166 mGain[msg.arg1][msg.arg2] = (Double)msg.obj;
192215 Message newValue = Message.obtain(mHandler, NEW_GUID_VECTOR, myVector);
193216 newValue.sendToTarget();
194217 }
218 }
219 }
220
221 /**
222 * Registers a receiver to receive Guidance updates.
223 * @param rec
224 */
225 public void registerReceiver(Receivable rec) {
226 synchronized (mRec) {
227 mRec.add(rec);
195228 }
196229 }
197230
263296 Log.w(TAG, "Nav Target lock not available.");
264297 }
265298 }
266 Log.v(TAG, "Relative target: " + mTarget[0] + ", " + mTarget[1] + ", " + mTarget[2]);
299 Log.v(TAG, "Relative target: " + mTarget[0] + ", " + mTarget[1] + ", " + mTarget[2] + ", " + mTarget[3]);
267300 }
268301
269302
345378 case 3: //Azimuth
346379 break;
347380 }
348 mTorques[i] = dmotor;
349 //Log.v(TAG, "phi: " + phi);
350 //Log.v(TAG, "dmotor: " + dmotor);
381 mTorques[i] = dmotor;
351382 }
352383 mLastUpdate = thistime;
353384
381412 else if (mTempMotorSpeed[i] > 1)
382413 mTempMotorSpeed[i] = 1;
383414 double diff = mTempMotorSpeed[i] - mMotorSpeed[i];
384 if (diff > 0)
385 mMotorSpeed[i] += Math.min(diff, MAX_DMOTOR);
386 else if (diff < 0)
387 mMotorSpeed[i] += Math.max(diff, -MAX_DMOTOR);
388
415 if (mStabilizing) {
416 if (diff > 0)
417 mMotorSpeed[i] += Math.min(diff, MAX_DSTABLE);
418 else if (diff < 0)
419 mMotorSpeed[i] += Math.max(diff, -MAX_DSTABLE);
420 }
421 else {
422 if (diff > 0)
423 mMotorSpeed[i] += Math.min(diff, MAX_DMOTOR);
424 else if (diff < 0)
425 mMotorSpeed[i] += Math.max(diff, -MAX_DMOTOR);
426 }
389427 //Linearizes system with regard to prop thrust, not prop RPMs
390428 mMotorSpeed[i] = Math.sqrt(mMotorSpeed[i]);
391429
412450
413451 //Pass motor values to motor controller!
414452 }
453
454 /**
455 * Updates all receivers
456 * @param str The message to send.
457 */
458 private void updateReceivers(String str) {
459 synchronized (mRec) {
460 ListIterator<Receivable> myList = mRec.listIterator();
461 while (myList.hasNext()) {
462 //myList.next().receiveMessage(str, this);
463 }
464 }
465 }
415466 }
416467
8585 private static PersistentThread myThread;
8686
8787 /** Registered receivers */
88 private LinkedList<Receivable> mRec = new LinkedList<Receivable>();
88 private LinkedList<Receivable> mRec;
8989
9090 /**
9191 * Constructs the thread, stores the surface for preview rendering.
9292 * @param sh The SurfaceHolder to which the preview will be rendered
9393 */
9494 public MakePicture(SurfaceHolder sh) {
95 mRec = new LinkedList<Receivable>();
9596 mPreviewHolder = sh;
9697 }
9798
8181 private static PersistentThread sThread;
8282
8383 /** Registered receivers */
84 private LinkedList<Receivable> mRec = new LinkedList<Receivable>();
84 private LinkedList<Receivable> mRec;
8585
8686 /**
8787 * Constructs a navigation object, initializes NavLists
9191 if (status == null) {
9292 throw new NullPointerException();
9393 }
94
94 mRec = new LinkedList<Receivable>();
9595 mLowPower = new NavList();
9696 mFlightPath = new NavList();
9797 mOnMyOwn = new NavList();
143143 evalNextVector();
144144 break;
145145 }
146 }
146 }
147147 };
148148
149149 //FOR TESTING ONLY:
150150 /*String taskList = "{ { VEL!0!10!0!0!300 VEL!5!10!5!10!180 } " +
151151 "{ DEST!300!-74.012345!40.74!10!100 { DEST!300!-77.07950!38.97300!100!250 " +
152152 " DEST!587!-117.15!32.72!10!600 } } }";*/
153 String taskList = "{VEL!0!0!0!0!600}";
153 String taskList = "{ VEL!0!0!0!180!600 }";
154154 setTask(BASIC_AUTO, taskList);
155155 updateStatus(BASIC_AUTO);
156156 autoPilot(true);
3838 private ChopperStatus mStatus;
3939
4040 /** List of registered receivers */
41 private LinkedList<Receivable> mRec = new LinkedList<Receivable>();;
41 private LinkedList<Receivable> mRec;
4242
4343 /** Handles task scheduling */
4444 private Handler mHandler;
4848 * @param status The ChopperStatus from which to compile status reports
4949 */
5050 public StatusReporter(ChopperStatus status) {
51 mRec = new LinkedList<Receivable>();
5152 mStatus = status;
5253 }
5354