Index: firmware/App/Controllers/LoadCell.c =================================================================== diff -u -r1285d4ad8e8335bee537d9e2a7bc7660fd52c2e9 -re06d369137e7ba150249b038c0649b434ac2f2f3 --- firmware/App/Controllers/LoadCell.c (.../LoadCell.c) (revision 1285d4ad8e8335bee537d9e2a7bc7660fd52c2e9) +++ firmware/App/Controllers/LoadCell.c (.../LoadCell.c) (revision e06d369137e7ba150249b038c0649b434ac2f2f3) @@ -1,96 +1,677 @@ -/**********************************************************************//** - * - * Copyright (c) 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 - * - * @date 25-Feb-2020 - * @author S. Nejatali - * - * @brief Processing sensor data. - * - **************************************************************************/ +/************************************************************************** +* +* Copyright (c) 2020-2022 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) Dong Nguyen +* @date (last) 27-Sep-2022 +* +* @author (original) Saeed Nejatali +* @date (original) 25-Feb-2020 +* +***************************************************************************/ +#include // For load cells calibration calculations + +#include "FPGA.h" #include "LoadCell.h" -#include "Common.h" +#include "MessageSupport.h" +#include "NVDataMgmt.h" +#include "PersistentAlarm.h" #include "SystemCommMessages.h" -#include "FPGA.h" #include "TaskPriority.h" -static F32 Load_cell_a1_ave; -static F32 Load_cell_a2_ave; -static F32 Load_cell_b1_ave; -static F32 Load_cell_b2_ave; - /**@}*/ +/** + * @addtogroup LoadCells + * @{ + */ +// ********** 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. +/// Conversion factor from ADC counts to grams. +static const F32 ADC2GRAM = (0.0894F * 1.1338F); + +#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 LOAD_CELL_MIN_ALLOWED_WEIGHT_GRAMS 0.0F ///< Load cell minimum allowed weight in grams. +#define LOAD_CELL_MAX_ALLOWED_WEIGHT_GRAMS 4500.0F ///< Load cell maximum allowed weight in grams. +#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.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 +{ + 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. + 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. + 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; ///< Load cell filtering timer count. +static U32 loadCellDataPublicationTimerCounter; ///< 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 BOOL hasLoadCellBeenTared[ NUM_OF_LOAD_CELLS ]; ///< Flags indicating whether loadcells have been tared yet. + +// ********** private function prototypes ********** + +static void monitorLoadCellsWeightOutOfRange( LOAD_CELL_ID_T loadCell ); +static void monitorLoadCellsPrimaryBackupDriftOutOfRange( void ); + /*********************************************************************//** * @brief - * The execLoadCell function gets load cell data from FPGA and advertises them over CAN. - * @details - * Inputs : none - * Outputs : Advertising load call data. + * The initLoadCell function initializes the LoadCell module. + * @details Inputs: none + * @details Outputs: LoadCell module initialized. * @return none *************************************************************************/ -void execLoadCell(void) + void initLoadCell( void ) { -#ifdef DEBUG_ENABLED - char debugStr[ 256 ]; -#endif - static U32 Counter = 0; - static U32 Load_cell_a1 = 0; - static U32 Load_cell_a2 = 0; - static U32 Load_cell_b1 = 0; - static U32 Load_cell_b2 = 0; - BOOL result; + U32 i; + U32 j; - Load_cell_a1 += getFPGALoadCellA1(); // No overflow since ADC output is 24 bits. - Load_cell_a2 += getFPGALoadCellA2(); - Load_cell_b1 += getFPGALoadCellB1(); - Load_cell_b2 += getFPGALoadCellB2(); + smallReadingsIdx = 0; + largeReadingsIdx = 0; + loadCellDataPublicationTimerCounter = DATA_PUBLISH_COUNTER_START_COUNT; + loadCellFilterTimerCount = 0; - Counter++; - if (Counter == LOAD_CELL_REPORT_PERIOD) - { - Counter = 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 ] ); - Load_cell_a1_ave = (F32)(Load_cell_a1)*ADC2GRAM; // division for averaging folded into ADC2GRAM - Load_cell_a2_ave = (F32)(Load_cell_a2)*ADC2GRAM; - Load_cell_b1_ave = (F32)(Load_cell_b1)*ADC2GRAM; - Load_cell_b2_ave = (F32)(Load_cell_b2)*ADC2GRAM; - result = broadcastLoadCellData( Load_cell_a1_ave, Load_cell_a2_ave, Load_cell_b1_ave, Load_cell_b2_ave ); -#ifdef DEBUG_ENABLED - if (result == FALSE) - sprintf( debugStr, "Adding load cell data to CAN buffer failed" ); -#else - (void)result; -#endif - Load_cell_a1 = 0; - Load_cell_a2 = 0; - Load_cell_b1 = 0; - Load_cell_b2 = 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.0F; + } + + for ( j = 0; j < SIZE_OF_LARGE_LOAD_CELL_AVG; j++ ) + { + loadcells[ i ].largeFilterReadings[ j ] = 0.0F; + } + + loadcells[ i ].loadCellVelocity_g_min = 0.0F; + } + + // Initialize persistent alarm(s) + 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 ); } -F32 getLoadCellA1Ave(void) +/*********************************************************************//** + * @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 ) { - return Load_cell_a1_ave; + U32 ii; + U32 a1 = getFPGALoadCellA1(); + U32 a2 = getFPGALoadCellA2(); + U32 b1 = getFPGALoadCellB1(); + U32 b2 = getFPGALoadCellB2(); + + // Check error bits from new readings + 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 ); + + 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 ); + + + /*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; + + if ( TRUE == isPersistentAlarmTriggered( ALARM_ID__AVAILABLE_13, ( ( a1 > 0 ) || ( a2 > 0 ) || ( b1 > 0 ) || ( b2 > 0 ) ) ) ) + { + SET_ALARM_WITH_1_U32_DATA( ALARM_ID__AVAILABLE_13, ( a1 | a2 | b1 | b2 ) ) + }*/ + + 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.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 + for ( ii = 0; ii < NUM_OF_LOAD_CELLS; ++ii ) + { + 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; + + // 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 ].smallFilterReadings[ smallReadingsIdx ] = 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.data = (F32)( loadcells[ ii ].smallFilterTotal / (F64)SIZE_OF_SMALL_LOAD_CELL_AVG ); + + // Apply the tare offset. NOTE: tare must be applied after checking the weight out of range. + loadcells[ ii ].smallFilteredWeight.data -= loadcells[ ii ].autoCalOffset; + } + + smallReadingsIdx = INC_WRAP( smallReadingsIdx, 0, SIZE_OF_SMALL_LOAD_CELL_AVG - 1 ); + + // filter every 100ms + if ( ++loadCellFilterTimerCount >= LOAD_CELL_REPORT_PERIOD ) + { + 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 ] = 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 ); + } + + // broadcast load cell data if we are at scheduled interval. + if ( ++loadCellDataPublicationTimerCounter >= getU32OverrideValue( &loadCellDataPublishInterval ) ) + { + LOAD_CELL_DATA_T loadCellData; + + loadCellData.loadCellA1inGram = getLoadCellSmallFilteredWeight( LOAD_CELL_RESERVOIR_1_PRIMARY ); + loadCellData.loadCellA2inGram = getLoadCellSmallFilteredWeight( LOAD_CELL_RESERVOIR_1_BACKUP ); + loadCellData.loadCellB1inGram = getLoadCellSmallFilteredWeight( LOAD_CELL_RESERVOIR_2_PRIMARY ); + loadCellData.loadCellB2inGram = getLoadCellSmallFilteredWeight( LOAD_CELL_RESERVOIR_2_BACKUP ); + + // Broadcast small filtered load cell data + 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(); } -F32 getLoadCellA2Ave(void) +/*********************************************************************//** + * @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: loadCellsCalRecord + * @return result of the load cell self test + *************************************************************************/ +SELF_TEST_STATUS_T execLoadCellsSelfTest ( void ) { - return Load_cell_a2_ave; + 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 ); + + if ( TRUE == calStatus ) + { + result = SELF_TEST_STATUS_PASSED; + } + else + { + result = SELF_TEST_STATUS_FAILED; + } + + return result; } -F32 getLoadCellB1Ave(void) +/*********************************************************************//** + * @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 ) { - return Load_cell_b1_ave; + BOOL tared = FALSE; + + if ( fabs( loadcells[ loadCellID ].autoCalOffset ) < NEARLY_ZERO ) + { + tared = FALSE; + } + else + { + tared = TRUE; + } + return tared; } -F32 getLoadCellB2Ave(void) +/*********************************************************************//** + * @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 ) { - return Load_cell_b2_ave; + BOOL isWeightOutOfRange = FALSE; + F32 weight = getLoadCellSmallFilteredWeight( loadCellID ); + + // Check if the load cell is being tared for the first time + 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 + F32 deltaWeight = fabs( weight - EMPTY_RESERVOIR_WEIGHT_GRAMS ); + isWeightOutOfRange = ( deltaWeight > MAX_ALLOWED_EXTRA_WEIGHT_BEFORE_FIRST_TARE_GRAMS ? TRUE : FALSE ); + } + else + { + isWeightOutOfRange = ( fabs( weight ) > MAX_ALLOWED_EXTRA_WEIGHT_BEFORE_TARE_GRAMS ? TRUE : FALSE ); + } + + if ( FALSE == isWeightOutOfRange ) + { + // Add old auto calibration offset to get back to actual weight value + 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 ); + } } +/*********************************************************************//** + * @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 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 weight for + * @return the load cell weight for the given load cell ID. + *************************************************************************/ +F32 getLoadCellWeight( LOAD_CELL_ID_T loadCellID ) +{ + F32 result = 0.0; + + if ( loadCellID < NUM_OF_LOAD_CELLS ) + { + result = loadcells[ loadCellID ].weight.data; + + if ( OVERRIDE_KEY == loadcells[ loadCellID ].weight.override ) + { + result = loadcells[ loadCellID ].weight.ovData; + } + } + else + { + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_INVALID_LOAD_CELL_ID, (U32)loadCellID ) + } + + 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 large filtered weight + * @return the small filtered load cell weight for the given load cell ID. + *************************************************************************/ +F32 getLoadCellSmallFilteredWeight( LOAD_CELL_ID_T loadCellID ) +{ + F32 result = 0.0; + + if ( loadCellID < NUM_OF_LOAD_CELLS ) + { + result = loadcells[ loadCellID ].smallFilteredWeight.data; + + if ( OVERRIDE_KEY == loadcells[ loadCellID ].smallFilteredWeight.override ) + { + result = loadcells[ loadCellID ].smallFilteredWeight.ovData; + } + } + else + { + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_INVALID_LOAD_CELL_ID, (U32)loadCellID ) + } + + 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 + * @return the large filtered load cell weight for the given load cell ID. + *************************************************************************/ +F32 getLoadCellLargeFilteredWeight( LOAD_CELL_ID_T loadCellID ) +{ + F32 result = 0.0; + + if ( loadCellID < NUM_OF_LOAD_CELLS ) + { + result = loadcells[ loadCellID ].largeFilteredWeight; + } + else + { + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_INVALID_LOAD_CELL_ID, (U32)loadCellID ) + } + + return result; +} + +/*********************************************************************//** + * @brief + * The getLoadCellVelocity function gets the current velocity (in g/min) + * for the given load cell. + * @details Inputs: loadcells[] + * @details Outputs: none + * @param loadCellID ID of load cell to get velocity + * @return the velocity (in g/min) for the given load cell ID. + *************************************************************************/ +F32 getLoadCellVelocity( LOAD_CELL_ID_T loadCellID ) +{ + F32 result = 0.0; + + if ( loadCellID < NUM_OF_LOAD_CELLS ) + { + result = loadcells[ loadCellID ].loadCellVelocity_g_min; + } + else + { + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_INVALID_LOAD_CELL_ID, (U32)loadCellID ) + } + + return result; +} + +/*********************************************************************//** + * @brief + * The monitorLoadCellsWeightOutOfRange function monitors the weight of the + * load cells and if they are not in range, it raises an alarm. + * Function must be called prior to applying tare so that un-tared weight + * is checked for out of range. + * @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 = 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 ); +} + +/*********************************************************************//** + * @brief + * The monitorLoadCellsPrimaryBackupDriftOutOfRange function monitors the + * load cells' primary and backup drift. + * @details Inputs: none + * @details Outputs: none + * @return none + *************************************************************************/ +static void monitorLoadCellsPrimaryBackupDriftOutOfRange( void ) +{ + F32 drift = 0.0; + F32 loadCellADrift = 0.0; + F32 loadCellBDrift = 0.0; + BOOL isDriftOutOfRange = FALSE; + + // 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 ) ); + } + 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 ) ); + } + + if ( ( loadCellADrift > LOAD_CELL_PRIMARY_BACKUP_MAX_ALLOWED_DRIFT_GRAMS ) || + ( loadCellBDrift > LOAD_CELL_PRIMARY_BACKUP_MAX_ALLOWED_DRIFT_GRAMS ) ) + { + isDriftOutOfRange = TRUE; + } + else + { + isDriftOutOfRange = FALSE; + } + + // 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 ); +} + + +/************************************************************************* + * 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 + * @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 raw ) +{ + BOOL result = FALSE; + + if ( loadCellID < NUM_OF_LOAD_CELLS ) + { + if ( TRUE == isTestingActivated() ) + { + result = TRUE; + 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; + } + } + } + + 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 + * @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 raw ) +{ + BOOL result = FALSE; + + if ( loadCellID < NUM_OF_LOAD_CELLS ) + { + if ( TRUE == isTestingActivated() ) + { + result = TRUE; + 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; + } + } + } + + 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; +} + +/**@}*/