git.haldean.org droidcopter / cc9637d
Adding unit tests for Guidance. Benjamin Bardin 11 years ago
30 changed file(s) with 1651 addition(s) and 910 deletion(s). Raw diff Collapse all Expand all
0 <?xml version="1.0" encoding="UTF-8"?>
1 <classpath>
2 <classpathentry kind="src" path="src"/>
3 <classpathentry kind="src" path="gen"/>
4 <classpathentry kind="con" path="com.android.ide.eclipse.adt.ANDROID_FRAMEWORK"/>
5 <classpathentry combineaccessrules="false" kind="src" path="/Pilot"/>
6 <classpathentry kind="output" path="bin"/>
7 </classpath>
0 <?xml version="1.0" encoding="UTF-8"?>
1 <projectDescription>
2 <name>PilotTest</name>
3 <comment></comment>
4 <projects>
5 </projects>
6 <buildSpec>
7 <buildCommand>
8 <name>com.android.ide.eclipse.adt.ResourceManagerBuilder</name>
9 <arguments>
10 </arguments>
11 </buildCommand>
12 <buildCommand>
13 <name>com.android.ide.eclipse.adt.PreCompilerBuilder</name>
14 <arguments>
15 </arguments>
16 </buildCommand>
17 <buildCommand>
18 <name>org.eclipse.jdt.core.javabuilder</name>
19 <arguments>
20 </arguments>
21 </buildCommand>
22 <buildCommand>
23 <name>com.android.ide.eclipse.adt.ApkBuilder</name>
24 <arguments>
25 </arguments>
26 </buildCommand>
27 </buildSpec>
28 <natures>
29 <nature>com.android.ide.eclipse.adt.AndroidNature</nature>
30 <nature>org.eclipse.jdt.core.javanature</nature>
31 </natures>
32 </projectDescription>
0 <?xml version="1.0" encoding="utf-8"?>
1 <manifest xmlns:android="http://schemas.android.com/apk/res/android"
2 package="org.haldean.chopper.pilot.test"
3 android:versionCode="1"
4 android:versionName="1.0">
5 <application android:icon="@drawable/icon" android:label="@string/app_name">
6
7 <uses-library android:name="android.test.runner" />
8 </application>
9 <uses-sdk android:minSdkVersion="10" />
10 <instrumentation android:targetPackage="org.haldean.chopper.pilot" android:name="android.test.InstrumentationTestRunner" />
11 </manifest>
0 # This file is automatically generated by Android Tools.
1 # Do not modify this file -- YOUR CHANGES WILL BE ERASED!
2 #
3 # This file must be checked in Version Control Systems.
4 #
5 # To customize properties used by the Ant build system use,
6 # "build.properties", and override values to adapt the script to your
7 # project structure.
8
9 # Project target.
10 target=android-10
0 /* AUTO-GENERATED FILE. DO NOT MODIFY.
1 *
2 * This class was automatically generated by the
3 * aapt tool from the resource data it found. It
4 * should not be modified by hand.
5 */
6
7 package org.haldean.chopper.pilot.test;
8
9 public final class R {
10 public static final class attr {
11 }
12 public static final class drawable {
13 public static final int icon=0x7f020000;
14 }
15 public static final class layout {
16 public static final int main=0x7f030000;
17 }
18 public static final class string {
19 public static final int app_name=0x7f040001;
20 public static final int hello=0x7f040000;
21 }
22 }
0 -optimizationpasses 5
1 -dontusemixedcaseclassnames
2 -dontskipnonpubliclibraryclasses
3 -dontpreverify
4 -verbose
5 -optimizations !code/simplification/arithmetic,!field/*,!class/merging/*
6
7 -keep public class * extends android.app.Activity
8 -keep public class * extends android.app.Application
9 -keep public class * extends android.app.Service
10 -keep public class * extends android.content.BroadcastReceiver
11 -keep public class * extends android.content.ContentProvider
12 -keep public class com.android.vending.licensing.ILicensingService
13
14 -keepclasseswithmembernames class * {
15 native <methods>;
16 }
17
18 -keepclasseswithmembernames class * {
19 public <init>(android.content.Context, android.util.AttributeSet);
20 }
21
22 -keepclasseswithmembernames class * {
23 public <init>(android.content.Context, android.util.AttributeSet, int);
24 }
25
26 -keepclassmembers enum * {
27 public static **[] values();
28 public static ** valueOf(java.lang.String);
29 }
30
31 -keep class * implements android.os.Parcelable {
32 public static final android.os.Parcelable$Creator *;
33 }
0 <?xml version="1.0" encoding="utf-8"?>
1 <LinearLayout xmlns:android="http://schemas.android.com/apk/res/android"
2 android:orientation="vertical"
3 android:layout_width="fill_parent"
4 android:layout_height="fill_parent"
5 >
6 <TextView
7 android:layout_width="fill_parent"
8 android:layout_height="wrap_content"
9 android:text="@string/hello"
10 />
11 </LinearLayout>
0 <?xml version="1.0" encoding="utf-8"?>
1 <resources>
2 <string name="hello">Hello World!</string>
3 <string name="app_name">PilotTest</string>
4 </resources>
0 ../../org
1515
1616 /**
1717 * Deserializes a NavList from valid serialized String form.
18 * @param msg Serialized form of the NavList
19 * @param cs The ChopperStatus with which to construct NavDests.
18 * @param str Serialized form of the NavList
2019 * May be null, in which case null we be passed to the NavDestReader constructor.
2120 */
2221
00 package org.haldean.chopper.pilot;
11
2 import android.util.Log;
2 public interface Angler {
33
4 public class Angler implements Constants {
54 /** Maximum permissible target velocity, in m/s; larger vectors will be resized */
65 public static final double MAX_VEL = 2.0;
7
86 /** The maximum angle, in degrees, guidance will permit the chopper to have */
97 public static final double MAX_ANGLE = 10;
10
118 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);
359
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 }
10 public abstract void getAngleTarget(double[] target);
11
12 }
0 package org.haldean.chopper.pilot;
1
2 import android.util.Log;
3
4 public class AnglerImpl implements Constants, Angler {
5 private Navigation mNav;
6 private ChopperStatus mStatus;
7 private double[] mNavTarget;
8 private double[] mRelativeTarget;
9 private double[] mCurrent;
10
11 public AnglerImpl(ChopperStatus status, Navigation nav) {
12 mNav = nav;
13 mStatus = status;
14 mCurrent = new double[4];
15 mNavTarget = new double[4];
16 mRelativeTarget = new double[4];
17
18 }
19
20 /* (non-Javadoc)
21 * @see org.haldean.chopper.pilot.Angler#getAngleTarget(double[])
22 */
23 public void getAngleTarget(double[] target) {
24 if ((target == null) || (target.length < 4)) {
25 return;
26 }
27 mNav.evalNextVector(mNavTarget);
28
29 double mAzimuth = mStatus.getReadingField(AZIMUTH);
30
31 // Transform target velocity components from absolute to relative.
32 double theta = mAzimuth * Math.PI / 180.0;
33 mRelativeTarget[0] = mNavTarget[0] * Math.cos(theta) - mNavTarget[1] * Math.sin(theta);
34 mRelativeTarget[1] = mNavTarget[0] * Math.sin(theta) + mNavTarget[1] * Math.cos(theta);
35 mRelativeTarget[2] = mNavTarget[2];
36 mRelativeTarget[3] = mNavTarget[3];
37
38 //Calculate target speed; if necessary, reduce to MAX_VEL by proportionately adjusting components.
39 double myVel = 0;
40 for (int i = 0; i < 3; i++) {
41 myVel += Math.pow(mRelativeTarget[i], 2);
42 }
43 myVel = Math.sqrt(myVel);
44 if (myVel > MAX_VEL) {
45 Log.v(TAG, "guid, Reducing requested velocity");
46 double adjustment = MAX_VEL / myVel;
47 for (int i = 0; i < 3; i++) {
48 mRelativeTarget[i] *= adjustment;
49 }
50 }
51
52 // Calculate current relative velocity.
53 double mGpsBearing = mStatus.getGpsField(BEARING);
54 double phi = (mGpsBearing - mAzimuth) * Math.PI / 180.0;
55
56 double mGpsSpeed = mStatus.getGpsField(SPEED);
57 mCurrent[0] = mGpsSpeed * Math.sin(phi);
58 mCurrent[1] = mGpsSpeed * Math.cos(phi);
59 mCurrent[2] = mStatus.getGpsField(dALT);
60 mCurrent[3] = mAzimuth;
61
62 if (mCurrent[0] < mRelativeTarget[0]) {
63 target[0] += 1.0;
64 } else {
65 target[0] -= 1.0;
66 }
67
68 if (mCurrent[1] < mRelativeTarget[1]) {
69 target[1] += 1.0;
70 } else {
71 target[1] -= 1.0;
72 }
73
74 target[0] = restrainedTarget(target[0]);
75 target[1] = restrainedTarget(target[1]);
76 target[2] = mRelativeTarget[2];
77 target[3] = mRelativeTarget[3];
78 }
79
80 private static double restrainedTarget(double requested) {
81 if (requested < -MAX_ANGLE) {
82 return -MAX_ANGLE;
83 }
84 if (requested > MAX_ANGLE) {
85 return MAX_ANGLE;
86 }
87 return requested;
88 }
89 }
00 package org.haldean.chopper.pilot;
11
2 import android.content.Intent;
3 import android.os.Handler;
4 import android.os.Looper;
52 import android.os.Message;
6 import at.abraxas.amarino.Amarino;
7 import at.abraxas.amarino.AmarinoIntent;
8 import at.abraxas.amarino.AmarinoService;
93
10 /**
11 * Interfaces Pilot with Amarino
12 */
13 public class BluetoothOutput implements Constants, Runnable {
14 static final String BT_DEVICE_ADDR = "00:06:66:04:B1:BE";
15 public Handler mHandler;
16 private AmarinoService amService;
17 public static final String TAG = "BluetoothOutput";
18
19 /**
20 * Initialize, start the message handler.
21 */
22 public void run() {
23 Looper.prepare();
24 Thread.currentThread().setName("BluetoothOutput");
25 /* Intent intent = new Intent(AmarinoIntent.ACTION_SEND);
26 boolean isbound = context.bindService(intent, mConnection, Activity.BIND_AUTO_CREATE);
27 Log.i(TAG, "binding success: " + isbound); */
28 mHandler = new Handler() {
29 public void handleMessage(Message msg) {
30 switch (msg.what) {
31 case SEND_MOTOR_SPEEDS:
32 double[] newspeeds = (double[])msg.obj;
33 setMotorSpeeds(newspeeds[0], newspeeds[1], newspeeds[2], newspeeds[3]);
34 break;
35 }
36 }
37 };
38 Looper.loop();
39 }
40
41 /**
42 * Send motor speeds to arduino.
43 * @param m1 First motor's speed.
44 * @param m2 Second motor's speed.
45 * @param m3 Third motor's speed.
46 * @param m4 Fourth motor's speed.
47 */
48 private void setMotorSpeeds(double m1, double m2, double m3, double m4) {
49 sendDataToArduino(BT_DEVICE_ADDR, 'A', (int) (100 * m1));
50 sendDataToArduino(BT_DEVICE_ADDR, 'C', (int) (100 * m2));
51 sendDataToArduino(BT_DEVICE_ADDR, 'B', (int) (100 * m3));
52 sendDataToArduino(BT_DEVICE_ADDR, 'D', (int) (100 * m4));
53 }
54
55 /**
56 * Sends an int value to Arduino
57 *
58 * @param context the context
59 * @param address the Bluetooth device you want to send data to
60 * @param flag the flag Arduino has registered a function for to receive this data
61 * @param data your data you want to send
4 public interface BluetoothOutput {
5
6 public static final String TAG = "BluetoothOutputImpl";
7
8 /**
9 * Initialize, start the message handler.
6210 */
63 private void sendDataToArduino(String address, char flag, int data) {
64 Intent intent = getSendIntent(address, AmarinoIntent.INT_EXTRA, flag);
65 intent.putExtra(AmarinoIntent.EXTRA_DATA, data);
66 amService = Amarino.getAmarinoService();
67 if (amService != null) {
68 amService.onStart(intent, 0); //Pretty sure that last 0 can be anything.
69 }
70 //else
71 //Log.e(TAG, "amarino service is null");
72 }
11 public void run();
7312
74 /**
75 * Form Intent to pass to AmarinoService
76 * @param address Address of the arduino
77 * @param dataType Add this as an "extra."
78 * @param flag Which motor to change.
79 * @return
80 */
81 private static Intent getSendIntent(String address, int dataType, char flag){
82 Intent intent = new Intent(AmarinoIntent.ACTION_SEND);
83 intent.putExtra(AmarinoIntent.EXTRA_DEVICE_ADDRESS, address);
84 intent.putExtra(AmarinoIntent.EXTRA_DATA_TYPE, dataType);
85 intent.putExtra(AmarinoIntent.EXTRA_FLAG, flag);
86 return intent;
87 }
13 public void sendMessageToHandler(Message msg);
8814 }
0 package org.haldean.chopper.pilot;
1
2 import android.content.Intent;
3 import android.os.Handler;
4 import android.os.Looper;
5 import android.os.Message;
6 import at.abraxas.amarino.Amarino;
7 import at.abraxas.amarino.AmarinoIntent;
8 import at.abraxas.amarino.AmarinoService;
9
10 /**
11 * Interfaces Pilot with Amarino
12 */
13 public class BluetoothOutputImpl implements Constants, Runnable, BluetoothOutput {
14 static final String BT_DEVICE_ADDR = "00:06:66:04:B1:BE";
15 public Handler mHandler;
16 private AmarinoService amService;
17 /* (non-Javadoc)
18 * @see org.haldean.chopper.pilot.BluetoothOutput#run()
19 */
20 public void run() {
21 Looper.prepare();
22 Thread.currentThread().setName("BluetoothOutputImpl");
23 /* Intent intent = new Intent(AmarinoIntent.ACTION_SEND);
24 boolean isbound = context.bindService(intent, mConnection, Activity.BIND_AUTO_CREATE);
25 Log.i(TAG, "binding success: " + isbound); */
26 mHandler = new Handler() {
27 public void handleMessage(Message msg) {
28 switch (msg.what) {
29 case SEND_MOTOR_SPEEDS:
30 double[] newspeeds = (double[])msg.obj;
31 setMotorSpeeds(newspeeds[0], newspeeds[1], newspeeds[2], newspeeds[3]);
32 break;
33 }
34 }
35 };
36 Looper.loop();
37 }
38
39 public void sendMessageToHandler(Message msg) {
40 mHandler.sendMessage(msg);
41 }
42
43 /**
44 * Send motor speeds to arduino.
45 * @param m1 First motor's speed.
46 * @param m2 Second motor's speed.
47 * @param m3 Third motor's speed.
48 * @param m4 Fourth motor's speed.
49 */
50 private void setMotorSpeeds(double m1, double m2, double m3, double m4) {
51 sendDataToArduino(BT_DEVICE_ADDR, 'A', (int) (100 * m1));
52 sendDataToArduino(BT_DEVICE_ADDR, 'C', (int) (100 * m2));
53 sendDataToArduino(BT_DEVICE_ADDR, 'B', (int) (100 * m3));
54 sendDataToArduino(BT_DEVICE_ADDR, 'D', (int) (100 * m4));
55 }
56
57 /**
58 * Sends an int value to Arduino
59 *
60 * @param context the context
61 * @param address the Bluetooth device you want to send data to
62 * @param flag the flag Arduino has registered a function for to receive this data
63 * @param data your data you want to send
64 */
65 private void sendDataToArduino(String address, char flag, int data) {
66 Intent intent = getSendIntent(address, AmarinoIntent.INT_EXTRA, flag);
67 intent.putExtra(AmarinoIntent.EXTRA_DATA, data);
68 amService = Amarino.getAmarinoService();
69 if (amService != null) {
70 amService.onStart(intent, 0); //Pretty sure that last 0 can be anything.
71 }
72 //else
73 //Log.e(TAG, "amarino service is null");
74 }
75
76 /**
77 * Form Intent to pass to AmarinoService
78 * @param address Address of the arduino
79 * @param dataType Add this as an "extra."
80 * @param flag Which motor to change.
81 * @return
82 */
83 private static Intent getSendIntent(String address, int dataType, char flag){
84 Intent intent = new Intent(AmarinoIntent.ACTION_SEND);
85 intent.putExtra(AmarinoIntent.EXTRA_DEVICE_ADDRESS, address);
86 intent.putExtra(AmarinoIntent.EXTRA_DATA_TYPE, dataType);
87 intent.putExtra(AmarinoIntent.EXTRA_FLAG, flag);
88 return intent;
89 }
90 }
2222
2323 /** Need a permanent reference to this, to destroy it. **/
2424 private Guidance guid;
25 private ChopperStatus status;
25 private ChopperStatusImpl status;
2626
2727 /**
2828 * Holds the wakelock; needed to keep the camera preview rendering on
4242 */
4343 public void onStart() {
4444 super.onStart();
45 Amarino.connect(this, BluetoothOutput.BT_DEVICE_ADDR);
45 Amarino.connect(this, BluetoothOutputImpl.BT_DEVICE_ADDR);
4646 }
4747
4848 /**
5050 */
5151 public void onStop() {
5252 super.onStop();
53 Amarino.disconnect(this, BluetoothOutput.BT_DEVICE_ADDR);
53 Amarino.disconnect(this, BluetoothOutputImpl.BT_DEVICE_ADDR);
5454 }
5555
5656 /**
7979 previewHolder.setType(SurfaceHolder.SURFACE_TYPE_PUSH_BUFFERS);
8080
8181 Comm comm = new Comm(true);
82 status = new ChopperStatus(getApplicationContext());
82 status = new ChopperStatusImpl(getApplicationContext());
8383 StatusReporter reporter = new StatusReporter(status);
8484 MakePicture pic = null;
8585 if (telemetry) {
8686 pic = new MakePicture(previewHolder);
8787 }
88 BluetoothOutput mBTooth = new BluetoothOutput();
89 Navigation nav = new Navigation(status);
90 guid = new Guidance(status, nav, mBTooth);
88 BluetoothOutputImpl mBTooth = new BluetoothOutputImpl();
89 NavigationImpl nav = new NavigationImpl(status);
90 Angler angler = new AnglerImpl(status, nav);
91 guid = new Guidance(status, mBTooth, angler);
9192
9293 if (telemetry) {
9394 comm.setTelemetrySource(pic);
00 package org.haldean.chopper.pilot;
11
2 import java.io.BufferedWriter;
3 import java.io.File;
4 import java.io.FileWriter;
5 import java.io.IOException;
6 import java.util.LinkedList;
7 import java.util.ListIterator;
8 import java.util.concurrent.atomic.AtomicInteger;
9
10 import android.content.BroadcastReceiver;
11 import android.content.Context;
12 import android.content.Intent;
13 import android.content.IntentFilter;
142 import android.hardware.Sensor;
153 import android.hardware.SensorEvent;
16 import android.hardware.SensorEventListener;
17 import android.hardware.SensorManager;
184 import android.location.Location;
19 import android.location.LocationListener;
20 import android.location.LocationManager;
21 import android.location.LocationProvider;
22 import android.os.BatteryManager;
235 import android.os.Bundle;
24 import android.os.Environment;
25 import android.os.Looper;
26 import android.util.Log;
276
28 /**
29 * Central "storehouse" for information about the chopper's status--maintains updated sensor readings, gps readings, etc.
30 * <P>
31 * May send the following messages to registered Receivables:<br>
32 * <pre>
33 * CSYS:LOWPOWER
34 * GPS:STATUS:
35 * DISABLED
36 * ENABLED
37 * OUT.OF.SERVICE
38 * TEMPORARILY.UNAVAILABLE
39 * AVAILABLE
40 * </pre>
41 * @author Benjamin Bardin
42 * @author Will Brown
43 */
44 public final class ChopperStatus implements Runnable, SensorEventListener, Constants, LocationListener
45 {
7 public interface ChopperStatus {
8
469 /** Point of "officially" low battery */
4710 public final static int LOW_BATT = 30;
48
49 public static final String logfilename = "fastest.txt";
50 public static BufferedWriter logwriter;
51
5211 /** Tag for logging */
5312 public static final String TAG = "chopper.ChopperStatus";
54
55 /** Parameter to specify GPS minimum update distance, with the usual trade-off between accuracy and power consumption.
56 * Value of '0' means maximum accuracy. */
57 private static final float GPS_MIN_DIST = 0;
58
59 /** Parameter to specify GPS minimum update time, with the usual trade-off between accuracy and power consumption.
60 * Value of '0' means maximum accuracy. */
61 private static final long GPS_MIN_TIME = 0;
62
63 /** Used to obtain location manager. */
64 private Context mContext;
65
66 /** Current battery level. */
67 private final AtomicInteger mCurrBatt = new AtomicInteger(0);
68
69 /** Holds GPS data. */
70 private double[] mGps = new double[GPS_FIELDS]; //last available GPS readings
71
72 /** Accuracy of last GPS reading. This field is only accessed by one thread, so no lock is needed. */
73 private float mGpsAccuracy; //accuracy of said reading
74
75 /** Lock for gps extras */
76 private Object mGpsExtrasLock;
77
78 /** Locks for fields in gps[]. */
79 private Object[] mGpsLock;
80
81 /** Number of satellites used to obtain last GPS reading. */
82 private int mGpsNumSats; //number of satellites used to collect last reading
83
84 /** Timestamp of last GPS reading. */
85 private long mGpsTimeStamp; //timestamp of the reading
86
87 /** Stores the location object last returned by the GPS. */
88 private Location mLastLoc;
89
90 /** Max battery level. */
91 private final AtomicInteger mMaxBatt = new AtomicInteger(100);
9213
93 /** Stores the speeds last submitted to the motors. */
94 private double[] mMotorSpeed = new double[4];
95
96 /** Stores power levels for the motors. */
97 private double[] mMotorPower = new double[4];
98
99 /** Holds data from various sensors, like gyroscope, acceleration and magnetic flux. */
100 private double[] mReading = new double[SENSORS]; //last known data for a given sensor
14 public void close();
10115
102 /** Locks for fields in reading[]. */
103 private Object[] mReadingLock;
104
105 /** List of registered receivers */
106 private LinkedList<Receivable> mRec;
107
108 private float[] orientation = new float[3];
109 private float[] rotationMatrix = new float[9];
110 private float[] flux = new float[3];
111
112 /**
113 * Initializes the locks, registers application context for runtime use.
114 * @param mycontext Application context
115 */
116 public ChopperStatus(Context mycontext) {
117 mContext = mycontext;
118 mRec = new LinkedList<Receivable>();
119
120 //Initialize the data locks
121 mReadingLock = new Object[SENSORS];
122
123 for (int i = 0; i < SENSORS; i++) {
124 mReadingLock[i] = new Object();
125 }
126
127 mGpsLock = new Object[GPS_FIELDS];
128 for (int i = 0; i < GPS_FIELDS; i++) {
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 }
142 }
143
14416 /**
14517 * Gets the phone battery level
14618 * @return The power level, as a float between 0 and 1.
14719 */
148 public float getBatteryLevel() {
149 return (float) mCurrBatt.get() / (float) mMaxBatt.get();
150 }
151
20 public float getBatteryLevel();
21
15222 /**
15323 * Gets GPS "extras" data, through a string of the form accuracy:numberOfSatellitesInLatFix:timeStampOfLastFix
15424 * @return The data string
15525 */
156 public String getGpsExtras() {
157 String gpsData = "";
158 synchronized (mGpsExtrasLock) {
159 gpsData += mGpsAccuracy +
160 ":" + mGpsNumSats +
161 ":" + mGpsTimeStamp;
162 }
163 return gpsData;
164 }
165
26 public String getGpsExtras();
27
16628 /**
16729 * Returns the value stored at the specified GPS index. If its lock is unavailable, blocks until it is.
16830 * @param whichField The index of the desired GPS data.
16931 * @return The desired GPS data.
17032 */
171 public double getGpsField(int whichField) {
172 double myValue;
173 synchronized (mGpsLock[whichField]) {
174 myValue = mGps[whichField];
175 }
176 return myValue;
177 }
178
179 public long getGpsTimeStamp() {
180 synchronized(mGpsExtrasLock) {
181 return mGpsTimeStamp;
182 }
183 }
184
33 public double getGpsField(int whichField);
34
35 public long getGpsTimeStamp();
36
18537 /**
18638 * Returns a copy of the most recent Location object delivered by the GPS.
18739 * @return The Location, if there is one; null if the GPS has not yet obtained a fix.
18840 */
189 public Location getLastLocation() {
190 if (mLastLoc == null)
191 return null;
192 Location myLocation;
193 synchronized (mLastLoc) {
194 myLocation = new Location(mLastLoc);
195 }
196 return myLocation;
197 }
198
41 public Location getLastLocation();
42
19943 /**
20044 * Obtains the current motor speeds.
20145 * @param myValues A copy of the array containing the motor speeds. Must have length >= 4.
20246 */
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 }
210 }
211
47 public void getMotorFields(double[] myValues);
48
21249 /** Obtains the current motor power levels.
21350 * @param myValues A copy of the array containing the motor power levels. Must have length >= 4.
21451 */
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 }
222 }
223
52 public void getMotorPowerFields(double[] myValues);
53
22454 /**
22555 * Runs the Thread
22656 */
227 public void run()
228 {
229 //System.out.println("ChopperStatus run() thread ID " + getId());
230 Looper.prepare();
231 Thread.currentThread().setName("ChopperStatus");
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 }
240 /* Register to receive battery status updates */
241 BroadcastReceiver batteryInfo = new BroadcastReceiver() {
242 public void onReceive(Context context, Intent intent) {
243 String action = intent.getAction();
244 if (Intent.ACTION_BATTERY_CHANGED.equals(action)) {
245 /* int read/writes are uninterruptible, no lock needed */
246 mCurrBatt.set(intent.getIntExtra(BatteryManager.EXTRA_LEVEL, 0));
247 mMaxBatt.set(intent.getIntExtra(BatteryManager.EXTRA_SCALE, 100));
248 float batteryPercent = (float) mCurrBatt.get() * 100F / (float) mMaxBatt.get();
249 if (batteryPercent <= LOW_BATT) {
250 updateReceivers("CSYS:LOWPOWER");
251 }
252 }
253 }
254 };
255
256 /* Gets a sensor manager */
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);
262
263 //sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_GYROSCOPE), SensorManager.SENSOR_DELAY_FASTEST);
264 //sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_PRESSURE), SensorManager.SENSOR_DELAY_NORMAL);
265 //sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_PROXIMITY), 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
57 public void run();
26958
270 /* Initialize GPS reading: */
271 LocationManager LocMan = (LocationManager) mContext.getSystemService(Context.LOCATION_SERVICE);
272 LocMan.requestLocationUpdates(LocationManager.GPS_PROVIDER, GPS_MIN_TIME, GPS_MIN_DIST, this);
273
274 mContext.registerReceiver(batteryInfo, new IntentFilter(Intent.ACTION_BATTERY_CHANGED));
275
276 Looper.loop();
277 }
278
27959 /**
28060 * Returns the reading specified by the supplied index, if its lock is available. Otherwise, throws an exception.
28161 * @param whichField The index of the desired reading.
28262 * @return The desired reading.
28363 */
284 public double getReadingField(int whichField) {
285 double myValue;
286 synchronized (mReadingLock[whichField]) {
287 myValue = mReading[whichField];
288 }
289 return myValue;
290 }
291
64 public double getReadingField(int whichField);
65
29266 /**
29367 * Registers a change in sensor accuracy. Not used in this application.
29468 * @param sensor Sensor registering change in accuracy.
29569 * @param newaccuracy New accuracy value.
29670 */
297 public void onAccuracyChanged(Sensor sensor, int newaccuracy) {
298 }
299
71 public void onAccuracyChanged(Sensor sensor, int newaccuracy);
72
30073 /**
30174 * Changes the value of the local GPS fields based on the data contained in a new GPS fix.
30275 * @param loc New GPS fix
30376 */
304 public void onLocationChanged(Location loc) {
305 if (loc != null && mGps != null) {
306 if (!loc.hasAltitude()) {
307 // loc.setAltitude(300.0);
308 Log.w(TAG, "No altitude fix");
309 }
310 double newalt = loc.getAltitude();
311 Log.i(TAG, "new altitude: " + newalt);
312 /* Vertical velocity does not update until vertical position does; prevents false conclusions that vertical velocity == 0 */
313 double oldAlt = getGpsField(ALTITUDE);
314 if (newalt != oldAlt) {
315 long timeElapsed;
316 synchronized (mGpsExtrasLock) {
317 timeElapsed = mGpsTimeStamp - loc.getTime();
318 }
319 double newdalt = ((newalt - oldAlt) / (double) timeElapsed) * 1000.0;
320 setGpsField(dALT, newdalt);
321 Log.i(TAG, "new dalt: " + newdalt);
322 setGpsField(ALTITUDE, newalt);
323 }
324 setGpsField(BEARING, loc.getBearing());
325 setGpsField(LONG, loc.getLongitude());
326 setGpsField(LAT, loc.getLatitude());
327 setGpsField(SPEED, loc.getSpeed());
328
329 synchronized (mGpsExtrasLock) {
330 mGpsAccuracy = loc.getAccuracy();
331 mGpsTimeStamp = loc.getTime();
332 if (loc.getExtras() != null)
333 mGpsNumSats = loc.getExtras().getInt("satellites");
334 }
335 if (mLastLoc != null) {
336 synchronized (mLastLoc) {
337 mLastLoc = loc;
338 }
339 }
340 else {
341 mLastLoc = loc;
342 }
343 }
344 }
345
77 public void onLocationChanged(Location loc);
78
34679 /**
34780 * Informs all receivers that a GPS provider is disabled.
34881 * @param provider The name of the provider that has been disabled
34982 */
350 public void onProviderDisabled(String provider) {
351 System.out.println("GPS disabled");
352 updateReceivers("GPS:STATUS:DISABLED");
353 }
354
83 public void onProviderDisabled(String provider);
84
35585 /**
35686 * Informs all receivers that a GPS provider is enabled.
35787 * @param provider The name of the provider that has been enabled
35888 */
359 public void onProviderEnabled(String provider) {
360 System.out.println("GPS enabled");
361 updateReceivers("GPS:STATUS:ENABLED.");
362 }
363
89 public void onProviderEnabled(String provider);
90
36491 /**
36592 * Registers a change in sensor data.
36693 * @param event Object containing new data--sensor type, accuracy, timestamp and value.
36794 */
368 public void onSensorChanged(SensorEvent event) {
369 long time = System.currentTimeMillis();
370 int type = event.sensor.getType();
371 switch (type) {
372 case Sensor.TYPE_ACCELEROMETER:
373 setReadingField(X_ACCEL, event.values[0]);
374 setReadingField(Y_ACCEL, event.values[1]);
375 setReadingField(Z_ACCEL, event.values[2]);
376 break;
377 case Sensor.TYPE_LIGHT:
378 setReadingField(LIGHT, event.values[0]);
379 break;
380 case Sensor.TYPE_MAGNETIC_FIELD:
381 System.arraycopy(event.values, 0, flux, 0, 3);
382 setReadingField(X_FLUX, event.values[0]);
383 setReadingField(Y_FLUX, event.values[1]);
384 setReadingField(Z_FLUX, event.values[2]);
385 break;
386 case Sensor.TYPE_PRESSURE:
387 setReadingField(PRESSURE, event.values[0]);
388 break;
389 case Sensor.TYPE_PROXIMITY:
390 setReadingField(PROXIMITY, event.values[0]);
391 break;
392 case Sensor.TYPE_TEMPERATURE:
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);
423 break;
424 }
425 }
426
95 public void onSensorChanged(SensorEvent event);
96
42797 /**
42898 * Informs all receivers that a GPS provider's status has changed.
42999 * @param provider The name of the provider whose status has changed
430100 * @param status The new status of the provider
431101 * @param extras Provider-specific extra data
432102 */
433 public void onStatusChanged(String provider, int status, Bundle extras) {
434 System.out.println("GPS status changed " + status);
435 switch (status) {
436 case LocationProvider.OUT_OF_SERVICE:
437 updateReceivers("GPS:STATUS:OUT.OF.SERVICE");
438 break;
439 case LocationProvider.TEMPORARILY_UNAVAILABLE:
440 updateReceivers("GPS:STATUS:TEMPORARILY.UNAVAILABLE");
441 break;
442 case LocationProvider.AVAILABLE:
443 updateReceivers("GPS:STATUS:AVAILABLE");
444 break;
445 }
446 }
447
103 public void onStatusChanged(String provider, int status, Bundle extras);
104
448105 /**
449 * Registers a receivable for certain status-related updates, especially a Navigation object.
106 * Registers a receivable for certain status-related updates, especially a NavigationImpl object.
450107 * @param rec The receivable to register.
451 * @see Navigation Navigation
108 * @see NavigationImpl NavigationImpl
452109 */
453 public void registerReceiver(Receivable rec) {
454 synchronized (mRec) {
455 mRec.add(rec);
456 }
457 }
110 public void registerReceiver(Receivable rec);
458111
459112 /**
460113 * Sets the motor speed data to the supplied array.
461114 * @param mySpeeds The data to which the motor speeds should be set.
462115 */
463 protected void setMotorFields(final double[] mySpeeds) {
464 if (mySpeeds.length < 4)
465 return;
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");
474 }
116 public void setMotorFields(double[] mySpeeds);
475117
476118 /**
477119 * Sets the motor power levels to the supplied array.
478120 * @param myPowers The data to which the motor power levels should be set.
479121 */
480 protected void setMotorPowerFields(final double[] myPowers) {
481 if (myPowers.length < 4)
482 return;
483 synchronized (mMotorPower) {
484 System.arraycopy(myPowers, 0, mMotorPower, 0, 4);
485 }
486 }
487
488 /**
489 * Writes the supplied GPS value at the specified GPS field.
490 * @param whichField The index of the GPS data to store.
491 * @param value The GPS data.
492 */
493 private void setGpsField(final int whichField, final double value) {
494 synchronized (mGpsLock[whichField]) {
495 mGps[whichField] = value;
496 }
497 }
498
499
500 /**
501 * Writes the supplied reading at the specified field.
502 * @param whichField The index of the reading to store.
503 * @param value The reading.
504 */
505 private void setReadingField(final int whichField, final double value) {
506 synchronized (mReadingLock[whichField]) {
507 mReading[whichField] = value;
508 }
509 }
510
511 /** Updates all registered receivers with the specified String */
512 private void updateReceivers(String str) {
513 synchronized (mRec) {
514 ListIterator<Receivable> myList = mRec.listIterator();
515 while (myList.hasNext()) {
516 myList.next().receiveMessage(str, null);
517 }
518 }
519 }
122 public void setMotorPowerFields(final double[] myPowers);
123
520124 }
0 package org.haldean.chopper.pilot;
1
2 import java.io.BufferedWriter;
3 import java.io.File;
4 import java.io.FileWriter;
5 import java.io.IOException;
6 import java.util.LinkedList;
7 import java.util.List;
8 import java.util.ListIterator;
9 import java.util.concurrent.atomic.AtomicInteger;
10
11 import android.content.BroadcastReceiver;
12 import android.content.Context;
13 import android.content.Intent;
14 import android.content.IntentFilter;
15 import android.hardware.Sensor;
16 import android.hardware.SensorEvent;
17 import android.hardware.SensorEventListener;
18 import android.hardware.SensorManager;
19 import android.location.Location;
20 import android.location.LocationListener;
21 import android.location.LocationManager;
22 import android.location.LocationProvider;
23 import android.os.BatteryManager;
24 import android.os.Bundle;
25 import android.os.Environment;
26 import android.os.Looper;
27 import android.util.Log;
28
29 /**
30 * Central "storehouse" for information about the chopper's status--maintains updated sensor readings, gps readings, etc.
31 * <P>
32 * May send the following messages to registered Receivables:<br>
33 * <pre>
34 * CSYS:LOWPOWER
35 * GPS:STATUS:
36 * DISABLED
37 * ENABLED
38 * OUT.OF.SERVICE
39 * TEMPORARILY.UNAVAILABLE
40 * AVAILABLE
41 * </pre>
42 * @author Benjamin Bardin
43 * @author Will Brown
44 */
45 public final class ChopperStatusImpl implements Runnable, SensorEventListener, Constants, LocationListener, ChopperStatus {
46 public static final String logfilename = "sensors.txt";
47 public static BufferedWriter logwriter;
48
49 /** Parameter to specify GPS minimum update distance, with the usual trade-off between accuracy and power consumption.
50 * Value of '0' means maximum accuracy. */
51 private static final float GPS_MIN_DIST = 0;
52
53 /** Parameter to specify GPS minimum update time, with the usual trade-off between accuracy and power consumption.
54 * Value of '0' means maximum accuracy. */
55 private static final long GPS_MIN_TIME = 0;
56
57 /** Used to obtain location manager. */
58 private Context mContext;
59
60 /** Current battery level. */
61 private final AtomicInteger mCurrBatt = new AtomicInteger(0);
62
63 /** Holds GPS data. */
64 private double[] mGps = new double[GPS_FIELDS]; //last available GPS readings
65
66 /** Accuracy of last GPS reading. This field is only accessed by one thread, so no lock is needed. */
67 private float mGpsAccuracy; //accuracy of said reading
68
69 /** Lock for gps extras */
70 private Object mGpsExtrasLock;
71
72 /** Locks for fields in gps[]. */
73 private Object[] mGpsLock;
74
75 /** Number of satellites used to obtain last GPS reading. */
76 private int mGpsNumSats; //number of satellites used to collect last reading
77
78 /** Timestamp of last GPS reading. */
79 private long mGpsTimeStamp; //timestamp of the reading
80
81 /** Stores the location object last returned by the GPS. */
82 private Location mLastLoc;
83
84 /** Max battery level. */
85 private final AtomicInteger mMaxBatt = new AtomicInteger(100);
86
87 /** Stores the speeds last submitted to the motors. */
88 private double[] mMotorSpeed = new double[4];
89
90 /** Stores power levels for the motors. */
91 private double[] mMotorPower = new double[4];
92
93 /** Holds data from various sensors, like gyroscope, acceleration and magnetic flux. */
94 private double[] mReading = new double[SENSORS]; //last known data for a given sensor
95
96 /** Locks for fields in reading[]. */
97 private Object[] mReadingLock;
98
99 /** List of registered receivers */
100 private LinkedList<Receivable> mRec;
101
102 private float[] orientation = new float[3];
103 private float[] rotationMatrix = new float[9];
104 private float[] flux = new float[3];
105
106 private long grav_time;
107
108 /**
109 * Initializes the locks, registers application context for runtime use.
110 * @param mycontext Application context
111 */
112 public ChopperStatusImpl(Context mycontext) {
113 mContext = mycontext;
114 mRec = new LinkedList<Receivable>();
115
116 //Initialize the data locks
117 mReadingLock = new Object[SENSORS];
118
119 for (int i = 0; i < SENSORS; i++) {
120 mReadingLock[i] = new Object();
121 }
122
123 mGpsLock = new Object[GPS_FIELDS];
124 for (int i = 0; i < GPS_FIELDS; i++) {
125 mGpsLock[i] = new Object();
126 }
127 mGpsExtrasLock = new Object();
128 }
129
130 /* (non-Javadoc)
131 * @see org.haldean.chopper.pilot.ChopperStatus#close()
132 */
133 public void close() {
134 if (logwriter != null) {
135 try {
136 logwriter.close();
137 } catch (IOException e) {
138 Log.e(TAG, "Canno close logfile");
139 }
140 }
141 }
142
143 /* (non-Javadoc)
144 * @see org.haldean.chopper.pilot.ChopperStatus#getBatteryLevel()
145 */
146 public float getBatteryLevel() {
147 return (float) mCurrBatt.get() / (float) mMaxBatt.get();
148 }
149
150 /* (non-Javadoc)
151 * @see org.haldean.chopper.pilot.ChopperStatus#getGpsExtras()
152 */
153 public String getGpsExtras() {
154 String gpsData = "";
155 synchronized (mGpsExtrasLock) {
156 gpsData += mGpsAccuracy +
157 ":" + mGpsNumSats +
158 ":" + mGpsTimeStamp;
159 }
160 return gpsData;
161 }
162
163 /* (non-Javadoc)
164 * @see org.haldean.chopper.pilot.ChopperStatus#getGpsField(int)
165 */
166 public double getGpsField(int whichField) {
167 double myValue;
168 synchronized (mGpsLock[whichField]) {
169 myValue = mGps[whichField];
170 }
171 return myValue;
172 }
173
174 /* (non-Javadoc)
175 * @see org.haldean.chopper.pilot.ChopperStatus#getGpsTimeStamp()
176 */
177 public long getGpsTimeStamp() {
178 synchronized(mGpsExtrasLock) {
179 return mGpsTimeStamp;
180 }
181 }
182
183 /* (non-Javadoc)
184 * @see org.haldean.chopper.pilot.ChopperStatus#getLastLocation()
185 */
186 public Location getLastLocation() {
187 if (mLastLoc == null)
188 return null;
189 Location myLocation;
190 synchronized (mLastLoc) {
191 myLocation = new Location(mLastLoc);
192 }
193 return myLocation;
194 }
195
196 /* (non-Javadoc)
197 * @see org.haldean.chopper.pilot.ChopperStatus#getMotorFields(double[])
198 */
199 public void getMotorFields (double[] myValues){
200 if (myValues.length < 4) {
201 throw new IllegalArgumentException();
202 }
203 synchronized (mMotorSpeed) {
204 System.arraycopy(mMotorSpeed, 0, myValues, 0, 4);
205 }
206 }
207
208 /* (non-Javadoc)
209 * @see org.haldean.chopper.pilot.ChopperStatus#getMotorPowerFields(double[])
210 */
211 public void getMotorPowerFields(double[] myValues) {
212 if (myValues.length < 4) {
213 throw new IllegalArgumentException();
214 }
215 synchronized (mMotorPower) {
216 System.arraycopy(mMotorPower, 0, myValues, 0, 4);
217 }
218 }
219
220 /* (non-Javadoc)
221 * @see org.haldean.chopper.pilot.ChopperStatus#run()
222 */
223 public void run()
224 {
225 //System.out.println("ChopperStatusImpl run() thread ID " + getId());
226 Looper.prepare();
227 Thread.currentThread().setName("ChopperStatusImpl");
228 File root = Environment.getExternalStorageDirectory();
229 if (root == null) Log.e(TAG, "No root directory found");
230 try {
231 logwriter = new BufferedWriter(new FileWriter(root + "/chopper/" + logfilename, false));
232 } catch (IOException e) {
233 Log.e(TAG, "No ChopperStatusImpl logfile");
234 e.printStackTrace();
235 }
236 /* Register to receive battery status updates */
237 BroadcastReceiver batteryInfo = new BroadcastReceiver() {
238 public void onReceive(Context context, Intent intent) {
239 String action = intent.getAction();
240 if (Intent.ACTION_BATTERY_CHANGED.equals(action)) {
241 /* int read/writes are uninterruptible, no lock needed */
242 mCurrBatt.set(intent.getIntExtra(BatteryManager.EXTRA_LEVEL, 0));
243 mMaxBatt.set(intent.getIntExtra(BatteryManager.EXTRA_SCALE, 100));
244 float batteryPercent = (float) mCurrBatt.get() * 100F / (float) mMaxBatt.get();
245 if (batteryPercent <= LOW_BATT) {
246 updateReceivers("CSYS:LOWPOWER");
247 }
248 }
249 }
250 };
251
252 /* Gets a sensor manager */
253 final SensorManager sensors = (SensorManager) mContext.getSystemService(Context.SENSOR_SERVICE);
254 List<Sensor> sensorList = sensors.getSensorList(Sensor.TYPE_ALL);
255 for (Sensor sensor : sensorList) {
256 try {
257 if (logwriter != null) {
258 logwriter.write("\nName : " + sensor.getName() + "\n");
259 logwriter.write("Type: : " + sensor.getType() + "\n");
260 logwriter.write("Max Range : " + sensor.getMaximumRange() + "\n");
261 logwriter.write("Min delay : " + sensor.getMinDelay() + "\n");
262 logwriter.write("Power : " + sensor.getPower() + "\n");
263 logwriter.write("Resolution: " + sensor.getResolution() + "\n");
264 logwriter.write("Vendor : " + sensor.getVendor() + "\n");
265 logwriter.write("Version : " + sensor.getVersion() + "\n\n");
266 logwriter.flush();
267 }
268 } catch (IOException e) {
269 e.printStackTrace();
270 }
271 }
272 /* Registers this class as a sensor listener for every necessary sensor. */
273 //sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_ACCELEROMETER), SensorManager.SENSOR_DELAY_FASTEST);
274 //sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_LIGHT), SensorManager.SENSOR_DELAY_NORMAL);
275 //sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD), SensorManager.SENSOR_DELAY_FASTEST);
276
277 //sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_GYROSCOPE), SensorManager.SENSOR_DELAY_FASTEST);
278 //sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_PRESSURE), SensorManager.SENSOR_DELAY_NORMAL);
279 //sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_PROXIMITY), SensorManager.SENSOR_DELAY_NORMAL);
280 //sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_TEMPERATURE), SensorManager.SENSOR_DELAY_NORMAL);
281 //sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_GRAVITY), SensorManager.SENSOR_DELAY_FASTEST);
282 sensors.registerListener(this, sensors.getDefaultSensor(Sensor.TYPE_ROTATION_VECTOR), SensorManager.SENSOR_DELAY_GAME);
283 Thread.currentThread().setPriority(Thread.MAX_PRIORITY);
284
285
286 /* Initialize GPS reading: */
287 LocationManager LocMan = (LocationManager) mContext.getSystemService(Context.LOCATION_SERVICE);
288 LocMan.requestLocationUpdates(LocationManager.GPS_PROVIDER, GPS_MIN_TIME, GPS_MIN_DIST, this);
289
290 mContext.registerReceiver(batteryInfo, new IntentFilter(Intent.ACTION_BATTERY_CHANGED));
291
292 Looper.loop();
293 }
294
295 /* (non-Javadoc)
296 * @see org.haldean.chopper.pilot.ChopperStatus#getReadingField(int)
297 */
298 public double getReadingField(int whichField) {
299 double myValue;
300 synchronized (mReadingLock[whichField]) {
301 myValue = mReading[whichField];
302 }
303 return myValue;
304 }
305
306 /* (non-Javadoc)
307 * @see org.haldean.chopper.pilot.ChopperStatus#onAccuracyChanged(android.hardware.Sensor, int)
308 */
309 public void onAccuracyChanged(Sensor sensor, int newaccuracy) {
310 }
311
312 /* (non-Javadoc)
313 * @see org.haldean.chopper.pilot.ChopperStatus#onLocationChanged(android.location.Location)
314 */
315 public void onLocationChanged(Location loc) {
316 if (loc != null && mGps != null) {
317 if (!loc.hasAltitude()) {
318 // loc.setAltitude(300.0);
319 Log.w(TAG, "No altitude fix");
320 }
321 double newalt = loc.getAltitude();
322 Log.i(TAG, "new altitude: " + newalt);
323 /* Vertical velocity does not update until vertical position does; prevents false conclusions that vertical velocity == 0 */
324 double oldAlt = getGpsField(ALTITUDE);
325 if (newalt != oldAlt) {
326 long timeElapsed;
327 synchronized (mGpsExtrasLock) {
328 timeElapsed = mGpsTimeStamp - loc.getTime();
329 }
330 double newdalt = ((newalt - oldAlt) / (double) timeElapsed) * 1000.0;
331 setGpsField(dALT, newdalt);
332 Log.i(TAG, "new dalt: " + newdalt);
333 setGpsField(ALTITUDE, newalt);
334 }
335 setGpsField(BEARING, loc.getBearing());
336 setGpsField(LONG, loc.getLongitude());
337 setGpsField(LAT, loc.getLatitude());
338 setGpsField(SPEED, loc.getSpeed());
339
340 synchronized (mGpsExtrasLock) {
341 mGpsAccuracy = loc.getAccuracy();
342 mGpsTimeStamp = loc.getTime();
343 if (loc.getExtras() != null)
344 mGpsNumSats = loc.getExtras().getInt("satellites");
345 }
346 if (mLastLoc != null) {
347 synchronized (mLastLoc) {
348 mLastLoc = loc;
349 }
350 }
351 else {
352 mLastLoc = loc;
353 }
354 }
355 }
356
357 /* (non-Javadoc)
358 * @see org.haldean.chopper.pilot.ChopperStatus#onProviderDisabled(java.lang.String)
359 */
360 public void onProviderDisabled(String provider) {
361 System.out.println("GPS disabled");
362 updateReceivers("GPS:STATUS:DISABLED");
363 }
364
365 /* (non-Javadoc)
366 * @see org.haldean.chopper.pilot.ChopperStatus#onProviderEnabled(java.lang.String)
367 */
368 public void onProviderEnabled(String provider) {
369 System.out.println("GPS enabled");
370 updateReceivers("GPS:STATUS:ENABLED.");
371 }
372
373 /* (non-Javadoc)
374 * @see org.haldean.chopper.pilot.ChopperStatus#onSensorChanged(android.hardware.SensorEvent)
375 */
376 public void onSensorChanged(SensorEvent event) {
377 long time = event.timestamp;
378 int type = event.sensor.getType();
379 switch (type) {
380 case Sensor.TYPE_ACCELEROMETER:
381 setReadingField(X_ACCEL, event.values[0]);
382 setReadingField(Y_ACCEL, event.values[1]);
383 setReadingField(Z_ACCEL, event.values[2]);
384 break;
385 case Sensor.TYPE_LIGHT:
386 setReadingField(LIGHT, event.values[0]);
387 break;
388 case Sensor.TYPE_MAGNETIC_FIELD:
389 System.arraycopy(event.values, 0, flux, 0, 3);
390 setReadingField(X_FLUX, event.values[0]);
391 setReadingField(Y_FLUX, event.values[1]);
392 setReadingField(Z_FLUX, event.values[2]);
393 break;
394 case Sensor.TYPE_PRESSURE:
395 setReadingField(PRESSURE, event.values[0]);
396 break;
397 case Sensor.TYPE_PROXIMITY:
398 setReadingField(PROXIMITY, event.values[0]);
399 break;
400 case Sensor.TYPE_TEMPERATURE:
401 setReadingField(TEMPERATURE, event.values[0]);
402 break;
403 case Sensor.TYPE_GRAVITY:
404
405 /*Log.v(TAG, "");
406 Log.v(TAG, "" + event.values[0]);
407 Log.v(TAG, "" + event.values[1]);
408 Log.v(TAG, "" + event.values[2]);
409 Log.v(TAG, ""); */
410 SensorManager.getRotationMatrix(rotationMatrix,
411 null,
412 event.values,
413 flux);
414 /*for (int i = 0; i < 9; i++) {
415 Log.v(TAG, "" + rotationMatrix[i]);
416 }*/
417 SensorManager.getOrientation(rotationMatrix, orientation);
418 //Log.v(TAG, "" + orientation[i]);
419 setReadingField(AZIMUTH, orientation[0] * 180.0 / Math.PI);
420 setReadingField(PITCH, orientation[1] * 180.0 / Math.PI);
421 setReadingField(ROLL, orientation[2] * -180.0 / Math.PI);
422 break;
423 case Sensor.TYPE_ROTATION_VECTOR:
424 Log.v(TAG, "my grav time: " + ((time - grav_time)/1000000));
425 grav_time = time;
426 String timestring = Long.toString(time);
427 try {
428 if (logwriter != null) {
429 logwriter.write(timestring + "\n");
430 logwriter.flush();
431 }
432 } catch (IOException e) {
433 // Do nothing.
434 }
435
436 SensorManager.getRotationMatrixFromVector(rotationMatrix, event.values);
437 SensorManager.getOrientation(rotationMatrix, orientation);
438 //Log.v(TAG, "" + orientation[i]);
439 setReadingField(AZIMUTH, orientation[0] * 180.0 / Math.PI);
440 setReadingField(PITCH, orientation[1] * 180.0 / Math.PI);
441 setReadingField(ROLL, orientation[2] * -180.0 / Math.PI);
442 break;
443 }
444 }
445
446 /* (non-Javadoc)
447 * @see org.haldean.chopper.pilot.ChopperStatus#onStatusChanged(java.lang.String, int, android.os.Bundle)
448 */
449 public void onStatusChanged(String provider, int status, Bundle extras) {
450 System.out.println("GPS status changed " + status);
451 switch (status) {
452 case LocationProvider.OUT_OF_SERVICE:
453 updateReceivers("GPS:STATUS:OUT.OF.SERVICE");
454 break;
455 case LocationProvider.TEMPORARILY_UNAVAILABLE:
456 updateReceivers("GPS:STATUS:TEMPORARILY.UNAVAILABLE");
457 break;
458 case LocationProvider.AVAILABLE:
459 updateReceivers("GPS:STATUS:AVAILABLE");
460 break;
461 }
462 }
463
464 /* (non-Javadoc)
465 * @see org.haldean.chopper.pilot.ChopperStatus#registerReceiver(org.haldean.chopper.pilot.Receivable)
466 */
467 public void registerReceiver(Receivable rec) {
468 synchronized (mRec) {
469 mRec.add(rec);
470 }
471 }
472
473 /**
474 * Sets the motor speed data to the supplied array.
475 * @param mySpeeds The data to which the motor speeds should be set.
476 */
477 public void setMotorFields(final double[] mySpeeds) {
478 if (mySpeeds.length < 4)
479 return;
480 synchronized (mMotorSpeed) {
481 System.arraycopy(mySpeeds, 0, mMotorSpeed, 0, 4);
482 /*Log.v(TAG, "vector " + mMotorSpeed[0] + ", "
483 + mMotorSpeed[1] + ", "
484 + mMotorSpeed[2] + ", "
485 + mMotorSpeed[3]);*/
486 }
487 //Log.v(TAG, "Done changing motorspeeds");
488 }
489
490 /**
491 * Sets the motor power levels to the supplied array.
492 * @param myPowers The data to which the motor power levels should be set.
493 */
494 public void setMotorPowerFields(final double[] myPowers) {
495 if (myPowers.length < 4)
496 return;
497 synchronized (mMotorPower) {
498 System.arraycopy(myPowers, 0, mMotorPower, 0, 4);
499 }
500 }
501
502 /**
503 * Writes the supplied GPS value at the specified GPS field.
504 * @param whichField The index of the GPS data to store.
505 * @param value The GPS data.
506 */
507 private void setGpsField(final int whichField, final double value) {
508 synchronized (mGpsLock[whichField]) {
509 mGps[whichField] = value;
510 }
511 }
512
513
514 /**
515 * Writes the supplied reading at the specified field.
516 * @param whichField The index of the reading to store.
517 * @param value The reading.
518 */
519 private void setReadingField(final int whichField, final double value) {
520 synchronized (mReadingLock[whichField]) {
521 mReading[whichField] = value;
522 }
523 }
524
525 /** Updates all registered receivers with the specified String */
526 private void updateReceivers(String str) {
527 synchronized (mRec) {
528 ListIterator<Receivable> myList = mRec.listIterator();
529 while (myList.hasNext()) {
530 myList.next().receiveMessage(str, null);
531 }
532 }
533 }
534 }
207207 }
208208
209209 /**
210 * Registers a receiver to receive a category of Comm updates, especially MakePicture and Navigation objects.
210 * Registers a receiver to receive a category of Comm updates, especially MakePicture and NavigationImpl objects.
211211 * @param msgType The type of updates for which to register.
212212 * @param receiver The receiver to register.
213213 * @see MakePicture MakePicture
214 * @see Navigation Navigation
214 * @see NavigationImpl NavigationImpl
215215 */
216216 public void registerReceiver(int msgType, Receivable receiver) {
217217 LinkedList<Receivable> myList = mMsgTypes.get(msgType);
2626 /** Message for MakePicture handler, instructing to send a list of available preview sizes to the server */
2727 public static final int SEND_SIZES = 105;
2828
29 /** Message for Navigation handler, instructing it to calculate the desired velocity vector */
29 /** Message for NavigationImpl handler, instructing it to calculate the desired velocity vector */
3030 public static final int EVAL_NAV = 106;
3131
3232 /** Message for Comm handler, instructing it to attempt to make a data (telemetry) connection with the server */
4141 /** Message for Guidance handler, passing a manual vector for motorspeeds **/
4242 public static final int NEW_GUID_VECTOR = 110;
4343
44 /** Message for BluetoothOutput, passing a vector to transmit to motors. **/
44 /** Message for BluetoothOutputImpl, passing a vector to transmit to motors. **/
4545 public static final int SEND_MOTOR_SPEEDS = 111;
4646
4747 /** Message for Guidance handler, instructing it to obtain PID values **/
111111 /**
112112 * Constructs a Guidance object
113113 * @param status The source status information.
114 * @param nav The source of navigation target information.
115 */
116 public Guidance(ChopperStatus status, Navigation nav, BluetoothOutput bT) {
117 if (status == null | nav == null) {
114 */
115 public Guidance(ChopperStatus status, BluetoothOutput bT, Angler angler) {
116 if (status == null || angler == null) {
118117 throw new NullPointerException();
119118 }
120119 mStatus = status;
121120 mRec = new LinkedList<Receivable>();
122121 mBt = bT;
123 mAngler = new Angler(status, nav);
122 mAngler = angler;
123 createHandler(); // Overridden at thread start. Here for testing.
124124
125125 //Temporary: need real tuning values at some point. Crap.
126126 for (int i = 0; i < 3; i++)
127 mGain[i][0] = .1;
128 mGain[3][0] = 0;
127 mGain[i][0] = .01;
128 mGain[3][0] = .01;
129129
130130 try {
131131 if (mEnableLogging)
161161 }
162162 }
163163
164 /**
165 * Starts the guidance thread
166 */
167 public void run() {
168 Looper.prepare();
169 Thread.currentThread().setName("Guidance");
170 Thread.currentThread().setPriority(Thread.MAX_PRIORITY);
164 private void createHandler() {
171165 mHandler = new Handler() {
172166 public void handleMessage(Message msg) {
173167 switch (msg.what) {
203197 }
204198 }
205199 };
200 }
201
202 /**
203 * Starts the guidance thread
204 */
205 public void run() {
206 Looper.prepare();
207 Thread.currentThread().setName("Guidance");
208 Thread.currentThread().setPriority(Thread.MAX_PRIORITY);
209 createHandler();
206210 //mHandler.sendEmptyMessage(EVAL_MOTOR_SPEED);
207211 receiveMessage("DIRECT:0:0:0:0", null);
208212 Looper.loop();
292296 }
293297
294298 /** Core of the class; calculates new motor speeds based on status */
295 private void reviseMotorSpeed() {
299 public void reviseMotorSpeed() {
296300 mHandler.removeMessages(EVAL_MOTOR_SPEED);
297301 long starttime = System.currentTimeMillis();
298 updateAngleTarget();
299
302
300303 //Retrieve current orientation.
301304
302305 mAzimuth = mStatus.getReadingField(AZIMUTH);
305308
306309 double[] errors = new double[4];
307310 synchronized (mAngleTarget) {
311 logArray("mAngleTarget", mAngleTarget);
308312 errors[0] = mAngleTarget[0] - mRollDeg;
309313 errors[1] = mAngleTarget[1] - mPitchDeg;
310314 errors[2] = mAngleTarget[2] - mStatus.getGpsField(dALT);
311315 errors[3] = mAngleTarget[3] - mAzimuth;
316 logArray("errors", errors);
312317 }
313318
314319 String errs = "errors: ";
329334
330335
331336 //Calculate derivative errors.
332 mErrors[i][2] = (err - mErrors[i][0]) * 1000.0 / (starttime - mLastUpdate);
337 long timeInterval = starttime - mLastUpdate;
338 if (timeInterval != 0) {
339 mErrors[i][2] = (err - mErrors[i][0]) * 1000.0 / (starttime - mLastUpdate);
340 } else {
341 mErrors[i][2] = 0.0;
342 }
333343
334344
335345 //Mark proportional error
370380
371381 //Send motor values to motors here:
372382 updateMotors();
373
383 updateAngleTarget();
374384 //Log.v(TAG, "motors: " + mMotorSpeed[0] + ", " + mMotorSpeed[1] + ", " + mMotorSpeed[2] + ", " + mMotorSpeed[3]);
375385 //Sleep a while
376386 long timetonext = (1000 / PIDREPS) - (System.currentTimeMillis() - starttime);
387397 }
388398
389399 private void controlVarsToMotorSpeeds() {
400 logArray("controlVars", mControlVars);
390401 double pitchrad = mPitchDeg * Math.PI / 180.0;
391402 double rollrad = mRollDeg * Math.PI / 180.0;
392403 double gradient = Math.sqrt(
399410
400411 double x = mControlVars[0];
401412 double y = mControlVars[1];
402 double z = mControlVars[2] / cosGrad;
413 double z;
414 if (cosGrad != 0) {
415 z = mControlVars[2] / cosGrad;
416 } else {
417 z = mControlVars[2];
418 }
403419 double t = mControlVars[3];
404420
405421 mMotorSpeed[0] = Math.sqrt(constrainValue(t - 2*y + z, 0, 1));
406422 mMotorSpeed[1] = Math.sqrt(constrainValue(t + 2*y + z, 0, 1));
407423 mMotorSpeed[2] = Math.sqrt(constrainValue(-t - 2*x + z, 0, 1));
408 mMotorSpeed[3] = Math.sqrt(constrainValue(-t + 2*x + z, 0 ,1));
424 mMotorSpeed[3] = Math.sqrt(constrainValue(-t + 2*x + z, 0, 1));
409425 }
410426
411427 private void updateAngleTarget() {
433449 }
434450
435451 /**
436 * Write motor values to ChopperStatus, BluetoothOutput, logfile.
452 * Write motor values to ChopperStatus, BluetoothOutputImpl, logfile.
437453 */
438454 private void updateMotors() {
439455 //Pass filtered values to ChopperStatus.
449465 //Log.e(TAG, "Cannot write to logfile");
450466 }
451467 //Pass motor values to motor controller!
452 Message msg = Message.obtain(mBt.mHandler, SEND_MOTOR_SPEEDS, mMotorSpeed);
453 msg.sendToTarget();
468 Message msg = Message.obtain();
469 msg.what = SEND_MOTOR_SPEEDS;
470 msg.obj = mMotorSpeed;
471 mBt.sendMessageToHandler(msg);
454472 //Log.i(TAG, "Guidance sending message.");
455473
456474 }
467485 }
468486 }
469487 }
488
489 private static void logArray(String id, double[] array) {
490 String output = id + ": ";
491 for (int i = 0; i < array.length; i++) {
492 output += array[i] + ", ";
493 }
494 Log.v(TAG, output);
495 }
470496 }
471497
00 package org.haldean.chopper.pilot;
11
2 import java.util.LinkedList;
3 import java.util.ListIterator;
4 import java.util.Vector;
5 import java.util.concurrent.atomic.AtomicInteger;
2 public interface Navigation {
63
7 import org.haldean.chopper.nav.NavData;
8 import org.haldean.chopper.nav.NavList;
9
10 import android.util.Log;
11
12 /**
13 * Handles Navigation routines; calculates next target velocity vector. <P>
14 *
15 * May send the following messages to registered Receivables:<br>
16 * <pre>
17 * NAV:AUTOTASK:&lt;travel_plan_index&gt;:&lt;serialized_NavTask&gt;
18 * </pre>
19 *
20 * May receive the following messages from Chopper components:
21 * <pre>
22 * NAV:
23 * SET:
24 * MANUAL:&lt;north_south_vel&gt;:&lt;east_west_vel&gt;:&lt;up_down_vel&gt;:&lt;orientation&gt;
25 * AUTOPILOT
26 * AUTOTASK:&lt;travel_plan_index&gt;:&lt;serialized_NavTask&gt;
27 * FLIGHTPLAN:&lt;new_plan&gt;
28 * GET:AUTOTASKS
29 * CSYS:
30 * NOCONN
31 * LOWPOWER
32 * </pre>
33 *
34 * @author Benjamin Bardin
35 */
36 public class Navigation implements Constants, Receivable {
37
384 /** Tag for logging */
395 public static final String TAG = "chopper.Navigation";
40
41 /** How long (in ms) Navigation should instruct the chopper to hover
6 /** How long (in ms) NavigationImpl should instruct the chopper to hover
427 * when autopilot has run out of NavTasks */
438 public static final int HOVER_PAUSE = 1000;
44
459 public static final String THREE_HRS_IN_MS = "10800000";
46
47 /** Velocity to achieve. Must be locked on each read/write. */
48 private double[] mTarget = new double[4];
49
50 /** Used to calculate next nav vector; prevents the thread
51 * from holding a lock on mTarget itself for too long */
52 private double[] mTempTarget = new double[4];
53
54 /** Chopper's navigation status */
55 private final AtomicInteger mNavStatus = new AtomicInteger(NAV_STATUSES - 1);
56
57 /** Holds all flight plans */
58 private Vector<NavData> mTravelPlans = new Vector<NavData>(); //Vector --> already thread-safe
59
60 /** Different flight plans depending on Nav status */
61 private NavData mLowPower;
62 private NavData mFlightPath;
63 private NavData mOnMyOwn;
64
65 /** Handle for other chopper components */
66 private ChopperStatus mStatus;
67
68 /** Registered receivers */
69 private LinkedList<Receivable> mRec;
70
71 private NavTask mTask;
72
73 /**
74 * Constructs a navigation object, initializes NavLists
75 * @param status The ChopperStatus from which to obtain location information.
76 */
77 public Navigation(ChopperStatus status) {
78 if (status == null) {
79 throw new NullPointerException();
80 }
81 mRec = new LinkedList<Receivable>();
82 mLowPower = NavList.fromString("{ -1}");
83 mFlightPath = NavList.fromString("{ -2}");
84 mOnMyOwn = NavList.fromString("{ -3}");
85
86 mTask = new NavTask(status);
87
88 mTravelPlans.add(mLowPower);
89 mTravelPlans.add(mFlightPath);
90 mTravelPlans.add(mOnMyOwn);
9110
92 mStatus = status;
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
11011 /**
11112 * Evaluates a new navigation vector, based on current status and the relevant NavTask.
11213 * @param newNavTarget If supplied and has length >= 4, writes the new target here. May be null.
11314 */
114 public void evalNextVector(double[] newNavTarget) {
15 public abstract void evalNextVector(double[] newNavTarget);
11516
116 /*Determine what the current task should be. Copies to a local variable in case
117 * 'status' changes during execution of the method */
118 int thisStatus = mNavStatus.get();
119
120 NavData myList = mTravelPlans.get(thisStatus);
121 Log.v(TAG, "Nav using index " + thisStatus + ", task " + myList.toString());
122 if (mTask.isComplete(myList)) {
123 Log.i(TAG, "Nav is Hovering");
124 hover();
125 return;
126 }
127 mTask.getVelocity(myList, mTempTarget);
128 setTarget(mTempTarget);
129 if (newNavTarget != null) {
130 getTarget(newNavTarget);
131 }
132
133 //long interval = mTask.getInterval(myList);
134 //Log.v(TAG, "Nav Interval is " + interval);
135 //Send the current NavList to the server, in case any tasks have been completed
136 updateReceivers("NAV:AUTOTASK:" + thisStatus + ":" + myList.toString());
137 }
138
13917 /**
14018 * Writes current navigation target vector into supplied
14119 * array. If the data is locked, immediately returns with
14422 * @param expectedValues The array in which to write the
14523 * vector--must be at least of length 4.
14624 */
147 public void getTarget(double[] navTarget) {
148 if (navTarget.length < 4)
149 return;
150 synchronized (mTarget) {
151 System.arraycopy(mTarget, 0, navTarget, 0, 4);
152 }
153 }
154
25 public abstract void getTarget(double[] navTarget);
26
15527 /**
15628 * Obtains all scheduled flight plans
15729 * @return An array of strings representing all flight plans (serialized form)
15830 */
159 public String[] getTasks() {
160 ListIterator<NavData> iterator = mTravelPlans.listIterator();
161 String[] myTasks = new String[mTravelPlans.size()];
162 while (iterator.hasNext()) {
163 myTasks[iterator.nextIndex()] = iterator.next().toString();
164 }
165 return myTasks;
166 }
167
168 /** Orders the chopper to remain in place */
169 private void hover() {
170
171 for (int i = 0; i < 3; i++) {
172 mTempTarget[i] = 0;
173 }
174 mTempTarget[3] = mStatus.getReadingField(AZIMUTH);
175 setTarget(mTempTarget);
176 }
177
31 public abstract String[] getTasks();
32
17833 /**
17934 * Receive a message.
18035 * @param source The source of the message, if a reply is needed. May be null.
18136 */
182 public void receiveMessage(String msg, Receivable source) {
183 String[] parts = msg.split(":");
184
185 if (parts[0].equals("NAV")) {
186 if (parts[1].equals("SET")) {
187 Log.v(TAG, "Updating Nav Status");
188
189 if (parts[2].equals("AUTOPILOT")) {
190 mNavStatus.set(BASIC_AUTO);
191 updateReceivers("GUID:ABSVEC");
192 updateReceivers("GUID:AUTOMATIC");
193 //autoPilot(true);
194 }
195 if (parts[2].equals("AUTOTASK")) {
196 Integer taskList = new Integer(parts[3]);
197 Log.v(TAG, "Nav setting index " + taskList + " to " + parts[4]);
198 setTask(taskList, parts[4]);
199 }
200 if (parts[2].equals("FLIGHTPLAN")) {
201 mNavStatus.set(new Integer(parts[3]));
202 }
203 }
204 if (parts[1].equals("GET")) {
205 if (parts[2].equals("AUTOTASKS")) {
206 String[] myTasks = getTasks();
207 for (int i = 0; i < myTasks.length; i++) {
208 if (source != null) {
209 source.receiveMessage("NAV:AUTOTASK:" + i + ":" + myTasks[i], this);
210 }
211 }
212 }
213 }
214 }
215 if (parts[0].equals("CSYS")) {
216 if (parts[1].equals("NOCONN")) {
217 updateStatus(NO_CONN);
218 //autoPilot(true);
219 }
220 if (parts[1].equals("LOWPOWER")) {
221 updateStatus(LOW_POWER);
222 //autoPilot(true);
223 }
224 }
225 }
226
37 public abstract void receiveMessage(String msg, Receivable source);
38
22739 /**
22840 * Registers a receiver to receive Nav updates.
22941 * @param rec
23042 */
231 public void registerReceiver(Receivable rec) {
232 synchronized (mRec) {
233 mRec.add(rec);
234 }
235 }
236
237 /**
238 * Sets the navigation target vector to the values contained by the supplied array (length must be at least 4).
239 * @param newTarget The new target vector.
240 */
241 private void setTarget(double[] newTarget) {
242 if (newTarget.length < 4) {
243 return;
244 }
245 synchronized (mTarget) {
246 String newNav = "New_Nav_Vector: ";
247 for (int i = 0; i < 4; i++) {
248 mTarget[i] = newTarget[i];
249 newNav += newTarget[i] + ":";
250 }
251 updateReceivers(newNav);
252 }
253 }
254
255 /**
256 * Sets a supplied NavList as flight plan for the specified Nav status.
257 * @param whichPlan The Nav status for which to set the new flight plan
258 * @param myTask The new flight plan
259 */
260 private void setTask(int whichPlan, String myTask) {
261 Log.v(TAG, "Nav about to myList");
262 NavList myList = null;
263 try {
264 myList = NavList.fromString(myTask);
265 } catch (Exception e) {
266 e.printStackTrace();
267 }
268 Log.v(TAG, "Nav myList is " + myList);
269 if (myList != null) {
270 //Make change:
271 mTravelPlans.set(whichPlan, myList);
272 Log.i(TAG, "Nav set index " + whichPlan + " to task " + myList);
273 //Confirm change to server:
274 updateReceivers("NAV:AUTOTASK:" + whichPlan + ":" + myList.toString());
275 }
276 else {
277 Log.e(TAG, "Nav received invalid task!");
278 }
279 }
280
281 /**
282 * Updates all receivers
283 * @param str The message to send.
284 */
285 private void updateReceivers(String str) {
286 synchronized (mRec) {
287 ListIterator<Receivable> myList = mRec.listIterator();
288 while (myList.hasNext()) {
289 myList.next().receiveMessage(str, this);
290 }
291 }
292 }
293
294 /**
295 * Compares the supplied status with the current status; stores the most important.
296 * @param newstatus The new (potential) status
297 */
298 private void updateStatus(int newstatus) {
299 mNavStatus.set(Math.min(mNavStatus.get(), newstatus));
300 }
301 }
43 public abstract void registerReceiver(Receivable rec);
44
45 }
0 package org.haldean.chopper.pilot;
1
2 import java.util.LinkedList;
3 import java.util.ListIterator;
4 import java.util.Vector;
5 import java.util.concurrent.atomic.AtomicInteger;
6
7 import org.haldean.chopper.nav.NavData;
8 import org.haldean.chopper.nav.NavList;
9
10 import android.util.Log;
11
12 /**
13 * Handles NavigationImpl routines; calculates next target velocity vector. <P>
14 *
15 * May send the following messages to registered Receivables:<br>
16 * <pre>
17 * NAV:AUTOTASK:&lt;travel_plan_index&gt;:&lt;serialized_NavTask&gt;
18 * </pre>
19 *
20 * May receive the following messages from Chopper components:
21 * <pre>
22 * NAV:
23 * SET:
24 * MANUAL:&lt;north_south_vel&gt;:&lt;east_west_vel&gt;:&lt;up_down_vel&gt;:&lt;orientation&gt;
25 * AUTOPILOT
26 * AUTOTASK:&lt;travel_plan_index&gt;:&lt;serialized_NavTask&gt;
27 * FLIGHTPLAN:&lt;new_plan&gt;
28 * GET:AUTOTASKS
29 * CSYS:
30 * NOCONN
31 * LOWPOWER
32 * </pre>
33 *
34 * @author Benjamin Bardin
35 */
36 public class NavigationImpl implements Constants, Receivable, Navigation {
37
38 /** Velocity to achieve. Must be locked on each read/write. */
39 private double[] mTarget = new double[4];
40
41 /** Used to calculate next nav vector; prevents the thread
42 * from holding a lock on mTarget itself for too long */
43 private double[] mTempTarget = new double[4];
44
45 /** Chopper's navigation status */
46 private final AtomicInteger mNavStatus = new AtomicInteger(NAV_STATUSES - 1);
47
48 /** Holds all flight plans */
49 private Vector<NavData> mTravelPlans = new Vector<NavData>(); //Vector --> already thread-safe
50
51 /** Different flight plans depending on Nav status */
52 private NavData mLowPower;
53 private NavData mFlightPath;
54 private NavData mOnMyOwn;
55
56 /** Handle for other chopper components */
57 private ChopperStatus mStatus;
58
59 /** Registered receivers */
60 private LinkedList<Receivable> mRec;
61
62 private NavTask mTask;
63
64 /**
65 * Constructs a navigation object, initializes NavLists
66 * @param status The ChopperStatusImpl from which to obtain location information.
67 */
68 public NavigationImpl(ChopperStatus status) {
69 if (status == null) {
70 throw new NullPointerException();
71 }
72 mRec = new LinkedList<Receivable>();
73 mLowPower = NavList.fromString("{ -1}");
74 mFlightPath = NavList.fromString("{ -2}");
75 mOnMyOwn = NavList.fromString("{ -3}");
76
77 mTask = new NavTask(status);
78
79 mTravelPlans.add(mLowPower);
80 mTravelPlans.add(mFlightPath);
81 mTravelPlans.add(mOnMyOwn);
82
83 mStatus = status;
84
85 //FOR TESTING ONLY:
86 /*String taskList = "{ " +
87 "{ DEST!targ1!300!-74.012345!40.74!10!100!232 { DEST!targ2!300!-77.07950!38.97300!100!250!233 " +
88 " DEST!targ3!587!-117.15!32.72!10!600!234 } } }";*/
89 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}";
90 setTask(BASIC_AUTO, taskList);
91 setTask(NO_CONN, "{ VEL!No_Conn!0!0!-1!0!" + THREE_HRS_IN_MS + "!-1!-5 -6}");
92 setTask(LOW_POWER, "{ VEL!Low_Power!0!0!-1!0!"+ THREE_HRS_IN_MS + "!-1!-7 -8}");
93 //mNavStatus.set(BASIC_AUTO);
94
95
96 //autoPilot(true);
97 }
98
99
100
101 /* (non-Javadoc)
102 * @see org.haldean.chopper.pilot.Navigation#evalNextVector(double[])
103 */
104 public void evalNextVector(double[] newNavTarget) {
105
106 /*Determine what the current task should be. Copies to a local variable in case
107 * 'status' changes during execution of the method */
108 int thisStatus = mNavStatus.get();
109
110 NavData myList = mTravelPlans.get(thisStatus);
111 Log.v(TAG, "Nav using index " + thisStatus + ", task " + myList.toString());
112 if (mTask.isComplete(myList)) {
113 Log.i(TAG, "Nav is Hovering");
114 hover();
115 return;
116 }
117 mTask.getVelocity(myList, mTempTarget);
118 setTarget(mTempTarget);
119 if (newNavTarget != null) {
120 getTarget(newNavTarget);
121 }
122
123 //long interval = mTask.getInterval(myList);
124 //Log.v(TAG, "Nav Interval is " + interval);
125 //Send the current NavList to the server, in case any tasks have been completed
126 updateReceivers("NAV:AUTOTASK:" + thisStatus + ":" + myList.toString());
127 }
128
129 /* (non-Javadoc)
130 * @see org.haldean.chopper.pilot.Navigation#getTarget(double[])
131 */
132 public void getTarget(double[] navTarget) {
133 if (navTarget.length < 4)
134 return;
135 synchronized (mTarget) {
136 System.arraycopy(mTarget, 0, navTarget, 0, 4);
137 }
138 }
139
140 /* (non-Javadoc)
141 * @see org.haldean.chopper.pilot.Navigation#getTasks()
142 */
143 public String[] getTasks() {
144 ListIterator<NavData> iterator = mTravelPlans.listIterator();
145 String[] myTasks = new String[mTravelPlans.size()];
146 while (iterator.hasNext()) {
147 myTasks[iterator.nextIndex()] = iterator.next().toString();
148 }
149 return myTasks;
150 }
151
152 /** Orders the chopper to remain in place */
153 private void hover() {
154
155 for (int i = 0; i < 3; i++) {
156 mTempTarget[i] = 0;
157 }
158 mTempTarget[3] = mStatus.getReadingField(AZIMUTH);
159 setTarget(mTempTarget);
160 }
161
162 /* (non-Javadoc)
163 * @see org.haldean.chopper.pilot.Navigation#receiveMessage(java.lang.String, org.haldean.chopper.pilot.Receivable)
164 */
165 public void receiveMessage(String msg, Receivable source) {
166 String[] parts = msg.split(":");
167
168 if (parts[0].equals("NAV")) {
169 if (parts[1].equals("SET")) {
170 Log.v(TAG, "Updating Nav Status");
171
172 if (parts[2].equals("AUTOPILOT")) {
173 mNavStatus.set(BASIC_AUTO);
174 updateReceivers("GUID:ABSVEC");
175 updateReceivers("GUID:AUTOMATIC");
176 //autoPilot(true);
177 }
178 if (parts[2].equals("AUTOTASK")) {
179 Integer taskList = new Integer(parts[3]);
180 Log.v(TAG, "Nav setting index " + taskList + " to " + parts[4]);
181 setTask(taskList, parts[4]);
182 }
183 if (parts[2].equals("FLIGHTPLAN")) {
184 mNavStatus.set(new Integer(parts[3]));
185 }
186 }
187 if (parts[1].equals("GET")) {
188 if (parts[2].equals("AUTOTASKS")) {
189 String[] myTasks = getTasks();
190 for (int i = 0; i < myTasks.length; i++) {
191 if (source != null) {
192 source.receiveMessage("NAV:AUTOTASK:" + i + ":" + myTasks[i], this);
193 }
194 }
195 }
196 }
197 }
198 if (parts[0].equals("CSYS")) {
199 if (parts[1].equals("NOCONN")) {
200 updateStatus(NO_CONN);
201 //autoPilot(true);
202 }
203 if (parts[1].equals("LOWPOWER")) {
204 updateStatus(LOW_POWER);
205 //autoPilot(true);
206 }
207 }
208 }
209
210 /* (non-Javadoc)
211 * @see org.haldean.chopper.pilot.Navigation#registerReceiver(org.haldean.chopper.pilot.Receivable)
212 */
213 public void registerReceiver(Receivable rec) {
214 synchronized (mRec) {
215 mRec.add(rec);
216 }
217 }
218
219 /**
220 * Sets the navigation target vector to the values contained by the supplied array (length must be at least 4).
221 * @param newTarget The new target vector.
222 */
223 private void setTarget(double[] newTarget) {
224 if (newTarget.length < 4) {
225 return;
226 }
227 synchronized (mTarget) {
228 String newNav = "New_Nav_Vector: ";
229 for (int i = 0; i < 4; i++) {
230 mTarget[i] = newTarget[i];
231 newNav += newTarget[i] + ":";
232 }
233 updateReceivers(newNav);
234 }
235 }
236
237 /**
238 * Sets a supplied NavList as flight plan for the specified Nav status.
239 * @param whichPlan The Nav status for which to set the new flight plan
240 * @param myTask The new flight plan
241 */
242 private void setTask(int whichPlan, String myTask) {
243 Log.v(TAG, "Nav about to myList");
244 NavList myList = null;
245 try {
246 myList = NavList.fromString(myTask);
247 } catch (Exception e) {
248 e.printStackTrace();
249 }
250 Log.v(TAG, "Nav myList is " + myList);
251 if (myList != null) {
252 //Make change:
253 mTravelPlans.set(whichPlan, myList);
254 Log.i(TAG, "Nav set index " + whichPlan + " to task " + myList);
255 //Confirm change to server:
256 updateReceivers("NAV:AUTOTASK:" + whichPlan + ":" + myList.toString());
257 }
258 else {
259 Log.e(TAG, "Nav received invalid task!");
260 }
261 }
262
263 /**
264 * Updates all receivers
265 * @param str The message to send.
266 */
267 private void updateReceivers(String str) {
268 synchronized (mRec) {
269 ListIterator<Receivable> myList = mRec.listIterator();
270 while (myList.hasNext()) {
271 myList.next().receiveMessage(str, this);
272 }
273 }
274 }
275
276 /**
277 * Compares the supplied status with the current status; stores the most important.
278 * @param newstatus The new (potential) status
279 */
280 private void updateStatus(int newstatus) {
281 mNavStatus.set(Math.min(mNavStatus.get(), newstatus));
282 }
283 }
2727 */
2828 public class StatusReporter implements Runnable, Constants {
2929
30 /** How often (in ms) status updates should be sent by ChopperStatus to the server */
30 /** How often (in ms) status updates should be sent by ChopperStatusImpl to the server */
3131 public int updateInterval = 350;
3232
3333 /** Tag for logging */
3434 public static final String TAG = "chopper.StatusReporter";
3535
36 /** The ChopperStatus from which to compile status reports */
36 /** The ChopperStatusImpl from which to compile status reports */
3737 private ChopperStatus mStatus;
3838
3939 /** List of registered receivers */
4444
4545 /**
4646 * Constructs the object.
47 * @param status The ChopperStatus from which to compile status reports
47 * @param status The ChopperStatusImpl from which to compile status reports
4848 */
4949 public StatusReporter(ChopperStatus status) {
5050 mRec = new LinkedList<Receivable>();
0 package org.haldean.chopper.pilot.test;
1
2 import org.haldean.chopper.pilot.BluetoothOutput;
3 import org.haldean.chopper.pilot.Constants;
4 import org.haldean.chopper.pilot.Guidance;
5
6 import android.os.Message;
7 import android.test.AndroidTestCase;
8 import android.util.Log;
9
10 public class GuidanceTest extends AndroidTestCase implements Constants {
11 public static final String TAG = "chopper.GuidanceTest";
12
13 private Guidance guid;
14 private MockChopperStatus mCs = new MockChopperStatus();
15 private MockAngler mAngler = new MockAngler();
16
17 public void setUp() throws Exception {
18 mCs.reset();
19 mAngler.reset();
20 guid = new Guidance(mCs,
21 new BluetoothOutput() {
22 public void run() {}
23 public void sendMessageToHandler(Message msg) {}
24 },
25 mAngler);
26
27 }
28
29 public void tearDown() throws Exception {
30
31 }
32
33 public void testFacingNorth() throws Exception {
34 guid.receiveMessage("GUID:AUTOPILOT", null);
35 double[] angleTarget = new double[4];
36 double[] oldSpeeds = new double[4];
37 double[] newSpeeds = new double[4];
38 double[] motorTarget = new double[4];
39
40 Log.v(TAG, " ");
41 Log.v(TAG, "10 degrees to the right");
42 angleTarget[0] = 10.0;
43 mAngler.setAngleTarget(angleTarget);
44 incrementGpsTimeStamp();
45 motorTarget[3] = 1.0;
46 motorTarget[2] = 0.0;
47 checkAngles(oldSpeeds, newSpeeds, motorTarget);
48
49 Log.v(TAG, " ");
50 Log.v(TAG, "10 degrees to the left");
51 angleTarget[0] = -10.0;
52 mAngler.setAngleTarget(angleTarget);
53 incrementGpsTimeStamp();
54 motorTarget[2] = 1.0;
55 motorTarget[3] = 0.0;
56 checkAngles(oldSpeeds, newSpeeds, motorTarget);
57
58 Log.v(TAG, " ");
59 Log.v(TAG, "10 degrees forward, 10 degrees to the left");
60 angleTarget[1] = 10.0;
61 mAngler.setAngleTarget(angleTarget);
62 incrementGpsTimeStamp();
63 motorTarget[1] = 1.0;
64 motorTarget[0] = 0.0;
65 checkAngles(oldSpeeds, newSpeeds, motorTarget);
66
67 Log.v(TAG, " ");
68 Log.v(TAG, "10 degrees backward, 10 degrees to the left");
69 angleTarget[1] = -10.0;
70 mAngler.setAngleTarget(angleTarget);
71 incrementGpsTimeStamp();
72 motorTarget[1] = 0.0;
73 motorTarget[0] = 1.0;
74 checkAngles(oldSpeeds, newSpeeds, motorTarget);
75
76 Log.v(TAG, " ");
77 Log.v(TAG, "10 Up");
78 angleTarget[0] = 0.0;
79 angleTarget[1] = 0.0;
80 angleTarget[2] = 10.0;
81 mAngler.setAngleTarget(angleTarget);
82 incrementGpsTimeStamp();
83 motorTarget[0] = 1.0;
84 motorTarget[1] = 1.0;
85 motorTarget[2] = 1.0;
86 motorTarget[3] = 1.0;
87 checkAngles(oldSpeeds, newSpeeds, motorTarget);
88
89 Log.v(TAG, " ");
90 Log.v(TAG, "10 Down");
91 angleTarget[2] = -10.0;
92 mAngler.setAngleTarget(angleTarget);
93 incrementGpsTimeStamp();
94 motorTarget[0] = 1.0; // Residue from back-left command.
95 motorTarget[1] = 0.0;
96 motorTarget[2] = 1.0; // Residue from back-left command.
97 motorTarget[3] = 0.0;
98 checkAngles(oldSpeeds, newSpeeds, motorTarget);
99 }
100
101 public void testRotation() throws Exception {
102 guid.receiveMessage("GUID:AUTOPILOT", null);
103 double[] angleTarget = new double[4];
104 double[] oldSpeeds = new double[4];
105 double[] newSpeeds = new double[4];
106 double[] motorTarget = new double[4];
107
108 mCs.setReadingField(AZIMUTH, 45);
109 motorTarget[0] = 0.0;
110 motorTarget[1] = 0.0;
111 motorTarget[2] = 1.0;
112 motorTarget[3] = 1.0;
113 checkAngles(oldSpeeds, newSpeeds, motorTarget);
114
115 mCs.setReadingField(AZIMUTH, 135);
116 motorTarget[0] = 0.0;
117 motorTarget[1] = 0.0;
118 motorTarget[2] = 1.0;
119 motorTarget[3] = 1.0;
120 checkAngles(oldSpeeds, newSpeeds, motorTarget);
121
122 mCs.setReadingField(AZIMUTH, -45);
123 motorTarget[0] = 1.0;
124 motorTarget[1] = 1.0;
125 motorTarget[2] = 0.0;
126 motorTarget[3] = 0.0;
127 checkAngles(oldSpeeds, newSpeeds, motorTarget);
128
129 mCs.setReadingField(AZIMUTH, -135);
130 motorTarget[0] = 1.0;
131 motorTarget[1] = 1.0;
132 motorTarget[2] = 0.0;
133 motorTarget[3] = 0.0;
134 checkAngles(oldSpeeds, newSpeeds, motorTarget);
135
136 mCs.setReadingField(AZIMUTH, 225);
137 motorTarget[0] = 1.0;
138 motorTarget[1] = 1.0;
139 motorTarget[2] = 0.0;
140 motorTarget[3] = 0.0;
141 checkAngles(oldSpeeds, newSpeeds, motorTarget);
142
143 mCs.setReadingField(AZIMUTH, 0);
144 angleTarget[3] = -45.0;
145 mAngler.setAngleTarget(angleTarget);
146 incrementGpsTimeStamp();
147 motorTarget[0] = 0.0;
148 motorTarget[1] = 0.0;
149 motorTarget[2] = 1.0;
150 motorTarget[3] = 1.0;
151 checkAngles(oldSpeeds, newSpeeds, motorTarget);
152
153 mCs.setReadingField(AZIMUTH, 0);
154 angleTarget[3] = 45.0;
155 mAngler.setAngleTarget(angleTarget);
156 incrementGpsTimeStamp();
157 motorTarget[0] = 1.0;
158 motorTarget[1] = 1.0;
159 motorTarget[2] = 0.0;
160 motorTarget[3] = 0.0;
161 checkAngles(oldSpeeds, newSpeeds, motorTarget);
162 }
163
164 private void checkAngles(double[] oldSpeed, double[] newSpeed, double[] finalSpeed) {
165 for (int j = 0; j < 40; j++) {
166 mCs.getMotorFields(oldSpeed);
167 guid.reviseMotorSpeed();
168 mCs.getMotorFields(newSpeed);
169 logArray("oldSpeed", oldSpeed);
170 logArray("newSpeed", newSpeed);
171 logArray("finalSpeed", finalSpeed);
172 for (int i = 0; i < finalSpeed.length; i++) {
173 if (finalSpeed[i] == oldSpeed[i]) {
174 assertEquals(finalSpeed[i], newSpeed[i]);
175 } else if (finalSpeed[i] > oldSpeed[i]) {
176 assertTrue(newSpeed[i] >= oldSpeed[i]);
177 assertTrue(finalSpeed[i] >= newSpeed[i]);
178 } else {
179 assertTrue(newSpeed[i] <= oldSpeed[i]);
180 assertTrue(finalSpeed[i] <= newSpeed[i]);
181 }
182 }
183 }
184 assertArrayEquals(finalSpeed, newSpeed);
185 }
186
187 private static void logArray(String id, double[] array) {
188 String output = id + ": ";
189 for (int i = 0; i < array.length; i++) {
190 output += array[i] + ", ";
191 }
192 Log.v(TAG, output);
193 }
194
195 private static void assertArrayEquals(double[] expected, double[] actual) {
196 assertEquals(expected.length, actual.length);
197 for (int i = 0; i < expected.length; i++) {
198 assertEquals(expected[i], actual[i]);
199 }
200 }
201
202 private void incrementGpsTimeStamp() {
203 mCs.setGpsTimeStamp(mCs.getGpsTimeStamp() + 1);
204 }
205 }
0 package org.haldean.chopper.pilot.test;
1
2 import java.util.Arrays;
3
4 import org.haldean.chopper.pilot.Angler;
5
6 public class MockAngler implements Angler {
7 private double[] angles = new double[4];
8
9 @Override
10 public void getAngleTarget(double[] target) {
11 System.arraycopy(angles, 0, target, 0, 4);
12 }
13
14 public void setAngleTarget(double[] target) {
15 System.arraycopy(target, 0, angles, 0, 4);
16 }
17
18 public void reset() {
19 Arrays.fill(angles, 0.0);
20 }
21 }
0 package org.haldean.chopper.pilot.test;
1
2 import java.util.Arrays;
3
4 import org.haldean.chopper.pilot.ChopperStatus;
5 import org.haldean.chopper.pilot.Constants;
6 import org.haldean.chopper.pilot.Receivable;
7
8 import android.hardware.Sensor;
9 import android.hardware.SensorEvent;
10 import android.location.Location;
11 import android.os.Bundle;
12
13 public class MockChopperStatus implements ChopperStatus, Constants {
14 /** Holds GPS data. */
15 private double[] mGps = new double[GPS_FIELDS]; //last available GPS readings
16
17 /** Timestamp of last GPS reading. */
18 private long mGpsTimeStamp; //timestamp of the reading
19
20 /** Stores the speeds last submitted to the motors. */
21 private double[] mMotorSpeed = new double[4];
22
23 /** Holds data from various sensors, like gyroscope, acceleration and magnetic flux. */
24 private double[] mReading = new double[SENSORS]; //last known data for a given sensor
25
26 public MockChopperStatus() {
27 reset();
28 }
29
30 @Override
31 public void close() {
32 }
33
34 @Override
35 public float getBatteryLevel() {
36 // TODO Auto-generated method stub
37 return 0;
38 }
39
40 @Override
41 public String getGpsExtras() {
42 // TODO Auto-generated method stub
43 return null;
44 }
45
46 @Override
47 public double getGpsField(int whichField) {
48 // TODO Auto-generated method stub
49 return 0;
50 }
51
52 @Override
53 public long getGpsTimeStamp() {
54 return mGpsTimeStamp;
55 }
56
57 @Override
58 public Location getLastLocation() {
59 // TODO Auto-generated method stub
60 return null;
61 }
62
63 @Override
64 public void getMotorFields(double[] myValues) {
65 System.arraycopy(mMotorSpeed, 0, myValues, 0, 4);
66
67 }
68
69 @Override
70 public void getMotorPowerFields(double[] myValues) {
71 // TODO Auto-generated method stub
72
73 }
74
75 @Override
76 public double getReadingField(int whichField) {
77 return mReading[whichField];
78 }
79
80 @Override
81 public void onAccuracyChanged(Sensor sensor, int newaccuracy) {
82 // TODO Auto-generated method stub
83
84 }
85
86 @Override
87 public void onLocationChanged(Location loc) {
88 // TODO Auto-generated method stub
89
90 }
91
92 @Override
93 public void onProviderDisabled(String provider) {
94 // TODO Auto-generated method stub
95
96 }
97
98 @Override
99 public void onProviderEnabled(String provider) {
100 // TODO Auto-generated method stub
101
102 }
103
104 @Override
105 public void onSensorChanged(SensorEvent event) {
106 // TODO Auto-generated method stub
107
108 }
109
110 @Override
111 public void onStatusChanged(String provider, int status, Bundle extras) {
112 // TODO Auto-generated method stub
113
114 }
115
116 @Override
117 public void registerReceiver(Receivable rec) {
118 // TODO Auto-generated method stub
119
120 }
121
122 public void reset() {
123 Arrays.fill(mReading, 0.0);
124 Arrays.fill(mMotorSpeed, 0.0);
125 }
126
127 @Override
128 public void run() {
129 // TODO Auto-generated method stub
130
131 }
132
133 public void setGpsField(int whichfield, int value) {
134 mGps[whichfield] = value;
135 }
136
137 public void setGpsTimeStamp(long time) {
138 mGpsTimeStamp = time;
139 }
140
141 @Override
142 public void setMotorFields(double[] mySpeeds) {
143 System.arraycopy(mySpeeds, 0, mMotorSpeed, 0, 4);
144 }
145
146 @Override
147 public void setMotorPowerFields(double[] myPowers) {
148 // TODO Auto-generated method stub
149
150 }
151
152 public void setReadingField(int whichfield, int value) {
153 mReading[whichfield] = value;
154 }
155 }