Index: Accel.c =================================================================== diff -u -ra682f934108923fe195a41cdb0039fc395f99aa3 -rde0a8fb5965a5dd2b3fe061f513da04ca1b121db --- Accel.c (.../Accel.c) (revision a682f934108923fe195a41cdb0039fc395f99aa3) +++ Accel.c (.../Accel.c) (revision de0a8fb5965a5dd2b3fe061f513da04ca1b121db) @@ -31,14 +31,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 @@ -416,9 +418,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 ] ) ); + } + } } /*********************************************************************//**