Index: firmware/App/Controllers/LoadCell.c =================================================================== diff -u -r87a22cbaa87daab0d7cedabc67452cead83d8630 -r72dd42b6a116e62d1b3ad5d60088c29e067d10d4 --- firmware/App/Controllers/LoadCell.c (.../LoadCell.c) (revision 87a22cbaa87daab0d7cedabc67452cead83d8630) +++ firmware/App/Controllers/LoadCell.c (.../LoadCell.c) (revision 72dd42b6a116e62d1b3ad5d60088c29e067d10d4) @@ -15,8 +15,11 @@ * ***************************************************************************/ +#include // For load cells calibration calculations + #include "FPGA.h" #include "LoadCell.h" +#include "NVDataMgmt.h" #include "SystemCommMessages.h" #include "TaskPriority.h" @@ -27,19 +30,31 @@ // ********** 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. -#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. +/// 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. -/// Load cell data structure +#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). + +/// 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 + U32 rawReading; ///< Latest raw load cell reading. + OVERRIDE_F32_T weight; ///< Latest load cell weight. + 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 (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 ********** @@ -50,9 +65,14 @@ 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 @@ -63,18 +83,51 @@ *************************************************************************/ void initLoadCell( void ) { + U32 cell; 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 ].rawReading = 0; - loadcells[ i ].measuredReadingSum = 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; + } } + + // 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; + } } /*********************************************************************//** @@ -90,40 +143,104 @@ U32 ii; // update sums for load cell average calculations - loadcells[ LOAD_CELL_A1 ].measuredReadingSum += getFPGALoadCellA1(); - loadcells[ LOAD_CELL_A2 ].measuredReadingSum += getFPGALoadCellA2(); - loadcells[ LOAD_CELL_B1 ].measuredReadingSum += getFPGALoadCellB1(); - loadcells[ LOAD_CELL_B2 ].measuredReadingSum += getFPGALoadCellB2(); + loadcells[ LOAD_CELL_RESERVOIR_1_PRIMARY ].rawReading = getFPGALoadCellA1(); + loadcells[ LOAD_CELL_RESERVOIR_1_BACKUP ].rawReading = getFPGALoadCellA2(); + loadcells[ LOAD_CELL_RESERVOIR_2_PRIMARY ].rawReading = getFPGALoadCellB1(); + loadcells[ LOAD_CELL_RESERVOIR_2_BACKUP ].rawReading = getFPGALoadCellB2(); + // 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; + + // 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_SAMPLES_TO_AVERAGE ) + if ( ++loadCellFilterTimerCount >= LOAD_CELL_REPORT_PERIOD ) { 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 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 load cell data - broadcastLoadCellData( getLoadCellFilteredWeight( LOAD_CELL_A1 ), getLoadCellFilteredWeight( LOAD_CELL_A2 ), - getLoadCellFilteredWeight( LOAD_CELL_B1 ), getLoadCellFilteredWeight( LOAD_CELL_B2 ) ); + // 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 @@ -132,7 +249,8 @@ *************************************************************************/ void tareLoadCell( LOAD_CELL_ID_T loadCellID ) { - loadcells[ loadCellID ].autoCalOffset = loadcells[ loadCellID ].filteredWeight.data; + // Add old auto calibration offset to get back to actual weight value + loadcells[ loadCellID ].autoCalOffset = ( loadcells[ loadCellID ].smallFilteredWeight + loadcells[ loadCellID ].autoCalOffset ); } /*********************************************************************//** @@ -150,26 +268,26 @@ /*********************************************************************//** * @brief - * The getLoadCellFilteredWeight function gets the measured filtered load cell - * weight for a given load cell ID. - * @details Inputs: load cell filtered weight + * 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 filtered weight for - * @return the filtered load cell weight for the given load cell ID. + * @param loadCellID ID of load cell to get weight for + * @return the load cell weight for the given load cell ID. *************************************************************************/ -F32 getLoadCellFilteredWeight( LOAD_CELL_ID_T loadCellID ) +F32 getLoadCellWeight( LOAD_CELL_ID_T loadCellID ) { F32 result = 0; if ( loadCellID < NUM_OF_LOAD_CELLS ) { - if ( OVERRIDE_KEY == loadcells[ loadCellID ].filteredWeight.override ) + if ( OVERRIDE_KEY == loadcells[ loadCellID ].weight.override ) { - result = loadcells[ loadCellID ].filteredWeight.ovData; + result = loadcells[ loadCellID ].weight.ovData; } else { - result = loadcells[ loadCellID ].filteredWeight.data; + result = loadcells[ loadCellID ].weight.data; } } else @@ -182,6 +300,56 @@ /*********************************************************************//** * @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 @@ -199,7 +367,50 @@ 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 *************************************************************************/ @@ -223,8 +434,8 @@ if ( TRUE == isTestingActivated() ) { result = TRUE; - loadcells[ loadCellID ].filteredWeight.ovData = value; - loadcells[ loadCellID ].filteredWeight.override = OVERRIDE_KEY; + loadcells[ loadCellID ].weight.ovData = value; + loadcells[ loadCellID ].weight.override = OVERRIDE_KEY; } } @@ -248,8 +459,8 @@ if ( TRUE == isTestingActivated() ) { result = TRUE; - loadcells[ loadCellID ].filteredWeight.override = OVERRIDE_RESET; - loadcells[ loadCellID ].filteredWeight.ovData = loadcells[ loadCellID ].filteredWeight.ovInitData; + loadcells[ loadCellID ].weight.override = OVERRIDE_RESET; + loadcells[ loadCellID ].weight.ovData = loadcells[ loadCellID ].weight.ovInitData; } }