git.haldean.org droidcopter / ae4c764
New PID loop/guidance structure (EnsignCrusher updated). Fixed bug in PadController where the back button would not be recognized. Benjamin Bardin 10 years ago
11 changed file(s) with 493 addition(s) and 782 deletion(s). Raw diff Collapse all Expand all
8585 <uses-feature android:name="android.hardware.camera.autofocus" />
8686
8787 <uses-sdk
88 android:minSdkVersion="8"
89 android:targetSdkVersion="8"/>
88 android:minSdkVersion="10"
89 android:targetSdkVersion="10"/>
9090
9191 </manifest>
99 # Indicates whether an apk should be generated for each density.
1010 split.density=false
1111 # Project target.
12 target=android-8
12 target=android-10
305305 localDevice.init(this, new ReadyListener() {
306306 @Override
307307 public void ready() {
308 RemoteDevice device = localDevice.getRemoteForAddr(address);
309 localDevice.destroy();
310 new ConnectThread(device).start();
308 try {
309 RemoteDevice device = localDevice.getRemoteForAddr(address);
310 localDevice.destroy();
311 new ConnectThread(device).start();
312 }
313 catch (NullPointerException e) {
314 Log.e(TAG, "Fuck external libraries.");
315 e.printStackTrace();
316 }
311317 }
312318 });
313319
0 package org.haldean.chopper.pilot;
1
2 import android.util.Log;
3
4 public class Angler implements Constants {
5 /** Maximum permissible target velocity, in m/s; larger vectors will be resized */
6 public static final double MAX_VEL = 2.0;
7
8 /** The maximum angle, in degrees, guidance will permit the chopper to have */
9 public static final double MAX_ANGLE = 10;
10
11 public static final String TAG = new String("chopper.Angler");
12
13 private Navigation mNav;
14 private ChopperStatus mStatus;
15 private double[] mNavTarget;
16 private double[] mRelativeTarget;
17 private double[] mCurrent;
18
19 public Angler(ChopperStatus status, Navigation nav) {
20 mNav = nav;
21 mStatus = status;
22 mCurrent = new double[4];
23 mNavTarget = new double[4];
24 mRelativeTarget = new double[4];
25
26 }
27
28 public void getAngleTarget(double[] target) {
29 if ((target == null) || (target.length < 4)) {
30 return;
31 }
32 mNav.evalNextVector(mNavTarget);
33
34 double mAzimuth = mStatus.getReadingField(AZIMUTH);
35
36 // Calculate target relative velocity.
37 double theta = mAzimuth * Math.PI / 180.0;
38 mRelativeTarget[0] = mNavTarget[0] * Math.cos(theta) - mNavTarget[1] * Math.sin(theta);
39 mRelativeTarget[1] = mNavTarget[0] * Math.sin(theta) + mNavTarget[1] * Math.cos(theta);
40 mRelativeTarget[2] = mNavTarget[2];
41 mRelativeTarget[3] = mNavTarget[3];
42
43 //Calculate recorded velocity; reduce, if necessary, to MAXVEL
44 double myVel = 0;
45 for (int i = 0; i < 3; i++) {
46 myVel += Math.pow(mRelativeTarget[i], 2);
47 }
48 myVel = Math.sqrt(myVel);
49 if (myVel > MAX_VEL) {
50 Log.v(TAG, "guid, Reducing requested velocity");
51 double adjustment = MAX_VEL / myVel;
52 for (int i = 0; i < 3; i++) {
53 mRelativeTarget[i] *= adjustment;
54 }
55 }
56
57 // Calculate current relative velocity.
58 double mGpsBearing = mStatus.getGpsField(BEARING);
59 double phi = (mGpsBearing - mAzimuth) * Math.PI / 180.0;
60
61 double mGpsSpeed = mStatus.getGpsField(SPEED);
62 mCurrent[0] = mGpsSpeed * Math.sin(phi);
63 mCurrent[1] = mGpsSpeed * Math.cos(phi);
64 mCurrent[2] = mStatus.getGpsField(dALT);
65 mCurrent[3] = mAzimuth;
66
67 if (mCurrent[0] < mRelativeTarget[0]) {
68 target[0] += 1.0;
69 } else {
70 target[0] -= 1.0;
71 }
72
73 if (mCurrent[1] < mRelativeTarget[1]) {
74 target[1] += 1.0;
75 } else {
76 target[1] -= 1.0;
77 }
78
79 target[0] = restrainedTarget(target[0]);
80 target[1] = restrainedTarget(target[1]);
81 target[2] = mRelativeTarget[2];
82 target[3] = mRelativeTarget[3];
83 }
84
85 private static double restrainedTarget(double requested) {
86 if (requested < -MAX_ANGLE) {
87 return -MAX_ANGLE;
88 }
89 if (requested > MAX_ANGLE) {
90 return MAX_ANGLE;
91 }
92 return requested;
93 }
94 }
1515 /** Tag for logging */
1616 public static final String TAG = "chopper.ChopperMain";
1717
18 private boolean telemetry = false;
18 private boolean telemetry = true;
1919
2020 /** Prevent duplicate launching of threads if app loses focus **/
2121 private static boolean mFirstRun = true;
2222
2323 /** Need a permanent reference to this, to destroy it. **/
2424 private Guidance guid;
25 private ChopperStatus status;
2526
2627 /**
2728 * Holds the wakelock; needed to keep the camera preview rendering on
7879 previewHolder.setType(SurfaceHolder.SURFACE_TYPE_PUSH_BUFFERS);
7980
8081 Comm comm = new Comm(true);
81 ChopperStatus status = new ChopperStatus(getApplicationContext());
82 status = new ChopperStatus(getApplicationContext());
8283 StatusReporter reporter = new StatusReporter(status);
8384 MakePicture pic = null;
8485 if (telemetry) {
9495 }
9596 comm.registerReceiver(NAV, nav);
9697 comm.registerReceiver(CSYS, nav);
98 comm.registerReceiver(CSYS, guid);
9799 comm.registerReceiver(GUID, guid);
98100 comm.registerReceiver(GUID, nav);
99101
116118 if (telemetry) {
117119 new Thread(pic).start();
118120 }
119 new Thread(nav).start();
120121 new Thread(guid).start();
121122 }
122123 catch (Exception e) {
134135 super.onDestroy();
135136 if (guid != null)
136137 guid.onDestroy();
138 if (status != null)
139 status.close();
137140 }
138141 }
00 package org.haldean.chopper.pilot;
11
2 import java.io.BufferedWriter;
3 import java.io.File;
4 import java.io.FileWriter;
5 import java.io.IOException;
26 import java.util.LinkedList;
37 import java.util.ListIterator;
4 import java.util.concurrent.ExecutorService;
5 import java.util.concurrent.Executors;
6 import java.util.concurrent.TimeUnit;
78 import java.util.concurrent.atomic.AtomicInteger;
8 import java.util.concurrent.locks.ReentrantLock;
99
1010 import android.content.BroadcastReceiver;
1111 import android.content.Context;
2121 import android.location.LocationProvider;
2222 import android.os.BatteryManager;
2323 import android.os.Bundle;
24 import android.os.Environment;
2425 import android.os.Looper;
2526 import android.util.Log;
2627
4546 /** Point of "officially" low battery */
4647 public final static int LOW_BATT = 30;
4748
49 public static final String logfilename = "fastest.txt";
50 public static BufferedWriter logwriter;
51
4852 /** Tag for logging */
4953 public static final String TAG = "chopper.ChopperStatus";
5054
5660 * Value of '0' means maximum accuracy. */
5761 private static final long GPS_MIN_TIME = 0;
5862
59 /** Number of threads in the mutator pool */
60 private static final int sNumMutatorThreads = 5;
61
6263 /** Used to obtain location manager. */
6364 private Context mContext;
6465
7273 private float mGpsAccuracy; //accuracy of said reading
7374
7475 /** Lock for gps extras */
75 private ReentrantLock mGpsExtrasLock;
76 private Object mGpsExtrasLock;
7677
7778 /** Locks for fields in gps[]. */
78 private ReentrantLock[] mGpsLock;
79
80 /** Number of satellites used to obtain last GPS reading. This field is only accessed by one thread, so no lock is needed. */
79 private Object[] mGpsLock;
80
81 /** Number of satellites used to obtain last GPS reading. */
8182 private int mGpsNumSats; //number of satellites used to collect last reading
8283
83 /** Timestamp of last GPS reading. This field is only accessed by one thread, so no lock is needed. */
84 /** Timestamp of last GPS reading. */
8485 private long mGpsTimeStamp; //timestamp of the reading
8586
8687 /** Stores the location object last returned by the GPS. */
8990 /** Max battery level. */
9091 private final AtomicInteger mMaxBatt = new AtomicInteger(100);
9192
92 /** Lock for motorspeed[]. */
93 private ReentrantLock mMotorLock;
94
9593 /** Stores the speeds last submitted to the motors. */
9694 private double[] mMotorSpeed = new double[4];
9795
98 /** Lock for mMotorPower[] */
99 private ReentrantLock mMotorPowerLock;
100
10196 /** Stores power levels for the motors. */
10297 private double[] mMotorPower = new double[4];
10398
104 /** Thread pool for mutator methods */
105 private ExecutorService mMutatorPool;
106
10799 /** Holds data from various sensors, like gyroscope, acceleration and magnetic flux. */
108100 private double[] mReading = new double[SENSORS]; //last known data for a given sensor
109101
110102 /** Locks for fields in reading[]. */
111 private ReentrantLock[] mReadingLock;
103 private Object[] mReadingLock;
112104
113105 /** List of registered receivers */
114106 private LinkedList<Receivable> mRec;
115107
108 private float[] orientation = new float[3];
109 private float[] rotationMatrix = new float[9];
110 private float[] flux = new float[3];
111
116112 /**
117113 * Initializes the locks, registers application context for runtime use.
118114 * @param mycontext Application context
122118 mRec = new LinkedList<Receivable>();
123119
124120 //Initialize the data locks
125 mReadingLock = new ReentrantLock[SENSORS];
121 mReadingLock = new Object[SENSORS];
126122
127123 for (int i = 0; i < SENSORS; i++) {
128 mReadingLock[i] = new ReentrantLock();
124 mReadingLock[i] = new Object();
129125 }
130126
131 mGpsLock = new ReentrantLock[GPS_FIELDS];
127 mGpsLock = new Object[GPS_FIELDS];
132128 for (int i = 0; i < GPS_FIELDS; i++) {
133 mGpsLock[i] = new ReentrantLock();
134 }
135
136 mMotorLock = new ReentrantLock();
137 mGpsExtrasLock = new ReentrantLock();
138 mMotorPowerLock = new ReentrantLock();
139
140 mMutatorPool = Executors.newFixedThreadPool(sNumMutatorThreads);
129 mGpsLock[i] = new Object();
130 }
131 mGpsExtrasLock = new Object();
132 }
133
134 public void close() {
135 if (logwriter != null) {
136 try {
137 logwriter.close();
138 } catch (IOException e) {
139 Log.e(TAG, "Canno close logfile");
140 }
141 }
141142 }
142143
143144 /**
151152 /**
152153 * Gets GPS "extras" data, through a string of the form accuracy:numberOfSatellitesInLatFix:timeStampOfLastFix
153154 * @return The data string
154 * @throws IllegalAccessException If the lock is currently in use.
155 */
156 public String getGpsExtrasNow() throws IllegalAccessException {
155 */
156 public String getGpsExtras() {
157157 String gpsData = "";
158 try {
159 if (mGpsExtrasLock.tryLock(0, TimeUnit.SECONDS)) {
160 gpsData += mGpsAccuracy +
161 ":" + mGpsNumSats +
162 ":" + mGpsTimeStamp;
163 mGpsExtrasLock.unlock();
164 }
165 else {
166 throw new InterruptedException();
167 }
168 }
169 catch (InterruptedException e) {
170 throw new IllegalAccessException();
158 synchronized (mGpsExtrasLock) {
159 gpsData += mGpsAccuracy +
160 ":" + mGpsNumSats +
161 ":" + mGpsTimeStamp;
171162 }
172163 return gpsData;
173164 }
179170 */
180171 public double getGpsField(int whichField) {
181172 double myValue;
182 mGpsLock[whichField].lock();
183 myValue = mGps[whichField];
184 mGpsLock[whichField].unlock();
173 synchronized (mGpsLock[whichField]) {
174 myValue = mGps[whichField];
175 }
185176 return myValue;
186177 }
187178
188 /**
189 * Returns the value stored at the specified GPS index, if its lock is available. Otherwise, throws an exception.
190 * @param whichField The index of the desired GPS data.
191 * @return The desired GPS data.
192 * @throws IllegalAccessException If the lock for the desired data is unavailable.
193 */
194 public double getGpsFieldNow(int whichField) throws IllegalAccessException {
195 double myValue;
196 try {
197 if (mGpsLock[whichField].tryLock(0, TimeUnit.SECONDS)) {
198 myValue = mGps[whichField];
199 mGpsLock[whichField].unlock();
200 }
201 else {
202 throw new InterruptedException();
203 }
204 }
205 catch (InterruptedException e) {
206 Log.w(TAG, "GPS Field " + whichField + " is locked.");
207 throw new IllegalAccessException();
208 }
209 return myValue;
210 }
211
212 /**
213 * Returns the value stored at the specified GPS index, if its lock is available. Otherwise, returns the supplied default value.
214 * @param whichField The index of the desired GPS data.
215 * @param expectedValue The default to return, should its lock be unavailable.
216 * @return Either the GPS data or the supplied default, depending on whether or not its lock is available.
217 */
218 public double getGpsFieldNow(int whichField, double expectedValue) {
219 double myValue;
220 try {
221 if (mGpsLock[whichField].tryLock(0, TimeUnit.SECONDS)) {
222 myValue = mGps[whichField];
223 mGpsLock[whichField].unlock();
224 }
225 else {
226 throw new InterruptedException();
227 }
228 }
229 catch (InterruptedException e) {
230 myValue = expectedValue;
231 }
232 return myValue;
179 public long getGpsTimeStamp() {
180 synchronized(mGpsExtrasLock) {
181 return mGpsTimeStamp;
182 }
233183 }
234184
235185 /**
248198
249199 /**
250200 * Obtains the current motor speeds.
251 * @return A copy of the array containing the motor speeds.
252 * @throws IllegalAccessException If the lock is unavailable.
253 */
254 public double[] getMotorFieldsNow() throws IllegalAccessException {
255 double[] myValues = new double[4];
256 try {
257 if (mMotorLock.tryLock(0, TimeUnit.SECONDS)) {
258 for (int i = 0; i < 4; i++) {
259 myValues[i] = mMotorSpeed[i];
260 }
261 mMotorLock.unlock();
262 }
263 else {
264 throw new InterruptedException();
265 }
266 }
267 catch (InterruptedException e) {
268 Log.w(TAG, "motorspeed is locked.");
269 throw new IllegalAccessException();
270 }
271 return myValues;
201 * @param myValues A copy of the array containing the motor speeds. Must have length >= 4.
202 */
203 public void getMotorFields (double[] myValues){
204 if (myValues.length < 4) {
205 throw new IllegalArgumentException();
206 }
207 synchronized (mMotorSpeed) {
208 System.arraycopy(mMotorSpeed, 0, myValues, 0, 4);
209 }
272210 }
273211
274212 /** Obtains the current motor power levels.
275 * @return A copy of the array containing the motor power levels.
276 * @throws IllegalAccessException If the lock is unavailable.
277 */
278 public double[] getMotorPowerFieldsNow() throws IllegalAccessException {
279 double[] myValues = new double[4];
280 try {
281 if (mMotorPowerLock.tryLock(0, TimeUnit.SECONDS)) {
282 for (int i = 0; i < 4; i++) {
283 myValues[i] = mMotorPower[i];
284 }
285 mMotorPowerLock.unlock();
286 }
287 else {
288 throw new InterruptedException();
289 }
290 }
291 catch (InterruptedException e) {
292 Log.w(TAG, "motorpowers is locked.");
293 throw new IllegalAccessException();
294 }
295 return myValues;
213 * @param myValues A copy of the array containing the motor power levels. Must have length >= 4.
214 */
215 public void getMotorPowerFields(double[] myValues) {
216 if (myValues.length < 4) {
217 throw new IllegalArgumentException();
218 }
219 synchronized (mMotorPower) {
220 System.arraycopy(mMotorPower, 0, myValues, 0, 4);
221 }
296222 }
297223
298224 /**
303229 //System.out.println("ChopperStatus run() thread ID " + getId());
304230 Looper.prepare();
305231 Thread.currentThread().setName("ChopperStatus");
306
232 File root = Environment.getExternalStorageDirectory();
233 if (root == null) Log.e(TAG, "No root directory found");
234 try {
235 logwriter = new BufferedWriter(new FileWriter(root + "/chopper/" + logfilename, false));
236 } catch (IOException e) {
237 Log.e(TAG, "No ChopperStatus logfile");
238 e.printStackTrace();
239 }
307240 /* Register to receive battery status updates */
308241 BroadcastReceiver batteryInfo = new BroadcastReceiver() {
309242 public void onReceive(Context context, Intent intent) {
321254 };
322255
323256 /* Gets a sensor manager */
324 SensorManager sensors = (SensorManager) mContext.getSystemService(Context.SENSOR_SERVICE);
257 final SensorManager sensors = (SensorManager) mContext.getSystemService(Context.SENSOR_SERVICE);
258 /* Registers this class as a sensor listener for every necessary sensor. */
259 //sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_ACCELEROMETER), SensorManager.SENSOR_DELAY_FASTEST);
260 //sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_LIGHT), SensorManager.SENSOR_DELAY_NORMAL);
261 sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD), SensorManager.SENSOR_DELAY_FASTEST);
325262
326 /* Registers this class as a sensor listener for every necessary sensor. */
327 sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_ACCELEROMETER), SensorManager.SENSOR_DELAY_NORMAL);
328 //sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_LIGHT), SensorManager.SENSOR_DELAY_NORMAL);
329 //sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD), SensorManager.SENSOR_DELAY_NORMAL);
330 sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_ORIENTATION), SensorManager.SENSOR_DELAY_FASTEST);
263 //sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_GYROSCOPE), SensorManager.SENSOR_DELAY_FASTEST);
331264 //sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_PRESSURE), SensorManager.SENSOR_DELAY_NORMAL);
332265 //sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_PROXIMITY), SensorManager.SENSOR_DELAY_NORMAL);
333 sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_TEMPERATURE), SensorManager.SENSOR_DELAY_NORMAL);
266 //sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_TEMPERATURE), SensorManager.SENSOR_DELAY_NORMAL);
267 sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_GRAVITY), SensorManager.SENSOR_DELAY_FASTEST);
268
334269
335270 /* Initialize GPS reading: */
336271 LocationManager LocMan = (LocationManager) mContext.getSystemService(Context.LOCATION_SERVICE);
341276 Looper.loop();
342277 }
343278
344 public double getReadingField(int whichField) {
345 double myValue;
346 mReadingLock[whichField].lock();
347 myValue = mReading[whichField];
348 mReadingLock[whichField].unlock();
349 return myValue;
350 }
351
352279 /**
353280 * Returns the reading specified by the supplied index, if its lock is available. Otherwise, throws an exception.
354281 * @param whichField The index of the desired reading.
355282 * @return The desired reading.
356 * @throws IllegalAccessException If the lock for the desired reading is unavailable.
357 */
358 public double getReadingFieldNow(int whichField) throws IllegalAccessException {
283 */
284 public double getReadingField(int whichField) {
359285 double myValue;
360 try {
361 if (mReadingLock[whichField].tryLock(0, TimeUnit.SECONDS)) {
362 myValue = mReading[whichField];
363 mReadingLock[whichField].unlock();
364 }
365 else {
366 throw new InterruptedException();
367 }
368 }
369 catch (InterruptedException e) {
370 Log.w(TAG, "Reading field " + whichField + " is locked.");
371 throw new IllegalAccessException();
372 }
373 return myValue;
374 }
375
376 /**
377 * Returns the reading specified by the supplied index, if its lock is available. Otherwise, returns the supplied default value.
378 * @param whichField The index of the desired reading.
379 * @param expectedValue The default to return, should its lock be unavailable.
380 * @return Either the reading or the supplied default, depending on whether or not its lock is available.
381 */
382 public double getReadingFieldNow(int whichField, double expectedValue) {
383 double myValue;
384 try {
385 if (mReadingLock[whichField].tryLock(0, TimeUnit.SECONDS)) {
386 myValue = mReading[whichField];
387 mReadingLock[whichField].unlock();
388 }
389 else {
390 throw new InterruptedException();
391 }
392 }
393 catch (InterruptedException e) {
394 Log.e(TAG, "Reading field " + whichField + " is locked.");
395 myValue = expectedValue;
286 synchronized (mReadingLock[whichField]) {
287 myValue = mReading[whichField];
396288 }
397289 return myValue;
398290 }
420312 /* Vertical velocity does not update until vertical position does; prevents false conclusions that vertical velocity == 0 */
421313 double oldAlt = getGpsField(ALTITUDE);
422314 if (newalt != oldAlt) {
423 mGpsExtrasLock.lock();
424 long timeElapsed = mGpsTimeStamp - loc.getTime();
425 mGpsExtrasLock.unlock();
315 long timeElapsed;
316 synchronized (mGpsExtrasLock) {
317 timeElapsed = mGpsTimeStamp - loc.getTime();
318 }
426319 double newdalt = ((newalt - oldAlt) / (double) timeElapsed) * 1000.0;
427320 setGpsField(dALT, newdalt);
428321 Log.i(TAG, "new dalt: " + newdalt);
433326 setGpsField(LAT, loc.getLatitude());
434327 setGpsField(SPEED, loc.getSpeed());
435328
436 mGpsExtrasLock.lock();
437 mGpsAccuracy = loc.getAccuracy();
438 mGpsTimeStamp = loc.getTime();
439 if (loc.getExtras() != null)
440 mGpsNumSats = loc.getExtras().getInt("satellites");
441 mGpsExtrasLock.unlock();
329 synchronized (mGpsExtrasLock) {
330 mGpsAccuracy = loc.getAccuracy();
331 mGpsTimeStamp = loc.getTime();
332 if (loc.getExtras() != null)
333 mGpsNumSats = loc.getExtras().getInt("satellites");
334 }
442335 if (mLastLoc != null) {
443336 synchronized (mLastLoc) {
444337 mLastLoc = loc;
473366 * @param event Object containing new data--sensor type, accuracy, timestamp and value.
474367 */
475368 public void onSensorChanged(SensorEvent event) {
369 long time = System.currentTimeMillis();
476370 int type = event.sensor.getType();
477 switch (type) {
371 switch (type) {
478372 case Sensor.TYPE_ACCELEROMETER:
479373 setReadingField(X_ACCEL, event.values[0]);
480374 setReadingField(Y_ACCEL, event.values[1]);
484378 setReadingField(LIGHT, event.values[0]);
485379 break;
486380 case Sensor.TYPE_MAGNETIC_FIELD:
381 System.arraycopy(event.values, 0, flux, 0, 3);
487382 setReadingField(X_FLUX, event.values[0]);
488383 setReadingField(Y_FLUX, event.values[1]);
489384 setReadingField(Z_FLUX, event.values[2]);
490385 break;
491 case Sensor.TYPE_ORIENTATION:
492 setReadingField(AZIMUTH, event.values[0]);
493 setReadingField(PITCH, event.values[1]);
494 setReadingField(ROLL, event.values[2]);
495 break;
496386 case Sensor.TYPE_PRESSURE:
497387 setReadingField(PRESSURE, event.values[0]);
498388 break;
501391 break;
502392 case Sensor.TYPE_TEMPERATURE:
503393 setReadingField(TEMPERATURE, event.values[0]);
394 break;
395 case Sensor.TYPE_GRAVITY:
396 //Log.v(TAG, "grav time: " + (time - grav_time));
397 String timestring = Long.toString(time);
398 try {
399 if (logwriter != null) {
400 logwriter.write(timestring + "\n");
401 logwriter.flush();
402 }
403 } catch (IOException e) {
404 // Do nothing.
405 }
406 /*Log.v(TAG, "");
407 Log.v(TAG, "" + event.values[0]);
408 Log.v(TAG, "" + event.values[1]);
409 Log.v(TAG, "" + event.values[2]);
410 Log.v(TAG, ""); */
411 SensorManager.getRotationMatrix(rotationMatrix,
412 null,
413 event.values,
414 flux);
415 /*for (int i = 0; i < 9; i++) {
416 Log.v(TAG, "" + rotationMatrix[i]);
417 }*/
418 SensorManager.getOrientation(rotationMatrix, orientation);
419 //Log.v(TAG, "" + orientation[i]);
420 setReadingField(AZIMUTH, orientation[0] * 180.0 / Math.PI);
421 setReadingField(PITCH, orientation[1] * 180.0 / Math.PI);
422 setReadingField(ROLL, orientation[2] * -180.0 / Math.PI);
504423 break;
505424 }
506425 }
541460 * Sets the motor speed data to the supplied array.
542461 * @param mySpeeds The data to which the motor speeds should be set.
543462 */
544 protected void setMotorFields(double[] mySpeeds) {
463 protected void setMotorFields(final double[] mySpeeds) {
545464 if (mySpeeds.length < 4)
546465 return;
547 final double[] speeds = mySpeeds.clone();
548 mMutatorPool.submit(new Runnable() {
549 public void run() {
550 //Log.v(TAG, "Changing motorspeeds:");
551
552 mMotorLock.lock();
553 for (int i = 0; i < 4; i++) {
554 mMotorSpeed[i] = speeds[i];
555 }
556 /*Log.v(TAG, "vector " + mMotorSpeed[0] + ", "
557 + mMotorSpeed[1] + ", "
558 + mMotorSpeed[2] + ", "
559 + mMotorSpeed[3]);*/
560 mMotorLock.unlock();
561 //Log.v(TAG, "Done changing motorspeeds");
562 }
563 });
466 synchronized (mMotorSpeed) {
467 System.arraycopy(mySpeeds, 0, mMotorSpeed, 0, 4);
468 /*Log.v(TAG, "vector " + mMotorSpeed[0] + ", "
469 + mMotorSpeed[1] + ", "
470 + mMotorSpeed[2] + ", "
471 + mMotorSpeed[3]);*/
472 }
473 //Log.v(TAG, "Done changing motorspeeds");
564474 }
565475
566476 /**
567477 * Sets the motor power levels to the supplied array.
568478 * @param myPowers The data to which the motor power levels should be set.
569479 */
570 protected void setMotorPowerFields(double[] myPowers) {
480 protected void setMotorPowerFields(final double[] myPowers) {
571481 if (myPowers.length < 4)
572482 return;
573 final double[] powers = myPowers.clone();
574 mMutatorPool.submit(new Runnable() {
575 public void run() {
576 mMotorPowerLock.lock();
577 for (int i = 0; i < 4; i++) {
578 mMotorPower[i] = powers[i];
579 }
580 mMotorPowerLock.unlock();
581 }
582 });
483 synchronized (mMotorPower) {
484 System.arraycopy(myPowers, 0, mMotorPower, 0, 4);
485 }
583486 }
584487
585488 /**
588491 * @param value The GPS data.
589492 */
590493 private void setGpsField(final int whichField, final double value) {
591 mMutatorPool.submit(new Runnable() {
592 public void run() {
593 mGpsLock[whichField].lock();
594 mGps[whichField] = value;
595 mGpsLock[whichField].unlock();
596 }
597 });
494 synchronized (mGpsLock[whichField]) {
495 mGps[whichField] = value;
496 }
598497 }
599498
600499
604503 * @param value The reading.
605504 */
606505 private void setReadingField(final int whichField, final double value) {
607 mMutatorPool.submit(new Runnable() {
608 public void run() {
609 mReadingLock[whichField].lock();
610 mReading[whichField] = value;
611 mReadingLock[whichField].unlock();
612 }
613 });
506 synchronized (mReadingLock[whichField]) {
507 mReading[whichField] = value;
508 }
614509 }
615510
616511 /** Updates all registered receivers with the specified String */
33 import java.io.IOException;
44 import java.util.LinkedList;
55 import java.util.ListIterator;
6 import java.util.concurrent.atomic.AtomicInteger;
67
78 import android.os.Handler;
89 import android.os.Looper;
2425 * PID:
2526 * SET:&lt;pid_loop_number&gt;:&lt;pid_parameter_index&gt;:&lt;pid_parameter_value&gt;
2627 * GET
27 * AUTOMATIC
28 * AUTOPILOT
2829 * VECTOR:&lt;north_motor_speed&gt;:&lt;south_motor_speed&gt;:&lt;east_motor_speed&gt;:&lt;west_motor_speed&gt;
2930 * LOCALVEC
3031 * ABSVEC
3637 public class Guidance implements Runnable, Constants, Receivable {
3738
3839 /** How many times per second the PID loop will run */
39 public static final int PIDREPS = 10;
40
41 /** Maximum permissible target velocity, in m/s; larger vectors will be resized */
42 public static final double MAX_VEL = 2.0;
43
44 /** The maximum angle, in degrees, guidance will permit the chopper to have */
45 public static final double MAX_ANGLE = 10;
40 public static final int PIDREPS = 40;
4641
4742 /** The maximum change in motor speed permitted at one time. Must be positive. */
48 public static final double MAX_DMOTOR = .01;
43 public static final double MAX_DMOTOR = .05;
4944
5045 /** The maximum change in motor speed permitted at one time if the chopper is stabilizing. Must be positive. */
51 public static final double MAX_DSTABLE = .05;
46 public static final double MAX_DSTABLE = .1;
5247
5348 /** Tag for logging */
5449 public static final String TAG = new String("chopper.Guidance");
5550
56 /** Used when a really big number is needed, still small enough to prevent overflow. */
57 private static final double sReallyBig = 2.0;
58
5951 /** Handles messages for the thread */
6052 private Handler mHandler;
6153
62 /** Stores whether or not a motor-eval loop should add itself to the queue again. **/
63 private boolean mReviseMotorSpeeds = true;
54 private Angler mAngler;
6455
6556 /** Stores orientation data persistently, as expected values in case lock is not immediately available*/
6657 private double mAzimuth;
6758 private double mPitchDeg;
6859 private double mRollDeg;
69 private double mGpsBearing;
70 private double mGpsSpeed;
71 private double mGpsDalt;
7260
7361 /** Log file name **/
7462 public static final String logname = "/sdcard/chopper/guidlog.txt";
8573 * remain persistent from iteration to iteration.
8674 */
8775
88 /** Stores desired velocity */
89 private double[] mTarget = new double[4];
90
91 /** Stores the current velocity (relative to the chopper) */
92 private double[] mCurrent = new double[4];
93
9476 /** Stores current PID error */
9577 private double[][] mErrors = new double[4][3];
9678
10183 /** Timestamp of last PID evaluation */
10284 private long mLastUpdate = 0;
10385
104 /** Sum of errors * tuning parameter for a given PID loop */
105 private double[] mTorques = new double[4];
106
107 /** Stores motor speeds temporarily */
108 private double[] mTempMotorSpeed = new double[4];
86 private double[] mControlVars = new double[4];
87 private double[] mAngleTarget = new double[4];
88 private long mLastGpsTimestamp;
89 private AtomicInteger mGuidanceMode = new AtomicInteger();
90
91 private static final int DIRECT = 0;
92 private static final int MANUAL = 1;
93 private static final int AUTOPILOT = 2;
10994
11095 /** Tuning parameters */
11196 private double[][] mGain = new double[4][3];
11398 /** Motor speed */
11499 private double[] mMotorSpeed = new double[4]; //ORDER: North, South, East, West
115100
116 /** If set to true, disregards lateral velocity commands
117 * Currently unused, though later may implement as extra safety protocol
118 * in the event of difficulty maintaining altitude */
119 private boolean mHorizontalDrift = false; //if true, does not consider dx, dy or azimuth error; makes for maximally efficient altitude control
120
121101 /** List of registered receivers */
122102 private LinkedList<Receivable> mRec;
123103
124104 /** Handles to other chopper components */
125105 private ChopperStatus mStatus;
126 private Navigation mNav;
127106 private BluetoothOutput mBt;
128
129 /** Controls whether N/S and E/W commands refer to absolute vectors or local **/
130 private boolean mAbsVec = true;
131107
132108 /** Flag for writing motor speeds to output file **/
133109 public final static boolean mEnableLogging = true;
142118 throw new NullPointerException();
143119 }
144120 mStatus = status;
145 mNav = nav;
146121 mRec = new LinkedList<Receivable>();
147122 mBt = bT;
123 mAngler = new Angler(status, nav);
148124
149125 //Temporary: need real tuning values at some point. Crap.
150 for (int i = 0; i < 2; i++)
151 for (int j = 0; j < 3; j++)
152 mGain[i][j] = .0003;
153 //mGain[i][j] = .05;
154
155 for (int j = 0; j < 3; j++) {
156 // mGain[3][j] = .0005;
157 mGain[2][j] = .0025;
158 mGain[3][j] = 0;
159 }
160
161 mGain[0][0] = .001;
162 mGain[0][1] = .0003;
163 mGain[0][2] = 0;
164 mGain[1][0] = .001;
165 mGain[1][1] = .0003;
166 mGain[1][2] = 0;
167 mGain[2][0] = .006;
168 mGain[2][1] = .006;
169 mGain[2][2] = 0;
126 for (int i = 0; i < 3; i++)
127 mGain[i][0] = .1;
128 mGain[3][0] = 0;
170129
171130 try {
172131 if (mEnableLogging)
245204 }
246205 };
247206 //mHandler.sendEmptyMessage(EVAL_MOTOR_SPEED);
248 receiveMessage("GUID:VECTOR:0:0:0:0", null);
207 receiveMessage("DIRECT:0:0:0:0", null);
249208 Looper.loop();
250209 }
251210
257216 public void receiveMessage(String msg, Receivable source) {
258217 //Log.d(TAG, "Receiving message " + msg);
259218 String[] parts = msg.split(":");
219 if (parts[0].equals("CSYS")) {
220 if (parts[1].equals("NOCONN")) {
221 Log.v(TAG, "NoConn in guidance");
222 receiveMessage("GUID:AUTOPILOT", source);
223 }
224 }
260225 if (parts[0].equals("GUID")) {
261226 if (parts[1].equals("PID")) {
262227 if (parts[2].equals("SET")) {
272237 getPids.sendToTarget();
273238 }
274239 }
275 if (parts[1].equals("AUTOMATIC")) {
240 if (parts[1].equals("AUTOPILOT")) {
241 Log.v(TAG, "AUTOPILOT mode");
242 mGuidanceMode.set(AUTOPILOT);
276243 mHandler.removeMessages(NEW_GUID_VECTOR);
277 mReviseMotorSpeeds = true;
278244 if (!mHandler.hasMessages(EVAL_MOTOR_SPEED))
279245 mHandler.sendEmptyMessage(EVAL_MOTOR_SPEED);
280246 }
281 if (parts[1].equals("LOCALVEC")) {
282 Log.v(TAG, "Setting mabsvec to false");
283 mAbsVec = false;
284 }
285 if (parts[1].equals("ABSVEC")) {
286 Log.v(TAG, "Setting mabsvec to true");
287 mAbsVec = true;
288 }
289 if (parts[1].equals("VECTOR")) {
290 mReviseMotorSpeeds = false;
247 if (parts[1].equals("DIRECT")) {
248 Log.v(TAG, "direct mode");
249 mGuidanceMode.set(DIRECT);
291250 mHandler.removeMessages(EVAL_MOTOR_SPEED);
292251 Double[] myVector = new Double[4];
293252 for (int i = 0; i < 4; i++) {
296255 Message newValue = Message.obtain(mHandler, NEW_GUID_VECTOR, myVector);
297256 newValue.sendToTarget();
298257 }
258 if (parts[1].equals("MANUAL")) {
259 //autoPilot(false);
260 String log = "manual mode: ";
261 if (parts.length > 2) {
262 double[] newTarget = new double[4];
263 for (int i = 0; i < 4; i++) {
264 newTarget[i] = new Double(parts[i + 2]);
265 log += newTarget[i] + ": ";
266 }
267 //Log.v(TAG, log);
268 newTarget[0] *= Angler.MAX_ANGLE;
269 newTarget[1] *= Angler.MAX_ANGLE;
270 newTarget[2] += 1.0;
271 //newTarget[2] *= 2.0;
272 synchronized (mAngleTarget) {
273 System.arraycopy(newTarget, 0, mAngleTarget, 0, 4);
274 }
275 }
276 mGuidanceMode.set(MANUAL);
277 mHandler.removeMessages(NEW_GUID_VECTOR);
278 if (!mHandler.hasMessages(EVAL_MOTOR_SPEED))
279 mHandler.sendEmptyMessage(EVAL_MOTOR_SPEED);
280 }
299281 }
300282 }
301283
312294 /** Core of the class; calculates new motor speeds based on status */
313295 private void reviseMotorSpeed() {
314296 mHandler.removeMessages(EVAL_MOTOR_SPEED);
315 //Log.v(TAG, "START MOTOR REVISION");
316297 long starttime = System.currentTimeMillis();
317
318 //Copying motor values to temporary array for working purposes.
298 updateAngleTarget();
299
300 //Retrieve current orientation.
301
302 mAzimuth = mStatus.getReadingField(AZIMUTH);
303 mPitchDeg = mStatus.getReadingField(PITCH);
304 mRollDeg = -mStatus.getReadingField(ROLL);
305
306 double[] errors = new double[4];
307 synchronized (mAngleTarget) {
308 errors[0] = mAngleTarget[0] - mRollDeg;
309 errors[1] = mAngleTarget[1] - mPitchDeg;
310 errors[2] = mAngleTarget[2] - mStatus.getGpsField(dALT);
311 errors[3] = mAngleTarget[3] - mAzimuth;
312 }
313
314 String errs = "errors: ";
319315 for (int i = 0; i < 4; i++) {
320 mTempMotorSpeed[i] = mMotorSpeed[i];
321 }
322
323 boolean mStabilizing = false; //initializing value
324 //Retrieve current orientation.
325
326 mAzimuth = mStatus.getReadingFieldNow(AZIMUTH, mAzimuth);
327 mPitchDeg = -mStatus.getReadingFieldNow(PITCH, -mPitchDeg);
328 mRollDeg = mStatus.getReadingFieldNow(ROLL, mRollDeg);
329
330 double pitchrad = mPitchDeg * Math.PI / 180.0;
331 double rollrad = mRollDeg * Math.PI / 180.0;
332
333 double gradient = Math.sqrt(
334 Math.pow(Math.tan(rollrad), 2) +
335 Math.pow(Math.tan(pitchrad), 2)
336 );
337 double ascentrad = Math.atan(gradient);
338 double mAscentDeg = ascentrad * 180.0 / Math.PI;
339 //if orientation is out-of-bounds,
340 if ((mAscentDeg > MAX_ANGLE) | (mPitchDeg > 90.0) | (mPitchDeg < -90.0)) {
341 mStabilizing = true;
342 //set target velocity to some big number in the direction of maximum ascent
343 double gradangle = Math.atan2(
344 Math.tan(rollrad) ,
345 Math.tan(pitchrad)
346 );
347 mTarget[0] = sReallyBig * Math.sin(gradangle);
348 mTarget[1] = sReallyBig * Math.cos(gradangle);
349
350 //Make sure the velocity vector components point in the right directions.
351 mTarget[0] *= Math.signum(mTarget[0]) * Math.signum(mRollDeg);
352 mTarget[1] *= Math.signum(mTarget[1]) * Math.signum(mPitchDeg);
353 mTarget[2] = 0;
354 mTarget[3] = mAzimuth;
355 //System.out.println(target[0] + ", " + target[1]);
356 }
357 else {
358 //Retrieve target velocity from nav,
359 //Transform absolute target velocity to relative target velocity
360 double theta = mAzimuth * Math.PI / 180.0;
361 if (mNav != null) {
362 try {
363 double[] absTarget = mNav.getTarget();
364 if (mAbsVec) {
365 //Log.v(TAG, "Abs vectors");
366 mTarget[0] = absTarget[0] * Math.cos(theta) - absTarget[1] * Math.sin(theta);
367 mTarget[1] = absTarget[0] * Math.sin(theta) + absTarget[1] * Math.cos(theta);
368 mTarget[2] = absTarget[2];
369 mTarget[3] = absTarget[3];
370 }
371 else {
372 //Log.v(TAG, "Local vectors");
373 String targ = "";
374 for (int i = 0; i < 4; i++) {
375 mTarget[i] = absTarget[i];
376 targ = targ + " " + mTarget[i];
377 }
378 //Log.v(TAG, "guid, nav targ: " + targ);
379 }
380
381 //Calculate recorded velocity; reduce, if necessary, to MAXVEL
382 double myVel = 0;
383 for (int i = 0; i < 3; i++) {
384 myVel += Math.pow(mTarget[i], 2);
385 }
386 myVel = Math.sqrt(myVel);
387 if (myVel > MAX_VEL) {
388 Log.v(TAG, "guid, Reducing requested velocity");
389 double adjustment = MAX_VEL / myVel;
390 for (int i = 0; i < 3; i++) {
391 mTarget[i] *= adjustment;
392 }
393 }
394 }
395 catch (IllegalAccessException e) {
396 Log.w(TAG, "Nav Target lock not available.");
397 }
398 }
399 //Log.v(TAG, "Relative target: " + mTarget[0] + ", " + mTarget[1] + ", " + mTarget[2] + ", " + mTarget[3]);
400 }
401
402
403
404 long thistime = System.currentTimeMillis();
405
406 //Retrieve current absolute velocity. For now, only from GPS data; later, maybe write a kalman filter to use accelerometer data as well.
407 //Transform current velocity from absolute to relative
408
409 if (mStabilizing) {
410 mCurrent[0] = 0;
411 mCurrent[1] = 0;
412 mCurrent[2] = 0;
413 mCurrent[3] = mAzimuth;
414 }
415 else {
416 mGpsBearing = mStatus.getGpsFieldNow(BEARING, mGpsBearing);
417 double theta = (mGpsBearing - mAzimuth) * Math.PI / 180.0;
418
419 mGpsSpeed = mStatus.getGpsFieldNow(SPEED, mGpsSpeed);
420 mCurrent[0] = mGpsSpeed * Math.sin(theta);
421 mCurrent[1] = mGpsSpeed * Math.cos(theta);
422
423 mGpsDalt = mStatus.getGpsFieldNow(dALT, mGpsDalt);
424 mCurrent[2] = mGpsDalt;
425 mCurrent[3] = mAzimuth;
426
427 String targ = "guid, curr vec: ";
428 for (int i = 0; i < 4; i++) {
429 targ = targ + " " + mCurrent[i];
430 }
431 //Log.v(TAG, targ);
432 }
316 errs += errors[i] + ": ";
317 }
318 //Log.v(TAG, errs);
319 //For azimuth, multiple possibilities exist for error, each equally valid; but only the error nearest zero makes practical sense.
320 if (errors[3] > 180.0)
321 errors[3] -= 360.0;
322 if (errors[3] < -180.0)
323 errors[3] += 360.0;
324
433325
434326 for (int i = 0; i < 4; i++) {
435327 //Calculate proportional errors
436 double err = mTarget[i] - mCurrent[i];
437 if (i == 3) { //For azimuth, multiple possibilities exist for error, each equally valid; but only the error nearest zero makes practical sense.
438 if (err > 180.0)
439 err -= 360.0;
440 if (err < -180.0)
441 err += 360.0;
442 }
328 double err = errors[i];//mTarget[i] - mCurrent[i];
329
443330
444331 //Calculate derivative errors.
445 mErrors[i][2] = (err - mErrors[i][0]) * 1000.0 / (thistime - mLastUpdate);
332 mErrors[i][2] = (err - mErrors[i][0]) * 1000.0 / (starttime - mLastUpdate);
446333
447334
448335 //Mark proportional error
455342 mErrors[i][1] += err;
456343 mIntegralIndex = ++mIntegralIndex % PIDREPS;
457344
458 double dmotor = 0;
459
460345 //Calculate changes in output
461346 for (int j = 0; j < 3; j++) {
462 /* if (i == 1) {
463 Log.v(TAG, "guid, dy error " + j + " is " + mErrors[i][j]);
464 Log.v(TAG, "guid, dy gain " + j + " is " + mGain[i][j]);
465 }*/
466 dmotor += mErrors[i][j] * mGain[i][j];
467 }
468 /*if (i == 1)
469 Log.v(TAG, "guid, dmotor 1 is " + dmotor);
470 if (i == 2)
471 Log.v(TAG, "guid, dmotor 2 is " + dmotor);*/
472 double phi = 0;
473 switch (i) {
474 case 0: //X velocity
475 if (!mStabilizing) {
476 phi = Math.sin(rollrad);
477 phi = Math.abs(phi);
478 if (phi == 0)
479 dmotor = 2 * dmotor;
480 else
481 dmotor = dmotor / phi;
482 }
483 break;
484 case 1: //Y velocity
485 if (!mStabilizing) {
486 phi = Math.sin(pitchrad);
487 phi = Math.abs(phi);
488 if (phi == 0)
489 dmotor = 2 * dmotor;
490 else
491 dmotor = dmotor / phi;
492 Log.v(TAG, "guid, phi 1 is " + phi);
493 Log.v(TAG, "guid, dmotor 1 is " + dmotor);
494 }
495 break;
496 case 2: //Z velocity
497 phi = Math.cos(ascentrad);
498 phi = Math.abs(phi);
499 if (phi == 0)
500 dmotor = 0; //Don't bother with altitude control, gives more efficiency to torque[0, 1] for stabilization
501 else
502 dmotor = dmotor / phi;
503 //Log.v(TAG, "guid, phi 2 is " + phi);
504 break;
505 case 3: //Azimuth
506 break;
507 }
508 mTorques[i] = dmotor;
509 //Log.v(TAG, "Torque " + i + " " + dmotor);
510 }
511 mLastUpdate = thistime;
512
513 /*String targ = "";
514 for (int i = 0; i < 4; i++)
515 targ = targ + " " + mTorques[i];
516 Log.v(TAG, "guid, torques is " + targ);
517 */
518 if ((!mHorizontalDrift) || (mStabilizing)) { //if horizontal drift is on, motor speeds give full efficiency to altitude control
519 //but if the chopper is stabilizing, under no circumstances ignore torques 0, 1
520 //changes torques to motor values
521 mTempMotorSpeed[0] -= mTorques[1] / 2.0;
522 mTempMotorSpeed[1] += mTorques[1] / 2.0;
523
524 //Log.v(TAG, "Temp 1 " + mTempMotorSpeed[2] + "\nTemp 2 " + mTempMotorSpeed[3]);
525 mTempMotorSpeed[2] -= mTorques[0] / 2.0;
526 mTempMotorSpeed[3] += mTorques[0] / 2.0;
527
528
529
530 double spintorque = mTorques[3] / 4.0;
531 mTempMotorSpeed[0] += spintorque;
532 mTempMotorSpeed[1] += spintorque;
533 mTempMotorSpeed[2] -= spintorque;
534 mTempMotorSpeed[3] -= spintorque;
535 }
536
537 double dalttorque = mTorques[2] / 4.0;
347 mControlVars[i] += mErrors[i][j] * mGain[i][j];
348 }
349 }
350 if (mGuidanceMode.get() == MANUAL) {
351 synchronized (mAngleTarget) {
352 mControlVars[2] = mAngleTarget[2];
353 }
354 }
355 mLastUpdate = starttime;
356
357 // Constrain control vars:
358 mControlVars[0] = constrainValue(mControlVars[0], -1, 1);
359 mControlVars[1] = constrainValue(mControlVars[1], -1, 1);
360 mControlVars[2] = constrainValue(mControlVars[2], 0, 4);
361 mControlVars[3] = constrainValue(mControlVars[3], -2, 2);
362
363 /*String vars = "Control vars: ";
538364 for (int i = 0; i < 4; i++) {
539 mTempMotorSpeed[i] += dalttorque;
540 }
541
542 //Bounds Check--values must be between zero and one.
543 for (int i = 0; i < 4; i++) {
544 if (mTempMotorSpeed[i] < 0)
545 mTempMotorSpeed[i] = 0;
546 else if (mTempMotorSpeed[i] > 1)
547 mTempMotorSpeed[i] = 1;
548 double diff = mTempMotorSpeed[i] - mMotorSpeed[i];
549 //if (i==1)
550 //Log.v(TAG, "guid, diff is " + diff);
551 if (mStabilizing) {
552 if (diff > 0)
553 mMotorSpeed[i] += Math.min(diff, MAX_DSTABLE);
554 else if (diff < 0)
555 mMotorSpeed[i] += Math.max(diff, -MAX_DSTABLE);
556 }
557 else {
558 if (diff > 0) {
559 mMotorSpeed[i] += Math.min(diff, MAX_DMOTOR);
560 }
561 else if (diff < 0) {
562 mMotorSpeed[i] += Math.max(diff, -MAX_DMOTOR);
563 }
564 }
565 mTempMotorSpeed[i] = mMotorSpeed[i];
566 //if (i == 1)
567 // Log.v(TAG, "guid, ms1 is " + mMotorSpeed[i]);
568 }
365 vars += mControlVars[i] + ": ";
366 }
367 Log.v(TAG, vars);*/
368
369 controlVarsToMotorSpeeds();
569370
570371 //Send motor values to motors here:
571372 updateMotors();
573374 //Log.v(TAG, "motors: " + mMotorSpeed[0] + ", " + mMotorSpeed[1] + ", " + mMotorSpeed[2] + ", " + mMotorSpeed[3]);
574375 //Sleep a while
575376 long timetonext = (1000 / PIDREPS) - (System.currentTimeMillis() - starttime);
576 if (mReviseMotorSpeeds) {
377 Log.v(TAG, "time to next: " + timetonext);
378 int currentMode = mGuidanceMode.get();
379 if ((currentMode == MANUAL) || (currentMode == AUTOPILOT)) {
577380 if (timetonext > 0)
578381 mHandler.sendEmptyMessageDelayed(EVAL_MOTOR_SPEED, timetonext);
579382 else {
581384 mHandler.sendEmptyMessage(EVAL_MOTOR_SPEED);
582385 }
583386 }
387 }
388
389 private void controlVarsToMotorSpeeds() {
390 double pitchrad = mPitchDeg * Math.PI / 180.0;
391 double rollrad = mRollDeg * Math.PI / 180.0;
392 double gradient = Math.sqrt(
393 Math.pow(Math.tan(rollrad), 2) +
394 Math.pow(Math.tan(pitchrad), 2)
395 );
396 double ascentRad = Math.atan(gradient);
397
398 double cosGrad = Math.cos(ascentRad);
399
400 double x = mControlVars[0];
401 double y = mControlVars[1];
402 double z = mControlVars[2] / cosGrad;
403 double t = mControlVars[3];
404
405 mMotorSpeed[0] = Math.sqrt(constrainValue(t - 2*y + z, 0, 1));
406 mMotorSpeed[1] = Math.sqrt(constrainValue(t + 2*y + z, 0, 1));
407 mMotorSpeed[2] = Math.sqrt(constrainValue(-t - 2*x + z, 0, 1));
408 mMotorSpeed[3] = Math.sqrt(constrainValue(-t + 2*x + z, 0 ,1));
409 }
410
411 private void updateAngleTarget() {
412 if (mGuidanceMode.get() != AUTOPILOT) {
413 return;
414 }
415 long currentGpsTimeStamp = mStatus.getGpsTimeStamp();
416 if (mLastGpsTimestamp == currentGpsTimeStamp) {
417 return;
418 }
419 mLastGpsTimestamp = currentGpsTimeStamp;
420 synchronized (mAngleTarget) {
421 mAngler.getAngleTarget(mAngleTarget);
422 }
423 }
424
425 private static double constrainValue(double requested, double min, double max) {
426 if (requested > max) {
427 return max;
428 }
429 if (requested < min) {
430 return min;
431 }
432 return requested;
584433 }
585434
586435 /**
597446 }
598447 }
599448 catch (IOException e) {
600 Log.e(TAG, "Cannot write to logfile");
449 //Log.e(TAG, "Cannot write to logfile");
601450 }
602451 //Pass motor values to motor controller!
603452 Message msg = Message.obtain(mBt.mHandler, SEND_MOTOR_SPEEDS, mMotorSpeed);
22 import java.util.LinkedList;
33 import java.util.ListIterator;
44 import java.util.Vector;
5 import java.util.concurrent.atomic.AtomicBoolean;
65 import java.util.concurrent.atomic.AtomicInteger;
7 import java.util.concurrent.locks.ReentrantLock;
86
97 import org.haldean.chopper.nav.NavData;
108 import org.haldean.chopper.nav.NavList;
119
12 import android.os.Handler;
13 import android.os.Looper;
14 import android.os.Message;
1510 import android.util.Log;
1611
1712 /**
3833 *
3934 * @author Benjamin Bardin
4035 */
41 public class Navigation implements Runnable, Constants, Receivable {
36 public class Navigation implements Constants, Receivable {
4237
4338 /** Tag for logging */
4439 public static final String TAG = "chopper.Navigation";
5247 /** Velocity to achieve. Must be locked on each read/write. */
5348 private double[] mTarget = new double[4];
5449
55 /** Lock for target[] */
56 private ReentrantLock mTargetLock;
57
5850 /** Used to calculate next nav vector; prevents the thread
5951 * from holding a lock on mTarget itself for too long */
6052 private double[] mTempTarget = new double[4];
61
62 /** True if autopilot is engaged */
63 private final AtomicBoolean mAutoPilot = new AtomicBoolean(false);
6453
6554 /** Chopper's navigation status */
6655 private final AtomicInteger mNavStatus = new AtomicInteger(NAV_STATUSES - 1);
7665 /** Handle for other chopper components */
7766 private ChopperStatus mStatus;
7867
79 /** Handles messages */
80 private Handler mHandler;
81
8268 /** Registered receivers */
8369 private LinkedList<Receivable> mRec;
8470
10288 mTravelPlans.add(mLowPower);
10389 mTravelPlans.add(mFlightPath);
10490 mTravelPlans.add(mOnMyOwn);
105
106 mTargetLock = new ReentrantLock();
10791
10892 mStatus = status;
109 }
110
111 /**
112 * Changes autopilot status (on or off)
113 * @param onoff The new autopilot status
114 */
115 public void autoPilot(boolean onoff) {
116 mAutoPilot.set(onoff);
117 mHandler.removeMessages(EVAL_NAV);
118 if (mAutoPilot.get()) {
119 mHandler.sendEmptyMessage(EVAL_NAV);
120 Log.i(TAG, "Autopilot engaged");
121 }
122 else
123 Log.i(TAG, "Autopilot disengaged");
124 }
125
126 /** Evaluates a new navigation vector, based on current status and the relevant NavTask */
127 private void evalNextVector() {
93
94 //FOR TESTING ONLY:
95 /*String taskList = "{ " +
96 "{ DEST!targ1!300!-74.012345!40.74!10!100!232 { DEST!targ2!300!-77.07950!38.97300!100!250!233 " +
97 " DEST!targ3!587!-117.15!32.72!10!600!234 } } }";*/
98 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}";
99 setTask(BASIC_AUTO, taskList);
100 setTask(NO_CONN, "{ VEL!No_Conn!0!0!-1!0!" + THREE_HRS_IN_MS + "!-1!-5 -6}");
101 setTask(LOW_POWER, "{ VEL!Low_Power!0!0!-1!0!"+ THREE_HRS_IN_MS + "!-1!-7 -8}");
102 //mNavStatus.set(BASIC_AUTO);
103
104
105 //autoPilot(true);
106 }
107
108
109
110 /**
111 * Evaluates a new navigation vector, based on current status and the relevant NavTask.
112 * @param newNavTarget If supplied and has length >= 4, writes the new target here. May be null.
113 */
114 public void evalNextVector(double[] newNavTarget) {
128115
129116 /*Determine what the current task should be. Copies to a local variable in case
130117 * 'status' changes during execution of the method */
139126 }
140127 mTask.getVelocity(myList, mTempTarget);
141128 setTarget(mTempTarget);
142
143
144
145 long interval = mTask.getInterval(myList);
146 Log.v(TAG, "Nav Interval is " + interval);
129 if (newNavTarget != null) {
130 getTarget(newNavTarget);
131 }
132
133 //long interval = mTask.getInterval(myList);
134 //Log.v(TAG, "Nav Interval is " + interval);
147135 //Send the current NavList to the server, in case any tasks have been completed
148136 updateReceivers("NAV:AUTOTASK:" + thisStatus + ":" + myList.toString());
149
150 mHandler.removeMessages(EVAL_NAV);
151 if (mAutoPilot.get()) {
152 if (interval > 0) {
153 mHandler.sendEmptyMessageDelayed(EVAL_NAV, interval);
154 }
155 else {
156 mHandler.sendEmptyMessage(EVAL_NAV);
157 }
158 }
159 }
160
161 /**
162 * Starts the navigation thread
163 */
164 public void run() {
165 Looper.prepare();
166 Thread.currentThread().setName("Navigation");
167 mHandler = new Handler() {
168 public void handleMessage(Message msg)
169 {
170 switch (msg.what) {
171 case EVAL_NAV:
172 if (mAutoPilot.get())
173 evalNextVector();
174 break;
175 }
176 }
177 };
178
179 //FOR TESTING ONLY:
180 /*String taskList = "{ " +
181 "{ DEST!targ1!300!-74.012345!40.74!10!100!232 { DEST!targ2!300!-77.07950!38.97300!100!250!233 " +
182 " DEST!targ3!587!-117.15!32.72!10!600!234 } } }";*/
183 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}";
184 setTask(BASIC_AUTO, taskList);
185 setTask(NO_CONN, "{ VEL!No_Conn!0!0!-1!0!" + THREE_HRS_IN_MS + "!-1!-5 -6}");
186 setTask(LOW_POWER, "{ VEL!Low_Power!0!0!-1!0!"+ THREE_HRS_IN_MS + "!-1!-7 -8}");
187 //mNavStatus.set(BASIC_AUTO);
188
189
190 //autoPilot(true);
191
192 Looper.loop();
193 }
194
195 /**
196 * Obtains current navigation target vector. If the data is
197 * locked, immediately throws an exception.
198 *
199 * @return A new array containing the navigation target vector.
200 * @throws IllegalAccessException If the data is currently locked.
201 */
202 public double[] getTarget() throws IllegalAccessException {
203 double[] myTarget = new double[4];
204 if (mTargetLock.tryLock()) {
205 for (int i = 0; i < 4; i++) {
206 myTarget[i] = mTarget[i];
207 }
208 mTargetLock.unlock();
209 }
210 else {
211 Log.w(TAG, "Target vector is locked.");
212 throw new IllegalAccessException();
213 }
214 return mTarget;
215137 }
216138
217139 /**
222144 * @param expectedValues The array in which to write the
223145 * vector--must be at least of length 4.
224146 */
225 public void getTarget(double[] expectedValues) {
226 if (expectedValues.length < 4)
147 public void getTarget(double[] navTarget) {
148 if (navTarget.length < 4)
227149 return;
228 if (mTargetLock.tryLock()) {
229 for (int i = 0; i < 4; i++) {
230 expectedValues[i] = mTarget[i];
231 }
232 mTargetLock.unlock();
233 }
234 else {
235 Log.w(TAG, "Target vector is locked.");
150 synchronized (mTarget) {
151 System.arraycopy(mTarget, 0, navTarget, 0, 4);
236152 }
237153 }
238154
255171 for (int i = 0; i < 3; i++) {
256172 mTempTarget[i] = 0;
257173 }
258 mTempTarget[3] = mStatus.getReadingFieldNow(AZIMUTH, mTempTarget[3]);
174 mTempTarget[3] = mStatus.getReadingField(AZIMUTH);
259175 setTarget(mTempTarget);
260 mHandler.removeMessages(EVAL_NAV);
261 mHandler.sendEmptyMessageDelayed(EVAL_NAV, HOVER_PAUSE);
262176 }
263177
264178 /**
266180 * @param source The source of the message, if a reply is needed. May be null.
267181 */
268182 public void receiveMessage(String msg, Receivable source) {
269 Log.d(TAG, "Receiving " + msg);
270183 String[] parts = msg.split(":");
271 if (parts[0].equals("GUID")) {
272 if (parts[1].equals("VECTOR")) {
273 autoPilot(false);
274 }
275 }
184
276185 if (parts[0].equals("NAV")) {
277186 if (parts[1].equals("SET")) {
278187 Log.v(TAG, "Updating Nav Status");
279 if (parts[2].equals("MANUAL")) {
280 autoPilot(false);
281 if (parts.length > 3) {
282 updateReceivers("GUID:LOCALVEC");
283 updateReceivers("GUID:AUTOMATIC");
284 double[] newTarget = new double[4];
285 for (int i = 0; i < 4; i++) {
286 newTarget[i] = new Double(parts[i + 3]);
287 }
288 setTarget(newTarget);
289 }
290 }
188
291189 if (parts[2].equals("AUTOPILOT")) {
292190 mNavStatus.set(BASIC_AUTO);
293191 updateReceivers("GUID:ABSVEC");
294192 updateReceivers("GUID:AUTOMATIC");
295 autoPilot(true);
193 //autoPilot(true);
296194 }
297195 if (parts[2].equals("AUTOTASK")) {
298196 Integer taskList = new Integer(parts[3]);
316214 }
317215 if (parts[0].equals("CSYS")) {
318216 if (parts[1].equals("NOCONN")) {
319 Log.d(TAG, "no conn in Nav");
320217 updateStatus(NO_CONN);
321 autoPilot(true);
322 updateReceivers("GUID:AUTOMATIC");
218 //autoPilot(true);
323219 }
324220 if (parts[1].equals("LOWPOWER")) {
325221 updateStatus(LOW_POWER);
326 autoPilot(true);
327 updateReceivers("GUID:AUTOMATIC");
222 //autoPilot(true);
328223 }
329224 }
330225 }
343238 * Sets the navigation target vector to the values contained by the supplied array (length must be at least 4).
344239 * @param newTarget The new target vector.
345240 */
346 protected void setTarget(double[] newTarget) {
241 private void setTarget(double[] newTarget) {
347242 if (newTarget.length < 4) {
348243 return;
349244 }
350 final double[] myCopy = newTarget.clone();
351 new Thread() {
352 public void run() {
245 synchronized (mTarget) {
353246 String newNav = "New_Nav_Vector: ";
354 mTargetLock.lock();
355 for (int i = 0; i < 4; i++) {
356 mTarget[i] = myCopy[i];
357 newNav += myCopy[i] + ":";
358 }
359 mTargetLock.unlock();
360 Log.i(TAG, newNav);
247 for (int i = 0; i < 4; i++) {
248 mTarget[i] = newTarget[i];
249 newNav += newTarget[i] + ":";
250 }
361251 updateReceivers(newNav);
362 }
363 }.start();
252 }
364253 }
365254
366255 /**
383272 Log.i(TAG, "Nav set index " + whichPlan + " to task " + myList);
384273 //Confirm change to server:
385274 updateReceivers("NAV:AUTOTASK:" + whichPlan + ":" + myList.toString());
386 if (mAutoPilot.get())
387 evalNextVector();
388275 }
389276 else {
390277 Log.e(TAG, "Nav received invalid task!");
55 import android.os.Handler;
66 import android.os.Looper;
77 import android.os.Message;
8 import android.util.Log;
98
109 /**
1110 * Sends regular chopper status reports to all registered receivers. <P>
6059 LinkedList<String> infoList = new LinkedList<String>();
6160
6261 /* Lock data, send it, unlock. If the lock is unavailable (unlikely), skip this datapiece for this iteration */
63 try {
64 double myAzimuth = mStatus.getReadingFieldNow(AZIMUTH);
65 double myPitch = mStatus.getReadingFieldNow(PITCH);
66 double myRoll = mStatus.getReadingFieldNow(ROLL);
67 infoList.add("ORIENT:" + myAzimuth + ":" + myPitch + ":" + myRoll);
68 }
69 catch (IllegalAccessException e) {
70 Log.w(TAG, "Orientation Report Unavailable");
71 }
72
73 try {
74 double myXaccel = mStatus.getReadingFieldNow(X_ACCEL);
75 double myYaccel = mStatus.getReadingFieldNow(Y_ACCEL);
76 double myZaccel = mStatus.getReadingFieldNow(Z_ACCEL);
77 infoList.add("ACCEL:" + myXaccel + ":" + myYaccel + ":" + myZaccel);
78 }
79 catch (IllegalAccessException e) {
80 Log.w(TAG, "Acceleration Report Unavailable");
81 }
82
62 double myAzimuth = mStatus.getReadingField(AZIMUTH);
63 double myPitch = mStatus.getReadingField(PITCH);
64 double myRoll = mStatus.getReadingField(ROLL);
65 infoList.add("ORIENT:" + myAzimuth + ":" + myPitch + ":" + myRoll);
66
67
68 double myXaccel = mStatus.getReadingField(X_ACCEL);
69 double myYaccel = mStatus.getReadingField(Y_ACCEL);
70 double myZaccel = mStatus.getReadingField(Z_ACCEL);
71 infoList.add("ACCEL:" + myXaccel + ":" + myYaccel + ":" + myZaccel);
72
8373 /*try {
8474 double myXflux = mStatus.getReadingFieldNow(X_FLUX);
8575 double myYflux = mStatus.getReadingFieldNow(Y_FLUX);
9080 Log.w(TAG, "Flux Report Unavailable");
9181 }*/
9282
93 try {
94 double[] mySpeeds = mStatus.getMotorFieldsNow();
95 infoList.add("MOTORSPEED:" + mySpeeds[0] +
96 ":" + mySpeeds[1] +
97 ":" + mySpeeds[2] +
98 ":" + mySpeeds[3]);
99 }
100 catch (IllegalAccessException e) {
101 Log.w(TAG, "MotorSpeeds Report Unavailable");
102 }
83 double[] mySpeeds = new double[4];
84 mStatus.getMotorFields(mySpeeds);
85 infoList.add("MOTORSPEED:" + mySpeeds[0] +
86 ":" + mySpeeds[1] +
87 ":" + mySpeeds[2] +
88 ":" + mySpeeds[3]);
89
10390 /*
10491 try {
10592 double[] myPowers = mStatus.getMotorFieldsNow();
135122 Log.w(TAG, "Pressure Report Unavailable");
136123 }
137124 */
138 try {
139 double myTemp = mStatus.getReadingFieldNow(TEMPERATURE);
140 infoList.add("TEMPERATURE:" + myTemp);
141 }
142 catch (IllegalAccessException e) {
143 Log.w(TAG, "Temperature Report Unavailable");
144 }
145
125 infoList.add("TEMPERATURE:" + mStatus.getReadingField(TEMPERATURE));
146126 infoList.add("BATTERY:" + mStatus.getBatteryLevel());
147127
148128 /* Send GPS data */
149129 String gpsData = new String("GPS");
150 try {
151 for (int i = 0; i < GPS_FIELDS; i++) {
152 double myValue = mStatus.getGpsFieldNow(i);
153 gpsData += ":" + myValue;
154 }
155
156 gpsData += ":" + mStatus.getGpsExtrasNow();
157 infoList.add(gpsData);
158 }
159 catch (IllegalAccessException e) {
160 Log.w(TAG, "GPS Report Unavailable");
161 }
130
131 for (int i = 0; i < GPS_FIELDS; i++) {
132 double myValue = mStatus.getGpsField(i);
133 gpsData += ":" + myValue;
134 }
135 gpsData += ":" + mStatus.getGpsExtras();
136 infoList.add(gpsData);
162137
163138 return infoList;
164139 }
1515 * @author William Brown
1616 */
1717 public class EnsignCrusher {
18 private static String navGoToManual = "NAV:SET:MANUAL";
19 private static String navGoToAutomatic = "NAV:SET:AUTOPILOT";
18 private static String navGoToDirect = "GUID:DIRECT"; // Direct control of motor speeds.
19 private static String navGoToManual = "GUID:MANUAL"; // Manual control of chopper target angles.
20 private static String navGoToAutomatic = "GUID:AUTO"; // Full automatic navigation.
2021 private static double bearing = 0;
2122
2223 /* In meters per second (we hope.) */
9899 * the motor closest to North.
99100 */
100101 public static void setMotorSpeeds(double[] speeds) {
101 String taskString = "GUID:VECTOR";
102 String taskString = navGoToDirect;
102103 for (int i=0; i<4; i++) {
103104 taskString += ":" + speeds[i];
104105 }
8989 buttons[BUTTON_X] = c;
9090 else if (id == Component.Identifier.Button.Y)
9191 buttons[BUTTON_Y] = c;
92 else if (id == Component.Identifier.Button.BACK)
92 else if ((id == Component.Identifier.Button.BACK) || (id == Component.Identifier.Button.SELECT))
9393 buttons[BUTTON_BACK] = c;
9494 else if (id == Component.Identifier.Button.LEFT_THUMB)
9595 buttons[BUTTON_L] = c;
119119 axes[AXIS_R_TRIGGER] = c;
120120 else if (id == Component.Identifier.Axis.POV)
121121 axes[D_PAD] = c;
122 }
122 }
123123 }
124124 }
125125