Index: Accel.c =================================================================== diff -u -r2a312d7c2448c2ad754532e71c27cbe91f671a15 -raa1221efc98233c54a97a2cbbf7fb1a5a35d385d --- Accel.c (.../Accel.c) (revision 2a312d7c2448c2ad754532e71c27cbe91f671a15) +++ Accel.c (.../Accel.c) (revision aa1221efc98233c54a97a2cbbf7fb1a5a35d385d) @@ -32,14 +32,16 @@ #define ACCEL_DATA_PUB_INTERVAL ( MS_PER_SECOND / TASK_PRIORITY_INTERVAL ) ///< interval (ms/task time) at which the accelerometer data is published on the CAN bus #define SIZE_OF_ROLLING_AVG ( MS_PER_SECOND / TASK_PRIORITY_INTERVAL ) ///< vector for tilt is filtered w/ moving average -#define G_PER_LSB ( 0.004 ) ///< conversion from counts (LSB) to gravities +#define G_PER_LSB ( 0.00390625 ) ///< conversion from counts (LSB) to gravities #define NO_NEW_ACCEL_SAMPLES_TIMEOUT ( 100 / TASK_PRIORITY_INTERVAL ) ///< maximum time w/o new accelerometer sample from FPGA. #define NOMINAL_ACCEL_VECTOR_LENGTH ( 1.0 ) ///< expect unit vector length when system is stable #define MAX_ACCEL_VECTOR_LENGTH_ERROR ( 0.1 ) ///< POST test looks at vector length at presumably stable moment - should be 1 +/- 0.1 #define MAX_TILT_ANGLE ( 7.0 ) ///< maximum tilt of system before alarm. #define MIN_TILT_ANGLE_TO_CLEAR_ALARM ( 5.0 ) ///< minimum tilt of system before alarm is cleared. #define MAX_TILT_PERSISTENCE ( 1 * MS_PER_SECOND / TASK_PRIORITY_INTERVAL ) ///< maximum time (in task intervals) that a tilt in excess of limit can persist before alarm #define MAX_SHOCK_ACCELERATION ( 2.0 ) ///< maximum shock (acceleration) measured on any axis before alarm. +#define MAX_TILT_G ( 1.0 ) ///< maximum tilt (in g) +#define MAX_TILT_ANGLE_DEG ( 90.0 ) ///< maximum tilt angle (in degrees) /// Enumeration of accelerometer monitor states. typedef enum Accelerometer_States @@ -165,9 +167,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 ); @@ -206,9 +209,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(); @@ -327,9 +342,9 @@ // publish accelerometer data on interval if ( ++accelDataPublicationTimerCounter >= getPublishAccelDataInterval() ) { - F32 x = getMeasuredAccelAxis( ACCEL_AXIS_X ); - F32 y = getMeasuredAccelAxis( ACCEL_AXIS_Y ); - F32 z = getMeasuredAccelAxis( ACCEL_AXIS_Z ); + F32 x = accelAvgVector[ ACCEL_AXIS_X ]; + F32 y = accelAvgVector[ ACCEL_AXIS_Y ]; + F32 z = accelAvgVector[ ACCEL_AXIS_Z ]; F32 xm = getMaxAccelAxis( ACCEL_AXIS_X ); F32 ym = getMaxAccelAxis( ACCEL_AXIS_Y ); F32 zm = getMaxAccelAxis( ACCEL_AXIS_Z ); @@ -338,7 +353,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; } } @@ -399,9 +419,21 @@ accelAvgVector[ ACCEL_AXIS_Y ] = accelReadingsTotal[ ACCEL_AXIS_Y ] / (F32)accelReadingsCount; accelAvgVector[ ACCEL_AXIS_Z ] = accelReadingsTotal[ ACCEL_AXIS_Z ] / (F32)accelReadingsCount; // calculate axis angles - accelTilt[ ACCEL_AXIS_X ] = RAD2DEG( asin( accelAvgVector[ ACCEL_AXIS_X ] ) ); - accelTilt[ ACCEL_AXIS_Y ] = RAD2DEG( asin( accelAvgVector[ ACCEL_AXIS_Y ] ) ); - accelTilt[ ACCEL_AXIS_Z ] = RAD2DEG( asin( accelAvgVector[ ACCEL_AXIS_Z ] ) ); + for ( axis = ACCEL_AXIS_X; axis < NUM_OF_ACCEL_AXES; axis++ ) + { + if ( accelAvgVector[ axis ] > MAX_TILT_G ) + { + accelTilt[ axis ] = MAX_TILT_ANGLE_DEG; + } + else if ( accelAvgVector[ axis ] < ( MAX_TILT_G * -1.0 ) ) + { + accelTilt[ axis ] = MAX_TILT_ANGLE_DEG * -1.0; + } + else + { + accelTilt[ axis ] = RAD2DEG( asin( accelAvgVector[ axis ] ) ); + } + } } /*********************************************************************//** @@ -411,9 +443,9 @@ * @details * Inputs : none * Outputs : none - * @param x : X axis magnitude of vector - * @param y : Y axis magnitude of vector - * @param z : Z axis magnitude of vector + * @param x X axis magnitude of vector + * @param y Y axis magnitude of vector + * @param z Z axis magnitude of vector * @return the length of the given vector. *************************************************************************/ static F32 calcVectorLength( F32 x, F32 y, F32 z ) @@ -476,7 +508,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 )); F32 maxAll = maxX; ACCEL_AXIS_T maxAxis = ACCEL_AXIS_X; @@ -594,9 +626,9 @@ * @details * Inputs : none * Outputs : accelCalOffsets[] - * @param offsetX : offset calibration factor for X axis - * @param offsetY : offset calibration factor for Y axis - * @param offsetZ : offset calibration factor for Z axis + * @param offsetX offset calibration factor for X axis + * @param offsetY offset calibration factor for Y axis + * @param offsetZ offset calibration factor for Z axis * @return TRUE if calibration factors successfully set/stored, FALSE if not *************************************************************************/ BOOL setAccelCalibration( F32 offsetX, F32 offsetY, F32 offsetZ ) @@ -630,9 +662,9 @@ * @details * Inputs : accelCalOffsets[] * Outputs : none - * @param offsetX : value to populate with X axis offset calibration factor - * @param offsetY : value to populate with Y axis offset calibration factor - * @param offsetZ : value to populate with Z axis offset calibration factor + * @param offsetX value to populate with X axis offset calibration factor + * @param offsetY value to populate with Y axis offset calibration factor + * @param offsetZ value to populate with Z axis offset calibration factor * @return none *************************************************************************/ void getAccelCalibration( F32 *offsetX, F32 *offsetY, F32 *offsetZ ) @@ -649,7 +681,7 @@ * @details * Inputs : none * Outputs : accelDataPublishInterval - * @param value : override accelerometer data publish interval with (in ms) + * @param value override accelerometer data publish interval with (in ms) * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testSetAccelDataPublishIntervalOverride( U32 value ) @@ -697,8 +729,8 @@ * specified accelerometer axis with a given value. * Inputs : none * Outputs : accelAxes[] - * @param axis : ID of sensor axis to override for - * @param value : override value for the given axis + * @param axis ID of sensor axis to override for + * @param value override value for the given axis * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testSetAccelAxisOverride( U32 axis, F32 value ) @@ -725,7 +757,7 @@ * @details * Inputs : none * Outputs : accelAxes[] - * @param axis : ID of accelerometer axis to reset override for + * @param axis ID of accelerometer axis to reset override for * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testResetAccelAxisOverride( U32 axis ) @@ -751,8 +783,8 @@ * specified accelerometer axis with a given value. * Inputs : none * Outputs : accelMaxs[] - * @param axis : ID of sensor axis to override for - * @param value : override value for the given axis maximum + * @param axis ID of sensor axis to override for + * @param value override value for the given axis maximum * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testSetAccelMaxOverride( U32 axis, F32 value ) @@ -779,7 +811,7 @@ * @details * Inputs : none * Outputs : accelMaxs[] - * @param axis : ID of accelerometer axis to reset override for + * @param axis ID of accelerometer axis to reset override for * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testResetAccelMaxOverride( U32 axis )