Index: Accel.c =================================================================== diff -u -r569a9f44c93e6489f1dc1a62a7937e100ea344c2 -r707cf591bec11cd3e8b2474811661011a2873b40 --- Accel.c (.../Accel.c) (revision 569a9f44c93e6489f1dc1a62a7937e100ea344c2) +++ Accel.c (.../Accel.c) (revision 707cf591bec11cd3e8b2474811661011a2873b40) @@ -1,23 +1,25 @@ -/**********************************************************************//** - * - * 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-2022 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 (original) Sean Nash +* @date (original) 29-Jul-2020 +* +***************************************************************************/ #include #include "Accel.h" -#include "FPGA.h" +#include "FPGA.h" +#include "MessageSupport.h" #include "NVDataMgmt.h" #include "SystemCommMessages.h" #include "TaskPriority.h" @@ -28,73 +30,91 @@ */ // ********** 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.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. +/// 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 DATA_PUBLISH_COUNTER_START_COUNT 30 ///< Data publish counter start count. -#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 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 ( 2.0 ) ///< maximum shock (acceleration) measured on any axis before alarm. - /// 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_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 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 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 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 -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 - // ********** private function prototypes ********** static ACCEL_STATE_T handleAccelMonitorState( void ); static void publishAccelData( void ); static void resetAccelMovingAverage( void ); static void filterAccelReadings( void ); -static F32 calcVectorLength( F32 x, F32 y, F32 z ); static void checkForTiltError( void ); -static void checkForShockError( 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 - * Outputs : Accel module initialized. + * @details Inputs: none + * @details Outputs: Accel module initialized * @return none *************************************************************************/ void initAccel( void ) @@ -116,18 +136,27 @@ 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 - * Outputs : accelState + * @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: @@ -139,7 +168,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; } @@ -149,25 +182,28 @@ /*********************************************************************//** * @brief - * The handleAccelMonitorState function handles the accelerometer monitor state \n - * of the accelerometer monitor state machine. - * @details - * Inputs : - * Outputs : + * 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 - 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 + // Read FPGA accelerometer registers getFPGAAccelAxes( &x, &y, &z ); getFPGAAccelMaxes( &xm, &ym, &zm ); getFPGAAccelStatus( &cnt, &accelFPGAFaultReg ); - + // check fresh sample if ( cnt != accelFPGASampleCtr ) { @@ -190,28 +226,32 @@ 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 } - // 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 + 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; + } - // 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(); @@ -224,12 +264,12 @@ /*********************************************************************//** * @brief - * The getPublishAccelDataInterval function gets the accelerometer vector data \n - * publication interval. - * @details - * Inputs : accelDataPublishInterval - * Outputs : none - * @return the current accelerometer vector data publication interval (in priority task periods). + * 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 ) { @@ -245,11 +285,11 @@ /*********************************************************************//** * @brief - * The getMeasuredAccelAxis function gets the current magnitude for the given \n - * accelerometer axis. - * @details - * Inputs : accelAxes[] - * Outputs : accelAxes[] + * 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 ) @@ -270,9 +310,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 } @@ -281,11 +321,11 @@ /*********************************************************************//** * @brief - * The getMaxAccelAxis function gets the current max magnitude for the given \n + * The getMaxAccelAxis function gets the current max magnitude for the given * accelerometer axis. - * @details - * Inputs : accelMaxs[] - * Outputs : accelMaxs[] + * @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 ) @@ -306,9 +346,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 } @@ -317,62 +357,81 @@ /*********************************************************************//** * @brief - * The publishAccelData function publishes accelerometer data at the set \n + * The publishAccelData function publishes accelerometer data at the set * interval. - * @details - * Inputs : accelAxes[] - * Outputs : Accelerometer data is published to CAN bus. + * @details Inputs: accelAvgVector, accelTilt, accelAxes, accelDataPublicationTimerCounter + * @details Outputs: accelAxes, accelDataPublicationTimerCounter * @return none *************************************************************************/ static void publishAccelData( void ) { // 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 xm = getMaxAccelAxis( ACCEL_AXIS_X ); - F32 ym = getMaxAccelAxis( ACCEL_AXIS_Y ); - F32 zm = getMaxAccelAxis( ACCEL_AXIS_Z ); - F32 xt = accelTilt[ ACCEL_AXIS_X ]; - F32 yt = accelTilt[ ACCEL_AXIS_Y ]; - F32 zt = accelTilt[ ACCEL_AXIS_Z ]; + { + ACCEL_DATA_PAYLOAD_T data; - broadcastAccelData( x, y, z, xm, ym, zm, xt, yt, zt ); + 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 \n + * The resetAccelMovingAverage function re-initializes the accelerometer * moving average sample buffer. - * @details - * Inputs : none - * Outputs : accelReadingsTotal, accelReadingsIdx, accelReadingsCount all set to zero. + * @details Inputs: accelReadingsTotal, accelAvgVector, accelReadings, accelReadingsIdx, + * accelReadingsCount + * @details Outputs: accelReadingsTotal, accelAvgVector, accelReadings, accelReadingsIdx, + * accelReadingsCount * @return none *************************************************************************/ 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; } /*********************************************************************//** * @brief - * The filterAccelReadings function adds a new axis samples to the filter. \n - * @details - * Inputs : none - * Outputs : flowReadings[], flowReadingsIdx, flowReadingsCount + * 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 ) @@ -397,36 +456,29 @@ 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 ] ) ); + } + } } /*********************************************************************//** * @brief - * The calcVectorLength function calculates the length of a vector with \n - * given vector axis magnitudes. - * @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 - * @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 checkForTiltError function checks for a tilt error. - * @details - * Inputs : accelTilt[], accelTiltErrorTimerCounter - * Outputs : alarm if persistent excessive tilt detected + * @details Inputs: accelTilt, accelTiltErrorTimerCounter + * @details Outputs: accelTiltErrorTimerCounter, alarm if persistent excessive + * tilt detected * @return none *************************************************************************/ static void checkForTiltError( void ) @@ -435,207 +487,201 @@ 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 ) { + 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 + else if ( ( fabs( x ) <= MAX_TILT_ANGLE_TO_CLEAR_ALARM ) && ( fabs( y ) <= MAX_TILT_ANGLE_TO_CLEAR_ALARM ) ) { - if ( accelTiltErrorTimerCounter > 0 ) - { - accelTiltErrorTimerCounter--; - } - else - { + accelTiltErrorTimerCounter = 0; + tiltErrorDetected = FALSE; #ifdef _DG_ - clearAlarm( ALARM_ID_DG_EXCESSIVE_TILT ); + clearAlarmCondition( ALARM_ID_DG_EXCESSIVE_TILT ); #else - clearAlarm( ALARM_ID_HD_EXCESSIVE_TILT ); + 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 : accelMaxs[] - * Outputs : alarm if excessive shock detected + * @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_X )); - F32 maxZ = FABS(getMaxAccelAxis( ACCEL_AXIS_X ) - 1.0); // when system level, expect Z axis to be at 1 g normally - F32 maxAll = MAX(maxX, maxY); + 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 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 ) { + shockErrorDetected = TRUE; #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, getMaxAccelAxis( maxAxis ) ) #else - SET_ALARM_WITH_1_F32_DATA( ALARM_ID_HD_SHOCK, maxAll ) + 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 \n - * accelerometer self test. - * @details - * Inputs : accelSelfTestState - * Outputs : accelSelfTestState - * @return the current state of the accelerometer self test. + * 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: - { - CALIBRATION_DATA_T cal; - - // retrieve blood flow sensor calibration data - if ( TRUE == getCalibrationData( &cal ) ) - { - accelCalOffsets[ ACCEL_AXIS_X ] = cal.accelXOffset; - accelCalOffsets[ ACCEL_AXIS_Y ] = cal.accelYOffset; - accelCalOffsets[ ACCEL_AXIS_Z ] = cal.accelZOffset; - 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; - - 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: - break; - - default: - // TODO - s/w fault - break; - } - - return result; + 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; result = SELF_TEST_STATUS_FAILED; + 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; } /************************************************************************* * TEST SUPPORT FUNCTIONS *************************************************************************/ - /*********************************************************************//** * @brief - * The setAccelCalibration function sets the accelerometer calibration \n - * factors and has them stored in non-volatile memory. - * @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 - * @return TRUE if calibration factors successfully set/stored, FALSE if not - *************************************************************************/ -BOOL setAccelCalibration( S32 offsetX, S32 offsetY, S32 offsetZ ) -{ - BOOL result = FALSE; - - if ( TRUE == isTestingActivated() ) - { - CALIBRATION_DATA_T cal; - - 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; - } - } - - return result; -} - -/*********************************************************************//** - * @brief - * The getAccelCalibration function retrieves the current accelerometer \n - * calibration factors. - * @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 - * @return none - *************************************************************************/ -void getAccelCalibration( S32 *offsetX, S32 *offsetY, S32 *offsetZ ) -{ - *offsetX = accelCalOffsets[ ACCEL_AXIS_X ]; - *offsetY = accelCalOffsets[ ACCEL_AXIS_Y ]; - *offsetZ = accelCalOffsets[ ACCEL_AXIS_Z ]; -} - -/*********************************************************************//** - * @brief - * The testSetAccelDataPublishIntervalOverride function overrides the \n + * The testSetAccelDataPublishIntervalOverride function overrides the * accelerometer data publish interval. - * @details - * Inputs : none - * Outputs : accelDataPublishInterval - * @param value : override accelerometer data publish interval with (in ms) + * @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 ) @@ -656,11 +702,10 @@ /*********************************************************************//** * @brief - * The testResetAccelDataPublishIntervalOverride function resets the override \n - * of the accelerometer data publish interval. - * @details - * Inputs : none - * Outputs : accelDataPublishInterval + * 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 ) @@ -679,12 +724,12 @@ /*********************************************************************//** * @brief - * The testSetAccelAxisOverride function overrides the value of the \n + * The testSetAccelAxisOverride function overrides the value of the * 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 + * @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 ) @@ -706,12 +751,11 @@ /*********************************************************************//** * @brief - * The testResetAccelAxisOverride function resets the override of the \n + * The testResetAccelAxisOverride function resets the override of the * specified accelerometer axis. - * @details - * Inputs : none - * Outputs : accelAxes[] - * @param axis : ID of accelerometer axis to reset override for + * @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 ) @@ -733,12 +777,12 @@ /*********************************************************************//** * @brief - * The testSetAccelMaxOverride function overrides the max. value of the \n + * The testSetAccelMaxOverride function overrides the max. value of the * 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 + * @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 ) @@ -760,12 +804,11 @@ /*********************************************************************//** * @brief - * The testResetAccelMaxOverride function resets the override of the \n + * The testResetAccelMaxOverride function resets the override of the * specified accelerometer axis maximum. - * @details - * Inputs : none - * Outputs : accelMaxs[] - * @param axis : ID of accelerometer axis to reset override for + * @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 ) @@ -786,4 +829,3 @@ } /**@}*/ -