Index: firmware/App/Controllers/LoadCell.c =================================================================== diff -u -r4d7d40a27130dc813d653f044cbb856b1b7d8481 -r4af713cb53b909ba87893dc814d39af89fb8e8d2 --- firmware/App/Controllers/LoadCell.c (.../LoadCell.c) (revision 4d7d40a27130dc813d653f044cbb856b1b7d8481) +++ firmware/App/Controllers/LoadCell.c (.../LoadCell.c) (revision 4af713cb53b909ba87893dc814d39af89fb8e8d2) @@ -38,9 +38,9 @@ static OVERRIDE_U32_T loadCellDataPublishInterval = { LOAD_CELL_REPORT_PERIOD, LOAD_CELL_REPORT_PERIOD, 0, 0 }; ///< Broadcast load cell data publish interval. -static U32 measuredLoadCellReadingsRaw[ NUM_OF_LOAD_CELLS ]; ///< Latest measured raw load cell readings. static U32 measuredLoadCellReadingsSum[ NUM_OF_LOAD_CELLS ]; ///< Raw load cell sums for averaging. static OVERRIDE_F32_T filteredLoadCellWeights[ NUM_OF_LOAD_CELLS ]; ///< Latest filtered load cell weights. +static F32 autoCalOffset[ NUM_OF_LOAD_CELLS ]; ///< Auto calibration offset for load cell. static U32 loadCellFilterTimerCount = 0; ///< Load cell filtering timer count. static U32 loadCellDataPublicationTimerCounter = 0; ///< Load cell data publication timer counter to CAN bus. @@ -67,7 +67,8 @@ filteredLoadCellWeights[ i ].override = OVERRIDE_RESET; measuredLoadCellReadingsSum[ i ] = 0; - measuredLoadCellReadingsRaw[ i ] = 0; + + autoCalOffset[ i ] = 0.0; } } @@ -81,16 +82,11 @@ *************************************************************************/ void execLoadCell( void ) { - // get latest raw load cell readings - measuredLoadCellReadingsRaw[ LOAD_CELL_A1 ] = getFPGALoadCellA1(); - measuredLoadCellReadingsRaw[ LOAD_CELL_A2 ] = getFPGALoadCellA2(); - measuredLoadCellReadingsRaw[ LOAD_CELL_B1 ] = getFPGALoadCellB1(); - measuredLoadCellReadingsRaw[ LOAD_CELL_B2 ] = getFPGALoadCellB2(); // update sums for load cell average calculations - measuredLoadCellReadingsSum[ LOAD_CELL_A1 ] += measuredLoadCellReadingsRaw[ LOAD_CELL_A1 ]; - measuredLoadCellReadingsSum[ LOAD_CELL_A2 ] += measuredLoadCellReadingsRaw[ LOAD_CELL_A2 ]; - measuredLoadCellReadingsSum[ LOAD_CELL_B1 ] += measuredLoadCellReadingsRaw[ LOAD_CELL_B1 ]; - measuredLoadCellReadingsSum[ LOAD_CELL_B2 ] += measuredLoadCellReadingsRaw[ LOAD_CELL_B2 ]; + measuredLoadCellReadingsSum[ LOAD_CELL_A1 ] += getFPGALoadCellA1(); + measuredLoadCellReadingsSum[ LOAD_CELL_A2 ] += getFPGALoadCellA2(); + measuredLoadCellReadingsSum[ LOAD_CELL_B1 ] += getFPGALoadCellB1(); + measuredLoadCellReadingsSum[ LOAD_CELL_B2 ] += getFPGALoadCellB2(); // filter every 100ms if ( ++loadCellFilterTimerCount >= LOAD_CELL_SAMPLES_TO_AVERAGE ) @@ -99,13 +95,13 @@ // calculate load cell average weights filteredLoadCellWeights[ LOAD_CELL_A1 ].data = (F32)measuredLoadCellReadingsSum[ LOAD_CELL_A1 ] * - LOAD_CELL_AVERAGE_MULTIPLIER * ADC2GRAM + LOAD_CELL_ZERO_OFFSET; + LOAD_CELL_AVERAGE_MULTIPLIER * ADC2GRAM + LOAD_CELL_ZERO_OFFSET - autoCalOffset[ LOAD_CELL_A1 ]; filteredLoadCellWeights[ LOAD_CELL_A2 ].data = (F32)measuredLoadCellReadingsSum[ LOAD_CELL_A2 ] * - LOAD_CELL_AVERAGE_MULTIPLIER * ADC2GRAM + LOAD_CELL_ZERO_OFFSET; + LOAD_CELL_AVERAGE_MULTIPLIER * ADC2GRAM + LOAD_CELL_ZERO_OFFSET - autoCalOffset[ LOAD_CELL_A2 ]; filteredLoadCellWeights[ LOAD_CELL_B1 ].data = (F32)measuredLoadCellReadingsSum[ LOAD_CELL_B1 ] * - LOAD_CELL_AVERAGE_MULTIPLIER * ADC2GRAM + LOAD_CELL_ZERO_OFFSET; + LOAD_CELL_AVERAGE_MULTIPLIER * ADC2GRAM + LOAD_CELL_ZERO_OFFSET - autoCalOffset[ LOAD_CELL_B1 ]; filteredLoadCellWeights[ LOAD_CELL_B2 ].data = (F32)measuredLoadCellReadingsSum[ LOAD_CELL_B2 ] * - LOAD_CELL_AVERAGE_MULTIPLIER * ADC2GRAM + LOAD_CELL_ZERO_OFFSET; + LOAD_CELL_AVERAGE_MULTIPLIER * ADC2GRAM + LOAD_CELL_ZERO_OFFSET - autoCalOffset[ LOAD_CELL_B2 ]; // reset sums for next averaging measuredLoadCellReadingsSum[ LOAD_CELL_A1 ] = 0; @@ -127,6 +123,20 @@ /*********************************************************************//** * @brief + * The setLoadCellAutoCal function sets the load cell auto calibration offset + * for a given load cell ID. + * @details Inputs: none + * @details Outputs: autoCalibrationOffset[] + * @param loadCellID ID of load cell to set calibration offset for + * @param calOffset auto calibration offset value + *************************************************************************/ +void setLoadCellAutoCal( LOAD_CELL_ID_T loadCellID, F32 calOffset ) +{ + autoCalOffset[ loadCellID ] = calOffset; +} + +/*********************************************************************//** + * @brief * The getLoadCellFilteredWeight function gets the measured filtered load cell * weight for a given load cell ID. * @details Inputs: filteredLoadCellWeights[] Index: firmware/App/Controllers/LoadCell.h =================================================================== diff -u -r54f45c387430e440ab4607451fc84dea61f273f1 -r4af713cb53b909ba87893dc814d39af89fb8e8d2 --- firmware/App/Controllers/LoadCell.h (.../LoadCell.h) (revision 54f45c387430e440ab4607451fc84dea61f273f1) +++ firmware/App/Controllers/LoadCell.h (.../LoadCell.h) (revision 4af713cb53b909ba87893dc814d39af89fb8e8d2) @@ -45,6 +45,7 @@ void initLoadCell( void ); // Initialize the LoadCell module. void execLoadCell( void ); // Execute the LoadCell monitor. +void setLoadCellAutoCal( LOAD_CELL_ID_T loadCellID, F32 calOffset ); F32 getLoadCellFilteredWeight( LOAD_CELL_ID_T loadCellID ); BOOL testSetLoadCellOverride( U32 loadCellID, F32 value ); Index: firmware/App/Modes/ModeDrain.c =================================================================== diff -u -r4d7d40a27130dc813d653f044cbb856b1b7d8481 -r4af713cb53b909ba87893dc814d39af89fb8e8d2 --- firmware/App/Modes/ModeDrain.c (.../ModeDrain.c) (revision 4d7d40a27130dc813d653f044cbb856b1b7d8481) +++ firmware/App/Modes/ModeDrain.c (.../ModeDrain.c) (revision 4af713cb53b909ba87893dc814d39af89fb8e8d2) @@ -22,6 +22,7 @@ #include "OperationModes.h" #include "Pressures.h" #include "Reservoirs.h" +#include "TaskGeneral.h" #include "TemperatureSensors.h" #include "Valves.h" @@ -32,11 +33,15 @@ // ********** private definitions ********** -#define TARGET_DRAIN_PUMP_RPM 2800 ///< Target drain pump speed (in RPM). +#define TARGET_DRAIN_PUMP_RPM 2800 ///< Target drain pump speed (in RPM). +#define DRAIN_WEIGHT_CHANGE_TOLERANCE 0.1 ///< Weight changes during draining tolerance. +#define DRAIN_WEIGH_UNCHANGE_TIMEOUT ( 10 * MS_PER_SECOND / TASK_GENERAL_INTERVAL ) ///< Time period of unchanged weight during draining before timeout. // ********** private data ********** -static DG_DRAIN_STATE_T drainState = DG_DRAIN_STATE_START; ///< Currently active drain state. +static DG_DRAIN_STATE_T drainState = DG_DRAIN_STATE_START; ///< Currently active drain state. +static F32 previousWeight; ///< Previous weight of the reservoir draining. +static U32 weightNotDroppingTimerCounter; ///< Timer counter for the period weight not dropping. // ********** private function prototypes ********** @@ -52,6 +57,8 @@ void initDrainMode( void ) { drainState = DG_DRAIN_STATE_START; + previousWeight = 0.0; + weightNotDroppingTimerCounter = 0; } /*********************************************************************//** @@ -117,15 +124,31 @@ { DG_DRAIN_STATE_T result = DG_DRAIN_STATE_DRAIN; LOAD_CELL_ID_T drainWeightLoadCell = LOAD_CELL_A1; + F32 currentFilteredWeight; + U32 const targetWeight = getReservoirDrainVolumeTargetMl(); // determine which load cell to use for drain volume - we want weight of inactive reservoir if ( RESERVOIR_1 == getActiveReservoir() ) { drainWeightLoadCell = LOAD_CELL_B1; } + currentFilteredWeight = getLoadCellFilteredWeight( drainWeightLoadCell ); + + // if weight stays the same for 10 seconds, timeout and auto calibration + if ( fabs( previousWeight - currentFilteredWeight ) < DRAIN_WEIGHT_CHANGE_TOLERANCE ) + { + previousWeight = currentFilteredWeight; + + if ( ++weightNotDroppingTimerCounter >= DRAIN_WEIGH_UNCHANGE_TIMEOUT ) + { + weightNotDroppingTimerCounter = 0; + setLoadCellAutoCal( drainWeightLoadCell, currentFilteredWeight - targetWeight ); + } + } + // if we've reached our target drain to volume (by weight), we're done draining - go back to re-circ mode - if ( getReservoirDrainVolumeTargetMl() >= getLoadCellFilteredWeight( drainWeightLoadCell ) ) + if ( targetWeight >= currentFilteredWeight ) { setDrainPumpTargetSpeed( 0 ); requestNewOperationMode( DG_MODE_CIRC );