Index: Accel.c =================================================================== diff -u -r3ef35af7d1ce059845d1dc682638dec735eea831 -r59871c9964559b5137781af9c2eeed6bab18ef73 --- Accel.c (.../Accel.c) (revision 3ef35af7d1ce059845d1dc682638dec735eea831) +++ Accel.c (.../Accel.c) (revision 59871c9964559b5137781af9c2eeed6bab18ef73) @@ -1,14 +1,14 @@ /************************************************************************** * -* Copyright (c) 2020-2022 Diality Inc. - All Rights Reserved. +* Copyright (c) 2020-2024 Diality Inc. - All Rights Reserved. * * THIS CODE MAY NOT BE COPIED OR REPRODUCED IN ANY FORM, IN PART OR IN * WHOLE, WITHOUT THE EXPLICIT PERMISSION OF THE COPYRIGHT OWNER. * * @file Accel.c * -* @author (last) Dara Navaei -* @date (last) 04-Jan-2022 +* @author (last) Vinayakam Mani +* @date (last) 31-Aug-2023 * * @author (original) Sean Nash * @date (original) 29-Jul-2020 @@ -35,20 +35,23 @@ #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. +#define G_PER_LSB ( 0.00390625F ) ///< 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 MAX_TILT_ANGLE_TO_CLEAR_ALARM ( 5.0 ) ///< Maximum tilt of system before alarm is cleared. +#define NOMINAL_ACCEL_VECTOR_LENGTH ( 1.0F ) ///< Expect unit vector length when system is stable. +#define MAX_ACCEL_VECTOR_LENGTH_ERROR ( 0.1F ) ///< POST test looks at vector length at presumably stable moment - should be 1 +/- 0.1. +#define MAX_TILT_ANGLE ( 3.0F ) ///< Maximum tilt of system before alarm. +#define MAX_TILT_ANGLE_TO_CLEAR_ALARM ( 2.0F ) ///< 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). +#define MAX_SHOCK_ACCELERATION ( 2.5F ) ///< Maximum shock (acceleration) measured on any axis before alarm. +#define MAX_SHOCK_TO_CLEAR_ALARM ( 1.0F ) ///< Maximum shock (acceleration) measured on any axis in order to clear alarm. +#define MAX_TILT_G ( 1.0F ) ///< Maximum tilt (in g). +#define MAX_TILT_ANGLE_DEG ( 90.0F ) ///< Maximum tilt angle (in degrees). #define DATA_PUBLISH_COUNTER_START_COUNT 30 ///< Data publish counter start count. +// POST test +#define ACCEL_POST_TEST_COUNT 5 ///< Number of times to repeat POST test +#define ACCEL_POST_TEST_FAIL_MAX 3 ///< Maximum number of times POST test may fail (<= ACCEl_POST_TEST_COUNT) /// Enumeration of accelerometer monitor states. typedef enum Accelerometer_States @@ -88,6 +91,8 @@ 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 ACCELEROMETER_SELF_TEST_STATE_T accelSelfTestState = ACCELEROMETER_SELF_TEST_STATE_START; ///< current accelerometer self-test state. +static U08 postTestCounter = 0; ///< POST test repeat counter +static U08 postFailCounter = 0; ///< POST test fail counter 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. @@ -159,8 +164,8 @@ switch ( accelState ) { - case ACCELEROMETER_START_STATE: - accelState = ACCELEROMETER_MONITOR_STATE; + case ACCELEROMETER_START_STATE: + accelState = ACCELEROMETER_MONITOR_STATE; break; case ACCELEROMETER_MONITOR_STATE: @@ -185,7 +190,7 @@ * The handleAccelMonitorState function handles the accelerometer monitor * state of the accelerometer monitor state machine. * @details Inputs: accelNoNewSampleTimerCounter, accelFPGASampleCtr, - * accelAxes, accelFPGAFaultReg, accelSensorCalRecord + * accelAxes, accelFPGAFaultReg, accelSensorCalRecord, accelSelfTestState * @details Outputs: accelNoNewSampleTimerCounter, accelFPGASampleCtr, * accelAxes * alarm if accelerometer failed @@ -222,16 +227,6 @@ } } - // check error status - if ( accelFPGAFaultReg != 0 ) - { -#ifdef _DG_ - SET_ALARM_WITH_1_U32_DATA( ALARM_ID_DG_ACCELEROMETER_FAILURE, 1 ) -#else - SET_ALARM_WITH_1_U32_DATA( ALARM_ID_HD_ACCELEROMETER_FAILURE, 1 ) -#endif - } - // convert to gravities and apply calibration (axis offsets) accelAxes[ ACCEL_AXIS_X ].data = (F32)x * G_PER_LSB + accelSensorCalRecord.accelXOffset; accelAxes[ ACCEL_AXIS_Y ].data = (F32)y * G_PER_LSB + accelSensorCalRecord.accelYOffset; @@ -255,9 +250,14 @@ // filter readings to get a stable vector for tilt filterAccelReadings(); - // check tilt and shock - checkForTiltError(); - checkForShockError(); + /* check tilt and shock once accelerometer self test completed that indicates + * calibration records read from Non volatile memory during power up. + */ + if ( ACCELEROMETER_SELF_TEST_STATE_COMPLETE == accelSelfTestState ) + { + checkForTiltError(); + checkForShockError(); + } return result; } @@ -604,14 +604,17 @@ /*********************************************************************//** * @brief * The execAccelTest function executes the state machine for the - * accelerometer self-test. + * accelerometer power on self-test (POST). Verifies Accelerometer + * reading is within tolerance. 5 Repeats on 50ms interval with + * maximum of 3 fails is intended to mitigate a false fail from + * moving or bumping the machine during test. * @details Inputs: accelSelfTestState, accelCalOffsets * @details Outputs: accelSelfTestState, accelCalOffsets, Alarm is * self-test failed * @return the current state of the accelerometer self-test. *************************************************************************/ SELF_TEST_STATUS_T execAccelTest( void ) -{ +{ SELF_TEST_STATUS_T result = SELF_TEST_STATUS_IN_PROGRESS; switch ( accelSelfTestState ) @@ -629,30 +632,49 @@ { accelSelfTestState = ACCELEROMETER_SELF_TEST_STATE_IN_PROGRESS; } + postTestCounter = 0; + postFailCounter = 0; } break; case ACCELEROMETER_SELF_TEST_STATE_IN_PROGRESS: { - F32 vectorLen = calcVectorLength( accelAxes[ ACCEL_AXIS_X ].data, accelAxes[ ACCEL_AXIS_Y ].data, accelAxes[ ACCEL_AXIS_Z ].data ); + F32 vectorLen = calcVectorLength( getMeasuredAccelAxis( ACCEL_AXIS_X ), + getMeasuredAccelAxis( ACCEL_AXIS_Y ), + getMeasuredAccelAxis( ACCEL_AXIS_Z ) ); - if ( fabs(NOMINAL_ACCEL_VECTOR_LENGTH - vectorLen) < MAX_ACCEL_VECTOR_LENGTH_ERROR ) + if ( fabs( NOMINAL_ACCEL_VECTOR_LENGTH - vectorLen ) >= MAX_ACCEL_VECTOR_LENGTH_ERROR ) { - result = SELF_TEST_STATUS_PASSED; - } - else - { - result = SELF_TEST_STATUS_FAILED; result = SELF_TEST_STATUS_FAILED; - result = SELF_TEST_STATUS_FAILED; - + // count the failures + postFailCounter++; + if ( postFailCounter >= ACCEL_POST_TEST_FAIL_MAX ) + { + result = SELF_TEST_STATUS_FAILED; + // Already failed, no need to repeat test + postTestCounter = ACCEL_POST_TEST_COUNT; #ifdef _DG_ - SET_ALARM_WITH_1_F32_DATA( ALARM_ID_DG_ACCELEROMETER_SELF_TEST_FAILURE, vectorLen ) + SET_ALARM_WITH_1_F32_DATA( ALARM_ID_DG_ACCELEROMETER_SELF_TEST_FAILURE, vectorLen ) #else - SET_ALARM_WITH_1_F32_DATA( ALARM_ID_HD_ACCELEROMETER_SELF_TEST_FAILURE, vectorLen ) + SET_ALARM_WITH_1_F32_DATA( ALARM_ID_HD_ACCELEROMETER_SELF_TEST_FAILURE, vectorLen ) #endif + } } - accelSelfTestState = ACCELEROMETER_SELF_TEST_STATE_COMPLETE; + if ( postTestCounter < ACCEL_POST_TEST_COUNT ) + { + // Repeat the test + postTestCounter++; + result = SELF_TEST_STATUS_IN_PROGRESS; + } + else + { + // Test complete, return Pass / Fail. + if ( SELF_TEST_STATUS_FAILED != result ) + { + result = SELF_TEST_STATUS_PASSED; + } + accelSelfTestState = ACCELEROMETER_SELF_TEST_STATE_COMPLETE; + } } break; @@ -665,9 +687,21 @@ SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_SOFTWARE_FAULT, SW_FAULT_ID_ACCEL_INVALID_SELF_TEST_STATE, accelSelfTestState ) #endif break; - } + } return result; +} + +/*********************************************************************//** + * @brief + * The resetAccelPOSTState function resets the accelerometers POST state. + * @details Inputs: none + * @details Outputs: accelSelfTestState + * @return none + *************************************************************************/ +void resetAccelPOSTState( void ) +{ + accelSelfTestState = ACCELEROMETER_SELF_TEST_STATE_START; }