Permalink
Browse files

adjustments to algorithm, and split up local log if running for more …

…than an hour.
  • Loading branch information...
1 parent 971f1c3 commit ba4ce005cd23388643915aa0280aa104cd0c2bdd Marije Baalman committed Feb 18, 2012
@@ -2,7 +2,6 @@
<LinearLayout android:id="@+id/LinearLayout01" android:layout_width="fill_parent" android:layout_height="fill_parent" xmlns:android="http://schemas.android.com/apk/res/android" android:orientation="vertical" android:padding="6dp">
<LinearLayout android:id="@+id/LinearLayout02" android:layout_width="fill_parent" android:layout_height="wrap_content" android:orientation="vertical">
-
<TextView android:textAppearance="?android:attr/textAppearanceLarge" android:layout_width="wrap_content" android:layout_height="wrap_content" android:id="@+id/MotionStatusTextView" android:text="Still" android:gravity="center_vertical|center_horizontal"></TextView>
<TableLayout android:id="@+id/LinearLayout04" android:layout_width="fill_parent" android:layout_height="wrap_content" android:stretchColumns="1" android:padding="6dp">
<TableRow>
@@ -47,6 +46,7 @@
</TableLayout>
</LinearLayout>
+
<ScrollView android:id="@+id/ScrollView01" android:layout_width="fill_parent" android:layout_height="wrap_content">
@@ -160,7 +160,12 @@
<EditText android:id="@+id/speed_decay" android:layout_width="wrap_content" android:layout_height="wrap_content" android:layout_weight="1" android:inputType="numberDecimal" android:text="0.99">
</EditText>
</TableRow>
- </TableLayout>
+ <TableRow>
+ <TextView android:layout_width="wrap_content" android:layout_height="wrap_content" android:id="@+id/MMALabelTextView" android:text="Mean MA"></TextView>
+ <EditText android:id="@+id/mean_coef" android:layout_width="wrap_content" android:layout_height="wrap_content" android:layout_weight="1" android:inputType="numberDecimal" android:text="0.99">
+ </EditText>
+ </TableRow>
+ </TableLayout>
<TextView android:textAppearance="?android:attr/textAppearanceLarge" android:layout_width="fill_parent" android:layout_height="wrap_content" android:id="@+id/SettingLabelTextView" android:text="SERVER" android:gravity="center_vertical|center_horizontal"></TextView>
<TableLayout android:id="@+id/TableLayout3" android:layout_width="fill_parent" android:layout_height="wrap_content" android:stretchColumns="1">
@@ -723,6 +723,14 @@ public void send_estimate_settings(){
System.out.println("Could not parse " + nfe);
}
+ ed = (EditText) findViewById(R.id.mean_coef);
+ float mean_coef = 0.99f;
+ try {
+ mean_coef = Float.parseFloat(ed.getText().toString());
+ } catch(NumberFormatException nfe) {
+ System.out.println("Could not parse " + nfe);
+ }
+
ed = (EditText) findViewById(R.id.offsetMA);
float offsetma = 0.99f;
try {
@@ -802,6 +810,7 @@ public void send_estimate_settings(){
b.putFloat("mean_weight", mean_weight );
b.putFloat("raw_weight", raw_weight );
b.putFloat("speed_decay", speed_decay );
+ b.putFloat("mean_coef", mean_coef );
b.putFloat("offsetma", offsetma );
b.putInt("signForward", signForward );
@@ -1003,7 +1012,9 @@ public void readPreferences(){
float mean_weight = mPrefs.getFloat("mean_weight", 0.65f );
float raw_weight = mPrefs.getFloat("raw_weight", 0.35f );
-
+
+ float mean_coef = mPrefs.getFloat("mean_coef", 0.99f );
+
float speed_decay = mPrefs.getFloat("speed_decay", 0.99f );
float offsetma = mPrefs.getFloat("offsetma", 0.99f );
@@ -1091,6 +1102,8 @@ public void readPreferences(){
ed = (EditText) findViewById(R.id.speed_decay );
ed.setText( Float.toString( speed_decay ) );
+ ed = (EditText) findViewById(R.id.mean_coef );
+ ed.setText( Float.toString( mean_coef ) );
ed = (EditText) findViewById(R.id.offsetMA);
ed.setText( Float.toString( offsetma ) );
ed = (EditText) findViewById(R.id.editWindow);
@@ -1251,6 +1264,13 @@ public void storePreferences(){
} catch(NumberFormatException nfe) {
System.out.println("Could not parse " + nfe);
}
+ ed = (EditText) findViewById(R.id.mean_coef);
+ float mean_coef = 0.99f;
+ try {
+ mean_coef = Float.parseFloat(ed.getText().toString());
+ } catch(NumberFormatException nfe) {
+ System.out.println("Could not parse " + nfe);
+ }
ed = (EditText) findViewById(R.id.offsetMA);
float offsetma = 0.99f;
@@ -1362,6 +1382,8 @@ public void storePreferences(){
mPrefsEdit.putFloat("deceleration_forward", dec_forward );
mPrefsEdit.putFloat("deceleration_mean", dec_mean );
mPrefsEdit.putFloat("speed_decay", speed_decay );
+ mPrefsEdit.putFloat("mean_coef", mean_coef );
+
mPrefsEdit.putFloat("still_forward", still_forward );
mPrefsEdit.putFloat("still_side", still_side );
mPrefsEdit.putFloat("motion_forward", motion_forward );
@@ -36,6 +36,8 @@
import java.io.OutputStream;
import java.io.PrintWriter;
import java.io.IOException;
+import java.lang.Math;
+
//import android.hardware.SensorEventListener;
@@ -71,6 +73,8 @@
static final int MSG_GUI_UPDATE_MSG = 15;
static final int MSG_GPS_LOC = 16;
+
+ private int mWrittenLines = 0;
// static final int MSG_REGISTER_VS_SRV = 5;
// static final int MSG_UNREGISTER_VS_SRV = 6;
@@ -123,7 +127,10 @@
private float [][] mCurrentStats = { { (float) 0.0, (float) 0.0, (float) 0.0 }, { (float) 0.0, (float) 0.0, (float) 0.0 } };
//float stats[][] = new float[2][3];
- private float mSpeed = (float) 0.0;
+ private double mSpeed = 0.0;
+ private float[] mamean = { (float) 0.0, (float) 0.0, (float) 0.0 };
+ private float[] mameanOff = { (float) 0.0, (float) 0.0, (float) 0.0 };
+ private float mameanCoef = 0.99f;
private int mForward = 1;
private int mSideways = 0;
@@ -199,6 +206,7 @@ public void handleMessage(Message msg)
threshold_motion_side = msg.getData().getFloat("motion_side");
mean_weight = msg.getData().getFloat("mean_weight");
raw_weight = msg.getData().getFloat("raw_weight");
+ mameanCoef = msg.getData().getFloat("mean_coef");
speed_decay = msg.getData().getFloat("speed_decay");
macoef = msg.getData().getFloat("offsetma");
forwardsign = msg.getData().getInt("signForward");
@@ -281,14 +289,17 @@ public void run() {
float [] currentGPSReadings = mGPSListener.getCurrentValues();
float [] logdata = {
(float) mState,
- mSpeed, mSpeed * 3.6f,
+ (float) mSpeed, (float) mSpeed * 3.6f,
mCurrentStats[0][0], mCurrentStats[1][0],
mCurrentStats[0][1], mCurrentStats[1][1],
mCurrentStats[0][2], mCurrentStats[1][2],
mOffsets[0],mOffsets[1],mOffsets[2],
//mStillTime,
currentReadings[0],currentReadings[1], currentReadings[2],
- currentGPSReadings[0],currentGPSReadings[1]
+ currentGPSReadings[0],currentGPSReadings[1],
+ mameanOff[0], mamean[0],
+ mameanOff[1], mamean[1],
+ mameanOff[2], mamean[2]
};
writeLogData( logdata );
}
@@ -482,6 +493,8 @@ private void readPreferences(){
mean_weight = mPrefs.getFloat("mean_weight", 0.65f );
raw_weight = mPrefs.getFloat("raw_weight", 0.35f );
+
+ mameanCoef = mPrefs.getFloat("mean_coef", 0.99f );
speed_decay = mPrefs.getFloat("speed_decay", 0.99f );
macoef = mPrefs.getFloat("offsetma", 0.99f );
@@ -692,7 +705,7 @@ private void send_gui_update_msg(){
Bundle b = new Bundle();
Message msg = Message.obtain(null, MSG_GUI_UPDATE_MSG );
b.putInt("motion", mState );
- b.putFloat("speed", mSpeed );
+ b.putFloat("speed", (float) mSpeed );
synchronized( this ){
b.putFloat("facc_mean", this.mCurrentStats[0][0] );
@@ -798,22 +811,26 @@ public void updateVelocityMeasurement(){
//mCurrentStats = mBuffer.getStats();
float [][] curStats = this.mListener.getCurrentStats();
+ for ( int axis = 0; axis < 3; axis++ ){
+ mamean[axis] = mamean[axis] * mameanCoef + curStats[0][axis]*(1.0f-mameanCoef);
+ }
+
// reset offsets when we are in still:
if ( this.mState == 0 ) {
for ( int axis = 0; axis < 3; axis++ ){
if ( curStats[0][axis] < 20.f ){ // just to make sure we're not killed by NaN's
- this.mOffsets[axis] = this.mOffsets[axis] * macoef + curStats[0][axis] * (1-macoef);
+ this.mOffsets[axis] = this.mOffsets[axis] * macoef + mamean[axis] * (1-macoef);
}
}
}
// substract offset from mean
for ( int axis = 0; axis < 3; axis++ ){
- curStats[0][axis] = curStats[0][axis] - this.mOffsets[axis];
+ mameanOff[axis] = mamean[axis] - this.mOffsets[axis];
}
// correct sign for forward axis
- curStats[0][0] = curStats[0][0] * forwardsign;
+ mameanOff[0] = mameanOff[0] * forwardsign;
currentReadings[0] = (currentReadings[0] - this.mOffsets[0]) * forwardsign;
// determine state
@@ -824,10 +841,10 @@ public void updateVelocityMeasurement(){
if ( curStats[1][0] < threshold_still_forward && curStats[1][1] < threshold_still_side ){
this.mState = 0; // still
}
- if ( curStats[1][0] > threshold_acceleration_forward && curStats[0][0] > threshold_acceleration_mean ){
+ if ( curStats[1][0] > threshold_acceleration_forward && mameanOff[0] > threshold_acceleration_mean ){
this.mState = 2; // accelerating
}
- if ( curStats[1][0] > threshold_deceleration_forward && curStats[0][0] < threshold_deceleration_mean ){
+ if ( curStats[1][0] > threshold_deceleration_forward && mameanOff[0] < threshold_deceleration_mean ){
this.mState = 3; // decelerating
}
@@ -870,13 +887,20 @@ public void updateVelocityMeasurement(){
}
}
*/
-
+
// calculate forward speed
+ // limit speed increase when getting faster:
+ double limiterFactor = Math.exp( -1. * this.mSpeed * Math.PI / 10. );
+ double deltaspeed = (mean_weight*mameanOff[0] + raw_weight*currentReadings[0]) * this.mDeltaTime * 0.001;
+ if ( deltaspeed > 0 ){
+ this.mSpeed += deltaspeed * limiterFactor;
+ } else {
+ this.mSpeed += deltaspeed;
+ }
if ( this.mState == 0 ){
this.mSpeed = this.mSpeed * speed_decay;
- } else {
- this.mSpeed += ( mean_weight*curStats[0][0] + raw_weight*currentReadings[0]) * this.mDeltaTime * 0.001;
}
+ this.mSpeed = Math.max( mSpeed, 0.0 );
/*
switch (this.mState){
@@ -896,7 +920,8 @@ public void updateVelocityMeasurement(){
*/
synchronized( this ){
for ( int axis = 0; axis < 3; axis++ ){
- mCurrentStats[0][axis] = curStats[0][axis];
+// mCurrentStats[0][axis] = curStats[0][axis];
+ mCurrentStats[0][axis] = mameanOff[axis];
mCurrentStats[1][axis] = curStats[1][axis];
}
}
@@ -951,6 +976,7 @@ public void createLocalLog() {
if ( mMakeLocalLog ){
try {
mLocalLog = new SensorOutputWriter(SensorOutputWriter.TYPE_GVB);
+ mWrittenLines = 0;
mWritingLocalLog = true;
} catch (StorageErrorException ex) {
ex.printStackTrace();
@@ -962,6 +988,11 @@ public void writeLogData( float[] values ){
if ( mWritingLocalLog ){
try {
mLocalLog.writeReadings(values);
+ mWrittenLines++;
+ if ( mWrittenLines > 360000 ){ // start a new file each hour
+ closeLocalLog();
+ createLocalLog();
+ }
} catch (StorageErrorException ex) {
ex.printStackTrace();
}
Oops, something went wrong.

0 comments on commit ba4ce00

Please sign in to comment.