/************************************************************************** * * Copyright (c) 2020-2023 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) 17-Jan-2023 * * @author (original) Sean Nash * @date (original) 29-Jul-2020 * ***************************************************************************/ #include #include "Accel.h" #include "FPGA.h" #include "MessageSupport.h" #include "NVDataMgmt.h" #include "SystemCommMessages.h" #include "TaskPriority.h" /** * @addtogroup Accel * @{ */ // ********** private definitions ********** /// 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.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.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 ( 7.0F ) ///< Maximum tilt of system before alarm. #define MAX_TILT_ANGLE_TO_CLEAR_ALARM ( 5.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.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. /// 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; /// 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; ///< 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. 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. #ifdef _HD_ static HD_ACCELEROMETER_SENSOR_CAL_RECORD_T accelSensorCalRecord; ///< HD accelerometer calibration record. static const ALARM_ID_T CAL_ALARM = ALARM_ID_HD_ACCELEROMETERS_INVALID_CAL_RECORD; ///< HD accelerometer calibration alarm. #endif #ifdef _DG_ static DG_ACCEL_SENSOR_CAL_RECORD_T accelSensorCalRecord; ///< DG accelerometer calibration record. static const ALARM_ID_T CAL_ALARM = ALARM_ID_DG_ACCELEROMETERS_INVALID_CAL_RECORD; ///< DG accelerometer calibration alarm. #endif // ********** private function prototypes ********** static ACCEL_STATE_T handleAccelMonitorState( void ); static void publishAccelData( void ); static void resetAccelMovingAverage( void ); static void filterAccelReadings( void ); static void checkForTiltError( void ); static void checkForShockError( void ); static F32 calcVectorLength( F32 x, F32 y, F32 z ); /*********************************************************************//** * @brief * The initAccel function initializes the Accel module. * @details Inputs: none * @details Outputs: Accel module initialized * @return none *************************************************************************/ void initAccel( void ) { ACCEL_AXIS_T axis; // zero rolling average buffer on accelerometer readings resetAccelMovingAverage(); // zero accel readings for ( axis = ACCEL_AXIS_X; axis < NUM_OF_ACCEL_AXES; axis++ ) { accelAxes[ axis ].data = 0.0; accelAxes[ axis ].ovData = 0.0; accelAxes[ axis ].ovInitData = 0.0; accelAxes[ axis ].override = OVERRIDE_RESET; accelMaxs[ axis ].data = 0.0; accelMaxs[ axis ].ovData = 0.0; accelMaxs[ axis ].ovInitData = 0.0; accelMaxs[ axis ].override = OVERRIDE_RESET; } tiltErrorDetected = FALSE; shockErrorDetected = FALSE; accelDataPublicationTimerCounter = DATA_PUBLISH_COUNTER_START_COUNT; } /*********************************************************************//** * @brief * The execAccel function executes the accelerometer monitor state machine. * @details Inputs: accelState * @details Outputs: accelState, alarm if software fault happened * @return none *************************************************************************/ void execAccel( void ) { // Check if a new calibration is available if ( TRUE == isNewCalibrationRecordAvailable() ) { getNVRecord2Driver( GET_CAL_ACCEL_SENSORS, (U08*)&accelSensorCalRecord, sizeof( accelSensorCalRecord ), 0, CAL_ALARM ); } switch ( accelState ) { case ACCELEROMETER_START_STATE: accelState = ACCELEROMETER_MONITOR_STATE; break; case ACCELEROMETER_MONITOR_STATE: accelState = handleAccelMonitorState(); break; default: #ifdef _DG_ SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_ACCEL_INVALID_STATE, accelState ) #else SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_SOFTWARE_FAULT, SW_FAULT_ID_ACCEL_INVALID_STATE, accelState ) #endif break; } // publish accelerometer data on interval publishAccelData(); } /*********************************************************************//** * @brief * The handleAccelMonitorState function handles the accelerometer monitor * state of the accelerometer monitor state machine. * @details Inputs: accelNoNewSampleTimerCounter, accelFPGASampleCtr, * accelAxes, accelFPGAFaultReg, accelSensorCalRecord * @details Outputs: accelNoNewSampleTimerCounter, accelFPGASampleCtr, * accelAxes * alarm if accelerometer failed * @return next state *************************************************************************/ 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 F32 xMax, yMax, zMax; // max axis readings (in gs) U16 cnt; // FPGA read counter // Read FPGA accelerometer registers getFPGAAccelAxes( &x, &y, &z ); getFPGAAccelMaxes( &xm, &ym, &zm ); getFPGAAccelStatus( &cnt, &accelFPGAFaultReg ); // check fresh sample if ( cnt != accelFPGASampleCtr ) { accelNoNewSampleTimerCounter = 0; accelFPGASampleCtr = cnt; } else { if ( ++accelNoNewSampleTimerCounter > NO_NEW_ACCEL_SAMPLES_TIMEOUT ) { #ifdef _DG_ SET_ALARM_WITH_1_U32_DATA( ALARM_ID_DG_ACCELEROMETER_FAILURE, 0 ) #else SET_ALARM_WITH_1_U32_DATA( ALARM_ID_HD_ACCELEROMETER_FAILURE, 0 ) #endif } } // 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; accelAxes[ ACCEL_AXIS_Z ].data = (F32)z * G_PER_LSB + accelSensorCalRecord.accelZOffset; xMax = fabs( (F32)xm * G_PER_LSB ); yMax = fabs( (F32)ym * G_PER_LSB ); zMax = fabs( (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(); // check tilt and shock checkForTiltError(); checkForShockError(); return result; } /*********************************************************************//** * @brief * The getPublishAccelDataInterval function gets the accelerometer vector * data publication interval. * @details Inputs: accelDataPublishInterval * @details Outputs: none * @return the current accelerometer vector data publication interval * (in priority task periods). *************************************************************************/ U32 getPublishAccelDataInterval( void ) { U32 result = accelDataPublishInterval.data; if ( OVERRIDE_KEY == accelDataPublishInterval.override ) { result = accelDataPublishInterval.ovData; } return result; } /*********************************************************************//** * @brief * The getMeasuredAccelAxis function gets the current magnitude for * the given accelerometer axis. * @details Inputs: accelAxes * @details Outputs: alarm if software fault happened * @param axis the axis to measure the acceleration from * @return the current magnitude for the given accelerometer axis (in g). *************************************************************************/ F32 getMeasuredAccelAxis( U32 axis ) { F32 result = 0.0; if ( axis < NUM_OF_ACCEL_AXES ) { if ( OVERRIDE_KEY == accelAxes[ axis ].override ) { result = accelAxes[ axis ].ovData; } else { result = accelAxes[ axis ].data; } } else { #ifdef _DG_ SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_ACCEL_GET_INVALID_AXIS, axis ) #else SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_SOFTWARE_FAULT, SW_FAULT_ID_ACCEL_GET_INVALID_AXIS, axis ) #endif } return result; } /*********************************************************************//** * @brief * The getMaxAccelAxis function gets the current max magnitude for the given * accelerometer axis. * @details Inputs: accelMaxs * @details Outputs: alarm software fault if happened * @param axis the axis to get the max acceleration * @return the current maximum magnitude for the given accelerometer axis (in g). *************************************************************************/ F32 getMaxAccelAxis( U32 axis ) { F32 result = 0.0; if ( axis < NUM_OF_ACCEL_AXES ) { if ( OVERRIDE_KEY == accelMaxs[ axis ].override ) { result = accelMaxs[ axis ].ovData; } else { result = accelMaxs[ axis ].data; } } else { #ifdef _DG_ SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_ACCEL_GET_MAX_INVALID_AXIS, axis ) #else SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_SOFTWARE_FAULT, SW_FAULT_ID_ACCEL_GET_MAX_INVALID_AXIS, axis ) #endif } return result; } /*********************************************************************//** * @brief * The publishAccelData function publishes accelerometer data at the set * interval. * @details Inputs: accelAvgVector, accelTilt, accelAxes, accelDataPublicationTimerCounter * @details Outputs: accelAxes, accelDataPublicationTimerCounter * @return none *************************************************************************/ static void publishAccelData( void ) { // publish accelerometer data on interval if ( ++accelDataPublicationTimerCounter >= getPublishAccelDataInterval() ) { ACCEL_DATA_PAYLOAD_T data; data.x = accelAvgVector[ ACCEL_AXIS_X ]; data.y = accelAvgVector[ ACCEL_AXIS_Y ]; data.z = accelAvgVector[ ACCEL_AXIS_Z ]; data.xMax = getMaxAccelAxis( ACCEL_AXIS_X ); data.yMax = getMaxAccelAxis( ACCEL_AXIS_Y ); data.zMax = getMaxAccelAxis( ACCEL_AXIS_Z ); data.xTilt = accelTilt[ ACCEL_AXIS_X ]; data.yTilt = accelTilt[ ACCEL_AXIS_Y ]; data.zTilt = accelTilt[ ACCEL_AXIS_Z ]; #ifdef _DG_ broadcastData( MSG_ID_DG_ACCELEROMETER_DATA, COMM_BUFFER_OUT_CAN_DG_BROADCAST, (U08*)&data, sizeof( ACCEL_DATA_PAYLOAD_T ) ); #endif #ifdef _HD_ broadcastData( MSG_ID_HD_ACCELEROMETER_DATA, COMM_BUFFER_OUT_CAN_HD_BROADCAST, (U08*)&data, sizeof( ACCEL_DATA_PAYLOAD_T ) ); #endif // 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; } } /*********************************************************************//** * @brief * The resetAccelMovingAverage function re-initializes the accelerometer * moving average sample buffer. * @details Inputs: accelReadingsTotal, accelAvgVector, accelReadings, accelReadingsIdx, * accelReadingsCount * @details Outputs: accelReadingsTotal, accelAvgVector, accelReadings, accelReadingsIdx, * accelReadingsCount * @return none *************************************************************************/ static void resetAccelMovingAverage( void ) { U32 axis, i; for ( axis = ACCEL_AXIS_X; axis < NUM_OF_ACCEL_AXES; axis++ ) { accelReadingsTotal[ axis ] = 0.0; accelAvgVector[ axis ] = 0.0; accelTilt[ axis ] = 0.0; for ( i = 0; i < SIZE_OF_ROLLING_AVG; i++ ) { accelReadings[ axis ][ i ] = 0.0; } } accelReadingsIdx = 0; accelReadingsCount = 0; } /*********************************************************************//** * @brief * The filterAccelReadings function adds a new axis samples to the filter. * @details Inputs: accelReadingsCount, accelReadingsTotal, accelReadings, * accelReadings, accelReadingsCount * @details Outputs: accelReadingsCount, accelReadingsTotal, accelReadings, * accelReadings, accelReadingsCount * @return none *************************************************************************/ static void filterAccelReadings( void ) { U32 axis; for ( axis = ACCEL_AXIS_X; axis < NUM_OF_ACCEL_AXES; axis++ ) { F32 rdg = getMeasuredAccelAxis( axis ); if ( accelReadingsCount >= SIZE_OF_ROLLING_AVG ) { accelReadingsTotal[ axis ] -= accelReadings[ axis ][ accelReadingsIdx ]; } accelReadings[ axis ][ accelReadingsIdx ] = rdg; accelReadingsTotal[ axis ] += rdg; } accelReadingsIdx = INC_WRAP( accelReadingsIdx, 0, SIZE_OF_ROLLING_AVG - 1 ); accelReadingsCount = INC_CAP( accelReadingsCount, SIZE_OF_ROLLING_AVG ); // calculate average vector axes accelAvgVector[ ACCEL_AXIS_X ] = accelReadingsTotal[ ACCEL_AXIS_X ] / (F32)accelReadingsCount; accelAvgVector[ ACCEL_AXIS_Y ] = accelReadingsTotal[ ACCEL_AXIS_Y ] / (F32)accelReadingsCount; accelAvgVector[ ACCEL_AXIS_Z ] = accelReadingsTotal[ ACCEL_AXIS_Z ] / (F32)accelReadingsCount; // calculate axis angles 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 ] ) ); } } } /*********************************************************************//** * @brief * The checkForTiltError function checks for a tilt error. * @details Inputs: accelTilt, accelTiltErrorTimerCounter * @details Outputs: accelTiltErrorTimerCounter, alarm if persistent excessive * tilt detected * @return none *************************************************************************/ static void checkForTiltError( void ) { F32 x = accelTilt[ ACCEL_AXIS_X ]; F32 y = accelTilt[ ACCEL_AXIS_Y ]; // is system tilted too much? if ( ( fabs( x ) > MAX_TILT_ANGLE ) || ( fabs( y ) > MAX_TILT_ANGLE ) ) { // 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 ( ( 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_ clearAlarmCondition( ALARM_ID_DG_EXCESSIVE_TILT ); #else clearAlarmCondition( ALARM_ID_HD_EXCESSIVE_TILT ); #endif } else { accelTiltErrorTimerCounter = ( accelTiltErrorTimerCounter > 0 ? accelTiltErrorTimerCounter - 1 : 0 ); } } /*********************************************************************//** * @brief * The checkForShockError function checks for a shock error. * @details Inputs: none * @details Outputs: alarm if excessive shock detected * @return none *************************************************************************/ static void checkForShockError( void ) { F32 maxX = fabs(getMaxAccelAxis( ACCEL_AXIS_X )); F32 maxY = fabs(getMaxAccelAxis( ACCEL_AXIS_Y )); F32 maxZ = fabs(getMaxAccelAxis( ACCEL_AXIS_Z )); F32 maxAll = maxX; ACCEL_AXIS_T maxAxis = ACCEL_AXIS_X; // determine axis with most acceleration if ( maxY > maxX || maxZ > maxX ) { if ( maxZ > maxY ) { maxAxis = ACCEL_AXIS_Z; maxAll = maxZ; } else { maxAxis = ACCEL_AXIS_Y; maxAll = maxY; } } // 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 } } /*********************************************************************//** * @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 * @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 ) { case ACCELEROMETER_SELF_TEST_STATE_START: { BOOL calStatus = getNVRecord2Driver( GET_CAL_ACCEL_SENSORS, (U08*)&accelSensorCalRecord, sizeof( accelSensorCalRecord ), 0, CAL_ALARM ); 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; } /*********************************************************************//** * @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; } /************************************************************************* * TEST SUPPORT FUNCTIONS *************************************************************************/ /*********************************************************************//** * @brief * The testSetAccelDataPublishIntervalOverride function overrides the * accelerometer data publish interval. * @details Inputs: accelDataPublishInterval * @details Outputs: accelDataPublishInterval * @param value override accelerometer data publish interval with (in ms) * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testSetAccelDataPublishIntervalOverride( U32 value ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { U32 intvl = value / TASK_PRIORITY_INTERVAL; result = TRUE; accelDataPublishInterval.ovData = intvl; accelDataPublishInterval.override = OVERRIDE_KEY; } return result; } /*********************************************************************//** * @brief * The testResetAccelDataPublishIntervalOverride function resets the * override of the accelerometer data publish interval. * @details Inputs: accelDataPublishInterval * @details Outputs: accelDataPublishInterval * @return TRUE if override reset successful, FALSE if not *************************************************************************/ BOOL testResetAccelDataPublishIntervalOverride( void ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { result = TRUE; accelDataPublishInterval.override = OVERRIDE_RESET; accelDataPublishInterval.ovData = accelDataPublishInterval.ovInitData; } return result; } /*********************************************************************//** * @brief * The testSetAccelAxisOverride function overrides the value of the * specified accelerometer axis with a given value. * @details Inputs: accelAxes * @details Outputs: accelAxes * @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 ) { BOOL result = FALSE; if ( axis < NUM_OF_ACCEL_AXES ) { if ( TRUE == isTestingActivated() ) { result = TRUE; accelAxes[ axis ].ovData = value; accelAxes[ axis ].override = OVERRIDE_KEY; } } return result; } /*********************************************************************//** * @brief * The testResetAccelAxisOverride function resets the override of the * specified accelerometer axis. * @details Inputs: accelAxes * @details Outputs: accelAxes * @param axis ID of accelerometer axis to reset override for * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testResetAccelAxisOverride( U32 axis ) { BOOL result = FALSE; if ( axis < NUM_OF_ACCEL_AXES ) { if ( TRUE == isTestingActivated() ) { result = TRUE; accelAxes[ axis ].override = OVERRIDE_RESET; accelAxes[ axis ].ovData = accelAxes[ axis ].ovInitData; } } return result; } /*********************************************************************//** * @brief * The testSetAccelMaxOverride function overrides the max. value of the * specified accelerometer axis with a given value. * @details Inputs: accelMaxs * @details Outputs: accelMaxs * @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 ) { BOOL result = FALSE; if ( axis < NUM_OF_ACCEL_AXES ) { if ( TRUE == isTestingActivated() ) { result = TRUE; accelMaxs[ axis ].ovData = value; accelMaxs[ axis ].override = OVERRIDE_KEY; } } return result; } /*********************************************************************//** * @brief * The testResetAccelMaxOverride function resets the override of the * specified accelerometer axis maximum. * @details Inputs: accelMaxs * @details Outputs: accelMaxs * @param axis ID of accelerometer axis to reset override for * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testResetAccelMaxOverride( U32 axis ) { BOOL result = FALSE; if ( axis < NUM_OF_ACCEL_AXES ) { if ( TRUE == isTestingActivated() ) { result = TRUE; accelMaxs[ axis ].override = OVERRIDE_RESET; accelMaxs[ axis ].ovData = accelMaxs[ axis ].ovInitData; } } return result; } /**@}*/