Index: Accel.c =================================================================== diff -u -r6a00bc5632cfdb84cf72e7dbbc55ff115e3481d5 -raa1221efc98233c54a97a2cbbf7fb1a5a35d385d --- Accel.c (.../Accel.c) (revision 6a00bc5632cfdb84cf72e7dbbc55ff115e3481d5) +++ Accel.c (.../Accel.c) (revision aa1221efc98233c54a97a2cbbf7fb1a5a35d385d) @@ -1,18 +1,19 @@ -/**********************************************************************//** - * - * Copyright (c) 2019-2020 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 - * - * @date 25-Sep-2019 - * @author S. Nash - * - * @brief Accelerometer monitor module. Monitors accelerometer sensor. - * - **************************************************************************/ +/************************************************************************** +* +* Copyright (c) 2019-2020 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) Sean Nash +* @date (last) 12-Aug-2020 +* +* @author (original) Sean Nash +* @date (original) 29-Jul-2020 +* +***************************************************************************/ #include @@ -31,13 +32,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 MAX_TILT_PERSISTENCE ( 2 * 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 ( 10.0 ) ///< maximum shock (acceleration) measured on any axis 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 @@ -60,7 +64,7 @@ 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 S32 accelCalOffsets[ NUM_OF_ACCEL_AXES ] = { 0, 0, 0 }; ///< accelerometer calibration offsets. +static F32 accelCalOffsets[ NUM_OF_ACCEL_AXES ] = { 0, 0, 0 }; ///< accelerometer calibration offsets. 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). @@ -139,7 +143,11 @@ break; default: - // TODO - s/w fault +#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; } @@ -159,9 +167,10 @@ 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 - U16 cnt; // FPGA read counter + 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 ); @@ -178,32 +187,44 @@ { if ( ++accelNoNewSampleTimerCounter > NO_NEW_ACCEL_SAMPLES_TIMEOUT ) { - // TODO - accelerometer fault +#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 ) { - // TODO - accelerometer fault +#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 } - // apply calibration (axis offsets) - x += accelCalOffsets[ ACCEL_AXIS_X ]; - y += accelCalOffsets[ ACCEL_AXIS_Y ]; - z += accelCalOffsets[ ACCEL_AXIS_Z ]; - xm += accelCalOffsets[ ACCEL_AXIS_X ]; - ym += accelCalOffsets[ ACCEL_AXIS_Y ]; - zm += accelCalOffsets[ ACCEL_AXIS_Z ]; + // convert to gravities and apply calibration (axis offsets) + accelAxes[ ACCEL_AXIS_X ].data = (F32)x * G_PER_LSB + accelCalOffsets[ ACCEL_AXIS_X ]; + accelAxes[ ACCEL_AXIS_Y ].data = (F32)y * G_PER_LSB + accelCalOffsets[ ACCEL_AXIS_Y ]; + accelAxes[ ACCEL_AXIS_Z ].data = (F32)z * G_PER_LSB + accelCalOffsets[ ACCEL_AXIS_Z ]; + xMax = (F32)xm * G_PER_LSB; + yMax = (F32)ym * G_PER_LSB; + zMax = (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; + } - // convert to gravities - accelAxes[ ACCEL_AXIS_X ].data = (F32)x * G_PER_LSB; - accelAxes[ ACCEL_AXIS_Y ].data = (F32)y * G_PER_LSB; - accelAxes[ ACCEL_AXIS_Z ].data = (F32)z * G_PER_LSB; - accelMaxs[ ACCEL_AXIS_X ].data = (F32)xm * G_PER_LSB; - accelMaxs[ ACCEL_AXIS_Y ].data = (F32)ym * G_PER_LSB; - accelMaxs[ ACCEL_AXIS_Z ].data = (F32)zm * G_PER_LSB; - // filter readings to get a stable vector for tilt filterAccelReadings(); @@ -262,9 +283,9 @@ else { #ifdef _DG_ - activateAlarmNoData( ALARM_ID_DG_SOFTWARE_FAULT ); + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_ACCEL_GET_INVALID_AXIS, axis ) #else - activateAlarmNoData( ALARM_ID_HD_SOFTWARE_FAULT ); + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_SOFTWARE_FAULT, SW_FAULT_ID_ACCEL_GET_INVALID_AXIS, axis ) #endif } @@ -298,9 +319,9 @@ else { #ifdef _DG_ - activateAlarmNoData( ALARM_ID_DG_SOFTWARE_FAULT ); + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_ACCEL_GET_MAX_INVALID_AXIS, axis ) #else - activateAlarmNoData( ALARM_ID_HD_SOFTWARE_FAULT ); + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_SOFTWARE_FAULT, SW_FAULT_ID_ACCEL_GET_MAX_INVALID_AXIS, axis ) #endif } @@ -321,9 +342,9 @@ // publish accelerometer data on interval if ( ++accelDataPublicationTimerCounter >= getPublishAccelDataInterval() ) { - F32 x = getMeasuredAccelAxis( ACCEL_AXIS_X ); - F32 y = getMeasuredAccelAxis( ACCEL_AXIS_Y ); - F32 z = getMeasuredAccelAxis( ACCEL_AXIS_Z ); + F32 x = accelAvgVector[ ACCEL_AXIS_X ]; + F32 y = accelAvgVector[ ACCEL_AXIS_Y ]; + F32 z = accelAvgVector[ ACCEL_AXIS_Z ]; F32 xm = getMaxAccelAxis( ACCEL_AXIS_X ); F32 ym = getMaxAccelAxis( ACCEL_AXIS_Y ); F32 zm = getMaxAccelAxis( ACCEL_AXIS_Z ); @@ -332,7 +353,12 @@ F32 zt = accelTilt[ ACCEL_AXIS_Z ]; broadcastAccelData( x, y, z, xm, ym, zm, xt, yt, zt ); + // 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; } } @@ -347,13 +373,17 @@ *************************************************************************/ static void resetAccelMovingAverage( void ) { - U32 axis; + 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; @@ -389,9 +419,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 ] ) ); + } + } } /*********************************************************************//** @@ -401,9 +443,9 @@ * @details * Inputs : none * Outputs : none - * @param x : X axis magnitude of vector - * @param y : Y axis magnitude of vector - * @param z : Z axis magnitude of vector + * @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 ) @@ -427,18 +469,31 @@ F32 y = accelTilt[ ACCEL_AXIS_Y ]; // is system tilted too much? - if ( ( FABS( x ) > MAX_TILT_ANGLE ) || ( FABS( y ) > MAX_TILT_ANGLE ) ) + if ( ( fabs( x ) > MAX_TILT_ANGLE ) || ( fabs( y ) > MAX_TILT_ANGLE ) ) { // excessive tilt must persist before triggering alarm if ( ++accelTiltErrorTimerCounter > MAX_TILT_PERSISTENCE ) { - // TODO - tilt 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 - { - accelTiltErrorTimerCounter = 0; - } } + else if ( ( fabs( x ) <= MIN_TILT_ANGLE_TO_CLEAR_ALARM ) && ( fabs( y ) <= MIN_TILT_ANGLE_TO_CLEAR_ALARM ) ) + { + accelTiltErrorTimerCounter = 0; +#ifdef _DG_ + clearAlarm( ALARM_ID_DG_EXCESSIVE_TILT ); +#else + clearAlarm( ALARM_ID_HD_EXCESSIVE_TILT ); +#endif + } + else + { + accelTiltErrorTimerCounter = ( accelTiltErrorTimerCounter > 0 ? accelTiltErrorTimerCounter - 1 : 0 ); + } } /*********************************************************************//** @@ -451,16 +506,35 @@ *************************************************************************/ static void checkForShockError( void ) { - F32 maxX = getMaxAccelAxis( ACCEL_AXIS_X ); - F32 maxY = getMaxAccelAxis( ACCEL_AXIS_X ); - F32 maxZ = getMaxAccelAxis( ACCEL_AXIS_X ) - 1.0; // when system level, expect Z axis to be at 1 g normally + 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 ( ( FABS( maxX ) > MAX_SHOCK_ACCELERATION ) || - ( FABS( maxY ) > MAX_SHOCK_ACCELERATION ) || - ( FABS( maxZ ) > MAX_SHOCK_ACCELERATION ) ) + if ( maxAll > MAX_SHOCK_ACCELERATION ) { - // TODO - shock alarm +#ifdef _DG_ + SET_ALARM_WITH_2_F32_DATA( ALARM_ID_DG_SHOCK, (F32)maxAxis, maxAll ) +#else + SET_ALARM_WITH_2_F32_DATA( ALARM_ID_HD_SHOCK, (F32)maxAxis, maxAll ) +#endif } } @@ -492,9 +566,14 @@ accelSelfTestState = ACCELEROMETER_SELF_TEST_STATE_IN_PROGRESS; } else - { + { // shouldn't get here - should have failed NV-Data POST prior result = SELF_TEST_STATUS_FAILED; accelSelfTestState = ACCELEROMETER_SELF_TEST_STATE_COMPLETE; +#ifdef _DG_ + SET_ALARM_WITH_1_U32_DATA( ALARM_ID_DG_ACCELEROMETER_SELF_TEST_FAILURE, 0 ) +#else + SET_ALARM_WITH_1_U32_DATA( ALARM_ID_HD_ACCELEROMETER_SELF_TEST_FAILURE, 0 ) +#endif } } break; @@ -503,23 +582,31 @@ { 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 ) + 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: - break; - default: - // TODO - s/w fault + 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; } @@ -539,33 +626,30 @@ * @details * Inputs : none * Outputs : accelCalOffsets[] - * @param offsetX : offset calibration factor for X axis - * @param offsetY : offset calibration factor for Y axis - * @param offsetZ : offset calibration factor for Z axis + * @param offsetX offset calibration factor for X axis + * @param offsetY offset calibration factor for Y axis + * @param offsetZ offset calibration factor for Z axis * @return TRUE if calibration factors successfully set/stored, FALSE if not *************************************************************************/ -BOOL setAccelCalibration( S32 offsetX, S32 offsetY, S32 offsetZ ) +BOOL setAccelCalibration( F32 offsetX, F32 offsetY, F32 offsetZ ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { CALIBRATION_DATA_T cal; - if ( TRUE == getCalibrationData( &cal ) ) - { // keep locally and apply immediately - accelCalOffsets[ ACCEL_AXIS_X ] = offsetX; - accelCalOffsets[ ACCEL_AXIS_Y ] = offsetY; - accelCalOffsets[ ACCEL_AXIS_Z ] = offsetZ; - // also update calibration record in non-volatile memory - cal.accelXOffset = offsetX; - cal.accelYOffset = offsetY; - cal.accelZOffset = offsetZ; - if ( TRUE == setCalibrationData( cal ) ) - { - result = TRUE; - } - } + getCalibrationData( &cal ); + // keep locally and apply immediately + accelCalOffsets[ ACCEL_AXIS_X ] = offsetX; + accelCalOffsets[ ACCEL_AXIS_Y ] = offsetY; + accelCalOffsets[ ACCEL_AXIS_Z ] = offsetZ; + // also update calibration record in non-volatile memory + cal.accelXOffset = offsetX; + cal.accelYOffset = offsetY; + cal.accelZOffset = offsetZ; + setCalibrationData( cal ); + result = TRUE; } return result; @@ -578,12 +662,12 @@ * @details * Inputs : accelCalOffsets[] * Outputs : none - * @param offsetX : value to populate with X axis offset calibration factor - * @param offsetY : value to populate with Y axis offset calibration factor - * @param offsetZ : value to populate with Z axis offset calibration factor + * @param offsetX value to populate with X axis offset calibration factor + * @param offsetY value to populate with Y axis offset calibration factor + * @param offsetZ value to populate with Z axis offset calibration factor * @return none *************************************************************************/ -void getAccelCalibration( S32 *offsetX, S32 *offsetY, S32 *offsetZ ) +void getAccelCalibration( F32 *offsetX, F32 *offsetY, F32 *offsetZ ) { *offsetX = accelCalOffsets[ ACCEL_AXIS_X ]; *offsetY = accelCalOffsets[ ACCEL_AXIS_Y ]; @@ -597,7 +681,7 @@ * @details * Inputs : none * Outputs : accelDataPublishInterval - * @param value : override accelerometer data publish interval with (in ms) + * @param value override accelerometer data publish interval with (in ms) * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testSetAccelDataPublishIntervalOverride( U32 value ) @@ -645,8 +729,8 @@ * specified accelerometer axis with a given value. * Inputs : none * Outputs : accelAxes[] - * @param axis : ID of sensor axis to override for - * @param value : override value for the given axis + * @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 ) @@ -673,7 +757,7 @@ * @details * Inputs : none * Outputs : accelAxes[] - * @param axis : ID of accelerometer axis to reset override for + * @param axis ID of accelerometer axis to reset override for * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testResetAccelAxisOverride( U32 axis ) @@ -699,8 +783,8 @@ * specified accelerometer axis with a given value. * Inputs : none * Outputs : accelMaxs[] - * @param axis : ID of sensor axis to override for - * @param value : override value for the given axis maximum + * @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 ) @@ -727,7 +811,7 @@ * @details * Inputs : none * Outputs : accelMaxs[] - * @param axis : ID of accelerometer axis to reset override for + * @param axis ID of accelerometer axis to reset override for * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testResetAccelMaxOverride( U32 axis )