Index: firmware/App/Controllers/LoadCell.c =================================================================== diff -u -r97d75c5bf1887d4ad6e8a9d15edac262af46042c -r9c3d402fd9ac55c4dc7116de8a61cc02c18b44bc --- firmware/App/Controllers/LoadCell.c (.../LoadCell.c) (revision 97d75c5bf1887d4ad6e8a9d15edac262af46042c) +++ firmware/App/Controllers/LoadCell.c (.../LoadCell.c) (revision 9c3d402fd9ac55c4dc7116de8a61cc02c18b44bc) @@ -1,207 +1,602 @@ -/**********************************************************************//** - * - * 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) 2019-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) Dara Navaei +* @date (last) 01-Jan-2022 +* +* @author (original) Saeed Nejatali +* @date (original) 25-Feb-2020 +* +***************************************************************************/ +#include // For load cells calibration calculations + +#include "FPGA.h" #include "LoadCell.h" +#include "MessageSupport.h" +#include "NVDataMgmt.h" +#include "PersistentAlarm.h" #include "SystemCommMessages.h" -#include "FPGA.h" #include "TaskPriority.h" -DATA_DECL( U32, MeasuredLoadCellA1, loadCellA1raw, 0, 0 ); -DATA_DECL( U32, MeasuredLoadCellA2, loadCellA2raw, 0, 0 ); -DATA_DECL( U32, MeasuredLoadCellB1, loadCellB1raw, 0, 0 ); -DATA_DECL( U32, MeasuredLoadCellB2, loadCellB2raw, 0, 0 ); -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.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 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. +#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 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 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; ///< 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 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; - loadCellA1raw.data = getFPGALoadCellA1(); - loadCellA2raw.data = getFPGALoadCellA2(); - loadCellB1raw.data = getFPGALoadCellB1(); - loadCellB2raw.data = getFPGALoadCellB2(); + smallReadingsIdx = 0; + largeReadingsIdx = 0; + loadCellDataPublicationTimerCounter = DATA_PUBLISH_COUNTER_START_COUNT; - Load_cell_a1 += getLoadCellA1raw(); - Load_cell_a2 += getLoadCellA2raw(); - Load_cell_b1 += getLoadCellB1raw(); - Load_cell_b2 += getLoadCellB2raw(); + for ( i = 0; i < NUM_OF_LOAD_CELLS; i++ ) + { + loadcells[ i ].rawReading = 0; - Counter++; - if (Counter == LOAD_CELL_REPORT_PERIOD) - { - Counter = 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; - 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; - } + 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; + } + + loadcells[ i ].loadCellVelocity_g_min = 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 ); } -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(); + + // 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 >> 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 ); + 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 ) ) + } + + // 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_CALIBRATION ); + + // 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 ) + { + 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; + + // 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 = (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 ); + + // 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 ] = loadcells[ ii ].smallFilteredWeight; + loadcells[ ii ].largeFilterTotal += loadcells[ ii ].smallFilteredWeight; + 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_CALIBRATION ); + + if ( TRUE == calStatus ) + { + result = SELF_TEST_STATUS_PASSED; + } + else + { + result = SELF_TEST_STATUS_FAILED; + } + + return result; } -F32 getLoadCellB1Ave(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_b1_ave; + BOOL isWeightOutOfRange = FALSE; + + F32 weight = getLoadCellSmallFilteredWeight( loadCellID ); + + // Check if the load cell is being tared for the first time + if ( fabs(loadcells[ loadCellID ].autoCalOffset ) < NEARLY_ZERO ) + { + // 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 + loadcells[ loadCellID ].autoCalOffset ); + } + else + { + SET_ALARM_WITH_1_F32_DATA( ALARM_ID_DG_LOAD_CELLS_TARE_WEIGHT_OUT_OF_RANGE, weight ) + } } -F32 getLoadCellB2Ave(void) +/*********************************************************************//** + * @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 ) { - return Load_cell_b2_ave; + loadcells[ loadCellID ].autoCalOffset = 0.0; } -/************************************************************************* - * @brief testSetLoadCellA1Override and testResetLoadCellA1Override - * The testSetLoadCellA1Override function overrides the measured \n - * load cell A1. \n - * The testResetLoadCellA1Override function resets the override of the \n - * load cell A1. \n - * @details - * Inputs : none - * Outputs : loadCellA1raw - * @param value : override measured load cell A1 raw - * @return TRUE if override successful, FALSE if not +/*********************************************************************//** + * @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. *************************************************************************/ -DATA_OVERRIDE_FUNC( U32, testSetLoadCellA1Override, testResetLoadCellA1Override, loadCellA1raw ) +F32 getLoadCellWeight( LOAD_CELL_ID_T loadCellID ) +{ + F32 result = loadcells[ loadCellID ].weight.data; -/************************************************************************* - * @brief testSetLoadCellA2Override and testResetLoadCellA2Override - * The testSetLoadCellA2Override function overrides the measured \n - * load cell A2. \n - * The testResetLoadCellA1Override function resets the override of the \n - * load cell A2. \n - * @details - * Inputs : none - * Outputs : loadCellA2raw - * @param value : override measured load cell A2 raw - * @return TRUE if override successful, FALSE if not + if ( loadCellID < NUM_OF_LOAD_CELLS ) + { + 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. *************************************************************************/ -DATA_OVERRIDE_FUNC( U32, testSetLoadCellA2Override, testResetLoadCellA2Override, loadCellA2raw ) +F32 getLoadCellSmallFilteredWeight( LOAD_CELL_ID_T loadCellID ) +{ + F32 result = 0.0; -/************************************************************************* - * @brief testSetLoadCellB1Override and testResetLoadCellB1Override - * The testSetLoadCellB1Override function overrides the measured \n - * load cell B1. \n - * The testResetLoadCellB1Override function resets the override of the \n - * load cell B1. \n - * @details - * Inputs : none - * Outputs : loadCellB1raw - * @param value : override measured load cell B1 raw - * @return TRUE if override successful, FALSE if not + if ( loadCellID < NUM_OF_LOAD_CELLS ) + { + result = loadcells[ loadCellID ].smallFilteredWeight; + } + 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. *************************************************************************/ -DATA_OVERRIDE_FUNC( U32, testSetLoadCellB1Override, testResetLoadCellB1Override, loadCellB1raw ) +F32 getLoadCellLargeFilteredWeight( LOAD_CELL_ID_T loadCellID ) +{ + F32 result = 0.0; -/************************************************************************* - * @brief testSetLoadCellB2Override and testResetLoadCellB2Override - * The testSetLoadCellB2Override function overrides the measured \n - * load cell B2. \n - * The testResetLoadCellB2Override function resets the override of the \n - * load cell B2. \n - * @details - * Inputs : none - * Outputs : loadCellB2raw - * @param value : override measured load cell B2 raw - * @return TRUE if override successful, FALSE if not + 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. *************************************************************************/ -DATA_OVERRIDE_FUNC( U32, testSetLoadCellB2Override, testResetLoadCellB2Override, loadCellB2raw ) +F32 getLoadCellVelocity( LOAD_CELL_ID_T loadCellID ) +{ + F32 result = 0.0; -/************************************************************************* - * @brief getLoadCellA1raw - * The getLoadCellA1raw function gets the measured load cell A1 \n - * current. - * @details - * Inputs : loadCellA1raw - * Outputs : none - * @param none - * @return the load cell A1 raw value. + 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. + * @details Inputs: none + * @details Outputs: none + * @param loadCell which is the load cell ID that its range is checked + * @return none *************************************************************************/ -DATA_GET( U32, getLoadCellA1raw, loadCellA1raw ) +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 ); -/************************************************************************* - * @brief getLoadCellA2raw - * The getLoadCellA2raw function gets the measured load cell A2 \n - * current. - * @details - * Inputs : loadCellA2raw - * Outputs : none - * @param none - * @return the load cell A2 raw value. + 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 *************************************************************************/ -DATA_GET( U32, getLoadCellA2raw, loadCellA2raw ) +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 ); +} + + /************************************************************************* - * @brief getLoadCellB1raw - * The getLoadCellB1raw function gets the measured load cell B1 \n - * current. - * @details - * Inputs : loadCellB1raw - * Outputs : none - * @param none - * @return the load cell B1 raw value. + * TEST SUPPORT FUNCTIONS *************************************************************************/ -DATA_GET( U32, getLoadCellB1raw, loadCellB1raw ) -/************************************************************************* - * @brief getLoadCellB2raw - * The getLoadCellB2raw function gets the measured load cell B2 \n - * current. - * @details - * Inputs : loadCellB2raw - * Outputs : none - * @param none - * @return the load cell B2 raw value. + +/*********************************************************************//** + * @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 + * @return TRUE if override successful, FALSE if not *************************************************************************/ -DATA_GET( U32, getLoadCellB2raw, loadCellB2raw ) +BOOL testSetLoadCellOverride( U32 loadCellID, F32 value ) +{ + BOOL result = FALSE; + + if ( loadCellID < NUM_OF_LOAD_CELLS ) + { + 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; + } + } + + 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 + * @return TRUE if reset successful, FALSE if not + *************************************************************************/ +BOOL testResetLoadCellOverride( U32 loadCellID ) +{ + BOOL result = FALSE; + + if ( loadCellID < NUM_OF_LOAD_CELLS ) + { + if ( TRUE == isTestingActivated() ) + { + result = TRUE; + loadcells[ loadCellID ].weight.override = OVERRIDE_RESET; + loadcells[ loadCellID ].weight.ovData = loadcells[ loadCellID ].weight.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; +} + +/**@}*/