Index: firmware/App/Controllers/LoadCell.c =================================================================== diff -u -r7fb16c92973a551dda1d801d5f255f88c0c81f12 -r696e732c9742535a58b9c65f243df7cd797d1423 --- firmware/App/Controllers/LoadCell.c (.../LoadCell.c) (revision 7fb16c92973a551dda1d801d5f255f88c0c81f12) +++ firmware/App/Controllers/LoadCell.c (.../LoadCell.c) (revision 696e732c9742535a58b9c65f243df7cd797d1423) @@ -7,8 +7,8 @@ * * @file LoadCell.c * -* @author (last) Dara Navaei -* @date (last) 03-Aug-2022 +* @author (last) Dong Nguyen +* @date (last) 27-Sep-2022 * * @author (original) Saeed Nejatali * @date (original) 25-Feb-2020 @@ -48,11 +48,12 @@ #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. +#define LOAD_CELL_FPGA_ERROR_TIMEOUT_MS ( 2 * MS_PER_SECOND ) ///< Load cell FPGA error timeout in milliseconds. /// Load cell data structure. typedef struct @@ -64,7 +65,7 @@ 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 +83,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,42 +107,48 @@ 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.data = 0.0F; + loadcells[ i ].weight.ovData = 0.0F; + loadcells[ i ].weight.ovInitData = 0.0F; + loadcells[ i ].weight.override = OVERRIDE_RESET; + loadcells[ i ].autoCalOffset = 0.0F; + loadcells[ i ].largeFilterTotal = 0.0F; + loadcells[ i ].largeFilteredWeight = 0.0F; + loadcells[ i ].smallFilterTotal = 0.0F; + loadcells[ i ].smallFilteredWeight.data = 0.0F; + loadcells[ i ].smallFilteredWeight.ovData = 0.0F; + loadcells[ i ].smallFilteredWeight.ovInitData = 0.0F; + loadcells[ i ].smallFilteredWeight.override = OVERRIDE_RESET; for ( j = 0; j < SIZE_OF_SMALL_LOAD_CELL_AVG; j++ ) { - loadcells[ i ].smallFilterReadings[ j ] = 0.0; + loadcells[ i ].smallFilterReadings[ j ] = 0.0F; } for ( j = 0; j < SIZE_OF_LARGE_LOAD_CELL_AVG; j++ ) { - loadcells[ i ].largeFilterReadings[ j ] = 0.0; + loadcells[ i ].largeFilterReadings[ j ] = 0.0F; } - loadcells[ i ].loadCellVelocity_g_min = 0.0; + loadcells[ i ].loadCellVelocity_g_min = 0.0F; } // Initialize persistent alarm(s) - initPersistentAlarm( ALARM_ID_DG_LOAD_CELL_ADC_ERROR, 0, LOAD_CELL_ADC_ERROR_PERSISTENCE ); - initPersistentAlarm( ALARM_ID_DG_LOAD_CELL_WEIGHT_OUT_OF_RANGE, LOAD_CELL_WEIGHT_OUT_RANGE_PERSISTENT_PERIOD_MS, LOAD_CELL_WEIGHT_OUT_RANGE_PERSISTENT_PERIOD_MS ); - initPersistentAlarm( ALARM_ID_DG_LOAD_CELL_PRIMARY_BACKUP_DRIFT_OUT_OF_RANGE, LOAD_CELL_PRIMARY_BACKUP_MAX_DRIFT_PERSISTENT_PERIOD_MS, LOAD_CELL_PRIMARY_BACKUP_MAX_DRIFT_PERSISTENT_PERIOD_MS ); + + // Initialize the FPGA persistent alarms + initFPGAPersistentAlarm( FPGA_PERS_ERROR_LOAD_CELL_A1_B1_SENSORS, ALARM_ID_DG_LOAD_CELL_A1_B1_FPGA_FAULT, LOAD_CELL_FPGA_ERROR_TIMEOUT_MS, LOAD_CELL_FPGA_ERROR_TIMEOUT_MS ); + initFPGAPersistentAlarm( FPGA_PERS_ERROR_LOAD_CELL_A2_B2_SENSORS, ALARM_ID_DG_LOAD_CELL_A2_B2_FPGA_FAULT, LOAD_CELL_FPGA_ERROR_TIMEOUT_MS, LOAD_CELL_FPGA_ERROR_TIMEOUT_MS ); } /*********************************************************************//** @@ -154,41 +162,36 @@ 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 >> 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 ) ) - } + U32 a1Err = ( a1 >> SHIFT_BITS_BY_31 ) << SHIFT_24_BITS; + U32 a2Err = ( a2 >> SHIFT_BITS_BY_31 ) << SHIFT_16_BITS_FOR_WORD_SHIFT; + U32 b1Err = ( b1 >> SHIFT_BITS_BY_31 ) << SHIFT_8_BITS_FOR_BYTE_SHIFT; + U32 b2Err = ( b2 >> SHIFT_BITS_BY_31 ); - // TODO use ALARM_ID_DG_LOAD_CELL_FPGA_READ_ERROR for read error + loadcells[ LOAD_CELL_RESERVOIR_1_PRIMARY ].rawReading = ( 0 == a1Err ? a1 & MASK_OFF_U32_MSB : loadcells[ LOAD_CELL_RESERVOIR_1_PRIMARY ].rawReading ); + loadcells[ LOAD_CELL_RESERVOIR_1_BACKUP ].rawReading = ( 0 == a2Err ? a2 & MASK_OFF_U32_MSB : loadcells[ LOAD_CELL_RESERVOIR_1_BACKUP ].rawReading ); + loadcells[ LOAD_CELL_RESERVOIR_2_PRIMARY ].rawReading = ( 0 == b1Err ? b1 & MASK_OFF_U32_MSB : loadcells[ LOAD_CELL_RESERVOIR_2_PRIMARY ].rawReading ); + loadcells[ LOAD_CELL_RESERVOIR_2_BACKUP ].rawReading = ( 0 == b2Err ? b2 & MASK_OFF_U32_MSB : loadcells[ LOAD_CELL_RESERVOIR_2_BACKUP ].rawReading ); + checkFPGAPersistentAlarms( FPGA_PERS_ERROR_LOAD_CELL_A1_B1_SENSORS, getFPGAADC1ErrorCount(), getFPGAADC1ReadCount() ); + checkFPGAPersistentAlarms( FPGA_PERS_ERROR_LOAD_CELL_A2_B2_SENSORS, getFPGAADC2ErrorCount(), getFPGAADC2ReadCount() ); + // Check if a new calibration is available if ( TRUE == isNewCalibrationRecordAvailable() ) { 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 ); // 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; + loadcells[ LOAD_CELL_RESERVOIR_1_PRIMARY ].autoCalOffset = 0.0F; + loadcells[ LOAD_CELL_RESERVOIR_1_BACKUP ].autoCalOffset = 0.0F; + loadcells[ LOAD_CELL_RESERVOIR_2_PRIMARY ].autoCalOffset = 0.0F; + loadcells[ LOAD_CELL_RESERVOIR_2_BACKUP ].autoCalOffset = 0.0F; } // Rolling average of last 100 raw samples in small filter @@ -204,22 +207,22 @@ loadCell * loadCellsCalRecord.loadCells[ (CAL_DATA_DG_LOAD_CELLS_T)ii ].gain + loadCellsCalRecord.loadCells[ (CAL_DATA_DG_LOAD_CELLS_T)ii ].offset; + // Monitor the load cells weight + monitorLoadCellsWeightOutOfRange( (LOAD_CELL_ID_T)ii ); + 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 ].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 ].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 ); @@ -230,14 +233,14 @@ 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 = (F32)( loadcells[ ii ].largeFilterTotal / (F64)SIZE_OF_LARGE_LOAD_CELL_AVG ); + loadcells[ ii ].largeFilterTotal -= loadcells[ ii ].largeFilterReadings[ largeReadingsIdx ]; + 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 ); } loadCellFilterTimerCount = 0; - largeReadingsIdx = INC_WRAP( largeReadingsIdx, 0, SIZE_OF_LARGE_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. @@ -325,7 +328,7 @@ 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 @@ -340,12 +343,13 @@ 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 { // Raise the alarm - SET_ALARM_WITH_1_F32_DATA( ALARM_ID_DG_LOAD_CELLS_TARE_WEIGHT_OUT_OF_RANGE, weight ) + SET_ALARM_WITH_1_F32_DATA( ALARM_ID_DG_LOAD_CELLS_TARE_WEIGHT_OUT_OF_RANGE, weight ); } } @@ -373,10 +377,12 @@ *************************************************************************/ F32 getLoadCellWeight( LOAD_CELL_ID_T loadCellID ) { - F32 result = loadcells[ loadCellID ].weight.data; + F32 result = 0.0F; if ( loadCellID < NUM_OF_LOAD_CELLS ) { + result = loadcells[ loadCellID ].weight.data; + if ( OVERRIDE_KEY == loadcells[ loadCellID ].weight.override ) { result = loadcells[ loadCellID ].weight.ovData; @@ -405,7 +411,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 { @@ -478,7 +489,7 @@ *************************************************************************/ static void monitorLoadCellsWeightOutOfRange( LOAD_CELL_ID_T loadCell ) { - F32 weight = getLoadCellSmallFilteredWeight( loadCell ); + F32 weight = getLoadCellWeight( loadCell ); BOOL isWeightOutOfRange = ( ( weight < LOAD_CELL_MIN_ALLOWED_WEIGHT_GRAMS ) || ( weight > LOAD_CELL_MAX_ALLOWED_WEIGHT_GRAMS ) ? TRUE : FALSE ); checkPersistentAlarm( ALARM_ID_DG_LOAD_CELL_WEIGHT_OUT_OF_RANGE, isWeightOutOfRange, weight, LOAD_CELL_MAX_ALLOWED_WEIGHT_GRAMS ); @@ -515,15 +526,14 @@ ( 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 ); } + // Pick the biggest drift to log w/ alarm if triggered + drift = ( loadCellADrift > loadCellBDrift ? loadCellADrift : loadCellBDrift ); checkPersistentAlarm( ALARM_ID_DG_LOAD_CELL_PRIMARY_BACKUP_DRIFT_OUT_OF_RANGE, isDriftOutOfRange, drift, LOAD_CELL_PRIMARY_BACKUP_MAX_ALLOWED_DRIFT_GRAMS ); } @@ -541,9 +551,11 @@ * @details Outputs: load cell filtered weight * @param loadCellID ID of the load cell to override * @param value override filtered load cell weight + * @param raw flag indicates whether override should apply to raw calibrated + * reading or filtered & tared reading * @return TRUE if override successful, FALSE if not *************************************************************************/ -BOOL testSetLoadCellOverride( U32 loadCellID, F32 value ) +BOOL testSetLoadCellOverride( U32 loadCellID, F32 value, BOOL raw ) { BOOL result = FALSE; @@ -552,9 +564,16 @@ if ( TRUE == isTestingActivated() ) { 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; + if ( TRUE == raw ) + { + loadcells[ loadCellID ].weight.ovData = value; + loadcells[ loadCellID ].weight.override = OVERRIDE_KEY; + } + else + { + loadcells[ loadCellID ].smallFilteredWeight.ovData = value; + loadcells[ loadCellID ].smallFilteredWeight.override = OVERRIDE_KEY; + } } } @@ -567,9 +586,11 @@ * @details Inputs: none * @details Outputs: load cell filtered weight * @param loadCellID ID of the load cell to override + * @param raw flag indicates whether override should apply to raw calibrated + * reading or filtered & tared reading * @return TRUE if reset successful, FALSE if not *************************************************************************/ -BOOL testResetLoadCellOverride( U32 loadCellID ) +BOOL testResetLoadCellOverride( U32 loadCellID, BOOL raw ) { BOOL result = FALSE; @@ -578,8 +599,16 @@ if ( TRUE == isTestingActivated() ) { result = TRUE; - loadcells[ loadCellID ].weight.override = OVERRIDE_RESET; - loadcells[ loadCellID ].weight.ovData = loadcells[ loadCellID ].weight.ovInitData; + if ( TRUE == raw ) + { + loadcells[ loadCellID ].weight.override = OVERRIDE_RESET; + loadcells[ loadCellID ].weight.ovData = loadcells[ loadCellID ].smallFilteredWeight.ovInitData; + } + else + { + loadcells[ loadCellID ].smallFilteredWeight.override = OVERRIDE_RESET; + loadcells[ loadCellID ].smallFilteredWeight.ovData = loadcells[ loadCellID ].smallFilteredWeight.ovInitData; + } } }