Index: Accel.c =================================================================== diff -u -rcf2afaec02799525f80e803b55a0f55948c75cad -r619684492ca3f5b37b507917a61df0bf7cee6a95 --- Accel.c (.../Accel.c) (revision cf2afaec02799525f80e803b55a0f55948c75cad) +++ Accel.c (.../Accel.c) (revision 619684492ca3f5b37b507917a61df0bf7cee6a95) @@ -164,9 +164,10 @@ static ACCEL_STATE_T handleAccelMonitorState( void ) { ACCEL_STATE_T result = ACCELEROMETER_MONITOR_STATE; - S16 x, y, z; // axis readings - S16 xm, ym, zm; // max axis readings since last time we read FPGA registers - U16 cnt; // FPGA read counter + S16 x, y, z; // axis readings + S16 xm, ym, zm; // max axis readings since last time we read FPGA registers + F32 xMax, yMax, zMax; // max axis readings (in gs) + U16 cnt; // FPGA read counter // read FPGA accelerometer registers getFPGAAccelAxes( &x, &y, &z ); @@ -205,9 +206,21 @@ accelAxes[ ACCEL_AXIS_X ].data = (F32)x * G_PER_LSB + accelCalOffsets[ ACCEL_AXIS_X ]; accelAxes[ ACCEL_AXIS_Y ].data = (F32)y * G_PER_LSB + accelCalOffsets[ ACCEL_AXIS_Y ]; accelAxes[ ACCEL_AXIS_Z ].data = (F32)z * G_PER_LSB + accelCalOffsets[ ACCEL_AXIS_Z ]; - accelMaxs[ ACCEL_AXIS_X ].data = (F32)xm * G_PER_LSB + accelCalOffsets[ ACCEL_AXIS_X ]; - accelMaxs[ ACCEL_AXIS_Y ].data = (F32)ym * G_PER_LSB + accelCalOffsets[ ACCEL_AXIS_Y ]; - accelMaxs[ ACCEL_AXIS_Z ].data = (F32)zm * G_PER_LSB + accelCalOffsets[ ACCEL_AXIS_Z ]; + xMax = (F32)xm * G_PER_LSB; + yMax = (F32)ym * G_PER_LSB; + zMax = (F32)zm * G_PER_LSB; + if ( xMax > accelMaxs[ ACCEL_AXIS_X ].data ) + { + accelMaxs[ ACCEL_AXIS_X ].data = xMax; + } + if ( yMax > accelMaxs[ ACCEL_AXIS_Y ].data ) + { + accelMaxs[ ACCEL_AXIS_Y ].data = yMax; + } + if ( zMax > accelMaxs[ ACCEL_AXIS_Z ].data ) + { + accelMaxs[ ACCEL_AXIS_Z ].data = zMax; + } // filter readings to get a stable vector for tilt filterAccelReadings(); @@ -337,7 +350,12 @@ F32 zt = accelTilt[ ACCEL_AXIS_Z ]; broadcastAccelData( x, y, z, xm, ym, zm, xt, yt, zt ); + // reset publication timer counter accelDataPublicationTimerCounter = 0; + // reset max axes + accelMaxs[ ACCEL_AXIS_X ].data = 0.0; + accelMaxs[ ACCEL_AXIS_Y ].data = 0.0; + accelMaxs[ ACCEL_AXIS_Z ].data = 0.0; } } @@ -475,7 +493,7 @@ { F32 maxX = fabs(getMaxAccelAxis( ACCEL_AXIS_X )); F32 maxY = fabs(getMaxAccelAxis( ACCEL_AXIS_Y )); - F32 maxZ = fabs(getMaxAccelAxis( ACCEL_AXIS_Z ) - 1.0); // when system level, expect Z axis to be at 1 g normally + F32 maxZ = fabs(getMaxAccelAxis( ACCEL_AXIS_Z )); // - 1.0); // when system level, expect Z axis to be at 1 g normally F32 maxAll = maxX; ACCEL_AXIS_T maxAxis = ACCEL_AXIS_X;