Index: firmware/App/Controllers/LoadCell.c =================================================================== diff -u -rcbcd41ec1ac2d5ae6446ec206ed6991a5306c249 -r622eebf6fb7f1c6c232ffd82bc072dd30a7b3f94 --- firmware/App/Controllers/LoadCell.c (.../LoadCell.c) (revision cbcd41ec1ac2d5ae6446ec206ed6991a5306c249) +++ firmware/App/Controllers/LoadCell.c (.../LoadCell.c) (revision 622eebf6fb7f1c6c232ffd82bc072dd30a7b3f94) @@ -19,6 +19,7 @@ #include "FPGA.h" #include "LoadCell.h" +#include "MessageSupport.h" #include "NVDataMgmt.h" #include "PersistentAlarm.h" #include "SystemCommMessages.h" @@ -34,51 +35,58 @@ // 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_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. +static const F32 ADC2GRAM = (0.0894 * 1.1338); -#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 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_FIRST_TARE_GRAMS 300 ///< Max allowed extra weight before first tare in grams. -#define MAX_ALLOWED_EXTRA_WEIGHT_BEFORE_TARE_GRAMS 15 ///< Max allowed extra weight before tare in grams. +#define LOAD_CELL_ADC_ERROR_PERSISTENCE 500 ///< Alarm persistence period (in ms) for load cell ADC errors. +#define LOAD_CELL_MIN_ALLOWED_WEIGHT_GRAMS 0.0 ///< Load cell minimum allowed weight in grams. +#define LOAD_CELL_MAX_ALLOWED_WEIGHT_GRAMS 4500.0 ///< Load cell maximum allowed weight in grams. +#define LOAD_CELL_MIN_ALLOWED_WEIGHT_BEFORE_TARE_GRAMS 1600.0 ///< 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 LOAD_CELL_PRIMARY_BACKUP_MAX_ALLOWED_DRIFT_GRAMS 60.0 ///< Load cell primary and backup maximum allowed weight drift 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. + 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. - 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. + 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. - 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. - F32 largeFilteredWeight; ///< Load cell large filtered (40 10Hz filtered 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. + 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. + 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. +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 BOOL processCalibrationData( void ); +static void monitorLoadCellsWeightOutOfRange( LOAD_CELL_ID_T loadCell ); +static void monitorLoadCellsPrimaryBackupDriftOutOfRange( void ); /*********************************************************************//** * @brief @@ -87,9 +95,8 @@ * @details Outputs: LoadCell module initialized. * @return none *************************************************************************/ -void initLoadCell( void ) + void initLoadCell( void ) { - U32 cell; U32 i; U32 j; @@ -126,18 +133,14 @@ loadcells[ i ].loadCellVelocity_g_min = 0.0; } - 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 ); + + 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 ); } /*********************************************************************//** @@ -188,8 +191,16 @@ // 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; + F32 loadCell = (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(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; @@ -199,14 +210,13 @@ loadcells[ ii ].smallFilterTotal += getLoadCellWeight( (LOAD_CELL_ID_T)ii ); // Calculate the load cell value before applying calibration to it - F32 loadCell = (F32)( loadcells[ ii ].smallFilterTotal / (F64)SIZE_OF_SMALL_LOAD_CELL_AVG ); - // 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 ].smallFilteredWeight = 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 ].autoCalOffset; + loadcells[ ii ].smallFilteredWeight = (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; } smallReadingsIdx = INC_WRAP( smallReadingsIdx, 0, SIZE_OF_SMALL_LOAD_CELL_AVG - 1 ); @@ -238,10 +248,13 @@ loadCellData.loadCellB2inGram = getLoadCellSmallFilteredWeight( LOAD_CELL_RESERVOIR_2_BACKUP ); // Broadcast small filtered load cell data - broadcastLoadCellData( &loadCellData ); + broadcastData( MSG_ID_LOAD_CELL_READINGS, COMM_BUFFER_OUT_CAN_DG_BROADCAST, (U08*)&loadCellData, sizeof( LOAD_CELL_DATA_T ) ); loadCellDataPublicationTimerCounter = 0; } + + // Monitor the load cells drift + monitorLoadCellsPrimaryBackupDriftOutOfRange(); } /*********************************************************************//** @@ -366,12 +379,6 @@ if ( loadCellID < NUM_OF_LOAD_CELLS ) { result = loadcells[ loadCellID ].smallFilteredWeight; - - // If the load cell is in override mode, return the override weight - if ( OVERRIDE_KEY == loadcells[ loadCellID ].weight.override ) - { - result = loadcells[ loadCellID ].weight.ovData; - } } else { @@ -472,7 +479,61 @@ return status; } +/*********************************************************************//** + * @brief + * The monitorLoadCellsWeightOutOfRange function monitors the weight of the + * load cells and if they are not in range, it raises an alarm. + * @details Inputs: none + * @details Outputs: none + * @param loadCell which is the load cell ID that its range is checked + * @return none + *************************************************************************/ +static void monitorLoadCellsWeightOutOfRange( LOAD_CELL_ID_T loadCell ) +{ + F32 weight = getLoadCellSmallFilteredWeight( loadCell ); + BOOL isWeightOutOfRange = ( weight < LOAD_CELL_MIN_ALLOWED_WEIGHT_GRAMS ) || ( weight > LOAD_CELL_MAX_ALLOWED_WEIGHT_GRAMS ); + checkPersistentAlarm( ALARM_ID_DG_LOAD_CELL_WEIGHT_OUT_OF_RANGE, isWeightOutOfRange, weight, LOAD_CELL_MAX_ALLOWED_WEIGHT_GRAMS ); +} + +/*********************************************************************//** + * @brief + * The monitorLoadCellsPrimaryBackupDriftOutOfRange function monitors the + * load cells' primary and backup drift. + * @details Inputs: none + * @details Outputs: none + * @return none + *************************************************************************/ +static void monitorLoadCellsPrimaryBackupDriftOutOfRange( void ) +{ + // TODO do we need this function at all? + F32 drift; + + F32 loadCellADrift = fabs( getLoadCellSmallFilteredWeight( LOAD_CELL_RESERVOIR_1_PRIMARY ) - + getLoadCellSmallFilteredWeight( LOAD_CELL_RESERVOIR_1_BACKUP ) ); + + F32 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 ) + { + drift = ( loadCellADrift > LOAD_CELL_PRIMARY_BACKUP_MAX_ALLOWED_DRIFT_GRAMS ? loadCellADrift : loadCellBDrift ); + } + else + { + // 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 ); +} + + /************************************************************************* * TEST SUPPORT FUNCTIONS *************************************************************************/ @@ -496,7 +557,8 @@ if ( TRUE == isTestingActivated() ) { result = TRUE; - loadcells[ loadCellID ].weight.ovData = value; + // 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; } }