Commit ae4c7643c39cd10742719cec356066a243d8e7e5 - droidcopter

New PID loop/guidance structure (EnsignCrusher updated). Fixed bug in PadController where the back button would not be recognized. Benjamin Bardin 10 years ago

11 changed file(s) with 493 addition(s) and 782 deletion(s). Raw diff Collapse all Expand all

-2

Pilot/AndroidManifest.xml less more

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> ⏎

-1

Pilot/default.properties less more

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

-3

at/abraxas/amarino/AmarinoService.java less more

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

+95

-0

org/haldean/chopper/pilot/Angler.java less more

	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	}

-3

org/haldean/chopper/pilot/ChopperMain.java less more

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	}

+147

-252

org/haldean/chopper/pilot/ChopperStatus.java less more

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 */

+155

-306

org/haldean/chopper/pilot/Guidance.java less more

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);

+46

-159

org/haldean/chopper/pilot/Navigation.java less more

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!");

+26

-51

org/haldean/chopper/pilot/StatusReporter.java less more

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	}

-3

org/haldean/chopper/server/EnsignCrusher.java less more

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	}

-2

org/haldean/chopper/server/PadController.java less more

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