New PID loop/guidance structure (EnsignCrusher updated). Fixed bug in PadController where the back button would not be recognized.
Benjamin Bardin
10 years ago
85 | 85 | <uses-feature android:name="android.hardware.camera.autofocus" /> |
86 | 86 | |
87 | 87 | <uses-sdk |
88 | android:minSdkVersion="8" | |
89 | android:targetSdkVersion="8"/> | |
88 | android:minSdkVersion="10" | |
89 | android:targetSdkVersion="10"/> | |
90 | 90 | |
91 | 91 | </manifest> ⏎ |
9 | 9 | # Indicates whether an apk should be generated for each density. |
10 | 10 | split.density=false |
11 | 11 | # Project target. |
12 | target=android-8 | |
12 | target=android-10 |
305 | 305 | localDevice.init(this, new ReadyListener() { |
306 | 306 | @Override |
307 | 307 | 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 | } | |
311 | 317 | } |
312 | 318 | }); |
313 | 319 |
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 | } |
15 | 15 | /** Tag for logging */ |
16 | 16 | public static final String TAG = "chopper.ChopperMain"; |
17 | 17 | |
18 | private boolean telemetry = false; | |
18 | private boolean telemetry = true; | |
19 | 19 | |
20 | 20 | /** Prevent duplicate launching of threads if app loses focus **/ |
21 | 21 | private static boolean mFirstRun = true; |
22 | 22 | |
23 | 23 | /** Need a permanent reference to this, to destroy it. **/ |
24 | 24 | private Guidance guid; |
25 | private ChopperStatus status; | |
25 | 26 | |
26 | 27 | /** |
27 | 28 | * Holds the wakelock; needed to keep the camera preview rendering on |
78 | 79 | previewHolder.setType(SurfaceHolder.SURFACE_TYPE_PUSH_BUFFERS); |
79 | 80 | |
80 | 81 | Comm comm = new Comm(true); |
81 | ChopperStatus status = new ChopperStatus(getApplicationContext()); | |
82 | status = new ChopperStatus(getApplicationContext()); | |
82 | 83 | StatusReporter reporter = new StatusReporter(status); |
83 | 84 | MakePicture pic = null; |
84 | 85 | if (telemetry) { |
94 | 95 | } |
95 | 96 | comm.registerReceiver(NAV, nav); |
96 | 97 | comm.registerReceiver(CSYS, nav); |
98 | comm.registerReceiver(CSYS, guid); | |
97 | 99 | comm.registerReceiver(GUID, guid); |
98 | 100 | comm.registerReceiver(GUID, nav); |
99 | 101 | |
116 | 118 | if (telemetry) { |
117 | 119 | new Thread(pic).start(); |
118 | 120 | } |
119 | new Thread(nav).start(); | |
120 | 121 | new Thread(guid).start(); |
121 | 122 | } |
122 | 123 | catch (Exception e) { |
134 | 135 | super.onDestroy(); |
135 | 136 | if (guid != null) |
136 | 137 | guid.onDestroy(); |
138 | if (status != null) | |
139 | status.close(); | |
137 | 140 | } |
138 | 141 | } |
0 | 0 | package org.haldean.chopper.pilot; |
1 | 1 | |
2 | import java.io.BufferedWriter; | |
3 | import java.io.File; | |
4 | import java.io.FileWriter; | |
5 | import java.io.IOException; | |
2 | 6 | import java.util.LinkedList; |
3 | 7 | import java.util.ListIterator; |
4 | import java.util.concurrent.ExecutorService; | |
5 | import java.util.concurrent.Executors; | |
6 | import java.util.concurrent.TimeUnit; | |
7 | 8 | import java.util.concurrent.atomic.AtomicInteger; |
8 | import java.util.concurrent.locks.ReentrantLock; | |
9 | 9 | |
10 | 10 | import android.content.BroadcastReceiver; |
11 | 11 | import android.content.Context; |
21 | 21 | import android.location.LocationProvider; |
22 | 22 | import android.os.BatteryManager; |
23 | 23 | import android.os.Bundle; |
24 | import android.os.Environment; | |
24 | 25 | import android.os.Looper; |
25 | 26 | import android.util.Log; |
26 | 27 | |
45 | 46 | /** Point of "officially" low battery */ |
46 | 47 | public final static int LOW_BATT = 30; |
47 | 48 | |
49 | public static final String logfilename = "fastest.txt"; | |
50 | public static BufferedWriter logwriter; | |
51 | ||
48 | 52 | /** Tag for logging */ |
49 | 53 | public static final String TAG = "chopper.ChopperStatus"; |
50 | 54 | |
56 | 60 | * Value of '0' means maximum accuracy. */ |
57 | 61 | private static final long GPS_MIN_TIME = 0; |
58 | 62 | |
59 | /** Number of threads in the mutator pool */ | |
60 | private static final int sNumMutatorThreads = 5; | |
61 | ||
62 | 63 | /** Used to obtain location manager. */ |
63 | 64 | private Context mContext; |
64 | 65 | |
72 | 73 | private float mGpsAccuracy; //accuracy of said reading |
73 | 74 | |
74 | 75 | /** Lock for gps extras */ |
75 | private ReentrantLock mGpsExtrasLock; | |
76 | private Object mGpsExtrasLock; | |
76 | 77 | |
77 | 78 | /** 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. */ | |
81 | 82 | private int mGpsNumSats; //number of satellites used to collect last reading |
82 | 83 | |
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. */ | |
84 | 85 | private long mGpsTimeStamp; //timestamp of the reading |
85 | 86 | |
86 | 87 | /** Stores the location object last returned by the GPS. */ |
89 | 90 | /** Max battery level. */ |
90 | 91 | private final AtomicInteger mMaxBatt = new AtomicInteger(100); |
91 | 92 | |
92 | /** Lock for motorspeed[]. */ | |
93 | private ReentrantLock mMotorLock; | |
94 | ||
95 | 93 | /** Stores the speeds last submitted to the motors. */ |
96 | 94 | private double[] mMotorSpeed = new double[4]; |
97 | 95 | |
98 | /** Lock for mMotorPower[] */ | |
99 | private ReentrantLock mMotorPowerLock; | |
100 | ||
101 | 96 | /** Stores power levels for the motors. */ |
102 | 97 | private double[] mMotorPower = new double[4]; |
103 | 98 | |
104 | /** Thread pool for mutator methods */ | |
105 | private ExecutorService mMutatorPool; | |
106 | ||
107 | 99 | /** Holds data from various sensors, like gyroscope, acceleration and magnetic flux. */ |
108 | 100 | private double[] mReading = new double[SENSORS]; //last known data for a given sensor |
109 | 101 | |
110 | 102 | /** Locks for fields in reading[]. */ |
111 | private ReentrantLock[] mReadingLock; | |
103 | private Object[] mReadingLock; | |
112 | 104 | |
113 | 105 | /** List of registered receivers */ |
114 | 106 | private LinkedList<Receivable> mRec; |
115 | 107 | |
108 | private float[] orientation = new float[3]; | |
109 | private float[] rotationMatrix = new float[9]; | |
110 | private float[] flux = new float[3]; | |
111 | ||
116 | 112 | /** |
117 | 113 | * Initializes the locks, registers application context for runtime use. |
118 | 114 | * @param mycontext Application context |
122 | 118 | mRec = new LinkedList<Receivable>(); |
123 | 119 | |
124 | 120 | //Initialize the data locks |
125 | mReadingLock = new ReentrantLock[SENSORS]; | |
121 | mReadingLock = new Object[SENSORS]; | |
126 | 122 | |
127 | 123 | for (int i = 0; i < SENSORS; i++) { |
128 | mReadingLock[i] = new ReentrantLock(); | |
124 | mReadingLock[i] = new Object(); | |
129 | 125 | } |
130 | 126 | |
131 | mGpsLock = new ReentrantLock[GPS_FIELDS]; | |
127 | mGpsLock = new Object[GPS_FIELDS]; | |
132 | 128 | 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 | } | |
141 | 142 | } |
142 | 143 | |
143 | 144 | /** |
151 | 152 | /** |
152 | 153 | * Gets GPS "extras" data, through a string of the form accuracy:numberOfSatellitesInLatFix:timeStampOfLastFix |
153 | 154 | * @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() { | |
157 | 157 | 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; | |
171 | 162 | } |
172 | 163 | return gpsData; |
173 | 164 | } |
179 | 170 | */ |
180 | 171 | public double getGpsField(int whichField) { |
181 | 172 | double myValue; |
182 | mGpsLock[whichField].lock(); | |
183 | myValue = mGps[whichField]; | |
184 | mGpsLock[whichField].unlock(); | |
173 | synchronized (mGpsLock[whichField]) { | |
174 | myValue = mGps[whichField]; | |
175 | } | |
185 | 176 | return myValue; |
186 | 177 | } |
187 | 178 | |
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 | } | |
233 | 183 | } |
234 | 184 | |
235 | 185 | /** |
248 | 198 | |
249 | 199 | /** |
250 | 200 | * 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 | } | |
272 | 210 | } |
273 | 211 | |
274 | 212 | /** 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 | } | |
296 | 222 | } |
297 | 223 | |
298 | 224 | /** |
303 | 229 | //System.out.println("ChopperStatus run() thread ID " + getId()); |
304 | 230 | Looper.prepare(); |
305 | 231 | 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 | } | |
307 | 240 | /* Register to receive battery status updates */ |
308 | 241 | BroadcastReceiver batteryInfo = new BroadcastReceiver() { |
309 | 242 | public void onReceive(Context context, Intent intent) { |
321 | 254 | }; |
322 | 255 | |
323 | 256 | /* 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); | |
325 | 262 | |
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); | |
331 | 264 | //sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_PRESSURE), SensorManager.SENSOR_DELAY_NORMAL); |
332 | 265 | //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 | ||
334 | 269 | |
335 | 270 | /* Initialize GPS reading: */ |
336 | 271 | LocationManager LocMan = (LocationManager) mContext.getSystemService(Context.LOCATION_SERVICE); |
341 | 276 | Looper.loop(); |
342 | 277 | } |
343 | 278 | |
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 | ||
352 | 279 | /** |
353 | 280 | * Returns the reading specified by the supplied index, if its lock is available. Otherwise, throws an exception. |
354 | 281 | * @param whichField The index of the desired reading. |
355 | 282 | * @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) { | |
359 | 285 | 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]; | |
396 | 288 | } |
397 | 289 | return myValue; |
398 | 290 | } |
420 | 312 | /* Vertical velocity does not update until vertical position does; prevents false conclusions that vertical velocity == 0 */ |
421 | 313 | double oldAlt = getGpsField(ALTITUDE); |
422 | 314 | 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 | } | |
426 | 319 | double newdalt = ((newalt - oldAlt) / (double) timeElapsed) * 1000.0; |
427 | 320 | setGpsField(dALT, newdalt); |
428 | 321 | Log.i(TAG, "new dalt: " + newdalt); |
433 | 326 | setGpsField(LAT, loc.getLatitude()); |
434 | 327 | setGpsField(SPEED, loc.getSpeed()); |
435 | 328 | |
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 | } | |
442 | 335 | if (mLastLoc != null) { |
443 | 336 | synchronized (mLastLoc) { |
444 | 337 | mLastLoc = loc; |
473 | 366 | * @param event Object containing new data--sensor type, accuracy, timestamp and value. |
474 | 367 | */ |
475 | 368 | public void onSensorChanged(SensorEvent event) { |
369 | long time = System.currentTimeMillis(); | |
476 | 370 | int type = event.sensor.getType(); |
477 | switch (type) { | |
371 | switch (type) { | |
478 | 372 | case Sensor.TYPE_ACCELEROMETER: |
479 | 373 | setReadingField(X_ACCEL, event.values[0]); |
480 | 374 | setReadingField(Y_ACCEL, event.values[1]); |
484 | 378 | setReadingField(LIGHT, event.values[0]); |
485 | 379 | break; |
486 | 380 | case Sensor.TYPE_MAGNETIC_FIELD: |
381 | System.arraycopy(event.values, 0, flux, 0, 3); | |
487 | 382 | setReadingField(X_FLUX, event.values[0]); |
488 | 383 | setReadingField(Y_FLUX, event.values[1]); |
489 | 384 | setReadingField(Z_FLUX, event.values[2]); |
490 | 385 | 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; | |
496 | 386 | case Sensor.TYPE_PRESSURE: |
497 | 387 | setReadingField(PRESSURE, event.values[0]); |
498 | 388 | break; |
501 | 391 | break; |
502 | 392 | case Sensor.TYPE_TEMPERATURE: |
503 | 393 | 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); | |
504 | 423 | break; |
505 | 424 | } |
506 | 425 | } |
541 | 460 | * Sets the motor speed data to the supplied array. |
542 | 461 | * @param mySpeeds The data to which the motor speeds should be set. |
543 | 462 | */ |
544 | protected void setMotorFields(double[] mySpeeds) { | |
463 | protected void setMotorFields(final double[] mySpeeds) { | |
545 | 464 | if (mySpeeds.length < 4) |
546 | 465 | 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"); | |
564 | 474 | } |
565 | 475 | |
566 | 476 | /** |
567 | 477 | * Sets the motor power levels to the supplied array. |
568 | 478 | * @param myPowers The data to which the motor power levels should be set. |
569 | 479 | */ |
570 | protected void setMotorPowerFields(double[] myPowers) { | |
480 | protected void setMotorPowerFields(final double[] myPowers) { | |
571 | 481 | if (myPowers.length < 4) |
572 | 482 | 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 | } | |
583 | 486 | } |
584 | 487 | |
585 | 488 | /** |
588 | 491 | * @param value The GPS data. |
589 | 492 | */ |
590 | 493 | 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 | } | |
598 | 497 | } |
599 | 498 | |
600 | 499 | |
604 | 503 | * @param value The reading. |
605 | 504 | */ |
606 | 505 | 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 | } | |
614 | 509 | } |
615 | 510 | |
616 | 511 | /** Updates all registered receivers with the specified String */ |
3 | 3 | import java.io.IOException; |
4 | 4 | import java.util.LinkedList; |
5 | 5 | import java.util.ListIterator; |
6 | import java.util.concurrent.atomic.AtomicInteger; | |
6 | 7 | |
7 | 8 | import android.os.Handler; |
8 | 9 | import android.os.Looper; |
24 | 25 | * PID: |
25 | 26 | * SET:<pid_loop_number>:<pid_parameter_index>:<pid_parameter_value> |
26 | 27 | * GET |
27 | * AUTOMATIC | |
28 | * AUTOPILOT | |
28 | 29 | * VECTOR:<north_motor_speed>:<south_motor_speed>:<east_motor_speed>:<west_motor_speed> |
29 | 30 | * LOCALVEC |
30 | 31 | * ABSVEC |
36 | 37 | public class Guidance implements Runnable, Constants, Receivable { |
37 | 38 | |
38 | 39 | /** 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; | |
46 | 41 | |
47 | 42 | /** 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; | |
49 | 44 | |
50 | 45 | /** 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; | |
52 | 47 | |
53 | 48 | /** Tag for logging */ |
54 | 49 | public static final String TAG = new String("chopper.Guidance"); |
55 | 50 | |
56 | /** Used when a really big number is needed, still small enough to prevent overflow. */ | |
57 | private static final double sReallyBig = 2.0; | |
58 | ||
59 | 51 | /** Handles messages for the thread */ |
60 | 52 | private Handler mHandler; |
61 | 53 | |
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; | |
64 | 55 | |
65 | 56 | /** Stores orientation data persistently, as expected values in case lock is not immediately available*/ |
66 | 57 | private double mAzimuth; |
67 | 58 | private double mPitchDeg; |
68 | 59 | private double mRollDeg; |
69 | private double mGpsBearing; | |
70 | private double mGpsSpeed; | |
71 | private double mGpsDalt; | |
72 | 60 | |
73 | 61 | /** Log file name **/ |
74 | 62 | public static final String logname = "/sdcard/chopper/guidlog.txt"; |
85 | 73 | * remain persistent from iteration to iteration. |
86 | 74 | */ |
87 | 75 | |
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 | ||
94 | 76 | /** Stores current PID error */ |
95 | 77 | private double[][] mErrors = new double[4][3]; |
96 | 78 | |
101 | 83 | /** Timestamp of last PID evaluation */ |
102 | 84 | private long mLastUpdate = 0; |
103 | 85 | |
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; | |
109 | 94 | |
110 | 95 | /** Tuning parameters */ |
111 | 96 | private double[][] mGain = new double[4][3]; |
113 | 98 | /** Motor speed */ |
114 | 99 | private double[] mMotorSpeed = new double[4]; //ORDER: North, South, East, West |
115 | 100 | |
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 | ||
121 | 101 | /** List of registered receivers */ |
122 | 102 | private LinkedList<Receivable> mRec; |
123 | 103 | |
124 | 104 | /** Handles to other chopper components */ |
125 | 105 | private ChopperStatus mStatus; |
126 | private Navigation mNav; | |
127 | 106 | private BluetoothOutput mBt; |
128 | ||
129 | /** Controls whether N/S and E/W commands refer to absolute vectors or local **/ | |
130 | private boolean mAbsVec = true; | |
131 | 107 | |
132 | 108 | /** Flag for writing motor speeds to output file **/ |
133 | 109 | public final static boolean mEnableLogging = true; |
142 | 118 | throw new NullPointerException(); |
143 | 119 | } |
144 | 120 | mStatus = status; |
145 | mNav = nav; | |
146 | 121 | mRec = new LinkedList<Receivable>(); |
147 | 122 | mBt = bT; |
123 | mAngler = new Angler(status, nav); | |
148 | 124 | |
149 | 125 | //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; | |
170 | 129 | |
171 | 130 | try { |
172 | 131 | if (mEnableLogging) |
245 | 204 | } |
246 | 205 | }; |
247 | 206 | //mHandler.sendEmptyMessage(EVAL_MOTOR_SPEED); |
248 | receiveMessage("GUID:VECTOR:0:0:0:0", null); | |
207 | receiveMessage("DIRECT:0:0:0:0", null); | |
249 | 208 | Looper.loop(); |
250 | 209 | } |
251 | 210 | |
257 | 216 | public void receiveMessage(String msg, Receivable source) { |
258 | 217 | //Log.d(TAG, "Receiving message " + msg); |
259 | 218 | 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 | } | |
260 | 225 | if (parts[0].equals("GUID")) { |
261 | 226 | if (parts[1].equals("PID")) { |
262 | 227 | if (parts[2].equals("SET")) { |
272 | 237 | getPids.sendToTarget(); |
273 | 238 | } |
274 | 239 | } |
275 | if (parts[1].equals("AUTOMATIC")) { | |
240 | if (parts[1].equals("AUTOPILOT")) { | |
241 | Log.v(TAG, "AUTOPILOT mode"); | |
242 | mGuidanceMode.set(AUTOPILOT); | |
276 | 243 | mHandler.removeMessages(NEW_GUID_VECTOR); |
277 | mReviseMotorSpeeds = true; | |
278 | 244 | if (!mHandler.hasMessages(EVAL_MOTOR_SPEED)) |
279 | 245 | mHandler.sendEmptyMessage(EVAL_MOTOR_SPEED); |
280 | 246 | } |
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); | |
291 | 250 | mHandler.removeMessages(EVAL_MOTOR_SPEED); |
292 | 251 | Double[] myVector = new Double[4]; |
293 | 252 | for (int i = 0; i < 4; i++) { |
296 | 255 | Message newValue = Message.obtain(mHandler, NEW_GUID_VECTOR, myVector); |
297 | 256 | newValue.sendToTarget(); |
298 | 257 | } |
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 | } | |
299 | 281 | } |
300 | 282 | } |
301 | 283 | |
312 | 294 | /** Core of the class; calculates new motor speeds based on status */ |
313 | 295 | private void reviseMotorSpeed() { |
314 | 296 | mHandler.removeMessages(EVAL_MOTOR_SPEED); |
315 | //Log.v(TAG, "START MOTOR REVISION"); | |
316 | 297 | 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: "; | |
319 | 315 | 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 | ||
433 | 325 | |
434 | 326 | for (int i = 0; i < 4; i++) { |
435 | 327 | //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 | ||
443 | 330 | |
444 | 331 | //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); | |
446 | 333 | |
447 | 334 | |
448 | 335 | //Mark proportional error |
455 | 342 | mErrors[i][1] += err; |
456 | 343 | mIntegralIndex = ++mIntegralIndex % PIDREPS; |
457 | 344 | |
458 | double dmotor = 0; | |
459 | ||
460 | 345 | //Calculate changes in output |
461 | 346 | 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: "; | |
538 | 364 | 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(); | |
569 | 370 | |
570 | 371 | //Send motor values to motors here: |
571 | 372 | updateMotors(); |
573 | 374 | //Log.v(TAG, "motors: " + mMotorSpeed[0] + ", " + mMotorSpeed[1] + ", " + mMotorSpeed[2] + ", " + mMotorSpeed[3]); |
574 | 375 | //Sleep a while |
575 | 376 | 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)) { | |
577 | 380 | if (timetonext > 0) |
578 | 381 | mHandler.sendEmptyMessageDelayed(EVAL_MOTOR_SPEED, timetonext); |
579 | 382 | else { |
581 | 384 | mHandler.sendEmptyMessage(EVAL_MOTOR_SPEED); |
582 | 385 | } |
583 | 386 | } |
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; | |
584 | 433 | } |
585 | 434 | |
586 | 435 | /** |
597 | 446 | } |
598 | 447 | } |
599 | 448 | catch (IOException e) { |
600 | Log.e(TAG, "Cannot write to logfile"); | |
449 | //Log.e(TAG, "Cannot write to logfile"); | |
601 | 450 | } |
602 | 451 | //Pass motor values to motor controller! |
603 | 452 | Message msg = Message.obtain(mBt.mHandler, SEND_MOTOR_SPEEDS, mMotorSpeed); |
2 | 2 | import java.util.LinkedList; |
3 | 3 | import java.util.ListIterator; |
4 | 4 | import java.util.Vector; |
5 | import java.util.concurrent.atomic.AtomicBoolean; | |
6 | 5 | import java.util.concurrent.atomic.AtomicInteger; |
7 | import java.util.concurrent.locks.ReentrantLock; | |
8 | 6 | |
9 | 7 | import org.haldean.chopper.nav.NavData; |
10 | 8 | import org.haldean.chopper.nav.NavList; |
11 | 9 | |
12 | import android.os.Handler; | |
13 | import android.os.Looper; | |
14 | import android.os.Message; | |
15 | 10 | import android.util.Log; |
16 | 11 | |
17 | 12 | /** |
38 | 33 | * |
39 | 34 | * @author Benjamin Bardin |
40 | 35 | */ |
41 | public class Navigation implements Runnable, Constants, Receivable { | |
36 | public class Navigation implements Constants, Receivable { | |
42 | 37 | |
43 | 38 | /** Tag for logging */ |
44 | 39 | public static final String TAG = "chopper.Navigation"; |
52 | 47 | /** Velocity to achieve. Must be locked on each read/write. */ |
53 | 48 | private double[] mTarget = new double[4]; |
54 | 49 | |
55 | /** Lock for target[] */ | |
56 | private ReentrantLock mTargetLock; | |
57 | ||
58 | 50 | /** Used to calculate next nav vector; prevents the thread |
59 | 51 | * from holding a lock on mTarget itself for too long */ |
60 | 52 | private double[] mTempTarget = new double[4]; |
61 | ||
62 | /** True if autopilot is engaged */ | |
63 | private final AtomicBoolean mAutoPilot = new AtomicBoolean(false); | |
64 | 53 | |
65 | 54 | /** Chopper's navigation status */ |
66 | 55 | private final AtomicInteger mNavStatus = new AtomicInteger(NAV_STATUSES - 1); |
76 | 65 | /** Handle for other chopper components */ |
77 | 66 | private ChopperStatus mStatus; |
78 | 67 | |
79 | /** Handles messages */ | |
80 | private Handler mHandler; | |
81 | ||
82 | 68 | /** Registered receivers */ |
83 | 69 | private LinkedList<Receivable> mRec; |
84 | 70 | |
102 | 88 | mTravelPlans.add(mLowPower); |
103 | 89 | mTravelPlans.add(mFlightPath); |
104 | 90 | mTravelPlans.add(mOnMyOwn); |
105 | ||
106 | mTargetLock = new ReentrantLock(); | |
107 | 91 | |
108 | 92 | 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) { | |
128 | 115 | |
129 | 116 | /*Determine what the current task should be. Copies to a local variable in case |
130 | 117 | * 'status' changes during execution of the method */ |
139 | 126 | } |
140 | 127 | mTask.getVelocity(myList, mTempTarget); |
141 | 128 | 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); | |
147 | 135 | //Send the current NavList to the server, in case any tasks have been completed |
148 | 136 | 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; | |
215 | 137 | } |
216 | 138 | |
217 | 139 | /** |
222 | 144 | * @param expectedValues The array in which to write the |
223 | 145 | * vector--must be at least of length 4. |
224 | 146 | */ |
225 | public void getTarget(double[] expectedValues) { | |
226 | if (expectedValues.length < 4) | |
147 | public void getTarget(double[] navTarget) { | |
148 | if (navTarget.length < 4) | |
227 | 149 | 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); | |
236 | 152 | } |
237 | 153 | } |
238 | 154 | |
255 | 171 | for (int i = 0; i < 3; i++) { |
256 | 172 | mTempTarget[i] = 0; |
257 | 173 | } |
258 | mTempTarget[3] = mStatus.getReadingFieldNow(AZIMUTH, mTempTarget[3]); | |
174 | mTempTarget[3] = mStatus.getReadingField(AZIMUTH); | |
259 | 175 | setTarget(mTempTarget); |
260 | mHandler.removeMessages(EVAL_NAV); | |
261 | mHandler.sendEmptyMessageDelayed(EVAL_NAV, HOVER_PAUSE); | |
262 | 176 | } |
263 | 177 | |
264 | 178 | /** |
266 | 180 | * @param source The source of the message, if a reply is needed. May be null. |
267 | 181 | */ |
268 | 182 | public void receiveMessage(String msg, Receivable source) { |
269 | Log.d(TAG, "Receiving " + msg); | |
270 | 183 | String[] parts = msg.split(":"); |
271 | if (parts[0].equals("GUID")) { | |
272 | if (parts[1].equals("VECTOR")) { | |
273 | autoPilot(false); | |
274 | } | |
275 | } | |
184 | ||
276 | 185 | if (parts[0].equals("NAV")) { |
277 | 186 | if (parts[1].equals("SET")) { |
278 | 187 | 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 | ||
291 | 189 | if (parts[2].equals("AUTOPILOT")) { |
292 | 190 | mNavStatus.set(BASIC_AUTO); |
293 | 191 | updateReceivers("GUID:ABSVEC"); |
294 | 192 | updateReceivers("GUID:AUTOMATIC"); |
295 | autoPilot(true); | |
193 | //autoPilot(true); | |
296 | 194 | } |
297 | 195 | if (parts[2].equals("AUTOTASK")) { |
298 | 196 | Integer taskList = new Integer(parts[3]); |
316 | 214 | } |
317 | 215 | if (parts[0].equals("CSYS")) { |
318 | 216 | if (parts[1].equals("NOCONN")) { |
319 | Log.d(TAG, "no conn in Nav"); | |
320 | 217 | updateStatus(NO_CONN); |
321 | autoPilot(true); | |
322 | updateReceivers("GUID:AUTOMATIC"); | |
218 | //autoPilot(true); | |
323 | 219 | } |
324 | 220 | if (parts[1].equals("LOWPOWER")) { |
325 | 221 | updateStatus(LOW_POWER); |
326 | autoPilot(true); | |
327 | updateReceivers("GUID:AUTOMATIC"); | |
222 | //autoPilot(true); | |
328 | 223 | } |
329 | 224 | } |
330 | 225 | } |
343 | 238 | * Sets the navigation target vector to the values contained by the supplied array (length must be at least 4). |
344 | 239 | * @param newTarget The new target vector. |
345 | 240 | */ |
346 | protected void setTarget(double[] newTarget) { | |
241 | private void setTarget(double[] newTarget) { | |
347 | 242 | if (newTarget.length < 4) { |
348 | 243 | return; |
349 | 244 | } |
350 | final double[] myCopy = newTarget.clone(); | |
351 | new Thread() { | |
352 | public void run() { | |
245 | synchronized (mTarget) { | |
353 | 246 | 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 | } | |
361 | 251 | updateReceivers(newNav); |
362 | } | |
363 | }.start(); | |
252 | } | |
364 | 253 | } |
365 | 254 | |
366 | 255 | /** |
383 | 272 | Log.i(TAG, "Nav set index " + whichPlan + " to task " + myList); |
384 | 273 | //Confirm change to server: |
385 | 274 | updateReceivers("NAV:AUTOTASK:" + whichPlan + ":" + myList.toString()); |
386 | if (mAutoPilot.get()) | |
387 | evalNextVector(); | |
388 | 275 | } |
389 | 276 | else { |
390 | 277 | Log.e(TAG, "Nav received invalid task!"); |
5 | 5 | import android.os.Handler; |
6 | 6 | import android.os.Looper; |
7 | 7 | import android.os.Message; |
8 | import android.util.Log; | |
9 | 8 | |
10 | 9 | /** |
11 | 10 | * Sends regular chopper status reports to all registered receivers. <P> |
60 | 59 | LinkedList<String> infoList = new LinkedList<String>(); |
61 | 60 | |
62 | 61 | /* 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 | ||
83 | 73 | /*try { |
84 | 74 | double myXflux = mStatus.getReadingFieldNow(X_FLUX); |
85 | 75 | double myYflux = mStatus.getReadingFieldNow(Y_FLUX); |
90 | 80 | Log.w(TAG, "Flux Report Unavailable"); |
91 | 81 | }*/ |
92 | 82 | |
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 | ||
103 | 90 | /* |
104 | 91 | try { |
105 | 92 | double[] myPowers = mStatus.getMotorFieldsNow(); |
135 | 122 | Log.w(TAG, "Pressure Report Unavailable"); |
136 | 123 | } |
137 | 124 | */ |
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)); | |
146 | 126 | infoList.add("BATTERY:" + mStatus.getBatteryLevel()); |
147 | 127 | |
148 | 128 | /* Send GPS data */ |
149 | 129 | 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); | |
162 | 137 | |
163 | 138 | return infoList; |
164 | 139 | } |
15 | 15 | * @author William Brown |
16 | 16 | */ |
17 | 17 | 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. | |
20 | 21 | private static double bearing = 0; |
21 | 22 | |
22 | 23 | /* In meters per second (we hope.) */ |
98 | 99 | * the motor closest to North. |
99 | 100 | */ |
100 | 101 | public static void setMotorSpeeds(double[] speeds) { |
101 | String taskString = "GUID:VECTOR"; | |
102 | String taskString = navGoToDirect; | |
102 | 103 | for (int i=0; i<4; i++) { |
103 | 104 | taskString += ":" + speeds[i]; |
104 | 105 | } |
89 | 89 | buttons[BUTTON_X] = c; |
90 | 90 | else if (id == Component.Identifier.Button.Y) |
91 | 91 | 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)) | |
93 | 93 | buttons[BUTTON_BACK] = c; |
94 | 94 | else if (id == Component.Identifier.Button.LEFT_THUMB) |
95 | 95 | buttons[BUTTON_L] = c; |
119 | 119 | axes[AXIS_R_TRIGGER] = c; |
120 | 120 | else if (id == Component.Identifier.Axis.POV) |
121 | 121 | axes[D_PAD] = c; |
122 | } | |
122 | } | |
123 | 123 | } |
124 | 124 | } |
125 | 125 |