Index: Accel.c =================================================================== diff -u -r13c32729e3186ebb5487e41a0cecd4cf15d357eb -rf7c61d2d14b40cfbbd2fa8e0968a1f972a39e4e0 --- Accel.c (.../Accel.c) (revision 13c32729e3186ebb5487e41a0cecd4cf15d357eb) +++ Accel.c (.../Accel.c) (revision f7c61d2d14b40cfbbd2fa8e0968a1f972a39e4e0) @@ -1,19 +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 -* -* @author (last) Sean Nash -* @date (last) 19-Aug-2020 -* -* @author (original) Sean Nash -* @date (original) 29-Jul-2020 -* -***************************************************************************/ +/************************************************************************** +* +* 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) 19-Aug-2020 +* +* @author (original) Sean Nash +* @date (original) 29-Jul-2020 +* +***************************************************************************/ #include @@ -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 ( 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 { - 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 ********** @@ -97,8 +97,8 @@ * @brief * The initAccel function initializes the Accel module. * @details - * Inputs : none - * Outputs : Accel module initialized. + * Inputs: none + * Outputs: Accel module initialized * @return none *************************************************************************/ void initAccel( void ) @@ -126,8 +126,8 @@ * @brief * The execAccel function executes the accelerometer monitor state machine. * @details - * Inputs : accelState - * Outputs : accelState + * Inputs: accelState + * Outputs: accelState, alarm if software fault happened * @return none *************************************************************************/ void execAccel( void ) @@ -157,11 +157,13 @@ /*********************************************************************//** * @brief - * The handleAccelMonitorState function handles the accelerometer monitor state \n - * of the accelerometer monitor state machine. + * The handleAccelMonitorState function handles the accelerometer monitor + * state of the accelerometer monitor state machine. * @details - * Inputs : - * Outputs : + * Inputs: accelNoNewSampleTimerCounter, accelFPGASampleCtr, accelAxes, + * accelFPGAFaultReg, accelCalOffsets + * Outputs: accelNoNewSampleTimerCounter, accelFPGASampleCtr, accelAxes + * alarm if accelerometer failed * @return next state *************************************************************************/ static ACCEL_STATE_T handleAccelMonitorState( void ) @@ -237,12 +239,13 @@ /*********************************************************************//** * @brief - * The getPublishAccelDataInterval function gets the accelerometer vector data \n - * publication interval. + * The getPublishAccelDataInterval function gets the accelerometer vector + * data publication interval. * @details - * Inputs : accelDataPublishInterval - * Outputs : none - * @return the current accelerometer vector data publication interval (in priority task periods). + * Inputs: accelDataPublishInterval + * Outputs: none + * @return the current accelerometer vector data publication interval + * (in priority task periods). *************************************************************************/ U32 getPublishAccelDataInterval( void ) { @@ -258,11 +261,12 @@ /*********************************************************************//** * @brief - * The getMeasuredAccelAxis function gets the current magnitude for the given \n - * accelerometer axis. + * The getMeasuredAccelAxis function gets the current magnitude for + * the given accelerometer axis. * @details - * Inputs : accelAxes[] - * Outputs : accelAxes[] + * Inputs: accelAxes + * 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 +298,12 @@ /*********************************************************************//** * @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[] + * Inputs: accelMaxs + * 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 +335,11 @@ /*********************************************************************//** * @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. + * Inputs: accelAvgVector, accelTilt, accelAxes, accelDataPublicationTimerCounter + * Outputs: accelAxes, accelDataPublicationTimerCounter * @return none *************************************************************************/ static void publishAccelData( void ) @@ -364,11 +369,13 @@ /*********************************************************************//** * @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. + * Inputs: accelReadingsTotal, accelAvgVector, accelReadings, accelReadingsIdx, + * accelReadingsCount + * Outputs: accelReadingsTotal, accelAvgVector, accelReadings, accelReadingsIdx, + * accelReadingsCount * @return none *************************************************************************/ static void resetAccelMovingAverage( void ) @@ -391,10 +398,12 @@ /*********************************************************************//** * @brief - * The filterAccelReadings function adds a new axis samples to the filter. \n + * The filterAccelReadings function adds a new axis samples to the filter. * @details - * Inputs : none - * Outputs : flowReadings[], flowReadingsIdx, flowReadingsCount + * Inputs: accelReadingsCount, accelReadingsTotal, accelReadings, + * accelReadings, accelReadingsCount + * Outputs: accelReadingsCount, accelReadingsTotal, accelReadings, + * accelReadings, accelReadingsCount * @return none *************************************************************************/ static void filterAccelReadings( void ) @@ -438,11 +447,11 @@ /*********************************************************************//** * @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 + * 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 @@ -459,8 +468,9 @@ * @brief * The checkForTiltError function checks for a tilt error. * @details - * Inputs : accelTilt[], accelTiltErrorTimerCounter - * Outputs : alarm if persistent excessive tilt detected + * Inputs: accelTilt, accelTiltErrorTimerCounter + * Outputs: accelTiltErrorTimerCounter, alarm if persistent excessive + * tilt detected * @return none *************************************************************************/ static void checkForTiltError( void ) @@ -500,8 +510,8 @@ * @brief * The checkForShockError function checks for a shock error. * @details - * Inputs : accelMaxs[] - * Outputs : alarm if excessive shock detected + * Inputs: none + * Outputs: alarm if excessive shock detected * @return none *************************************************************************/ static void checkForShockError( void ) @@ -540,12 +550,12 @@ /*********************************************************************//** * @brief - * The execAccelTest function executes the state machine for the \n - * accelerometer self test. + * The execAccelTest function executes the state machine for the + * accelerometer self-test. * @details - * Inputs : accelSelfTestState - * Outputs : accelSelfTestState - * @return the current state of the accelerometer self test. + * Inputs: accelSelfTestState, accelCalOffsets + * 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 +631,11 @@ /*********************************************************************//** * @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[] + * Inputs: accelCalOffsets + * 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 +667,11 @@ /*********************************************************************//** * @brief - * The getAccelCalibration function retrieves the current accelerometer \n + * The getAccelCalibration function retrieves the current accelerometer * calibration factors. * @details - * Inputs : accelCalOffsets[] - * Outputs : none + * 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 @@ -676,11 +686,11 @@ /*********************************************************************//** * @brief - * The testSetAccelDataPublishIntervalOverride function overrides the \n + * The testSetAccelDataPublishIntervalOverride function overrides the * accelerometer data publish interval. * @details - * Inputs : none - * Outputs : accelDataPublishInterval + * Inputs: accelDataPublishInterval + * Outputs: accelDataPublishInterval * @param value override accelerometer data publish interval with (in ms) * @return TRUE if override successful, FALSE if not *************************************************************************/ @@ -702,11 +712,11 @@ /*********************************************************************//** * @brief - * The testResetAccelDataPublishIntervalOverride function resets the override \n - * of the accelerometer data publish interval. + * The testResetAccelDataPublishIntervalOverride function resets the + * override of the accelerometer data publish interval. * @details - * Inputs : none - * Outputs : accelDataPublishInterval + * Inputs: accelDataPublishInterval + * Outputs: accelDataPublishInterval * @return TRUE if override reset successful, FALSE if not *************************************************************************/ BOOL testResetAccelDataPublishIntervalOverride( void ) @@ -725,10 +735,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[] + * Inputs: accelAxes + * 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 +762,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[] + * Inputs: accelAxes + * Outputs: accelAxes * @param axis ID of accelerometer axis to reset override for * @return TRUE if override successful, FALSE if not *************************************************************************/ @@ -779,10 +789,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[] + * Inputs: accelMaxs + * 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 +816,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[] + * Inputs: accelMaxs + * Outputs: accelMaxs * @param axis ID of accelerometer axis to reset override for * @return TRUE if override successful, FALSE if not *************************************************************************/ @@ -832,4 +842,3 @@ } /**@}*/ -