/************************************************************************** * * 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 LoadCell.c * * @author (last) Quang Nguyen * @date (last) 26-Aug-2020 * * @author (original) Saeed Nejatali * @date (original) 25-Feb-2020 * ***************************************************************************/ #include // For load cells calibration calculations #include "FPGA.h" #include "LoadCell.h" #include "NVDataMgmt.h" #include "PersistentAlarm.h" #include "SystemCommMessages.h" #include "TaskPriority.h" /** * @addtogroup LoadCells * @{ */ // ********** private definitions ********** // TODO check the maximum weight on the load cells in tare. There was 1500 grams limit // but it has been removed. Check the load cells data sheet. #define LOAD_CELL_REPORT_PERIOD (100 / TASK_PRIORITY_INTERVAL) ///< Broadcast load cell values message every 100 ms. /// Conversion factor from ADC counts to grams. static const F32 ADC2GRAM = (0.0894 * 1.1338); #define LOAD_CELL_FILTER_ALPHA 0.05 ///< Alpha factor for the alpha filter used on load cell readings. #define SIZE_OF_SMALL_LOAD_CELL_AVG 100 ///< Small load cell moving average has 100 raw samples @ 10ms intervals (1-second). #define SIZE_OF_LARGE_LOAD_CELL_AVG 40 ///< Large load cell moving average has 40 samples from small filter @ 100ms intervals (4-second). #define LOAD_CELL_ADC_ERROR_PERSISTENCE 500 ///< Alarm persistence period (in ms) for load cell ADC errors. #define EMPTY_RESERVOIR_WEIGHT_GRAMS 1600 ///< Reservoirs empty weight in grams. #define MAX_ALLOWED_EXTRA_WEIGHT_BEFORE_TARE_GRAMS 300 ///< Max allowed extra weight before tare in grams. /// Load cell data structure. typedef struct { U32 rawReading; ///< Latest raw load cell reading OVERRIDE_F32_T weight; ///< Latest load cell weight F32 autoCalOffset; ///< Load cell auto-calibration offset F32 loadCellVelocity_g_min; ///< Velocity (in g/min) of load cell. F32 smallFilterReadings[ SIZE_OF_SMALL_LOAD_CELL_AVG ]; ///< Load cell samples for small load cell moving average. F32 smallFilterTotal; ///< Small filter rolling total - used to calc small load cell moving average. F32 smallFilteredWeight; ///< Load cell small filtered (100 100Hz raw sample) weight. F32 largeFilterReadings[ SIZE_OF_LARGE_LOAD_CELL_AVG ]; ///< Load cell samples for large load cell moving average. F32 largeFilterTotal; ///< Large filter rolling total - used to calc small load cell moving average. F32 largeFilteredWeight; ///< Load cell large filtered (40 10Hz filtered sample) weight. } LOADCELL_T; // ********** private data ********** static OVERRIDE_U32_T loadCellDataPublishInterval = { LOAD_CELL_REPORT_PERIOD, LOAD_CELL_REPORT_PERIOD, 0, 0 }; ///< Broadcast load cell data publish interval. static LOADCELL_T loadcells[ NUM_OF_LOAD_CELLS ]; ///< Load cell data structures. static U32 loadCellFilterTimerCount = 0; ///< Load cell filtering timer count. static U32 loadCellDataPublicationTimerCounter = 0; ///< Load cell data publication timer counter to CAN bus. static U32 smallReadingsIdx; ///< Index for next sample in load cell small rolling average sample array. static U32 largeReadingsIdx; ///< Index for next sample in load cell large rolling average sample array. static DG_LOAD_CELLS_CAL_RECORD_T loadCellsCalRecord; ///< Load cells calibration record. // ********** private function prototypes ********** static U32 getLoadCellDataPublishInterval( void ); static BOOL processCalibrationData( void ); /*********************************************************************//** * @brief * The initLoadCell function initializes the LoadCell module. * @details Inputs: none * @details Outputs: LoadCell module initialized. * @return none *************************************************************************/ void initLoadCell( void ) { U32 cell; U32 i; U32 j; smallReadingsIdx = 0; largeReadingsIdx = 0; for ( i = 0; i < NUM_OF_LOAD_CELLS; i++ ) { loadcells[ i ].rawReading = 0; loadcells[ i ].weight.data = 0.0; loadcells[ i ].weight.ovData = 0.0; loadcells[ i ].weight.ovInitData = 0.0; loadcells[ i ].weight.override = OVERRIDE_RESET; loadcells[ i ].autoCalOffset = 0.0; loadcells[ i ].largeFilterTotal = 0.0; loadcells[ i ].largeFilteredWeight = 0.0; loadcells[ i ].smallFilterTotal = 0.0; loadcells[ i ].smallFilteredWeight = 0.0; for ( j = 0; j < SIZE_OF_SMALL_LOAD_CELL_AVG; j++ ) { loadcells[ i ].smallFilterReadings[ j ] = 0.0; } for ( j = 0; j < SIZE_OF_LARGE_LOAD_CELL_AVG; j++ ) { loadcells[ i ].largeFilterReadings[ j ] = 0.0; } loadcells[ i ].loadCellVelocity_g_min = 0.0; } // Set all the load cells' calibration values to benign values for ( cell = CAL_DATA_LOAD_CELL_A1; cell < NUM_OF_CAL_DATA_LOAD_CELLS; cell++ ) { // Reset the calibration variables loadCellsCalRecord.loadCells[ cell ].fourthOrderCoeff = 0.0; loadCellsCalRecord.loadCells[ cell ].thirdOrderCoeff = 0.0; loadCellsCalRecord.loadCells[ cell ].secondOrderCoeff = 0.0; loadCellsCalRecord.loadCells[ cell ].gain = 1.0; loadCellsCalRecord.loadCells[ cell ].offset = 0.0; } // Initialize persistent alarm(s) initPersistentAlarm( ALARM_ID_DG_LOAD_CELL_ADC_ERROR, 0, LOAD_CELL_ADC_ERROR_PERSISTENCE ); } /*********************************************************************//** * @brief * The execLoadCell function gets load cell data from FPGA, applies filters, * and advertises them over CAN. * @details Inputs: none * @details Outputs: Filtered and advertised load cell data. * @return none *************************************************************************/ void execLoadCell( void ) { U32 ii; U32 a1 = getFPGALoadCellA1(); U32 a2 = getFPGALoadCellA2(); U32 b1 = getFPGALoadCellB1(); U32 b2 = getFPGALoadCellB2(); // update sums for load cell average calculations loadcells[ LOAD_CELL_RESERVOIR_1_PRIMARY ].rawReading = a1 & MASK_OFF_U32_MSB; loadcells[ LOAD_CELL_RESERVOIR_1_BACKUP ].rawReading = a2 & MASK_OFF_U32_MSB; loadcells[ LOAD_CELL_RESERVOIR_2_PRIMARY ].rawReading = b1 & MASK_OFF_U32_MSB; loadcells[ LOAD_CELL_RESERVOIR_2_BACKUP ].rawReading = b2 & MASK_OFF_U32_MSB; // Check error bits from new readings a1 = ( a1 >> 31 ) << SHIFT_24_BITS; a2 = ( a2 >> 31 ) << SHIFT_16_BITS_FOR_WORD_SHIFT; b1 = ( b1 >> 31 ) << SHIFT_8_BITS_FOR_BYTE_SHIFT; b2 = ( b2 >> 31 ); if ( TRUE == isPersistentAlarmTriggered( ALARM_ID_DG_LOAD_CELL_ADC_ERROR, ( ( a1 > 0 ) || ( a2 > 0 ) || ( b1 > 0 ) || ( b2 > 0 ) ) ) ) { SET_ALARM_WITH_1_U32_DATA( ALARM_ID_DG_LOAD_CELL_ADC_ERROR, ( a1 | a2 | b1 | b2 ) ) } // Check if a new calibration is available if ( isNewCalibrationRecordAvailable() == TRUE ) { // Get the new calibration data and check its validity processCalibrationData(); // Zero the current tare values when new calibration data is available loadcells[ LOAD_CELL_RESERVOIR_1_PRIMARY ].autoCalOffset = 0.0; loadcells[ LOAD_CELL_RESERVOIR_1_BACKUP ].autoCalOffset = 0.0; loadcells[ LOAD_CELL_RESERVOIR_2_PRIMARY ].autoCalOffset = 0.0; loadcells[ LOAD_CELL_RESERVOIR_2_BACKUP ].autoCalOffset = 0.0; } // Rolling average of last 100 raw samples in small filter for ( ii = 0; ii < NUM_OF_LOAD_CELLS; ++ii ) { loadcells[ ii ].weight.data = (F32)loadcells[ ii ].rawReading * ADC2GRAM; // Apply the calibration factors to the data. // load_cell_weight = fourth_order_coeff * (load_cell^4) + third_order_coeff * (load_cell^3) + second_order_coeff * (load_cell^2) + gain * load_cell + offset loadcells[ ii ].weight.data = pow(loadcells[ ii ].weight.data, 4) * loadCellsCalRecord.loadCells[ (CAL_DATA_DG_LOAD_CELLS_T)ii ].fourthOrderCoeff + pow(loadcells[ ii ].weight.data, 3) * loadCellsCalRecord.loadCells[ (CAL_DATA_DG_LOAD_CELLS_T)ii ].thirdOrderCoeff + pow(loadcells[ ii ].weight.data, 2) * loadCellsCalRecord.loadCells[ (CAL_DATA_DG_LOAD_CELLS_T)ii ].secondOrderCoeff + loadcells[ ii ].weight.data * loadCellsCalRecord.loadCells[ (CAL_DATA_DG_LOAD_CELLS_T)ii ].gain + loadCellsCalRecord.loadCells[ (CAL_DATA_DG_LOAD_CELLS_T)ii ].offset; loadcells[ ii ].weight.data = loadcells[ ii ].weight.data - loadcells[ ii ].autoCalOffset; loadcells[ ii ].loadCellVelocity_g_min = ( getLoadCellWeight( (LOAD_CELL_ID_T)ii ) - loadcells[ ii ].smallFilterReadings[ smallReadingsIdx ] ) * (F32)SEC_PER_MIN; // Update small filter with new weight sample loadcells[ ii ].smallFilterTotal -= loadcells[ ii ].smallFilterReadings[ smallReadingsIdx ]; loadcells[ ii ].smallFilterReadings[ smallReadingsIdx ] = getLoadCellWeight( (LOAD_CELL_ID_T)ii ); loadcells[ ii ].smallFilterTotal += getLoadCellWeight( (LOAD_CELL_ID_T)ii ); loadcells[ ii ].smallFilteredWeight = loadcells[ ii ].smallFilterTotal / (F32)SIZE_OF_SMALL_LOAD_CELL_AVG; } smallReadingsIdx = INC_WRAP( smallReadingsIdx, 0, SIZE_OF_SMALL_LOAD_CELL_AVG - 1 ); // filter every 100ms if ( ++loadCellFilterTimerCount >= LOAD_CELL_REPORT_PERIOD ) { for ( ii = 0; ii < NUM_OF_LOAD_CELLS; ++ii ) { // Update large filter with new small filter weight sample loadcells[ ii ].largeFilterTotal -= loadcells[ ii ].largeFilterReadings[ largeReadingsIdx ]; loadcells[ ii ].largeFilterReadings[ largeReadingsIdx ] = loadcells[ ii ].smallFilteredWeight; loadcells[ ii ].largeFilterTotal += loadcells[ ii ].smallFilteredWeight; loadcells[ ii ].largeFilteredWeight = loadcells[ ii ].largeFilterTotal / (F32)SIZE_OF_LARGE_LOAD_CELL_AVG; } loadCellFilterTimerCount = 0; largeReadingsIdx = INC_WRAP( largeReadingsIdx, 0, SIZE_OF_LARGE_LOAD_CELL_AVG - 1 ); } // broadcast load cell data if we are at scheduled interval. if ( ++loadCellDataPublicationTimerCounter >= getLoadCellDataPublishInterval() ) { loadCellDataPublicationTimerCounter = 0; // broadcast small filtered load cell data broadcastLoadCellData( loadcells[ LOAD_CELL_RESERVOIR_1_PRIMARY ].smallFilteredWeight, loadcells[ LOAD_CELL_RESERVOIR_1_BACKUP ].smallFilteredWeight, loadcells[ LOAD_CELL_RESERVOIR_2_PRIMARY ].smallFilteredWeight, loadcells[ LOAD_CELL_RESERVOIR_2_BACKUP ].smallFilteredWeight ); } } /*********************************************************************//** * @brief * The execLoadCellsSelfTest function executes the load cell self test. * It gets the calibration record from NVDataMgmt and checks whether the * values have a calibration date. * @details Inputs: none * @details Outputs: * @return result of the load cell self test *************************************************************************/ SELF_TEST_STATUS_T execLoadCellsSelfTest ( void ) { SELF_TEST_STATUS_T result = SELF_TEST_STATUS_IN_PROGRESS; BOOL calStatus = processCalibrationData(); if ( TRUE == calStatus ) { result = SELF_TEST_STATUS_PASSED; } else { result = SELF_TEST_STATUS_FAILED; } return result; } /*********************************************************************//** * @brief * The tareLoadCell function sets the load cell auto calibration offset * for a given load cell ID. * @details Inputs: none * @details Outputs: load cell autoCalOffset * @param loadCellID ID of load cell to set calibration offset for *************************************************************************/ void tareLoadCell( LOAD_CELL_ID_T loadCellID ) { BOOL isWeightOutOfRange = FALSE; F32 deltaWeight = 0.0; F32 weight = getLoadCellSmallFilteredWeight( loadCellID ); // Check if the load cell is being tared for the first time if ( fabs(loadcells[ loadCellID ].autoCalOffset) < NEARLY_ZERO ) { deltaWeight = fabs(weight - ( EMPTY_RESERVOIR_WEIGHT_GRAMS + MAX_ALLOWED_EXTRA_WEIGHT_BEFORE_TARE_GRAMS )); isWeightOutOfRange = ( weight > deltaWeight ? TRUE : FALSE ); } else { deltaWeight = fabs(weight - MAX_ALLOWED_EXTRA_WEIGHT_BEFORE_TARE_GRAMS ); isWeightOutOfRange = ( weight > deltaWeight ? TRUE : FALSE ); } if ( FALSE == isWeightOutOfRange ) { // Add old auto calibration offset to get back to actual weight value loadcells[ loadCellID ].autoCalOffset = ( loadcells[ loadCellID ].smallFilteredWeight + loadcells[ loadCellID ].autoCalOffset ); } else { SET_ALARM_WITH_1_F32_DATA( ALARM_ID_DG_LOAD_CELLS_TARE_WEIGHT_OUT_OF_RANGE, weight ) } } /*********************************************************************//** * @brief * The resetLoadCellOffset function resets the load cell auto calibration offset * to zero for a given load cell ID. * @details Inputs: none * @details Outputs: load cell autoCalOffset * @param loadCellID ID of load cell to set calibration offset for *************************************************************************/ void resetLoadCellOffset( LOAD_CELL_ID_T loadCellID ) { loadcells[ loadCellID ].autoCalOffset = 0.0; } /*********************************************************************//** * @brief * The getLoadCellWeight function gets the measured load cell weight for * a given load cell ID. * @details Inputs: load cell weight * @details Outputs: none * @param loadCellID ID of load cell to get weight for * @return the load cell weight for the given load cell ID. *************************************************************************/ F32 getLoadCellWeight( LOAD_CELL_ID_T loadCellID ) { F32 result = 0; if ( loadCellID < NUM_OF_LOAD_CELLS ) { if ( OVERRIDE_KEY == loadcells[ loadCellID ].weight.override ) { result = loadcells[ loadCellID ].weight.ovData; } else { result = loadcells[ loadCellID ].weight.data; } } else { SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_INVALID_LOAD_CELL_ID, (U32)loadCellID ) } return result; } /*********************************************************************//** * @brief * The getLoadCellSmallFilteredWeight function gets the small filtered load cell * weight for a given load cell ID. * @details Inputs: load cell filtered weight * @details Outputs: none * @param loadCellID ID of load cell to get large filtered weight * @return the small filtered load cell weight for the given load cell ID. *************************************************************************/ F32 getLoadCellSmallFilteredWeight( LOAD_CELL_ID_T loadCellID ) { F32 result = 0; if ( loadCellID < NUM_OF_LOAD_CELLS ) { result = loadcells[ loadCellID ].smallFilteredWeight; } else { SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_INVALID_LOAD_CELL_ID, (U32)loadCellID ) } return result; } /*********************************************************************//** * @brief * The getLoadCellSmallFilteredWeight function gets the large filtered load cell * weight for a given load cell ID. * @details Inputs: load cell filtered weight * @details Outputs: none * @param loadCellID ID of load cell to get large filtered weight * @return the large filtered load cell weight for the given load cell ID. *************************************************************************/ F32 getLoadCellLargeFilteredWeight( LOAD_CELL_ID_T loadCellID ) { F32 result = 0.0; if ( loadCellID < NUM_OF_LOAD_CELLS ) { result = loadcells[ loadCellID ].largeFilteredWeight; } else { SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_INVALID_LOAD_CELL_ID, (U32)loadCellID ) } return result; } /*********************************************************************//** * @brief * The getLoadCellVelocity function gets the current velocity (in g/min) * for the given load cell. * @details Inputs: loadcells[] * @details Outputs: none * @param loadCellID ID of load cell to get velocity * @return the velocity (in g/min) for the given load cell ID. *************************************************************************/ F32 getLoadCellVelocity( LOAD_CELL_ID_T loadCellID ) { F32 result = 0.0; if ( loadCellID < NUM_OF_LOAD_CELLS ) { result = loadcells[ loadCellID ].loadCellVelocity_g_min; } else { SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_INVALID_LOAD_CELL_ID, (U32)loadCellID ) } return result; } /*********************************************************************//** * @brief * The getLoadCellDataPublishInterval function gets the load cell data publish interval. * @details Inputs: loadCellDataPublishInterval * @details Outputs: none * @return the current load cell data publication interval (in ms/task interval). *************************************************************************/ static U32 getLoadCellDataPublishInterval( void ) { U32 result = loadCellDataPublishInterval.data; if ( OVERRIDE_KEY == loadCellDataPublishInterval.override ) { result = loadCellDataPublishInterval.ovData; } return result; } /*********************************************************************//** * @brief * The processCalibrationData function gets the calibration data and makes * sure it is valid by checking the calibration date. The calibration date * should not be 0. * @details Inputs: loadCellsCalRecord * @details Outputs: loadCellsCalRecord * @return TRUE if the calibration record is valid, otherwise FALSE *************************************************************************/ static BOOL processCalibrationData( void ) { BOOL status = TRUE; U32 cell; // Get the calibration record from NVDataMgmt DG_LOAD_CELLS_CAL_RECORD_T calData = getDGLoadCellsCalibrationRecord(); for ( cell = 0; cell < NUM_OF_CAL_DATA_LOAD_CELLS; cell++ ) { // Check if the calibration data that was received from NVDataMgmt is legitimate // The calibration date item should not be zero. If the calibration date is 0, // then the load cells data is not stored in the NV memory or it was corrupted. if ( calData.loadCells[ cell ].calibrationTime == 0 ) { #ifndef DISABLE_CAL_CHECK SET_ALARM_WITH_1_U32_DATA( ALARM_ID_DG_LOAD_CELLS_INVALID_CALIBRATION, (U32)cell ); #endif status = FALSE; } else { // The calibration data was valid, update the local copy loadCellsCalRecord.loadCells[ cell ].fourthOrderCoeff = calData.loadCells[ cell ].fourthOrderCoeff; loadCellsCalRecord.loadCells[ cell ].thirdOrderCoeff = calData.loadCells[ cell ].thirdOrderCoeff; loadCellsCalRecord.loadCells[ cell ].secondOrderCoeff = calData.loadCells[ cell ].secondOrderCoeff; loadCellsCalRecord.loadCells[ cell ].gain = calData.loadCells[ cell ].gain; loadCellsCalRecord.loadCells[ cell ].offset = calData.loadCells[ cell ].offset; } } return status; } /************************************************************************* * TEST SUPPORT FUNCTIONS *************************************************************************/ /*********************************************************************//** * @brief * The testSetLoadCellOverride function overrides the measured load cell data. * @details Inputs: none * @details Outputs: load cell filtered weight * @param loadCellID ID of the load cell to override * @param value override filtered load cell weight * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testSetLoadCellOverride( U32 loadCellID, F32 value ) { BOOL result = FALSE; if ( loadCellID < NUM_OF_LOAD_CELLS ) { if ( TRUE == isTestingActivated() ) { result = TRUE; loadcells[ loadCellID ].weight.ovData = value; loadcells[ loadCellID ].weight.override = OVERRIDE_KEY; } } return result; } /*********************************************************************//** * @brief * The testResetLoadCellOverride function resets the override of the load cell. * @details Inputs: none * @details Outputs: load cell filtered weight * @param loadCellID ID of the load cell to override * @return TRUE if reset successful, FALSE if not *************************************************************************/ BOOL testResetLoadCellOverride( U32 loadCellID ) { BOOL result = FALSE; if ( loadCellID < NUM_OF_LOAD_CELLS ) { if ( TRUE == isTestingActivated() ) { result = TRUE; loadcells[ loadCellID ].weight.override = OVERRIDE_RESET; loadcells[ loadCellID ].weight.ovData = loadcells[ loadCellID ].weight.ovInitData; } } return result; } /*********************************************************************//** * @brief * The testSetLoadCellDataPublishIntervalOverride function overrides the * load cell data publish interval. * @details Inputs: none * @details Outputs: loadCellDataPublishInterval * @param value override load cell data publish interval with (in ms) * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testSetLoadCellDataPublishIntervalOverride( U32 value ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { U32 intvl = value / TASK_PRIORITY_INTERVAL; result = TRUE; loadCellDataPublishInterval.ovData = intvl; loadCellDataPublishInterval.override = OVERRIDE_KEY; } return result; } /*********************************************************************//** * @brief * The testResetLoadCellDataPublishIntervalOverride function resets the override * of the load cell data publish interval. * @details Inputs: none * @details Outputs: loadCellDataPublishInterval * @return TRUE if override reset successful, FALSE if not *************************************************************************/ BOOL testResetLoadCellDataPublishIntervalOverride( void ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { result = TRUE; loadCellDataPublishInterval.override = OVERRIDE_RESET; loadCellDataPublishInterval.ovData = loadCellDataPublishInterval.ovInitData; } return result; } /**@}*/