Guidance logging
Benjamin Bardin
11 years ago
39 | 39 | <uses-permission android:name="android.permission.PERSISTENT_ACTIVITY"></uses-permission> |
40 | 40 | <uses-permission android:name="android.permission.WAKE_LOCK"></uses-permission> |
41 | 41 | <uses-permission android:name="android.permission.DEVICE_POWER"></uses-permission> |
42 | <uses-permission android:name="android.permission.WRITE_EXTERNAL_STORAGE"></uses-permission> | |
42 | 43 | |
43 | 44 | <uses-feature android:name="android.hardware.camera" /> |
44 | 45 | <uses-feature android:name="android.hardware.camera.autofocus" /> |
17 | 17 | private boolean telemetry = true; |
18 | 18 | |
19 | 19 | private static boolean mFirstRun = true; |
20 | private Guidance guid; | |
20 | 21 | |
21 | 22 | /** |
22 | 23 | * Holds the wakelock; needed to keep the camera preview rendering on |
77 | 78 | } |
78 | 79 | |
79 | 80 | Navigation nav = new Navigation(status); |
80 | Guidance guid = new Guidance(status, nav); | |
81 | guid = new Guidance(status, nav); | |
81 | 82 | |
82 | 83 | if (telemetry) { |
83 | 84 | comm.setTelemetrySource(pic); |
116 | 117 | |
117 | 118 | mFirstRun = false; |
118 | 119 | } |
119 | /* | |
120 | ||
120 | 121 | /** |
121 | 122 | * Releases the wakelock, destroys activity. |
122 | ||
123 | */ | |
123 | 124 | protected void onDestroy() { |
124 | 125 | //mWakeLock.release(); |
125 | 126 | super.onDestroy(); |
126 | } */ | |
127 | guid.onDestroy(); | |
128 | } | |
127 | 129 | } |
3 | 3 | import java.util.ListIterator; |
4 | 4 | import java.util.concurrent.ExecutorService; |
5 | 5 | import java.util.concurrent.Executors; |
6 | import java.util.concurrent.TimeUnit; | |
6 | 7 | import java.util.concurrent.atomic.AtomicInteger; |
7 | 8 | import java.util.concurrent.locks.ReentrantLock; |
8 | 9 | |
154 | 155 | */ |
155 | 156 | public String getGpsExtrasNow() throws IllegalAccessException { |
156 | 157 | String gpsData = ""; |
157 | if (mGpsExtrasLock.tryLock()) { | |
158 | gpsData += mGpsAccuracy + | |
159 | ":" + mGpsNumSats + | |
160 | ":" + mGpsTimeStamp; | |
161 | mGpsExtrasLock.unlock(); | |
162 | } | |
163 | else { | |
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) { | |
164 | 170 | throw new IllegalAccessException(); |
165 | 171 | } |
166 | 172 | return gpsData; |
187 | 193 | */ |
188 | 194 | public double getGpsFieldNow(int whichField) throws IllegalAccessException { |
189 | 195 | double myValue; |
190 | if (mGpsLock[whichField].tryLock()) { | |
191 | myValue = mGps[whichField]; | |
192 | mGpsLock[whichField].unlock(); | |
193 | } | |
194 | else { | |
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) { | |
195 | 206 | Log.w(TAG, "GPS Field " + whichField + " is locked."); |
196 | 207 | throw new IllegalAccessException(); |
197 | 208 | } |
206 | 217 | */ |
207 | 218 | public double getGpsFieldNow(int whichField, double expectedValue) { |
208 | 219 | double myValue; |
209 | if (mGpsLock[whichField].tryLock()) { | |
210 | myValue = mGps[whichField]; | |
211 | mGpsLock[whichField].unlock(); | |
212 | } | |
213 | else { | |
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) { | |
214 | 230 | myValue = expectedValue; |
215 | 231 | } |
216 | 232 | return myValue; |
237 | 253 | */ |
238 | 254 | public double[] getMotorFieldsNow() throws IllegalAccessException { |
239 | 255 | double[] myValues = new double[4]; |
240 | if (mMotorLock.tryLock()) { | |
241 | for (int i = 0; i < 4; i++) { | |
242 | myValues[i] = mMotorSpeed[i]; | |
243 | } | |
244 | mMotorLock.unlock(); | |
245 | } | |
246 | else { | |
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) { | |
247 | 268 | Log.w(TAG, "motorspeed is locked."); |
248 | 269 | throw new IllegalAccessException(); |
249 | 270 | } |
256 | 277 | */ |
257 | 278 | public double[] getMotorPowerFieldsNow() throws IllegalAccessException { |
258 | 279 | double[] myValues = new double[4]; |
259 | if (mMotorPowerLock.tryLock()) { | |
260 | for (int i = 0; i < 4; i++) { | |
261 | myValues[i] = mMotorPower[i]; | |
262 | } | |
263 | mMotorPowerLock.unlock(); | |
264 | } | |
265 | else { | |
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) { | |
266 | 292 | Log.w(TAG, "motorpowers is locked."); |
267 | 293 | throw new IllegalAccessException(); |
268 | 294 | } |
298 | 324 | SensorManager sensors = (SensorManager) mContext.getSystemService(Context.SENSOR_SERVICE); |
299 | 325 | |
300 | 326 | /* Registers this class as a sensor listener for every necessary sensor. */ |
301 | sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_ACCELEROMETER), SensorManager.SENSOR_DELAY_FASTEST); | |
302 | sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_LIGHT), SensorManager.SENSOR_DELAY_NORMAL); | |
303 | sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD), SensorManager.SENSOR_DELAY_NORMAL); | |
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); | |
304 | 330 | sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_ORIENTATION), SensorManager.SENSOR_DELAY_FASTEST); |
305 | sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_PRESSURE), SensorManager.SENSOR_DELAY_NORMAL); | |
306 | sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_PROXIMITY), SensorManager.SENSOR_DELAY_NORMAL); | |
331 | //sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_PRESSURE), SensorManager.SENSOR_DELAY_NORMAL); | |
332 | //sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_PROXIMITY), SensorManager.SENSOR_DELAY_NORMAL); | |
307 | 333 | sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_TEMPERATURE), SensorManager.SENSOR_DELAY_NORMAL); |
308 | 334 | |
309 | 335 | /* Initialize GPS reading: */ |
331 | 357 | */ |
332 | 358 | public double getReadingFieldNow(int whichField) throws IllegalAccessException { |
333 | 359 | double myValue; |
334 | if (mReadingLock[whichField].tryLock()) { | |
335 | myValue = mReading[whichField]; | |
336 | mReadingLock[whichField].unlock(); | |
337 | } | |
338 | else { | |
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) { | |
339 | 370 | Log.w(TAG, "Reading field " + whichField + " is locked."); |
340 | 371 | throw new IllegalAccessException(); |
341 | 372 | } |
350 | 381 | */ |
351 | 382 | public double getReadingFieldNow(int whichField, double expectedValue) { |
352 | 383 | double myValue; |
353 | if (mReadingLock[whichField].tryLock()) { | |
354 | myValue = mReading[whichField]; | |
355 | mReadingLock[whichField].unlock(); | |
356 | } | |
357 | else { | |
358 | Log.w(TAG, "Reading field " + whichField + " is locked."); | |
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."); | |
359 | 395 | myValue = expectedValue; |
360 | 396 | } |
361 | 397 | return myValue; |
0 | 0 | package org.haldean.chopper.pilot; |
1 | 1 | |
2 | import java.io.FileWriter; | |
3 | import java.io.IOException; | |
2 | 4 | import java.util.LinkedList; |
3 | 5 | import java.util.ListIterator; |
4 | 6 | |
34 | 36 | public class Guidance implements Runnable, Constants, Receivable { |
35 | 37 | |
36 | 38 | /** How many times per second the PID loop will run */ |
37 | public int PIDREPS = 20; | |
39 | public int PIDREPS = 10; | |
38 | 40 | |
39 | 41 | /** Maximum permissible target velocity, in m/s; larger vectors will be resized */ |
40 | 42 | public static final double MAX_VEL = 2.0; |
41 | 43 | |
42 | 44 | /** The maximum angle, in degrees, guidance will permit the chopper to have */ |
43 | public static final double MAX_ANGLE = 5; | |
45 | public static final double MAX_ANGLE = 10; | |
44 | 46 | |
45 | 47 | /** The maximum change in motor speed permitted at one time. Must be positive. */ |
46 | 48 | public static final double MAX_DMOTOR = .05; |
67 | 69 | private double mGpsBearing; |
68 | 70 | private double mGpsSpeed; |
69 | 71 | private double mGpsDalt; |
72 | ||
73 | public static final String logname = "/sdcard/chopper/guidlog.txt"; | |
74 | private FileWriter logfile; | |
70 | 75 | |
71 | 76 | /** Note that some of the following objects are declared outside their smallest scope. |
72 | 77 | * This is to relieve unnecessary stress on the GC. Many of these data holders |
119 | 124 | /** Controls whether N/S and E/W commands refer to absolute vectors or local **/ |
120 | 125 | private boolean mAbsVec = true; |
121 | 126 | |
127 | public final static boolean mEnableLogging = true; | |
122 | 128 | /** |
123 | 129 | * Constructs a Guidance object |
124 | 130 | * @param status The source status information. |
135 | 141 | //Temporary: need real tuning values at some point. Crap. |
136 | 142 | for (int i = 0; i < 3; i++) |
137 | 143 | for (int j = 0; j < 3; j++) |
138 | mGain[i][j] = .00005; | |
144 | mGain[i][j] = .0001; | |
145 | //mGain[i][j] = .05; | |
139 | 146 | for (int j = 0; j < 3; j++) { |
140 | 147 | // mGain[3][j] = .0005; |
141 | 148 | mGain[3][j] = 0; |
149 | } | |
150 | try { | |
151 | if (mEnableLogging) | |
152 | logfile = new FileWriter(logname, false); | |
153 | } | |
154 | catch (IOException e) { | |
155 | e.printStackTrace(); | |
156 | Log.e(TAG, "Cannot open log file."); | |
142 | 157 | } |
143 | 158 | } |
144 | 159 | |
147 | 162 | + ":" + mErrors[1][0] |
148 | 163 | + ":" + mErrors[2][0] |
149 | 164 | + ":" + mErrors[3][0]; |
165 | } | |
166 | ||
167 | public void onDestroy() { | |
168 | try { | |
169 | if (logfile != null) | |
170 | logfile.close(); | |
171 | } | |
172 | catch (IOException e) { | |
173 | Log.e(TAG, "Cannot close logfile."); | |
174 | } | |
150 | 175 | } |
151 | 176 | |
152 | 177 | /** |
449 | 474 | mTempMotorSpeed[i] += dalttorque; |
450 | 475 | } |
451 | 476 | |
452 | //Sanity Check--values must be between zero and one. | |
477 | //Bounds Check--values must be between zero and one. | |
453 | 478 | for (int i = 0; i < 4; i++) { |
454 | 479 | if (mTempMotorSpeed[i] < 0) |
455 | 480 | mTempMotorSpeed[i] = 0; |
480 | 505 | if (mReviseMotorSpeeds) { |
481 | 506 | if (timetonext > 0) |
482 | 507 | mHandler.sendEmptyMessageDelayed(EVAL_MOTOR_SPEED, timetonext); |
483 | else | |
508 | else { | |
509 | Log.e(TAG, "Guidance too slow"); | |
484 | 510 | mHandler.sendEmptyMessage(EVAL_MOTOR_SPEED); |
511 | } | |
485 | 512 | } |
486 | 513 | } |
487 | 514 | |
489 | 516 | private void updateMotors() { |
490 | 517 | //Pass filtered values to ChopperStatus. |
491 | 518 | mStatus.setMotorFields(mMotorSpeed); |
492 | ||
519 | String logline = Long.toString(System.currentTimeMillis()) + " " + mMotorSpeed[0] + " " + mMotorSpeed[1] + " " + mMotorSpeed[2] + " " + mMotorSpeed[3] + "\n"; | |
520 | try { | |
521 | if (logfile != null) { | |
522 | logfile.write(logline); | |
523 | logfile.flush(); | |
524 | } | |
525 | } | |
526 | catch (IOException e) { | |
527 | Log.e(TAG, "Cannot write to logfile"); | |
528 | } | |
493 | 529 | //Pass motor values to motor controller! |
494 | 530 | BluetoothOutput.setMotorSpeeds(mMotorSpeed[0], mMotorSpeed[1], mMotorSpeed[2], mMotorSpeed[3]); |
495 | 531 | } |
144 | 144 | updateReceivers("NAV:AUTOTASK:" + thisStatus + ":" + myList.toString()); |
145 | 145 | |
146 | 146 | mHandler.removeMessages(EVAL_NAV); |
147 | if (interval > 0) { | |
148 | mHandler.sendEmptyMessageDelayed(EVAL_NAV, interval); | |
149 | } | |
150 | else { | |
151 | mHandler.sendEmptyMessage(EVAL_NAV); | |
147 | if (mAutoPilot.get()) { | |
148 | if (interval > 0) { | |
149 | mHandler.sendEmptyMessageDelayed(EVAL_NAV, interval); | |
150 | } | |
151 | else { | |
152 | mHandler.sendEmptyMessage(EVAL_NAV); | |
153 | } | |
152 | 154 | } |
153 | 155 | } |
154 | 156 |
80 | 80 | Log.w(TAG, "Acceleration Report Unavailable"); |
81 | 81 | } |
82 | 82 | |
83 | try { | |
83 | /*try { | |
84 | 84 | double myXflux = mStatus.getReadingFieldNow(X_FLUX); |
85 | 85 | double myYflux = mStatus.getReadingFieldNow(Y_FLUX); |
86 | 86 | double myZflux = mStatus.getReadingFieldNow(Z_FLUX); |
88 | 88 | } |
89 | 89 | catch (IllegalAccessException e) { |
90 | 90 | Log.w(TAG, "Flux Report Unavailable"); |
91 | } | |
91 | }*/ | |
92 | 92 | |
93 | 93 | try { |
94 | 94 | double[] mySpeeds = mStatus.getMotorFieldsNow(); |
100 | 100 | catch (IllegalAccessException e) { |
101 | 101 | Log.w(TAG, "MotorSpeeds Report Unavailable"); |
102 | 102 | } |
103 | ||
103 | /* | |
104 | 104 | try { |
105 | 105 | double[] myPowers = mStatus.getMotorFieldsNow(); |
106 | 106 | infoList.add("MOTORPOWER:" + myPowers[0] + |
134 | 134 | catch (IllegalAccessException e) { |
135 | 135 | Log.w(TAG, "Pressure Report Unavailable"); |
136 | 136 | } |
137 | ||
137 | */ | |
138 | 138 | try { |
139 | 139 | double myTemp = mStatus.getReadingFieldNow(TEMPERATURE); |
140 | 140 | infoList.add("TEMPERATURE:" + myTemp); |