Permalink
Browse files

Add movement end checks

  • Loading branch information...
1 parent 268a3a3 commit 0ea2a4423e2d696bb06ebb00647d1da11cdcdbb9 @grantmd committed May 1, 2011
Showing with 21 additions and 1 deletion.
  1. +19 −1 INS.cpp
  2. +2 −0 INS.h
View
20 INS.cpp
@@ -39,16 +39,34 @@ void INS::update(int dT, float x_accel, float y_accel, float z_accel, float head
updateAxis(ZAXIS, dT, z_accel);
}
+// Combine acceleration data with previous measurements to get our velocity and position
void INS::updateAxis(byte axis, int dT, float accel){
// first integration
float velocity = _velocity[axis] + _acceleration[axis] + ((accel - _acceleration[axis]) / 2);
// second integration
- _position[axis] = _position[axis] + _velocity[axis] + ((velocity - _velocity[axis]) / 2);
+ int position = _position[axis] + _velocity[axis] + ((velocity - _velocity[axis]) / 2);
// Store for next time
+ _position[axis] = position;
_velocity[axis] = velocity;
_acceleration[axis] = accel;
+
+ movementEndCheck(axis);
+}
+
+// Maintain a count of how many ticks at 0 acceleration we've seen. If we've seen "enough", zero out velocity
+void INS::movementEndCheck(byte axis){
+ if (_acceleration[axis] == 0){
+ _zeroCount[axis]++;
+ }
+ else{
+ _zeroCount[axis] = 0;
+ }
+
+ if (_zeroCount[axis] >= 25){
+ _velocity[axis] = 0;
+ }
}
///////////
View
2 INS.h
@@ -37,10 +37,12 @@ class INS
private:
void updateAxis(byte, int, float);
+ void movementEndCheck(byte);
float _acceleration[3];
float _velocity[3];
int _position[3];
+ int _zeroCount[3];
};
#endif

0 comments on commit 0ea2a44

Please sign in to comment.