0 | 0 |
package org.haldean.chopper.pilot;
|
|
1 |
|
|
2 |
import java.util.LinkedList;
|
|
3 |
import java.util.ListIterator;
|
1 | 4 |
|
2 | 5 |
import android.os.Handler;
|
3 | 6 |
import android.os.Looper;
|
|
6 | 9 |
|
7 | 10 |
/**
|
8 | 11 |
* 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:<loop_1_error>:<loop_2_error>:<loop_3_error>:<loop_4_error>
|
|
16 |
* </pre>
|
9 | 17 |
*
|
10 | 18 |
* May receive the following messages from Chopper components:
|
11 | 19 |
* <pre>
|
|
21 | 29 |
public class Guidance implements Constants, Receivable {
|
22 | 30 |
|
23 | 31 |
/** How many times per second the PID loop will run */
|
24 | |
public int PIDREPS = 20;
|
|
32 |
public int PIDREPS = 1;
|
25 | 33 |
|
26 | 34 |
/** Maximum permissible target velocity, in m/s; larger vectors will be resized */
|
27 | 35 |
public static final double MAX_VEL = 2.0;
|
|
30 | 38 |
public static final double MAX_ANGLE = 20;
|
31 | 39 |
|
32 | 40 |
/** 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;
|
34 | 45 |
|
35 | 46 |
/** Tag for logging */
|
36 | 47 |
public static final String TAG = new String("chopper.Guidance");
|
|
91 | 102 |
* in the event of difficulty maintaining altitude */
|
92 | 103 |
private boolean mHorizontalDrift = false; //if true, does not consider dx, dy or azimuth error; makes for maximally efficient altitude control
|
93 | 104 |
|
|
105 |
private LinkedList<Receivable> mRec;
|
|
106 |
|
94 | 107 |
/** Handles to other chopper components */
|
95 | 108 |
private ChopperStatus mStatus;
|
96 | 109 |
private Navigation mNav;
|
|
110 | 123 |
}
|
111 | 124 |
mStatus = status;
|
112 | 125 |
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.
|
114 | 129 |
for (int i = 0; i < 4; i++)
|
115 | 130 |
for (int j = 0; j < 3; j++)
|
116 | 131 |
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];
|
117 | 139 |
}
|
118 | 140 |
|
119 | 141 |
/**
|
|
138 | 160 |
switch (msg.what) {
|
139 | 161 |
case EVAL_MOTOR_SPEED:
|
140 | 162 |
reviseMotorSpeed();
|
|
163 |
updateReceivers(getErrorString());
|
141 | 164 |
break;
|
142 | 165 |
case NEW_PID_VALUE:
|
143 | 166 |
mGain[msg.arg1][msg.arg2] = (Double)msg.obj;
|
|
192 | 215 |
Message newValue = Message.obtain(mHandler, NEW_GUID_VECTOR, myVector);
|
193 | 216 |
newValue.sendToTarget();
|
194 | 217 |
}
|
|
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);
|
195 | 228 |
}
|
196 | 229 |
}
|
197 | 230 |
|
|
263 | 296 |
Log.w(TAG, "Nav Target lock not available.");
|
264 | 297 |
}
|
265 | 298 |
}
|
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]);
|
267 | 300 |
}
|
268 | 301 |
|
269 | 302 |
|
|
345 | 378 |
case 3: //Azimuth
|
346 | 379 |
break;
|
347 | 380 |
}
|
348 | |
mTorques[i] = dmotor;
|
349 | |
//Log.v(TAG, "phi: " + phi);
|
350 | |
//Log.v(TAG, "dmotor: " + dmotor);
|
|
381 |
mTorques[i] = dmotor;
|
351 | 382 |
}
|
352 | 383 |
mLastUpdate = thistime;
|
353 | 384 |
|
|
381 | 412 |
else if (mTempMotorSpeed[i] > 1)
|
382 | 413 |
mTempMotorSpeed[i] = 1;
|
383 | 414 |
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 |
}
|
389 | 427 |
//Linearizes system with regard to prop thrust, not prop RPMs
|
390 | 428 |
mMotorSpeed[i] = Math.sqrt(mMotorSpeed[i]);
|
391 | 429 |
|
|
412 | 450 |
|
413 | 451 |
//Pass motor values to motor controller!
|
414 | 452 |
}
|
|
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 |
}
|
415 | 466 |
}
|
416 | 467 |
|