Commit d27eec3bfe5915a89368bded1696ae39322a02f3 - droidcopter

Guidance logging Benjamin Bardin 10 years ago

6 changed file(s) with 143 addition(s) and 66 deletion(s). Raw diff Collapse all Expand all

-0

Pilot/AndroidManifest.xml less more

39	39	<uses-permission android:name="android.permission.PERSISTENT_ACTIVITY"></uses-permission>
40	40	<uses-permission android:name="android.permission.WAKE_LOCK"></uses-permission>
41	41	<uses-permission android:name="android.permission.DEVICE_POWER"></uses-permission>
	42	<uses-permission android:name="android.permission.WRITE_EXTERNAL_STORAGE"></uses-permission>
42	43
43	44	<uses-feature android:name="android.hardware.camera" />
44	45	<uses-feature android:name="android.hardware.camera.autofocus" />

-4

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

17	17	private boolean telemetry = true;
18	18
19	19	private static boolean mFirstRun = true;
	20	private Guidance guid;
20	21
21	22	/**
22	23	* Holds the wakelock; needed to keep the camera preview rendering on

77	78	}
78	79
79	80	Navigation nav = new Navigation(status);
80		Guidance guid = new Guidance(status, nav);
	81	guid = new Guidance(status, nav);
81	82
82	83	if (telemetry) {
83	84	comm.setTelemetrySource(pic);

116	117
117	118	mFirstRun = false;
118	119	}
119		/*
	120
120	121	/**
121	122	* Releases the wakelock, destroys activity.
122
	123	*/
123	124	protected void onDestroy() {
124	125	//mWakeLock.release();
125	126	super.onDestroy();
126		} */
	127	guid.onDestroy();
	128	}
127	129	}

+83

-47

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

3	3	import java.util.ListIterator;
4	4	import java.util.concurrent.ExecutorService;
5	5	import java.util.concurrent.Executors;
	6	import java.util.concurrent.TimeUnit;
6	7	import java.util.concurrent.atomic.AtomicInteger;
7	8	import java.util.concurrent.locks.ReentrantLock;
8	9

154	155	*/
155	156	public String getGpsExtrasNow() throws IllegalAccessException {
156	157	String gpsData = "";
157		if (mGpsExtrasLock.tryLock()) {
158		gpsData += mGpsAccuracy +
159		":" + mGpsNumSats +
160		":" + mGpsTimeStamp;
161		mGpsExtrasLock.unlock();
162		}
163		else {
	158	try {
	159	if (mGpsExtrasLock.tryLock(0, TimeUnit.SECONDS)) {
	160	gpsData += mGpsAccuracy +
	161	":" + mGpsNumSats +
	162	":" + mGpsTimeStamp;
	163	mGpsExtrasLock.unlock();
	164	}
	165	else {
	166	throw new InterruptedException();
	167	}
	168	}
	169	catch (InterruptedException e) {
164	170	throw new IllegalAccessException();
165	171	}
166	172	return gpsData;

187	193	*/
188	194	public double getGpsFieldNow(int whichField) throws IllegalAccessException {
189	195	double myValue;
190		if (mGpsLock[whichField].tryLock()) {
191		myValue = mGps[whichField];
192		mGpsLock[whichField].unlock();
193		}
194		else {
	196	try {
	197	if (mGpsLock[whichField].tryLock(0, TimeUnit.SECONDS)) {
	198	myValue = mGps[whichField];
	199	mGpsLock[whichField].unlock();
	200	}
	201	else {
	202	throw new InterruptedException();
	203	}
	204	}
	205	catch (InterruptedException e) {
195	206	Log.w(TAG, "GPS Field " + whichField + " is locked.");
196	207	throw new IllegalAccessException();
197	208	}

206	217	*/
207	218	public double getGpsFieldNow(int whichField, double expectedValue) {
208	219	double myValue;
209		if (mGpsLock[whichField].tryLock()) {
210		myValue = mGps[whichField];
211		mGpsLock[whichField].unlock();
212		}
213		else {
	220	try {
	221	if (mGpsLock[whichField].tryLock(0, TimeUnit.SECONDS)) {
	222	myValue = mGps[whichField];
	223	mGpsLock[whichField].unlock();
	224	}
	225	else {
	226	throw new InterruptedException();
	227	}
	228	}
	229	catch (InterruptedException e) {
214	230	myValue = expectedValue;
215	231	}
216	232	return myValue;

237	253	*/
238	254	public double[] getMotorFieldsNow() throws IllegalAccessException {
239	255	double[] myValues = new double[4];
240		if (mMotorLock.tryLock()) {
241		for (int i = 0; i < 4; i++) {
242		myValues[i] = mMotorSpeed[i];
243		}
244		mMotorLock.unlock();
245		}
246		else {
	256	try {
	257	if (mMotorLock.tryLock(0, TimeUnit.SECONDS)) {
	258	for (int i = 0; i < 4; i++) {
	259	myValues[i] = mMotorSpeed[i];
	260	}
	261	mMotorLock.unlock();
	262	}
	263	else {
	264	throw new InterruptedException();
	265	}
	266	}
	267	catch (InterruptedException e) {
247	268	Log.w(TAG, "motorspeed is locked.");
248	269	throw new IllegalAccessException();
249	270	}

256	277	*/
257	278	public double[] getMotorPowerFieldsNow() throws IllegalAccessException {
258	279	double[] myValues = new double[4];
259		if (mMotorPowerLock.tryLock()) {
260		for (int i = 0; i < 4; i++) {
261		myValues[i] = mMotorPower[i];
262		}
263		mMotorPowerLock.unlock();
264		}
265		else {
	280	try {
	281	if (mMotorPowerLock.tryLock(0, TimeUnit.SECONDS)) {
	282	for (int i = 0; i < 4; i++) {
	283	myValues[i] = mMotorPower[i];
	284	}
	285	mMotorPowerLock.unlock();
	286	}
	287	else {
	288	throw new InterruptedException();
	289	}
	290	}
	291	catch (InterruptedException e) {
266	292	Log.w(TAG, "motorpowers is locked.");
267	293	throw new IllegalAccessException();
268	294	}

298	324	SensorManager sensors = (SensorManager) mContext.getSystemService(Context.SENSOR_SERVICE);
299	325
300	326	/* Registers this class as a sensor listener for every necessary sensor. */
301		sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_ACCELEROMETER), SensorManager.SENSOR_DELAY_FASTEST);
302		sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_LIGHT), SensorManager.SENSOR_DELAY_NORMAL);
303		sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD), SensorManager.SENSOR_DELAY_NORMAL);
	327	sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_ACCELEROMETER), SensorManager.SENSOR_DELAY_NORMAL);
	328	//sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_LIGHT), SensorManager.SENSOR_DELAY_NORMAL);
	329	//sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD), SensorManager.SENSOR_DELAY_NORMAL);
304	330	sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_ORIENTATION), SensorManager.SENSOR_DELAY_FASTEST);
305		sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_PRESSURE), SensorManager.SENSOR_DELAY_NORMAL);
306		sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_PROXIMITY), SensorManager.SENSOR_DELAY_NORMAL);
	331	//sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_PRESSURE), SensorManager.SENSOR_DELAY_NORMAL);
	332	//sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_PROXIMITY), SensorManager.SENSOR_DELAY_NORMAL);
307	333	sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_TEMPERATURE), SensorManager.SENSOR_DELAY_NORMAL);
308	334
309	335	/* Initialize GPS reading: */

331	357	*/
332	358	public double getReadingFieldNow(int whichField) throws IllegalAccessException {
333	359	double myValue;
334		if (mReadingLock[whichField].tryLock()) {
335		myValue = mReading[whichField];
336		mReadingLock[whichField].unlock();
337		}
338		else {
	360	try {
	361	if (mReadingLock[whichField].tryLock(0, TimeUnit.SECONDS)) {
	362	myValue = mReading[whichField];
	363	mReadingLock[whichField].unlock();
	364	}
	365	else {
	366	throw new InterruptedException();
	367	}
	368	}
	369	catch (InterruptedException e) {
339	370	Log.w(TAG, "Reading field " + whichField + " is locked.");
340	371	throw new IllegalAccessException();
341	372	}

350	381	*/
351	382	public double getReadingFieldNow(int whichField, double expectedValue) {
352	383	double myValue;
353		if (mReadingLock[whichField].tryLock()) {
354		myValue = mReading[whichField];
355		mReadingLock[whichField].unlock();
356		}
357		else {
358		Log.w(TAG, "Reading field " + whichField + " is locked.");
	384	try {
	385	if (mReadingLock[whichField].tryLock(0, TimeUnit.SECONDS)) {
	386	myValue = mReading[whichField];
	387	mReadingLock[whichField].unlock();
	388	}
	389	else {
	390	throw new InterruptedException();
	391	}
	392	}
	393	catch (InterruptedException e) {
	394	Log.e(TAG, "Reading field " + whichField + " is locked.");
359	395	myValue = expectedValue;
360	396	}
361	397	return myValue;

+42

-6

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

0	0	package org.haldean.chopper.pilot;
1	1
	2	import java.io.FileWriter;
	3	import java.io.IOException;
2	4	import java.util.LinkedList;
3	5	import java.util.ListIterator;
4	6

34	36	public class Guidance implements Runnable, Constants, Receivable {
35	37
36	38	/** How many times per second the PID loop will run */
37		public int PIDREPS = 20;
	39	public int PIDREPS = 10;
38	40
39	41	/** Maximum permissible target velocity, in m/s; larger vectors will be resized */
40	42	public static final double MAX_VEL = 2.0;
41	43
42	44	/** The maximum angle, in degrees, guidance will permit the chopper to have */
43		public static final double MAX_ANGLE = 5;
	45	public static final double MAX_ANGLE = 10;
44	46
45	47	/** The maximum change in motor speed permitted at one time. Must be positive. */
46	48	public static final double MAX_DMOTOR = .05;

67	69	private double mGpsBearing;
68	70	private double mGpsSpeed;
69	71	private double mGpsDalt;
	72
	73	public static final String logname = "/sdcard/chopper/guidlog.txt";
	74	private FileWriter logfile;
70	75
71	76	/** Note that some of the following objects are declared outside their smallest scope.
72	77	* This is to relieve unnecessary stress on the GC. Many of these data holders

119	124	/ Controls whether N/S and E/W commands refer to absolute vectors or local /
120	125	private boolean mAbsVec = true;
121	126
	127	public final static boolean mEnableLogging = true;
122	128	/**
123	129	* Constructs a Guidance object
124	130	* @param status The source status information.

135	141	//Temporary: need real tuning values at some point. Crap.
136	142	for (int i = 0; i < 3; i++)
137	143	for (int j = 0; j < 3; j++)
138		mGain[i][j] = .00005;
	144	mGain[i][j] = .0001;
	145	//mGain[i][j] = .05;
139	146	for (int j = 0; j < 3; j++) {
140	147	// mGain[3][j] = .0005;
141	148	mGain[3][j] = 0;
	149	}
	150	try {
	151	if (mEnableLogging)
	152	logfile = new FileWriter(logname, false);
	153	}
	154	catch (IOException e) {
	155	e.printStackTrace();
	156	Log.e(TAG, "Cannot open log file.");
142	157	}
143	158	}
144	159

147	162	+ ":" + mErrors[1][0]
148	163	+ ":" + mErrors[2][0]
149	164	+ ":" + mErrors[3][0];
	165	}
	166
	167	public void onDestroy() {
	168	try {
	169	if (logfile != null)
	170	logfile.close();
	171	}
	172	catch (IOException e) {
	173	Log.e(TAG, "Cannot close logfile.");
	174	}
150	175	}
151	176
152	177	/**

449	474	mTempMotorSpeed[i] += dalttorque;
450	475	}
451	476
452		//Sanity Check--values must be between zero and one.
	477	//Bounds Check--values must be between zero and one.
453	478	for (int i = 0; i < 4; i++) {
454	479	if (mTempMotorSpeed[i] < 0)
455	480	mTempMotorSpeed[i] = 0;

480	505	if (mReviseMotorSpeeds) {
481	506	if (timetonext > 0)
482	507	mHandler.sendEmptyMessageDelayed(EVAL_MOTOR_SPEED, timetonext);
483		else
	508	else {
	509	Log.e(TAG, "Guidance too slow");
484	510	mHandler.sendEmptyMessage(EVAL_MOTOR_SPEED);
	511	}
485	512	}
486	513	}
487	514

489	516	private void updateMotors() {
490	517	//Pass filtered values to ChopperStatus.
491	518	mStatus.setMotorFields(mMotorSpeed);
492
	519	String logline = Long.toString(System.currentTimeMillis()) + " " + mMotorSpeed[0] + " " + mMotorSpeed[1] + " " + mMotorSpeed[2] + " " + mMotorSpeed[3] + "\n";
	520	try {
	521	if (logfile != null) {
	522	logfile.write(logline);
	523	logfile.flush();
	524	}
	525	}
	526	catch (IOException e) {
	527	Log.e(TAG, "Cannot write to logfile");
	528	}
493	529	//Pass motor values to motor controller!
494	530	BluetoothOutput.setMotorSpeeds(mMotorSpeed[0], mMotorSpeed[1], mMotorSpeed[2], mMotorSpeed[3]);
495	531	}

-5

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

144	144	updateReceivers("NAV:AUTOTASK:" + thisStatus + ":" + myList.toString());
145	145
146	146	mHandler.removeMessages(EVAL_NAV);
147		if (interval > 0) {
148		mHandler.sendEmptyMessageDelayed(EVAL_NAV, interval);
149		}
150		else {
151		mHandler.sendEmptyMessage(EVAL_NAV);
	147	if (mAutoPilot.get()) {
	148	if (interval > 0) {
	149	mHandler.sendEmptyMessageDelayed(EVAL_NAV, interval);
	150	}
	151	else {
	152	mHandler.sendEmptyMessage(EVAL_NAV);
	153	}
152	154	}
153	155	}
154	156

-4

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

80	80	Log.w(TAG, "Acceleration Report Unavailable");
81	81	}
82	82
83		try {
	83	/*try {
84	84	double myXflux = mStatus.getReadingFieldNow(X_FLUX);
85	85	double myYflux = mStatus.getReadingFieldNow(Y_FLUX);
86	86	double myZflux = mStatus.getReadingFieldNow(Z_FLUX);

88	88	}
89	89	catch (IllegalAccessException e) {
90	90	Log.w(TAG, "Flux Report Unavailable");
91		}
	91	}*/
92	92
93	93	try {
94	94	double[] mySpeeds = mStatus.getMotorFieldsNow();

100	100	catch (IllegalAccessException e) {
101	101	Log.w(TAG, "MotorSpeeds Report Unavailable");
102	102	}
103
	103	/*
104	104	try {
105	105	double[] myPowers = mStatus.getMotorFieldsNow();
106	106	infoList.add("MOTORPOWER:" + myPowers[0] +

134	134	catch (IllegalAccessException e) {
135	135	Log.w(TAG, "Pressure Report Unavailable");
136	136	}
137
	137	*/
138	138	try {
139	139	double myTemp = mStatus.getReadingFieldNow(TEMPERATURE);
140	140	infoList.add("TEMPERATURE:" + myTemp);