Index: firmware/App/Controllers/LoadCell.c =================================================================== diff -u -r87a22cbaa87daab0d7cedabc67452cead83d8630 -r484b185f0cf4b2ea0ba9de331573952b1b5124b4 --- firmware/App/Controllers/LoadCell.c (.../LoadCell.c) (revision 87a22cbaa87daab0d7cedabc67452cead83d8630) +++ firmware/App/Controllers/LoadCell.c (.../LoadCell.c) (revision 484b185f0cf4b2ea0ba9de331573952b1b5124b4) @@ -34,12 +34,23 @@ #define ADC2GRAM (0.0894 * 1.1338) ///< Conversion factor from ADC counts to grams. #define LOAD_CELL_ZERO_OFFSET -1215.0 ///< Zero offset (in grams). TODO - right now, this is empty reservoir weight. +#define SIZE_OF_SMALL_LOAD_CELL_AVG 10 ///< Small load cell moving average has 10 samples. +#define SIZE_OF_LARGE_LOAD_CELL_AVG 40 ///< Large load cell moving average has 40 samples. + /// Load cell data structure typedef struct { - U32 measuredReadingSum; ///< Raw load cell sums for averaging - OVERRIDE_F32_T filteredWeight; ///< Latest filtered load cell weights - F32 autoCalOffset; ///< Load cell auto-calibration offset + U32 measuredReadingSum; ///< Raw load cell sums for averaging + OVERRIDE_F32_T filteredWeight; ///< Latest filtered load cell weights + F32 autoCalOffset; ///< Load cell auto-calibration offset + + F32 smallFilterReadings[ SIZE_OF_SMALL_LOAD_CELL_AVG ]; ///< Load cell samples for small load cell moving average + F32 smallFilterTotal; ///< Small filter rolling total - used to calc small load cell moving average + F32 smallFilteredWeight; ///< Load cell small filtered (8 sample) weight. + + F32 largeFilterReadings[ SIZE_OF_LARGE_LOAD_CELL_AVG ]; ///< Load cell samples for large load cell moving average + F32 largeFilterTotal; ///< Large filter rolling total - used to calc small load cell moving average + F32 largeFilteredWeight; ///< Load cell large filtered (32 sample) weight. } LOADCELL_T; // ********** private data ********** @@ -50,6 +61,9 @@ 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 + // ********** private function prototypes ********** static U32 getLoadCellDataPublishInterval( void ); @@ -64,7 +78,11 @@ void initLoadCell( void ) { U32 i; + U32 j; + smallReadingsIdx = 0; + largeReadingsIdx = 0; + for ( i = 0; i < NUM_OF_LOAD_CELLS; i++ ) { loadcells[ i ].filteredWeight.data = 0.0; @@ -74,6 +92,22 @@ loadcells[ i ].measuredReadingSum = 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; + } } } @@ -90,10 +124,10 @@ U32 ii; // update sums for load cell average calculations - loadcells[ LOAD_CELL_A1 ].measuredReadingSum += getFPGALoadCellA1(); - loadcells[ LOAD_CELL_A2 ].measuredReadingSum += getFPGALoadCellA2(); - loadcells[ LOAD_CELL_B1 ].measuredReadingSum += getFPGALoadCellB1(); - loadcells[ LOAD_CELL_B2 ].measuredReadingSum += getFPGALoadCellB2(); + loadcells[ LOAD_CELL_RESERVOIR_1_PRIMARY ].measuredReadingSum += getFPGALoadCellA1(); + loadcells[ LOAD_CELL_RESERVOIR_1_BACKUP ].measuredReadingSum += getFPGALoadCellA2(); + loadcells[ LOAD_CELL_RESERVOIR_2_PRIMARY ].measuredReadingSum += getFPGALoadCellB1(); + loadcells[ LOAD_CELL_RESERVOIR_2_BACKUP ].measuredReadingSum += getFPGALoadCellB2(); // filter every 100ms if ( ++loadCellFilterTimerCount >= LOAD_CELL_SAMPLES_TO_AVERAGE ) @@ -106,9 +140,22 @@ // reset sums for next averaging loadcells[ ii ].measuredReadingSum = 0; + + // Update filtered with new filtered weight sample and moving averages + loadcells[ ii ].smallFilterTotal -= loadcells[ ii ].smallFilterReadings[ smallReadingsIdx ]; + loadcells[ ii ].smallFilterReadings[ smallReadingsIdx ] = loadcells[ ii ].filteredWeight.data; + loadcells[ ii ].smallFilterTotal += loadcells[ ii ].filteredWeight.data; + loadcells[ ii ].smallFilteredWeight = loadcells[ ii ].smallFilterTotal / (F32)SIZE_OF_SMALL_LOAD_CELL_AVG; + + loadcells[ ii ].largeFilterTotal -= loadcells[ ii ].largeFilterReadings[ largeReadingsIdx ]; + loadcells[ ii ].largeFilterReadings[ largeReadingsIdx ] = loadcells[ ii ].filteredWeight.data; + loadcells[ ii ].largeFilterTotal += loadcells[ ii ].filteredWeight.data; + loadcells[ ii ].largeFilteredWeight = loadcells[ ii ].largeFilterTotal / (F32)SIZE_OF_LARGE_LOAD_CELL_AVG; } loadCellFilterTimerCount = 0; + smallReadingsIdx = INC_WRAP( smallReadingsIdx, 0, SIZE_OF_SMALL_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. @@ -117,8 +164,8 @@ loadCellDataPublicationTimerCounter = 0; // broadcast load cell data - broadcastLoadCellData( getLoadCellFilteredWeight( LOAD_CELL_A1 ), getLoadCellFilteredWeight( LOAD_CELL_A2 ), - getLoadCellFilteredWeight( LOAD_CELL_B1 ), getLoadCellFilteredWeight( LOAD_CELL_B2 ) ); + broadcastLoadCellData( getLoadCellFilteredWeight( LOAD_CELL_RESERVOIR_1_PRIMARY ), getLoadCellFilteredWeight( LOAD_CELL_RESERVOIR_1_BACKUP ), + getLoadCellFilteredWeight( LOAD_CELL_RESERVOIR_2_PRIMARY ), getLoadCellFilteredWeight( LOAD_CELL_RESERVOIR_2_BACKUP ) ); } } @@ -182,6 +229,56 @@ /*********************************************************************//** * @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 small filtered weight for + * @return the small filtered load cell weight for the given load cell ID. + *************************************************************************/ +F32 getLoadCellSmallFilteredWeight( LOAD_CELL_ID_T loadCellID ) +{ + F32 result = 0; + + if ( loadCellID < NUM_OF_LOAD_CELLS ) + { + result = loadcells[ loadCellID ].smallFilteredWeight; + } + else + { + activateAlarmNoData( ALARM_ID_DG_SOFTWARE_FAULT ); + } + + 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 for + * @return the large filtered load cell weight for the given load cell ID. + *************************************************************************/ +F32 getLoadCellLargeFilteredWeight( LOAD_CELL_ID_T loadCellID ) +{ + F32 result = 0; + + if ( loadCellID < NUM_OF_LOAD_CELLS ) + { + result = loadcells[ loadCellID ].largeFilteredWeight; + } + else + { + activateAlarmNoData( ALARM_ID_DG_SOFTWARE_FAULT ); + } + + return result; +} + +/*********************************************************************//** + * @brief * The getLoadCellDataPublishInterval function gets the load cell data publish interval. * @details Inputs: loadCellDataPublishInterval * @details Outputs: none