/************************************************************************** * * 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 "FPGA.h" #include "LoadCell.h" #include "SystemCommMessages.h" #include "TaskPriority.h" /** * @addtogroup LoadCells * @{ */ // ********** private definitions ********** #define LOAD_CELL_REPORT_PERIOD (100 / TASK_PRIORITY_INTERVAL) ///< Broadcast load cell values message every 100 ms. #define LOAD_CELL_SAMPLES_TO_AVERAGE (100 / TASK_PRIORITY_INTERVAL) ///< Averaging load cell data over the reporting interval. #define LOAD_CELL_AVERAGE_MULTIPLIER (1.0 / (F32)LOAD_CELL_SAMPLES_TO_AVERAGE) ///< Optimization - multiplying is faster than dividing. // TODO - gain and offset for load cells should be read from NV Data calibration record. #define ADC2GRAM (0.0894 * 1.1338) ///< Conversion factor from ADC counts to grams. #define LOAD_CELL_ZERO_OFFSET -1215.0 ///< Zero offset (in grams). TODO - right now, this is empty reservoir weight. #define SIZE_OF_SMALL_LOAD_CELL_AVG 10 ///< Small load cell moving average has 10 samples. #define SIZE_OF_LARGE_LOAD_CELL_AVG 40 ///< Large load cell moving average has 40 samples. /// Load cell data structure. typedef struct { U32 measuredReadingSum; ///< Raw load cell sums for averaging OVERRIDE_F32_T filteredWeight; ///< Latest filtered load cell weights F32 autoCalOffset; ///< Load cell auto-calibration offset 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 (8 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 (32 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. // ********** private function prototypes ********** static U32 getLoadCellDataPublishInterval( void ); /*********************************************************************//** * @brief * The initLoadCell function initializes the LoadCell module. * @details Inputs: none * @details Outputs: LoadCell module initialized. * @return none *************************************************************************/ void initLoadCell( void ) { U32 i; U32 j; smallReadingsIdx = 0; largeReadingsIdx = 0; for ( i = 0; i < NUM_OF_LOAD_CELLS; i++ ) { loadcells[ i ].filteredWeight.data = 0.0; loadcells[ i ].filteredWeight.ovData = 0.0; loadcells[ i ].filteredWeight.ovInitData = 0.0; loadcells[ i ].filteredWeight.override = OVERRIDE_RESET; loadcells[ i ].measuredReadingSum = 0; 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; } } } /*********************************************************************//** * @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; // update sums for load cell average calculations loadcells[ LOAD_CELL_RESERVOIR_1_PRIMARY ].measuredReadingSum += getFPGALoadCellA1(); loadcells[ LOAD_CELL_RESERVOIR_1_BACKUP ].measuredReadingSum += getFPGALoadCellA2(); loadcells[ LOAD_CELL_RESERVOIR_2_PRIMARY ].measuredReadingSum += getFPGALoadCellB1(); loadcells[ LOAD_CELL_RESERVOIR_2_BACKUP ].measuredReadingSum += getFPGALoadCellB2(); // filter every 100ms if ( ++loadCellFilterTimerCount >= LOAD_CELL_SAMPLES_TO_AVERAGE ) { for ( ii = 0; ii < NUM_OF_LOAD_CELLS; ++ii ) { // calculate load cell average weights loadcells[ ii ].filteredWeight.data = (F32)loadcells[ ii ].measuredReadingSum * LOAD_CELL_AVERAGE_MULTIPLIER * ADC2GRAM + LOAD_CELL_ZERO_OFFSET - loadcells[ ii ].autoCalOffset; // reset sums for next averaging loadcells[ ii ].measuredReadingSum = 0; // Update filtered with new filtered weight sample and moving averages loadcells[ ii ].smallFilterTotal -= loadcells[ ii ].smallFilterReadings[ smallReadingsIdx ]; loadcells[ ii ].smallFilterReadings[ smallReadingsIdx ] = loadcells[ ii ].filteredWeight.data; loadcells[ ii ].smallFilterTotal += loadcells[ ii ].filteredWeight.data; loadcells[ ii ].smallFilteredWeight = loadcells[ ii ].smallFilterTotal / (F32)SIZE_OF_SMALL_LOAD_CELL_AVG; loadcells[ ii ].largeFilterTotal -= loadcells[ ii ].largeFilterReadings[ largeReadingsIdx ]; loadcells[ ii ].largeFilterReadings[ largeReadingsIdx ] = loadcells[ ii ].filteredWeight.data; loadcells[ ii ].largeFilterTotal += loadcells[ ii ].filteredWeight.data; loadcells[ ii ].largeFilteredWeight = loadcells[ ii ].largeFilterTotal / (F32)SIZE_OF_LARGE_LOAD_CELL_AVG; } loadCellFilterTimerCount = 0; smallReadingsIdx = INC_WRAP( smallReadingsIdx, 0, SIZE_OF_SMALL_LOAD_CELL_AVG - 1 ); 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 load cell data broadcastLoadCellData( getLoadCellFilteredWeight( LOAD_CELL_RESERVOIR_1_PRIMARY ), getLoadCellFilteredWeight( LOAD_CELL_RESERVOIR_1_BACKUP ), getLoadCellFilteredWeight( LOAD_CELL_RESERVOIR_2_PRIMARY ), getLoadCellFilteredWeight( LOAD_CELL_RESERVOIR_2_BACKUP ) ); } } /*********************************************************************//** * @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 ) { // Add old auto calibration offset to get back to actual weight value loadcells[ loadCellID ].autoCalOffset = ( loadcells[ loadCellID ].filteredWeight.data + loadcells[ loadCellID ].autoCalOffset ); } /*********************************************************************//** * @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 getLoadCellFilteredWeight function gets the measured 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 filtered weight for * @return the filtered load cell weight for the given load cell ID. *************************************************************************/ F32 getLoadCellFilteredWeight( LOAD_CELL_ID_T loadCellID ) { F32 result = 0; if ( loadCellID < NUM_OF_LOAD_CELLS ) { if ( OVERRIDE_KEY == loadcells[ loadCellID ].filteredWeight.override ) { result = loadcells[ loadCellID ].filteredWeight.ovData; } else { result = loadcells[ loadCellID ].filteredWeight.data; } } else { activateAlarmNoData( ALARM_ID_DG_SOFTWARE_FAULT ); } 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 small filtered weight for * @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 { activateAlarmNoData( ALARM_ID_DG_SOFTWARE_FAULT ); } 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 for * @return the large filtered load cell weight for the given load cell ID. *************************************************************************/ F32 getLoadCellLargeFilteredWeight( LOAD_CELL_ID_T loadCellID ) { F32 result = 0; if ( loadCellID < NUM_OF_LOAD_CELLS ) { result = loadcells[ loadCellID ].largeFilteredWeight; } else { activateAlarmNoData( ALARM_ID_DG_SOFTWARE_FAULT ); } 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; } /************************************************************************* * 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 ].filteredWeight.ovData = value; loadcells[ loadCellID ].filteredWeight.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 ].filteredWeight.override = OVERRIDE_RESET; loadcells[ loadCellID ].filteredWeight.ovData = loadcells[ loadCellID ].filteredWeight.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; } /**@}*/