Index: firmware/App/Controllers/LoadCell.c =================================================================== diff -u -r22176ce95e49213c48454f34ddf5d29b8109f2cb -r3b70632c04247a6973960e1f37ae73eb4384a6b7 --- firmware/App/Controllers/LoadCell.c (.../LoadCell.c) (revision 22176ce95e49213c48454f34ddf5d29b8109f2cb) +++ firmware/App/Controllers/LoadCell.c (.../LoadCell.c) (revision 3b70632c04247a6973960e1f37ae73eb4384a6b7) @@ -7,8 +7,8 @@ * * @file LoadCell.c * -* @author (last) Dara Navaei -* @date (last) 02-May-2022 +* @author (last) Michael Garthwaite +* @date (last) 07-Sep-2022 * * @author (original) Saeed Nejatali * @date (original) 25-Feb-2020 @@ -48,23 +48,23 @@ #define LOAD_CELL_MIN_ALLOWED_WEIGHT_BEFORE_TARE_GRAMS 1600.0F ///< Load cell minimum allowed weight before tare in grams. #define LOAD_CELL_WEIGHT_OUT_RANGE_PERSISTENT_PERIOD_MS (5 * MS_PER_SECOND) ///< Load cell weight out of range persistent period in milliseconds. #define LOAD_CELL_PRIMARY_BACKUP_MAX_DRIFT_PERSISTENT_PERIOD_MS (5 * MS_PER_SECOND) ///< Load cell primary and backup maximum allowed weight drift persistent period in milliseconds. -#define EMPTY_RESERVOIR_WEIGHT_GRAMS 1600 ///< Reservoirs empty weight in grams. -#define MAX_ALLOWED_EXTRA_WEIGHT_BEFORE_FIRST_TARE_GRAMS 300 ///< Max allowed extra weight before first tare in grams. -#define MAX_ALLOWED_EXTRA_WEIGHT_BEFORE_TARE_GRAMS 60 ///< Max allowed extra weight before tare in grams. +#define EMPTY_RESERVOIR_WEIGHT_GRAMS 1600.0F ///< Reservoirs empty weight in grams. +#define MAX_ALLOWED_EXTRA_WEIGHT_BEFORE_FIRST_TARE_GRAMS 300.0F ///< Max allowed extra weight before first tare in grams. +#define MAX_ALLOWED_EXTRA_WEIGHT_BEFORE_TARE_GRAMS 60.0F ///< Max allowed extra weight before tare in grams. #define LOAD_CELL_PRIMARY_BACKUP_MAX_ALLOWED_DRIFT_GRAMS 60.0F ///< Load cell primary and backup maximum allowed weight drift in grams. #define DATA_PUBLISH_COUNTER_START_COUNT 0 ///< Data publish counter start count. /// Load cell data structure. typedef struct { U32 rawReading; ///< Latest raw load cell reading. - OVERRIDE_F32_T weight; ///< Latest load cell weight. + F32 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. F64 smallFilterTotal; ///< Small filter rolling total - used to calc small load cell moving average. - F32 smallFilteredWeight; ///< Load cell small filtered (100 100Hz raw sample) weight. + OVERRIDE_F32_T 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. F64 largeFilterTotal; ///< Large filter rolling total - used to calc small load cell moving average. @@ -82,6 +82,7 @@ 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. +static BOOL hasLoadCellBeenTared[ NUM_OF_LOAD_CELLS ]; ///< Flags indicating whether loadcells have been tared yet. // ********** private function prototypes ********** @@ -105,21 +106,24 @@ loadCellDataPublicationTimerCounter = DATA_PUBLISH_COUNTER_START_COUNT; loadCellFilterTimerCount = 0; + // Initialize load cell filter data and set calibration data as benign until loaded from non-volatile memory for ( i = 0; i < NUM_OF_LOAD_CELLS; i++ ) { benignPolynomialCalRecord( &loadCellsCalRecord.loadCells[ 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; + hasLoadCellBeenTared[ i ] = FALSE; + loadcells[ i ].rawReading = 0; + loadcells[ i ].weight = 0.0; + loadcells[ i ].autoCalOffset = 0.0; + loadcells[ i ].largeFilterTotal = 0.0; + loadcells[ i ].largeFilteredWeight = 0.0; + loadcells[ i ].smallFilterTotal = 0.0; + loadcells[ i ].smallFilteredWeight.data = 0.0; + loadcells[ i ].smallFilteredWeight.ovData = 0.0; + loadcells[ i ].smallFilteredWeight.ovInitData = 0.0; + loadcells[ i ].smallFilteredWeight.override = OVERRIDE_RESET; + for ( j = 0; j < SIZE_OF_SMALL_LOAD_CELL_AVG; j++ ) { loadcells[ i ].smallFilterReadings[ j ] = 0.0; @@ -167,10 +171,10 @@ 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 ); + a1 = ( a1 >> SHIFT_BITS_BY_31 ) << SHIFT_24_BITS; + a2 = ( a2 >> SHIFT_BITS_BY_31 ) << SHIFT_16_BITS_FOR_WORD_SHIFT; + b1 = ( b1 >> SHIFT_BITS_BY_31 ) << SHIFT_8_BITS_FOR_BYTE_SHIFT; + b2 = ( b2 >> SHIFT_BITS_BY_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 ) ) @@ -198,11 +202,11 @@ // 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(loadCell, 4) * loadCellsCalRecord.loadCells[ (CAL_DATA_DG_LOAD_CELLS_T)ii ].fourthOrderCoeff + - pow(loadCell, 3) * loadCellsCalRecord.loadCells[ (CAL_DATA_DG_LOAD_CELLS_T)ii ].thirdOrderCoeff + - pow(loadCell, 2) * loadCellsCalRecord.loadCells[ (CAL_DATA_DG_LOAD_CELLS_T)ii ].secondOrderCoeff + - loadCell * loadCellsCalRecord.loadCells[ (CAL_DATA_DG_LOAD_CELLS_T)ii ].gain + - loadCellsCalRecord.loadCells[ (CAL_DATA_DG_LOAD_CELLS_T)ii ].offset; + loadcells[ ii ].weight = pow(loadCell, 4) * loadCellsCalRecord.loadCells[ (CAL_DATA_DG_LOAD_CELLS_T)ii ].fourthOrderCoeff + + pow(loadCell, 3) * loadCellsCalRecord.loadCells[ (CAL_DATA_DG_LOAD_CELLS_T)ii ].thirdOrderCoeff + + pow(loadCell, 2) * loadCellsCalRecord.loadCells[ (CAL_DATA_DG_LOAD_CELLS_T)ii ].secondOrderCoeff + + loadCell * loadCellsCalRecord.loadCells[ (CAL_DATA_DG_LOAD_CELLS_T)ii ].gain + + loadCellsCalRecord.loadCells[ (CAL_DATA_DG_LOAD_CELLS_T)ii ].offset; loadcells[ ii ].loadCellVelocity_g_min = ( getLoadCellWeight( (LOAD_CELL_ID_T)ii ) - loadcells[ ii ].smallFilterReadings[ smallReadingsIdx ] ) * (F32)SEC_PER_MIN; @@ -213,13 +217,13 @@ loadcells[ ii ].smallFilterTotal += getLoadCellWeight( (LOAD_CELL_ID_T)ii ); // Calculate the load cell value before applying calibration to it - loadcells[ ii ].smallFilteredWeight = (F32)( loadcells[ ii ].smallFilterTotal / (F64)SIZE_OF_SMALL_LOAD_CELL_AVG ); + loadcells[ ii ].smallFilteredWeight.data = (F32)( loadcells[ ii ].smallFilterTotal / (F64)SIZE_OF_SMALL_LOAD_CELL_AVG ); // Monitor the load cells weight monitorLoadCellsWeightOutOfRange( (LOAD_CELL_ID_T)ii ); // Apply the tare offset. NOTE: tare must be applied after checking the weight out of range. - loadcells[ ii ].smallFilteredWeight -= loadcells[ ii ].autoCalOffset; + loadcells[ ii ].smallFilteredWeight.data -= loadcells[ ii ].autoCalOffset; } smallReadingsIdx = INC_WRAP( smallReadingsIdx, 0, SIZE_OF_SMALL_LOAD_CELL_AVG - 1 ); @@ -231,8 +235,8 @@ { // 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 ].largeFilterReadings[ largeReadingsIdx ] = getLoadCellSmallFilteredWeight((LOAD_CELL_ID_T) ii); + loadcells[ ii ].largeFilterTotal += getLoadCellSmallFilteredWeight((LOAD_CELL_ID_T) ii); loadcells[ ii ].largeFilteredWeight = (F32)( loadcells[ ii ].largeFilterTotal / (F64)SIZE_OF_LARGE_LOAD_CELL_AVG ); } @@ -274,7 +278,7 @@ SELF_TEST_STATUS_T result = SELF_TEST_STATUS_IN_PROGRESS; BOOL calStatus = getNVRecord2Driver( GET_CAL_LOAD_CELL_SENSORS, (U08*)&loadCellsCalRecord, sizeof( DG_LOAD_CELLS_CAL_RECORD_T ), - NUM_OF_CAL_DATA_LOAD_CELLS, ALARM_ID_DG_LOAD_CELLS_INVALID_CAL_RECORD ); + NUM_OF_CAL_DATA_LOAD_CELLS, ALARM_ID_DG_LOAD_CELLS_INVALID_CAL_RECORD ); if ( TRUE == calStatus ) { @@ -290,6 +294,29 @@ /*********************************************************************//** * @brief + * The isLoadCellTared function returns load cell tare status + * for a given load cell ID. + * @details Inputs: none + * @details Outputs: load cell tare status + * @param loadCellID ID of load cell tare status + *************************************************************************/ +BOOL isLoadCellTared( LOAD_CELL_ID_T loadCellID ) +{ + BOOL tared = FALSE; + + if ( fabs( loadcells[ loadCellID ].autoCalOffset ) < NEARLY_ZERO ) + { + tared = FALSE; + } + else + { + tared = TRUE; + } + return tared; +} + +/*********************************************************************//** + * @brief * The tareLoadCell function sets the load cell auto calibration offset * for a given load cell ID. * @details Inputs: none @@ -299,11 +326,10 @@ void tareLoadCell( LOAD_CELL_ID_T loadCellID ) { BOOL isWeightOutOfRange = FALSE; + F32 weight = getLoadCellSmallFilteredWeight( loadCellID ); - F32 weight = getLoadCellSmallFilteredWeight( loadCellID ); - // Check if the load cell is being tared for the first time - if ( fabs( loadcells[ loadCellID ].autoCalOffset ) < NEARLY_ZERO ) + if ( hasLoadCellBeenTared[ loadCellID ] != TRUE ) { // For the first tare, the weight of the reservoir should be considered // The current weight of the load cell should not be greater than the weight of the reservoir + the extra weight @@ -318,7 +344,8 @@ if ( FALSE == isWeightOutOfRange ) { // Add old auto calibration offset to get back to actual weight value - loadcells[ loadCellID ].autoCalOffset = ( loadcells[ loadCellID ].smallFilteredWeight + loadcells[ loadCellID ].autoCalOffset ); + loadcells[ loadCellID ].autoCalOffset = ( loadcells[ loadCellID ].smallFilteredWeight.data + loadcells[ loadCellID ].autoCalOffset ); + hasLoadCellBeenTared[ loadCellID ] = TRUE; } else { @@ -351,14 +378,11 @@ *************************************************************************/ F32 getLoadCellWeight( LOAD_CELL_ID_T loadCellID ) { - F32 result = loadcells[ loadCellID ].weight.data; + F32 result = 0.0; if ( loadCellID < NUM_OF_LOAD_CELLS ) { - if ( OVERRIDE_KEY == loadcells[ loadCellID ].weight.override ) - { - result = loadcells[ loadCellID ].weight.ovData; - } + result = loadcells[ loadCellID ].weight; } else { @@ -383,7 +407,12 @@ if ( loadCellID < NUM_OF_LOAD_CELLS ) { - result = loadcells[ loadCellID ].smallFilteredWeight; + result = loadcells[ loadCellID ].smallFilteredWeight.data; + + if ( OVERRIDE_KEY == loadcells[ loadCellID ].smallFilteredWeight.override ) + { + result = loadcells[ loadCellID ].smallFilteredWeight.ovData; + } } else { @@ -470,31 +499,38 @@ *************************************************************************/ static void monitorLoadCellsPrimaryBackupDriftOutOfRange( void ) { - // TODO do we need this function at all? - F32 drift; + F32 drift = 0.0; + F32 loadCellADrift = 0.0; + F32 loadCellBDrift = 0.0; + BOOL isDriftOutOfRange = FALSE; - F32 loadCellADrift = fabs( getLoadCellSmallFilteredWeight( LOAD_CELL_RESERVOIR_1_PRIMARY ) - + // Test is valid after load cells are tared + if ( isLoadCellTared( LOAD_CELL_RESERVOIR_1_PRIMARY ) && isLoadCellTared( LOAD_CELL_RESERVOIR_1_BACKUP ) ) + { + loadCellADrift = fabs( getLoadCellSmallFilteredWeight( LOAD_CELL_RESERVOIR_1_PRIMARY ) - getLoadCellSmallFilteredWeight( LOAD_CELL_RESERVOIR_1_BACKUP ) ); - - F32 loadCellBDrift = fabs( getLoadCellSmallFilteredWeight( LOAD_CELL_RESERVOIR_2_PRIMARY ) - + } + if ( isLoadCellTared( LOAD_CELL_RESERVOIR_2_PRIMARY ) && isLoadCellTared( LOAD_CELL_RESERVOIR_2_BACKUP ) ) + { + loadCellBDrift = fabs( getLoadCellSmallFilteredWeight( LOAD_CELL_RESERVOIR_2_PRIMARY ) - getLoadCellSmallFilteredWeight( LOAD_CELL_RESERVOIR_2_BACKUP ) ); + } - BOOL isDriftOutOfRange = ( loadCellADrift > LOAD_CELL_PRIMARY_BACKUP_MAX_ALLOWED_DRIFT_GRAMS ) || - ( loadCellBDrift > LOAD_CELL_PRIMARY_BACKUP_MAX_ALLOWED_DRIFT_GRAMS ); - - if ( TRUE == isDriftOutOfRange ) + if ( ( loadCellADrift > LOAD_CELL_PRIMARY_BACKUP_MAX_ALLOWED_DRIFT_GRAMS ) || + ( loadCellBDrift > LOAD_CELL_PRIMARY_BACKUP_MAX_ALLOWED_DRIFT_GRAMS ) ) { + isDriftOutOfRange = TRUE; drift = ( loadCellADrift > LOAD_CELL_PRIMARY_BACKUP_MAX_ALLOWED_DRIFT_GRAMS ? loadCellADrift : loadCellBDrift ); } else { + isDriftOutOfRange = FALSE; // Pick the biggest drift in between the two load cells when none of the is above range drift = ( loadCellADrift > loadCellBDrift ? loadCellADrift : loadCellBDrift ); } - // TODO this alarm is disabled until a better drift algorithm is figured out. Drift check might be removed from the load cells - //checkPersistentAlarm( ALARM_ID_DG_LOAD_CELL_PRIMARY_BACKUP_DRIFT_OUT_OF_RANGE, isDriftOutOfRange, drift, - // LOAD_CELL_PRIMARY_BACKUP_MAX_ALLOWED_DRIFT_GRAMS ); + checkPersistentAlarm( ALARM_ID_DG_LOAD_CELL_PRIMARY_BACKUP_DRIFT_OUT_OF_RANGE, isDriftOutOfRange, drift, + LOAD_CELL_PRIMARY_BACKUP_MAX_ALLOWED_DRIFT_GRAMS ); } @@ -522,8 +558,8 @@ { result = TRUE; // Add tare to given value so reported load will be what is requested when tare is subtracted later - loadcells[ loadCellID ].weight.ovData = value + loadcells[ loadCellID ].autoCalOffset; - loadcells[ loadCellID ].weight.override = OVERRIDE_KEY; + loadcells[ loadCellID ].smallFilteredWeight.ovData = value; + loadcells[ loadCellID ].smallFilteredWeight.override = OVERRIDE_KEY; } } @@ -547,8 +583,8 @@ if ( TRUE == isTestingActivated() ) { result = TRUE; - loadcells[ loadCellID ].weight.override = OVERRIDE_RESET; - loadcells[ loadCellID ].weight.ovData = loadcells[ loadCellID ].weight.ovInitData; + loadcells[ loadCellID ].smallFilteredWeight.override = OVERRIDE_RESET; + loadcells[ loadCellID ].smallFilteredWeight.ovData = loadcells[ loadCellID ].smallFilteredWeight.ovInitData; } }