git.haldean.org droidcopter / d27eec3
Guidance logging Benjamin Bardin 10 years ago
6 changed file(s) with 143 addition(s) and 66 deletion(s). Raw diff Collapse all Expand all
3939 <uses-permission android:name="android.permission.PERSISTENT_ACTIVITY"></uses-permission>
4040 <uses-permission android:name="android.permission.WAKE_LOCK"></uses-permission>
4141 <uses-permission android:name="android.permission.DEVICE_POWER"></uses-permission>
42 <uses-permission android:name="android.permission.WRITE_EXTERNAL_STORAGE"></uses-permission>
4243
4344 <uses-feature android:name="android.hardware.camera" />
4445 <uses-feature android:name="android.hardware.camera.autofocus" />
1717 private boolean telemetry = true;
1818
1919 private static boolean mFirstRun = true;
20 private Guidance guid;
2021
2122 /**
2223 * Holds the wakelock; needed to keep the camera preview rendering on
7778 }
7879
7980 Navigation nav = new Navigation(status);
80 Guidance guid = new Guidance(status, nav);
81 guid = new Guidance(status, nav);
8182
8283 if (telemetry) {
8384 comm.setTelemetrySource(pic);
116117
117118 mFirstRun = false;
118119 }
119 /*
120
120121 /**
121122 * Releases the wakelock, destroys activity.
122
123 */
123124 protected void onDestroy() {
124125 //mWakeLock.release();
125126 super.onDestroy();
126 } */
127 guid.onDestroy();
128 }
127129 }
33 import java.util.ListIterator;
44 import java.util.concurrent.ExecutorService;
55 import java.util.concurrent.Executors;
6 import java.util.concurrent.TimeUnit;
67 import java.util.concurrent.atomic.AtomicInteger;
78 import java.util.concurrent.locks.ReentrantLock;
89
154155 */
155156 public String getGpsExtrasNow() throws IllegalAccessException {
156157 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) {
164170 throw new IllegalAccessException();
165171 }
166172 return gpsData;
187193 */
188194 public double getGpsFieldNow(int whichField) throws IllegalAccessException {
189195 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) {
195206 Log.w(TAG, "GPS Field " + whichField + " is locked.");
196207 throw new IllegalAccessException();
197208 }
206217 */
207218 public double getGpsFieldNow(int whichField, double expectedValue) {
208219 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) {
214230 myValue = expectedValue;
215231 }
216232 return myValue;
237253 */
238254 public double[] getMotorFieldsNow() throws IllegalAccessException {
239255 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) {
247268 Log.w(TAG, "motorspeed is locked.");
248269 throw new IllegalAccessException();
249270 }
256277 */
257278 public double[] getMotorPowerFieldsNow() throws IllegalAccessException {
258279 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) {
266292 Log.w(TAG, "motorpowers is locked.");
267293 throw new IllegalAccessException();
268294 }
298324 SensorManager sensors = (SensorManager) mContext.getSystemService(Context.SENSOR_SERVICE);
299325
300326 /* 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);
304330 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);
307333 sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_TEMPERATURE), SensorManager.SENSOR_DELAY_NORMAL);
308334
309335 /* Initialize GPS reading: */
331357 */
332358 public double getReadingFieldNow(int whichField) throws IllegalAccessException {
333359 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) {
339370 Log.w(TAG, "Reading field " + whichField + " is locked.");
340371 throw new IllegalAccessException();
341372 }
350381 */
351382 public double getReadingFieldNow(int whichField, double expectedValue) {
352383 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.");
359395 myValue = expectedValue;
360396 }
361397 return myValue;
00 package org.haldean.chopper.pilot;
11
2 import java.io.FileWriter;
3 import java.io.IOException;
24 import java.util.LinkedList;
35 import java.util.ListIterator;
46
3436 public class Guidance implements Runnable, Constants, Receivable {
3537
3638 /** How many times per second the PID loop will run */
37 public int PIDREPS = 20;
39 public int PIDREPS = 10;
3840
3941 /** Maximum permissible target velocity, in m/s; larger vectors will be resized */
4042 public static final double MAX_VEL = 2.0;
4143
4244 /** 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;
4446
4547 /** The maximum change in motor speed permitted at one time. Must be positive. */
4648 public static final double MAX_DMOTOR = .05;
6769 private double mGpsBearing;
6870 private double mGpsSpeed;
6971 private double mGpsDalt;
72
73 public static final String logname = "/sdcard/chopper/guidlog.txt";
74 private FileWriter logfile;
7075
7176 /** Note that some of the following objects are declared outside their smallest scope.
7277 * This is to relieve unnecessary stress on the GC. Many of these data holders
119124 /** Controls whether N/S and E/W commands refer to absolute vectors or local **/
120125 private boolean mAbsVec = true;
121126
127 public final static boolean mEnableLogging = true;
122128 /**
123129 * Constructs a Guidance object
124130 * @param status The source status information.
135141 //Temporary: need real tuning values at some point. Crap.
136142 for (int i = 0; i < 3; i++)
137143 for (int j = 0; j < 3; j++)
138 mGain[i][j] = .00005;
144 mGain[i][j] = .0001;
145 //mGain[i][j] = .05;
139146 for (int j = 0; j < 3; j++) {
140147 // mGain[3][j] = .0005;
141148 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.");
142157 }
143158 }
144159
147162 + ":" + mErrors[1][0]
148163 + ":" + mErrors[2][0]
149164 + ":" + 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 }
150175 }
151176
152177 /**
449474 mTempMotorSpeed[i] += dalttorque;
450475 }
451476
452 //Sanity Check--values must be between zero and one.
477 //Bounds Check--values must be between zero and one.
453478 for (int i = 0; i < 4; i++) {
454479 if (mTempMotorSpeed[i] < 0)
455480 mTempMotorSpeed[i] = 0;
480505 if (mReviseMotorSpeeds) {
481506 if (timetonext > 0)
482507 mHandler.sendEmptyMessageDelayed(EVAL_MOTOR_SPEED, timetonext);
483 else
508 else {
509 Log.e(TAG, "Guidance too slow");
484510 mHandler.sendEmptyMessage(EVAL_MOTOR_SPEED);
511 }
485512 }
486513 }
487514
489516 private void updateMotors() {
490517 //Pass filtered values to ChopperStatus.
491518 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 }
493529 //Pass motor values to motor controller!
494530 BluetoothOutput.setMotorSpeeds(mMotorSpeed[0], mMotorSpeed[1], mMotorSpeed[2], mMotorSpeed[3]);
495531 }
144144 updateReceivers("NAV:AUTOTASK:" + thisStatus + ":" + myList.toString());
145145
146146 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 }
152154 }
153155 }
154156
8080 Log.w(TAG, "Acceleration Report Unavailable");
8181 }
8282
83 try {
83 /*try {
8484 double myXflux = mStatus.getReadingFieldNow(X_FLUX);
8585 double myYflux = mStatus.getReadingFieldNow(Y_FLUX);
8686 double myZflux = mStatus.getReadingFieldNow(Z_FLUX);
8888 }
8989 catch (IllegalAccessException e) {
9090 Log.w(TAG, "Flux Report Unavailable");
91 }
91 }*/
9292
9393 try {
9494 double[] mySpeeds = mStatus.getMotorFieldsNow();
100100 catch (IllegalAccessException e) {
101101 Log.w(TAG, "MotorSpeeds Report Unavailable");
102102 }
103
103 /*
104104 try {
105105 double[] myPowers = mStatus.getMotorFieldsNow();
106106 infoList.add("MOTORPOWER:" + myPowers[0] +
134134 catch (IllegalAccessException e) {
135135 Log.w(TAG, "Pressure Report Unavailable");
136136 }
137
137 */
138138 try {
139139 double myTemp = mStatus.getReadingFieldNow(TEMPERATURE);
140140 infoList.add("TEMPERATURE:" + myTemp);