Index: Accel.c =================================================================== diff -u -raa1221efc98233c54a97a2cbbf7fb1a5a35d385d -r0f1d0c443daee3e30ae823711e85f3410bbf49fe --- Accel.c (.../Accel.c) (revision aa1221efc98233c54a97a2cbbf7fb1a5a35d385d) +++ Accel.c (.../Accel.c) (revision 0f1d0c443daee3e30ae823711e85f3410bbf49fe) @@ -1,14 +1,14 @@ /************************************************************************** * -* Copyright (c) 2019-2020 Diality Inc. - All Rights Reserved. +* Copyright (c) 2019-2021 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 +* @date (last) 16-Nov-2020 * * @author (original) Sean Nash * @date (original) 29-Jul-2020 @@ -30,58 +30,58 @@ // ********** private definitions ********** -#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.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 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) +#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.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 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 ( 4.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 { - ACCELEROMETER_START_STATE = 0, ///< Accelerometer start state. - ACCELEROMETER_MONITOR_STATE, ///< Accelerometer monitor state. - NUM_OF_ACCELEROMETER_STATES ///< Number of 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. +/// 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_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 F32 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). -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 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 ACCELEROMETER_SELF_TEST_STATE_T accelSelfTestState = ACCELEROMETER_SELF_TEST_STATE_START; ///< current accelerometer self-test state // ********** private function prototypes ********** @@ -96,9 +96,8 @@ /*********************************************************************//** * @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 ) @@ -125,9 +124,8 @@ /*********************************************************************//** * @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 ) @@ -157,11 +155,12 @@ /*********************************************************************//** * @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, accelCalOffsets + * @details Outputs: accelNoNewSampleTimerCounter, accelFPGASampleCtr, accelAxes + * alarm if accelerometer failed * @return next state *************************************************************************/ static ACCEL_STATE_T handleAccelMonitorState( void ) @@ -209,9 +208,9 @@ 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; + 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; @@ -237,12 +236,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 ) { @@ -258,11 +257,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 ) @@ -294,11 +293,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 ) @@ -330,11 +329,10 @@ /*********************************************************************//** * @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 ) @@ -364,11 +362,12 @@ /*********************************************************************//** * @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 ) @@ -391,10 +390,11 @@ /*********************************************************************//** * @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 ) @@ -438,11 +438,10 @@ /*********************************************************************//** * @brief - * The calcVectorLength function calculates the length of a vector with \n + * The calcVectorLength function calculates the length of a vector with * given vector axis magnitudes. - * @details - * Inputs : none - * Outputs : none + * @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 @@ -458,9 +457,9 @@ /*********************************************************************//** * @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 ) @@ -499,9 +498,8 @@ /*********************************************************************//** * @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 ) @@ -531,21 +529,20 @@ if ( maxAll > MAX_SHOCK_ACCELERATION ) { #ifdef _DG_ - SET_ALARM_WITH_2_F32_DATA( ALARM_ID_DG_SHOCK, (F32)maxAxis, maxAll ) + 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, maxAll ) + SET_ALARM_WITH_2_F32_DATA( ALARM_ID_HD_SHOCK, (F32)maxAxis, getMaxAccelAxis( maxAxis ) ) #endif } } /*********************************************************************//** * @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 ) { @@ -621,11 +618,10 @@ /*********************************************************************//** * @brief - * The setAccelCalibration function sets the accelerometer calibration \n + * The setAccelCalibration function sets the accelerometer calibration * factors and has them stored in non-volatile memory. - * @details - * Inputs : none - * Outputs : accelCalOffsets[] + * @details Inputs: accelCalOffsets + * @details 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 @@ -657,11 +653,10 @@ /*********************************************************************//** * @brief - * The getAccelCalibration function retrieves the current accelerometer \n + * The getAccelCalibration function retrieves the current accelerometer * calibration factors. - * @details - * Inputs : accelCalOffsets[] - * Outputs : none + * @details Inputs: accelCalOffsets + * @details 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 @@ -676,11 +671,10 @@ /*********************************************************************//** * @brief - * The testSetAccelDataPublishIntervalOverride function overrides the \n + * The testSetAccelDataPublishIntervalOverride function overrides the * accelerometer data publish interval. - * @details - * Inputs : none - * Outputs : accelDataPublishInterval + * @details Inputs: accelDataPublishInterval + * @details Outputs: accelDataPublishInterval * @param value override accelerometer data publish interval with (in ms) * @return TRUE if override successful, FALSE if not *************************************************************************/ @@ -702,11 +696,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 ) @@ -725,10 +718,10 @@ /*********************************************************************//** * @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[] + * @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 @@ -752,11 +745,10 @@ /*********************************************************************//** * @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[] + * @details Inputs: accelAxes + * @details Outputs: accelAxes * @param axis ID of accelerometer axis to reset override for * @return TRUE if override successful, FALSE if not *************************************************************************/ @@ -779,10 +771,10 @@ /*********************************************************************//** * @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[] + * @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 @@ -806,11 +798,10 @@ /*********************************************************************//** * @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[] + * @details Inputs: accelMaxs + * @details Outputs: accelMaxs * @param axis ID of accelerometer axis to reset override for * @return TRUE if override successful, FALSE if not *************************************************************************/ @@ -832,4 +823,3 @@ } /**@}*/ -