Index: Accel.c =================================================================== diff -u -rcd8032ab289cafa3585fcb67dc9fbff5d8e18e70 -re6b38d1e5fbb5eab5cf62e850890632ebcfeb1a7 --- Accel.c (.../Accel.c) (revision cd8032ab289cafa3585fcb67dc9fbff5d8e18e70) +++ Accel.c (.../Accel.c) (revision e6b38d1e5fbb5eab5cf62e850890632ebcfeb1a7) @@ -14,7 +14,9 @@ * **************************************************************************/ -#include +#ifndef _VECTORCAST_ + #include +#endif #include "Accel.h" #include "FPGA.h" @@ -36,7 +38,8 @@ #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 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. /// Enumeration of accelerometer monitor states. @@ -139,7 +142,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; } @@ -190,9 +197,9 @@ if ( accelFPGAFaultReg != 0 ) { #ifdef _DG_ - SET_ALARM_WITH_1_U32_DATA( ALARM_ID_DG_ACCELEROMETER_FAILURE, 1 ) + 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 ) + SET_ALARM_WITH_1_U32_DATA( ALARM_ID_HD_ACCELEROMETER_FAILURE, 1 ) #endif } @@ -270,9 +277,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 } @@ -306,9 +313,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 } @@ -355,13 +362,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; @@ -447,21 +458,19 @@ #endif } } - else + else if ( ( FABS( x ) <= MIN_TILT_ANGLE_TO_CLEAR_ALARM ) && ( FABS( y ) <= MIN_TILT_ANGLE_TO_CLEAR_ALARM ) ) { - if ( accelTiltErrorTimerCounter > 0 ) - { - accelTiltErrorTimerCounter--; - } - else - { + accelTiltErrorTimerCounter = 0; #ifdef _DG_ - clearAlarm( ALARM_ID_DG_EXCESSIVE_TILT ); + clearAlarm( ALARM_ID_DG_EXCESSIVE_TILT ); #else - clearAlarm( ALARM_ID_HD_EXCESSIVE_TILT ); + clearAlarm( ALARM_ID_HD_EXCESSIVE_TILT ); #endif - } } + else + { + accelTiltErrorTimerCounter = ( accelTiltErrorTimerCounter > 0 ? accelTiltErrorTimerCounter - 1 : 0 ); + } } /*********************************************************************//** @@ -477,18 +486,31 @@ F32 maxX = FABS(getMaxAccelAxis( ACCEL_AXIS_X )); F32 maxY = FABS(getMaxAccelAxis( ACCEL_AXIS_Y )); F32 maxZ = FABS(getMaxAccelAxis( ACCEL_AXIS_Z ) - 1.0); // when system level, expect Z axis to be at 1 g normally - F32 maxAll = MAX(maxX, maxY); + F32 maxAll = maxX; + ACCEL_AXIS_T maxAxis = ACCEL_AXIS_X; - // determine largest acceleration - maxAll = MAX(maxAll, maxZ); + // 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 ) { #ifdef _DG_ - SET_ALARM_WITH_1_F32_DATA( ALARM_ID_DG_SHOCK, maxAll ) + SET_ALARM_WITH_2_F32_DATA( ALARM_ID_DG_SHOCK, (F32)maxAxis, maxAll ) #else - SET_ALARM_WITH_1_F32_DATA( ALARM_ID_HD_SHOCK, maxAll ) + SET_ALARM_WITH_2_F32_DATA( ALARM_ID_HD_SHOCK, (F32)maxAxis, maxAll ) #endif } } @@ -555,10 +577,13 @@ 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; } @@ -600,10 +625,8 @@ cal.accelXOffset = offsetX; cal.accelYOffset = offsetY; cal.accelZOffset = offsetZ; - if ( TRUE == setCalibrationData( cal ) ) - { - result = TRUE; - } + setCalibrationData( cal ); + result = TRUE; } return result; Index: Common.h =================================================================== diff -u -rbbf79dce31e555e123cd8dc947206724ce793afd -re6b38d1e5fbb5eab5cf62e850890632ebcfeb1a7 --- Common.h (.../Common.h) (revision bbf79dce31e555e123cd8dc947206724ce793afd) +++ Common.h (.../Common.h) (revision e6b38d1e5fbb5eab5cf62e850890632ebcfeb1a7) @@ -180,6 +180,8 @@ #define _disable_FIQ() #define fabs(v) ((v) < 0.0 ? ((v) * -1.0) : (v)) + extern F32 asin( F32 val ); + extern F32 sqrt( F32 val ); #endif