Index: Accel.c =================================================================== diff -u -r85db35aef9a57d1efc02bb3b42fc802b65aa3646 -r01695bc10ee97cdeb3e68387668c1d234d05438e --- Accel.c (.../Accel.c) (revision 85db35aef9a57d1efc02bb3b42fc802b65aa3646) +++ Accel.c (.../Accel.c) (revision 01695bc10ee97cdeb3e68387668c1d234d05438e) @@ -29,17 +29,22 @@ */ // ********** private definitions ********** - -#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.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 + +/// Interval (ms/task time) at which the accelerometer data is published on the CAN bus +#define ACCEL_DATA_PUB_INTERVAL ( MS_PER_SECOND / TASK_PRIORITY_INTERVAL ) +/// Vector for tilt is filtered w/ moving average +#define SIZE_OF_ROLLING_AVG ( MS_PER_SECOND / TASK_PRIORITY_INTERVAL ) +#define G_PER_LSB ( 0.00390625 ) ///< Conversion from counts (LSB) to gravities +/// Maximum time w/o new accelerometer sample from FPGA +static const U32 NO_NEW_ACCEL_SAMPLES_TIMEOUT = ( 100 / TASK_PRIORITY_INTERVAL ); #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 ( 4.0 ) ///< Maximum shock (acceleration) measured on any axis before alarm +#define MAX_TILT_ANGLE_TO_CLEAR_ALARM ( 5.0 ) ///< Maximum tilt of system before alarm is cleared +/// Maximum time (in task intervals) that a tilt in excess of limit can persist before alarm +static const U32 MAX_TILT_PERSISTENCE = ( 1 * MS_PER_SECOND / TASK_PRIORITY_INTERVAL ); +#define MAX_SHOCK_ACCELERATION ( 2.5 ) ///< Maximum shock (acceleration) measured on any axis before alarm +#define MAX_SHOCK_TO_CLEAR_ALARM ( 1.0 ) ///< Maximum shock (acceleration) measured on any axis in order to clear alarm #define MAX_TILT_G ( 1.0 ) ///< Maximum tilt (in g) #define MAX_TILT_ANGLE_DEG ( 90.0 ) ///< Maximum tilt angle (in degrees) @@ -80,7 +85,10 @@ static F32 accelAvgVector[ NUM_OF_ACCEL_AXES ]; ///< Filtered accelerometer vector for tilt static F32 accelTilt[ NUM_OF_ACCEL_AXES ]; ///< Axis angles for tilt determination (filtered and converted to degrees) static U32 accelTiltErrorTimerCounter = 0; ///< used for persistence requirement on tilt error - + +static BOOL tiltErrorDetected; ///< Flag indicates a tilt error has been detected and tilt must now come below alarm clear threshold to clear alarm. +static BOOL shockErrorDetected; ///< Flag indicates a shock error has been detected and g-force must now come below alarm clear threshold to clear alarm. + static ACCELEROMETER_SELF_TEST_STATE_T accelSelfTestState = ACCELEROMETER_SELF_TEST_STATE_START; ///< current accelerometer self-test state // ********** private function prototypes ********** @@ -119,6 +127,9 @@ accelMaxs[ axis ].ovInitData = 0.0; accelMaxs[ axis ].override = OVERRIDE_RESET; } + + tiltErrorDetected = FALSE; + shockErrorDetected = FALSE; } /*********************************************************************//** @@ -171,7 +182,7 @@ F32 xMax, yMax, zMax; // max axis readings (in gs) U16 cnt; // FPGA read counter - // read FPGA accelerometer registers + // Read FPGA accelerometer registers getFPGAAccelAxes( &x, &y, &z ); getFPGAAccelMaxes( &xm, &ym, &zm ); getFPGAAccelStatus( &cnt, &accelFPGAFaultReg ); @@ -351,9 +362,9 @@ F32 zt = accelTilt[ ACCEL_AXIS_Z ]; broadcastAccelData( x, y, z, xm, ym, zm, xt, yt, zt ); - // reset publication timer counter + // Reset publication timer counter accelDataPublicationTimerCounter = 0; - // reset max axes + // Reset max axes accelMaxs[ ACCEL_AXIS_X ].data = 0.0; accelMaxs[ ACCEL_AXIS_Y ].data = 0.0; accelMaxs[ ACCEL_AXIS_Z ].data = 0.0; @@ -473,20 +484,30 @@ // excessive tilt must persist before triggering alarm if ( ++accelTiltErrorTimerCounter > MAX_TILT_PERSISTENCE ) { + tiltErrorDetected = TRUE; #ifdef _DG_ SET_ALARM_WITH_2_F32_DATA( ALARM_ID_DG_EXCESSIVE_TILT, x, y ) #else SET_ALARM_WITH_2_F32_DATA( ALARM_ID_HD_EXCESSIVE_TILT, x, y ) #endif } } - else if ( ( fabs( x ) <= MIN_TILT_ANGLE_TO_CLEAR_ALARM ) && ( fabs( y ) <= MIN_TILT_ANGLE_TO_CLEAR_ALARM ) ) + else if ( ( TRUE == tiltErrorDetected ) && ( ( fabs( x ) > MAX_TILT_ANGLE_TO_CLEAR_ALARM ) || ( fabs( y ) > MAX_TILT_ANGLE_TO_CLEAR_ALARM ) ) ) + { +#ifdef _DG_ + SET_ALARM_WITH_2_F32_DATA( ALARM_ID_DG_EXCESSIVE_TILT, x, y ) +#else + SET_ALARM_WITH_2_F32_DATA( ALARM_ID_HD_EXCESSIVE_TILT, x, y ) +#endif + } + else if ( ( fabs( x ) <= MAX_TILT_ANGLE_TO_CLEAR_ALARM ) && ( fabs( y ) <= MAX_TILT_ANGLE_TO_CLEAR_ALARM ) ) { accelTiltErrorTimerCounter = 0; + tiltErrorDetected = FALSE; #ifdef _DG_ - clearAlarm( ALARM_ID_DG_EXCESSIVE_TILT ); + clearAlarmCondition( ALARM_ID_DG_EXCESSIVE_TILT ); #else - clearAlarm( ALARM_ID_HD_EXCESSIVE_TILT ); + clearAlarmCondition( ALARM_ID_HD_EXCESSIVE_TILT ); #endif } else @@ -528,11 +549,29 @@ // has system just experienced an excessive shock? if ( maxAll > MAX_SHOCK_ACCELERATION ) { + shockErrorDetected = TRUE; #ifdef _DG_ SET_ALARM_WITH_2_F32_DATA( ALARM_ID_DG_SHOCK, (F32)maxAxis, getMaxAccelAxis( maxAxis ) ) #else SET_ALARM_WITH_2_F32_DATA( ALARM_ID_HD_SHOCK, (F32)maxAxis, getMaxAccelAxis( maxAxis ) ) #endif + } + else if ( ( TRUE == shockErrorDetected ) && ( maxAll > MAX_SHOCK_TO_CLEAR_ALARM ) ) + { +#ifdef _DG_ + SET_ALARM_WITH_2_F32_DATA( ALARM_ID_DG_SHOCK, (F32)maxAxis, getMaxAccelAxis( maxAxis ) ) +#else + SET_ALARM_WITH_2_F32_DATA( ALARM_ID_HD_SHOCK, (F32)maxAxis, getMaxAccelAxis( maxAxis ) ) +#endif + } + else if ( maxAll <= MAX_SHOCK_TO_CLEAR_ALARM ) + { + shockErrorDetected = FALSE; +#ifdef _DG_ + clearAlarmCondition( ALARM_ID_DG_SHOCK ); +#else + clearAlarmCondition( ALARM_ID_HD_SHOCK ); +#endif } } @@ -554,7 +593,7 @@ { CALIBRATION_DATA_T cal; - // retrieve blood flow sensor calibration data + // Retrieve blood flow sensor calibration data if ( TRUE == getCalibrationData( &cal ) ) { accelCalOffsets[ ACCEL_AXIS_X ] = cal.accelXOffset;