Permalink
Browse files

add gravity threshold and correct for gravity angle when on slopes

  • Loading branch information...
1 parent dcc08d1 commit cd113f5fd4d561c05f7e5a3fadce8959b7d2c38f Marije Baalman committed Feb 19, 2012
@@ -66,8 +66,13 @@
</TableRow>
<TableRow>
<TextView android:textAppearance="?android:attr/textAppearanceLarge" android:layout_width="wrap_content" android:layout_height="wrap_content" android:id="@+id/ThresholdLabelTextView" android:text="Thresholds" android:layout_span="2"></TextView>
- </TableRow>
+ </TableRow>
<TableRow>
+ <TextView android:layout_width="wrap_content" android:layout_height="wrap_content" android:id="@+id/Still1LabelTextView" android:text="Gravity"></TextView>
+ <EditText android:id="@+id/threshold_gravity" android:layout_width="wrap_content" android:layout_height="wrap_content" android:layout_weight="1" android:inputType="numberDecimal" android:text="0.1">
+ </EditText>
+ </TableRow>
+ <TableRow>
<TextView android:layout_width="wrap_content" android:layout_height="wrap_content" android:id="@+id/Still1LabelTextView" android:text="Still forward"></TextView>
<EditText android:id="@+id/still_forward" android:layout_width="wrap_content" android:layout_height="wrap_content" android:layout_weight="1" android:inputType="numberDecimal" android:text="0.04">
</EditText>
@@ -673,6 +673,14 @@ public void send_estimate_settings(){
} catch(NumberFormatException nfe) {
System.out.println("Could not parse " + nfe);
}
+ ed = (EditText) findViewById(R.id.threshold_gravity);
+ float thr_gravity = 0.1f;
+
+ try {
+ thr_gravity = Float.parseFloat(ed.getText().toString());
+ } catch(NumberFormatException nfe) {
+ System.out.println("Could not parse " + nfe);
+ }
ed = (EditText) findViewById(R.id.acceleration_mean);
float acc_mean = 0.1f;
@@ -823,6 +831,7 @@ public void send_estimate_settings(){
b.putInt("window", window );
b.putInt("updateTime", updateTime );
+ b.putFloat("threshold_gravity", thr_gravity );
b.putFloat("acceleration_forward", acc_forward );
b.putFloat("acceleration_mean", acc_mean );
b.putFloat("deceleration_forward", dec_forward );
@@ -1028,6 +1037,7 @@ public void readPreferences(){
int window = mPrefs.getInt("window", 200 );
int updateTime = mPrefs.getInt("updateTime", 10 );
+ float thr_gravity = mPrefs.getFloat("threshold_gravity", 0.1f );
float acc_forward = mPrefs.getFloat("acceleration_forward", 0.2f );
float acc_mean = mPrefs.getFloat("acceleration_mean", 0.1f );
float dec_forward = mPrefs.getFloat("deceleration_forward", 0.3f );
@@ -1113,6 +1123,8 @@ public void readPreferences(){
break;
}
+ ed = (EditText) findViewById(R.id.threshold_gravity);
+ ed.setText( Float.toString( thr_gravity ) );
ed = (EditText) findViewById(R.id.acceleration_forward);
ed.setText( Float.toString( acc_forward ) );
ed = (EditText) findViewById(R.id.acceleration_mean);
@@ -1238,6 +1250,14 @@ public void storePreferences(){
} catch(NumberFormatException nfe) {
System.out.println("Could not parse " + nfe);
}
+ ed = (EditText) findViewById(R.id.threshold_gravity);
+ float thr_gravity = 0.1f;
+
+ try {
+ thr_gravity = Float.parseFloat(ed.getText().toString());
+ } catch(NumberFormatException nfe) {
+ System.out.println("Could not parse " + nfe);
+ }
ed = (EditText) findViewById(R.id.acceleration_mean);
float acc_mean = 0.1f;
@@ -1423,6 +1443,7 @@ public void storePreferences(){
mPrefsEdit.putInt("window", window );
mPrefsEdit.putInt("updateTime", updateTime );
+ mPrefsEdit.putFloat("threshold_gravity", thr_gravity );
mPrefsEdit.putFloat("acceleration_forward", acc_forward );
mPrefsEdit.putFloat("acceleration_mean", acc_mean );
mPrefsEdit.putFloat("deceleration_forward", dec_forward );
@@ -145,6 +145,8 @@
private int mState = 0;
//private float mStillTime = 0.0f;
+ private float threshold_gravity = 0.1f;
+
private float threshold_still_side = 0.04f;
private float threshold_still_forward = 0.04f;
@@ -203,6 +205,7 @@ public void handleMessage(Message msg)
set_gravity_axis( msg.getData().getInt("gravity") );
mWindowSize = msg.getData().getInt("window");
mUpdateTime = msg.getData().getInt("updateTime");
+ threshold_gravity = msg.getData().getFloat("threshold_gravity");
threshold_acceleration_forward = msg.getData().getFloat("acceleration_forward");
threshold_acceleration_mean = msg.getData().getFloat("acceleration_mean");
threshold_deceleration_forward = msg.getData().getFloat("deceleration_forward");
@@ -495,6 +498,8 @@ private void readPreferences(){
mWindowSize = mPrefs.getInt("window", 200 );
mUpdateTime = mPrefs.getInt("updateTime", 10 );
+
+ threshold_gravity = mPrefs.getFloat("threshold_gravity", 0.1f );
threshold_acceleration_forward = mPrefs.getFloat("acceleration_forward", 0.2f );
threshold_acceleration_mean = mPrefs.getFloat("acceleration_mean", 0.1f );
@@ -848,6 +853,12 @@ public void updateVelocityMeasurement(){
mameanOff[axis] = mamean[axis] - this.mOffsets[axis];
}
+ if ( Math.abs( mameanOff[2] ) > threshold_gravity ){
+ // sensor is tilted
+ float correctionFactor = mamean[2] / this.mOffsets[2];
+ mameanOff[0] = correctionFactor * mamean[0] - this.mOffsets[0];
+ }
+
// correct sign for forward axis
mameanOff[0] = mameanOff[0] * forwardsign;
currentReadings[0] = (currentReadings[0] - this.mOffsets[0]) * forwardsign;

0 comments on commit cd113f5

Please sign in to comment.