Index: Accel.c =================================================================== diff -u -r6e948bace1cb93579a09c9e5571c06ccce29a170 -r14434061a23aa02218e1f3c6cc3024123e79b153 --- Accel.c (.../Accel.c) (revision 6e948bace1cb93579a09c9e5571c06ccce29a170) +++ Accel.c (.../Accel.c) (revision 14434061a23aa02218e1f3c6cc3024123e79b153) @@ -34,56 +34,66 @@ #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.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 MAX_TILT_ANGLE_TO_CLEAR_ALARM ( 5.0 ) ///< Maximum tilt of system before alarm is cleared. +#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. /// 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.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). /// Enumeration of accelerometer monitor states. typedef enum Accelerometer_States { - ACCELEROMETER_START_STATE = 0, ///< Accelerometer start state - ACCELEROMETER_MONITOR_STATE, ///< Accelerometer monitor state - NUM_OF_ACCELEROMETER_STATES ///< Number of accelerometer states -} ACCEL_STATE_T; + ACCELEROMETER_START_STATE = 0, ///< Accelerometer start state + ACCELEROMETER_MONITOR_STATE, ///< Accelerometer monitor state + NUM_OF_ACCELEROMETER_STATES ///< Number of accelerometer states +} ACCEL_STATE_T; + +/// Enumeration of accelerometer self-test states. +typedef enum Accelerometer_Self_Test_States +{ + ACCELEROMETER_SELF_TEST_STATE_START = 0, ///< Accelerometer self-test start state + ACCELEROMETER_SELF_TEST_STATE_IN_PROGRESS, ///< Accelerometer self-test in progress state + ACCELEROMETER_SELF_TEST_STATE_COMPLETE, ///< Accelerometer self-test completed state + NUM_OF_ACCELEROMETER_SELF_TEST_STATES ///< Number of accelerometer self-test states +} ACCELEROMETER_SELF_TEST_STATE_T; // ********** private data ********** -static ACCEL_STATE_T accelState = ACCELEROMETER_START_STATE; ///< current state of accelerometer monitor state machine. -static U32 accelDataPublicationTimerCounter = 0; ///< used to schedule accelerometer data publication to CAN bus. +static ACCEL_STATE_T accelState = ACCELEROMETER_START_STATE; ///< current state of accelerometer monitor state machine. +static U32 accelDataPublicationTimerCounter = 0; ///< used to schedule accelerometer data publication to CAN bus. static OVERRIDE_U32_T accelDataPublishInterval = { ACCEL_DATA_PUB_INTERVAL, - ACCEL_DATA_PUB_INTERVAL, 0, 0 }; ///< interval (in ms/task interval) at which to publish accelerometer data to CAN bus. -static OVERRIDE_F32_T accelAxes[ NUM_OF_ACCEL_AXES ]; ///< Measured accelerometer axis readings (calibrated, converted to gravities). -static OVERRIDE_F32_T accelMaxs[ NUM_OF_ACCEL_AXES ]; ///< Maximum axis readings since last sample (calibrated, converted to gravities). -static U16 accelFPGAFaultReg = 0; ///< FPGA accelerometer fault register value indicates whether issues with reading accelerometer. -static U16 accelFPGASampleCtr = 0; ///< Sample counter from FPGA indicates when new sample(s) are available. -static U32 accelNoNewSampleTimerCounter = 0; ///< used to enforce timeout on no new samples. + ACCEL_DATA_PUB_INTERVAL, 0, 0 }; ///< interval (in ms/task interval) at which to publish accelerometer data to CAN bus. +static OVERRIDE_F32_T accelAxes[ NUM_OF_ACCEL_AXES ]; ///< Measured accelerometer axis readings (calibrated, converted to gravities). +static OVERRIDE_F32_T accelMaxs[ NUM_OF_ACCEL_AXES ]; ///< Maximum axis readings since last sample (calibrated, converted to gravities). +static U16 accelFPGAFaultReg = 0; ///< FPGA accelerometer fault register value indicates whether issues with reading accelerometer. +static U16 accelFPGASampleCtr = 0; ///< Sample counter from FPGA indicates when new sample(s) are available. +static U32 accelNoNewSampleTimerCounter = 0; ///< used to enforce timeout on no new samples. -static F32 accelReadings[ NUM_OF_ACCEL_AXES ][ SIZE_OF_ROLLING_AVG ]; ///< holds flow samples for a rolling average. -static F32 accelReadingsTotal[ NUM_OF_ACCEL_AXES ]; ///< rolling total - used to calc average. -static U32 accelReadingsIdx = 0; ///< index for next sample in rolling average array. -static U32 accelReadingsCount = 0; ///< number of samples in flow rolling average buffer. -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 F32 accelReadings[ NUM_OF_ACCEL_AXES ][ SIZE_OF_ROLLING_AVG ]; ///< holds flow samples for a rolling average. +static F32 accelReadingsTotal[ NUM_OF_ACCEL_AXES ]; ///< rolling total - used to calc average. +static U32 accelReadingsIdx = 0; ///< index for next sample in rolling average array. +static U32 accelReadingsCount = 0; ///< number of samples in flow rolling average buffer. +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 ACCELEROMETER_SELF_TEST_STATE_T accelSelfTestState = ACCELEROMETER_SELF_TEST_STATE_START; ///< current accelerometer self-test state. -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 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. #ifdef _HD_ -static HD_ACCELEROMETER_SENSOR_CAL_RECORD_T accelSensorCalRecord; ///< HD accelerometer calibration record. +static HD_ACCELEROMETER_SENSOR_CAL_RECORD_T accelSensorCalRecord; ///< HD accelerometer calibration record. #endif #ifdef _DG_ -static DG_ACCELEROMETER_SENSOR_CAL_RECORD_T accelSensorCalRecord; ///< DG accelerometer calibration record. +static DG_ACCELEROMETER_SENSOR_CAL_RECORD_T accelSensorCalRecord; ///< DG accelerometer calibration record. #endif // ********** private function prototypes ********** @@ -94,7 +104,8 @@ static void filterAccelReadings( void ); static void checkForTiltError( void ); static void checkForShockError( void ); -static BOOL processCalibrationData( void ); +static BOOL processCalibrationData( void ); +static F32 calcVectorLength( F32 x, F32 y, F32 z ); /*********************************************************************//** * @brief @@ -610,32 +621,98 @@ accelSensorCalRecord.accelZOffset = calData.accelZOffset; return status; +} + +/*********************************************************************//** + * @brief + * The calcVectorLength function calculates the length of a vector with + * given vector axis magnitudes. + * @details Inputs: none + * @details Outputs: none + * @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 ) +{ + F32 result = sqrt( x * x + y * y + z * z ); + + return result; } /*********************************************************************//** * @brief * The execAccelTest function executes the state machine for the * accelerometer self-test. * @details Inputs: accelSelfTestState, accelCalOffsets - * @details Outputs: accelSelfTestState, accelCalOffsets, Alarm is self-test failed + * @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; - BOOL calStatus = processCalibrationData(); - - if ( TRUE == calStatus ) + switch ( accelSelfTestState ) { - result = SELF_TEST_STATUS_PASSED; - } - else - { - result = SELF_TEST_STATUS_FAILED; - } - - return result; + case ACCELEROMETER_SELF_TEST_STATE_START: + { +#ifdef _HD_ + HD_ACCELEROMETER_SENSOR_CAL_RECORD_T cal = getHDAccelerometerSensorCalibrationRecord(); +#endif +#ifdef _DG_ + DG_ACCELEROMETER_SENSOR_CAL_RECORD_T cal = getDGAccelerometerSensorCalibrationRecord(); +#endif + BOOL calStatus = processCalibrationData(); + + if ( FALSE == calStatus ) + { + result = SELF_TEST_STATUS_FAILED; + accelSelfTestState = ACCELEROMETER_SELF_TEST_STATE_COMPLETE; + } + else + { + accelSelfTestState = ACCELEROMETER_SELF_TEST_STATE_IN_PROGRESS; + } + } + 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 ); + + if ( fabs(NOMINAL_ACCEL_VECTOR_LENGTH - vectorLen) < MAX_ACCEL_VECTOR_LENGTH_ERROR ) + { + result = SELF_TEST_STATUS_PASSED; + } + else + { + result = SELF_TEST_STATUS_FAILED; +#ifdef _DG_ + 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 ) +#endif + } + + accelSelfTestState = ACCELEROMETER_SELF_TEST_STATE_COMPLETE; + } + break; + + case ACCELEROMETER_SELF_TEST_STATE_COMPLETE: + default: + result = SELF_TEST_STATUS_FAILED; +#ifdef _DG_ + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_ACCEL_INVALID_SELF_TEST_STATE, accelSelfTestState ) +#else + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_SOFTWARE_FAULT, SW_FAULT_ID_ACCEL_INVALID_SELF_TEST_STATE, accelSelfTestState ) +#endif + break; + } + + return result; + }