Index: firmware/App/Controllers/Valves.c =================================================================== diff -u -r2b8e15565dbb160eeaca94e3126fc3e8a079a982 -r7ce686fa967206c341805b79916298ae187854de --- firmware/App/Controllers/Valves.c (.../Valves.c) (revision 2b8e15565dbb160eeaca94e3126fc3e8a079a982) +++ firmware/App/Controllers/Valves.c (.../Valves.c) (revision 7ce686fa967206c341805b79916298ae187854de) @@ -50,23 +50,21 @@ #define ADC_TO_CURRENT_CONVERSION_CONSTANT 2048.0 #define INITIAL_EDGE_OFFSET_READ_COUNT 100 -#define INITIAL_DEENERGIZED_POSITION ( 0 - INITIAL_EDGE_OFFSET_READ_COUNT ) //TODO remove -#define INITIAL_ENERGIZED_POSITION ( 12000 + INITIAL_EDGE_OFFSET_READ_COUNT ) //TODO Remove +#define INITIAL_DEENERGIZED_POSITION ( 0 - INITIAL_EDGE_OFFSET_READ_COUNT ) +#define INITIAL_ENERGIZED_POSITION ( 12000 + INITIAL_EDGE_OFFSET_READ_COUNT ) #define STEP_CHANGE_IN_COUNTS 1000 #define MAX_POS_DEVIATION_FROM_TARGET_COUNTS 1000 //TODO test this tolerance #define VALVE_TRANSITION_TIMEOUT_MS 5000 //TODO test the timeout -#define MAX_POSITION_DEVIATION_FROM_EDGES_IN_COUNTS 150U +#define MAX_POSITION_DEVIATION_FROM_TARGET_IN_COUNTS 150U #define MAX_ALLOWED_FAILED_HOMINGS 3U -#define HOMING_EDGE_DETECTION_TARGET_COUNTER 2U +#define HOMING_EDGE_DETECTION_TARGET_COUNTER 12U #define VALVES_CURRENT_THRESHOLD_AMPS 1U #define OVER_CURRENT_COUNTER 100U #define OUT_OF_RANGE_COUNTER 100U #define NEXT_STEP_COMD_TIME_INTERVAL_COUNTER 10U -#define DEENERGIZED_EDGE_CHECK_COUNTER 100U - // TODO changed the interval count to every 10 calls in priority task which is 100 ms #define VALVES_DATA_PUB_INTERVAL ( MS_PER_SECOND / ( 10 * TASK_PRIORITY_INTERVAL ) ) @@ -103,20 +101,18 @@ S16 targetPositionInCounts; BOOL hasTransitionBeenRequested; VALVE_STATE_T execState; - U32 transitionStartTime; + U32 transitionStartTime; //TODO REMOVE? S16 positions[ NUM_OF_VALVE_POSITIONS ]; F32 valveCurrent; U32 valveOverCurrentCounter; U32 valvePositionOutOfRangeCounter; U32 valveDataPublishCounter; - U32 valveRestCounter; + U32 valveRestCounter; //TODO REMOVE? // Homing variables - S16 previousPositionInCounts; U32 numberOfFailedHomings; U32 homingEdgeDetectionCounter; BOOL hasHomingBeenRequested; BOOL hasValveBeenHomed; - U32 deenergizedEdgeCheckStartCounter; } VALVE_STATUS_T; #pragma pack(pop) @@ -178,15 +174,11 @@ * @details * Inputs: valvesStatus * Outputs: valvesStatus (The flag that sets the homing request) - * @return: none (TODO DO WE HAVE TO RETURN ANYTHING?) + * @return: none *************************************************************************/ -BOOL homeValve( VALVE_T valve ) +void homeValve( VALVE_T valve ) { - BOOL result = FALSE; - valvesStatus[ valve ].hasHomingBeenRequested = TRUE; - - return result; } /*********************************************************************//** @@ -225,7 +217,7 @@ * @param: position : Enumeration of the commanded position * @return: returns TRUE if the requested position was legal *************************************************************************/ -VALVE_POSITION_T getValvePosition( VALVE_T valve) +VALVE_POSITION_T getValvePosition( VALVE_T valve ) { return valvesStatus[ valve ].currentPosition; } @@ -542,7 +534,9 @@ static void handleValveStateHomingNotStarted( VALVE_T valve ) { VALVE_STATE_T state = VALVE_STATE_HOMING_NOT_STARTED; + // TODO connect this flag to the doors driver later + // For now it is assumed the door is closed BOOL isDoorClosed = TRUE; // If homing has been requested or POST is completed and the door has been close for the specified @@ -551,15 +545,11 @@ && isDoorClosed ) { // Get ready for the energized state - valvesStatus[ valve ].transitionStartTime = getMSTimerCount(); - valvesStatus[ valve ].homingEdgeDetectionCounter = 0; - valvesStatus[ valve ].numberOfFailedHomings = 0; - valvesStatus[ valve ].valveRestCounter = 0; - valvesStatus[ valve ].targetPositionInCounts = 0; - valvesStatus[ valve ].previousPositionInCounts = 0; - valvesStatus[ valve ].hasValveBeenHomed = FALSE; - valvesStatus[ valve ].deenergizedEdgeCheckStartCounter = 0; - valvesStatus[ valve ].currentPosition = VALVE_POSITION_NOT_IN_POSITION; + valvesStatus[ valve ].homingEdgeDetectionCounter = 0; + valvesStatus[ valve ].numberOfFailedHomings = 0; + valvesStatus[ valve ].targetPositionInCounts = STEP_CHANGE_IN_COUNTS; + valvesStatus[ valve ].hasValveBeenHomed = FALSE; + valvesStatus[ valve ].currentPosition = VALVE_POSITION_NOT_IN_POSITION; setFPGAValue( valve, valvesStatus[ valve ].targetPositionInCounts ); state = VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE; } @@ -580,44 +570,30 @@ { VALVE_STATE_T state = VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE; - S16 prevPosition = valvesStatus[ valve ].previousPositionInCounts; S16 currentPosition = valvesStatus[ valve ].currentPositionInCounts; S16 targetPosition = valvesStatus[ valve ].targetPositionInCounts; - if ( fabs( fabs(prevPosition) - fabs(currentPosition) ) <= MAX_POSITION_DEVIATION_FROM_EDGES_IN_COUNTS && + if ( fabs( fabs(targetPosition) - fabs(currentPosition) ) > MAX_POSITION_DEVIATION_FROM_TARGET_IN_COUNTS && ++valvesStatus[ valve ].homingEdgeDetectionCounter >= HOMING_EDGE_DETECTION_TARGET_COUNTER ) { // Current position (positive or negative) will be stored in the Position B of the current valve - // Do not use the above variables because they absolute valves using fabs valvesStatus[ valve ].positions[ VALVE_POSITION_B_OPEN ] = currentPosition; - valvesStatus[ valve ].transitionStartTime = getMSTimerCount(); valvesStatus[ valve ].homingEdgeDetectionCounter = 0; + valvesStatus[ valve ].targetPositionInCounts = currentPosition - STEP_CHANGE_IN_COUNTS; + setFPGAValue( valve, valvesStatus[ valve ].targetPositionInCounts ); + state = VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE; } - - else if ( fabs( fabs(currentPosition) - fabs(targetPosition) ) <= 100 ) //TODO Add a #define for the edge tolerance + else if ( fabs( fabs(currentPosition) - fabs(targetPosition) ) <= MAX_POSITION_DEVIATION_FROM_TARGET_IN_COUNTS ) { - U32 restStatus = valvesStatus[ valve ].valveRestCounter; - - if ( ++restStatus >= 0 ) //TODO add a #define for the rest time. Do we need the rest? + if ( ++valvesStatus[ valve ].valveRestCounter >= 0 ) //TODO add a #define for the rest time. Do we need the rest? { + valvesStatus[ valve ].homingEdgeDetectionCounter = 0; valvesStatus[ valve ].targetPositionInCounts = currentPosition + STEP_CHANGE_IN_COUNTS; setFPGAValue( valve, valvesStatus[ valve ].targetPositionInCounts ); } } - // Check if the transition has timed out - /*else if ( didTimeout( valvesStatus[ valve ].transitionStartTime, VALVE_TRANSITION_TIMEOUT_MS ) ) - { - // If the valve has exceeded the max number of failed homings, raise an alarm - if ( ++valvesStatus[ valve ].numberOfFailedHomings >= MAX_ALLOWED_FAILED_HOMINGS ) - { - //TODO fault alarm - } - state = VALVE_STATE_HOMING_NOT_STARTED; - }*/ - - valvesStatus[ valve ].previousPositionInCounts = currentPosition; // TODO check is this too fast? valvesStatus[ valve ].execState = state; } @@ -634,18 +610,16 @@ { VALVE_STATE_T state = VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE; - S16 prevPosition = valvesStatus[ valve ].previousPositionInCounts; S16 currentPosition = valvesStatus[ valve ].currentPositionInCounts; S16 targetPosition = valvesStatus[ valve ].targetPositionInCounts; - if ( fabs( fabs(prevPosition) - fabs(currentPosition) ) <= MAX_POSITION_DEVIATION_FROM_EDGES_IN_COUNTS && - ++valvesStatus[ valve ].homingEdgeDetectionCounter >= HOMING_EDGE_DETECTION_TARGET_COUNTER && - ++valvesStatus[ valve ].deenergizedEdgeCheckStartCounter >= DEENERGIZED_EDGE_CHECK_COUNTER ) + if ( fabs( fabs(targetPosition) - fabs(currentPosition) ) > MAX_POSITION_DEVIATION_FROM_TARGET_IN_COUNTS && + ++valvesStatus[ valve ].homingEdgeDetectionCounter >= HOMING_EDGE_DETECTION_TARGET_COUNTER ) { S16 positionB = valvesStatus[ valve ].positions[ VALVE_POSITION_B_OPEN ]; // Positions B and C will have an offset from the edge to make sure each time the valve will not hit the edge valvesStatus[ valve ].positions[ VALVE_POSITION_B_OPEN ] = positionB - INITIAL_EDGE_OFFSET_READ_COUNT; - valvesStatus[ valve ].positions[ VALVE_POSITION_C_CLOSE ] = currentPosition - INITIAL_EDGE_OFFSET_READ_COUNT; + valvesStatus[ valve ].positions[ VALVE_POSITION_C_CLOSE ] = currentPosition + INITIAL_EDGE_OFFSET_READ_COUNT; // Position A is the average of the Position B that was read last time and position C that was // the target of this state valvesStatus[ valve ].positions[ VALVE_POSITION_A_INSERT_EJECT ] = ( valvesStatus[ valve ].positions[ VALVE_POSITION_B_OPEN ] + @@ -662,31 +636,16 @@ // Idle will initiate a transition state = VALVE_STATE_IDLE; } - - else if ( fabs( fabs(currentPosition) - fabs(targetPosition) ) <= 100 ) //TODO Add a #define for the edge tolerance + else if ( fabs( fabs(currentPosition) - fabs(targetPosition) ) <= MAX_POSITION_DEVIATION_FROM_TARGET_IN_COUNTS ) { - U32 restStatus = valvesStatus[ valve ].valveRestCounter; - - if ( ++restStatus >= 0 ) //TODO add a #define for the rest time. Do we need the rest? + if ( ++valvesStatus[ valve ].valveRestCounter >= 0 ) //TODO add a #define for the rest time. Do we need the rest? { + valvesStatus[ valve ].homingEdgeDetectionCounter = 0; valvesStatus[ valve ].targetPositionInCounts = currentPosition - STEP_CHANGE_IN_COUNTS; setFPGAValue( valve, valvesStatus[ valve ].targetPositionInCounts ); } } - // Check if the transition has timed out - /*else if ( didTimeout( valvesStatus[ valve ].transitionStartTime, VALVE_TRANSITION_TIMEOUT_MS ) ) - { - // If the valve has exceeded the max number of failed homings, raise an alarm - if ( ++valvesStatus[ valve ].numberOfFailedHomings >= MAX_ALLOWED_FAILED_HOMINGS ) - { - //TODO fault alarm - } - - state = VALVE_STATE_HOMING_NOT_STARTED; - }*/ - - valvesStatus[ valve ].previousPositionInCounts = currentPosition; // TODO check is this too fast? valvesStatus[ valve ].execState = state; } @@ -730,11 +689,11 @@ U16 status = getFPGAValvesStatus(); // VDi status - if ( ! status & VDI_INIT_STATUS_BIT_MASK ) + if ( ! ( status & VDI_INIT_STATUS_BIT_MASK ) ) { result = FALSE; } - if ( ! status & VDI_ENABLE_PID_STATUS_BIT_MASK ) + if ( ! ( status & VDI_ENABLE_PID_STATUS_BIT_MASK ) ) { result = FALSE; } @@ -744,11 +703,11 @@ } // VDo status - if ( ! status & VDO_INIT_STATUS_BIT_MASK ) + if ( ! ( status & VDO_INIT_STATUS_BIT_MASK ) ) { result = FALSE; } - if ( ! status & VDO_ENABLE_PID_STATUS_BIT_MASK ) + if ( ! ( status & VDO_ENABLE_PID_STATUS_BIT_MASK ) ) { result = FALSE; } @@ -758,11 +717,11 @@ } // VBA status - if ( ! status & VBA_INIT_STATUS_BIT_MASK ) + if ( ! ( status & VBA_INIT_STATUS_BIT_MASK ) ) { result = FALSE; } - if ( ! status & VBA_ENABLE_PID_STATUS_BIT_MASK ) + if ( ! ( status & VBA_ENABLE_PID_STATUS_BIT_MASK ) ) { result = FALSE; } @@ -772,11 +731,11 @@ } // VBV status - if ( ! status & VBV_INIT_STATUS_BIT_MASK ) + if ( ! ( status & VBV_INIT_STATUS_BIT_MASK ) ) { result = FALSE; } - if ( ! status & VBV_ENABLE_PID_STATUS_BIT_MASK ) + if ( ! ( status & VBV_ENABLE_PID_STATUS_BIT_MASK ) ) { result = FALSE; } @@ -859,14 +818,14 @@ for ( i = 0; i < NUM_OF_VALVES; i++) { // Check if the current is over the threshold for the defined amount of time - if ( valvesStatus[ i ].valveCurrent > VALVES_CURRENT_THRESHOLD_AMPS && + if ( fabs(valvesStatus[ i ].valveCurrent) > VALVES_CURRENT_THRESHOLD_AMPS && ++valvesStatus[ i ].valveOverCurrentCounter > OVER_CURRENT_COUNTER ) { //TODO fault } // If the current is below the threshold again and the counter for the time is greater than // zero, set the counter to zero - else if ( valvesStatus[ i ].valveCurrent < VALVES_CURRENT_THRESHOLD_AMPS && + else if ( fabs(valvesStatus[ i ].valveCurrent) < VALVES_CURRENT_THRESHOLD_AMPS && valvesStatus[ i ].valveOverCurrentCounter > 0 ) { valvesStatus[ i ].valveOverCurrentCounter = 0; @@ -962,13 +921,15 @@ // Check the counter if ( ++valvesStatus[ valve ].valveDataPublishCounter > getPublishValvesDataInterval() ) { - valveData.valveID = (U32)valve; - valveData.state = (U32)valvesStatus[ valve ].execState; + valveData.valveID = (U32)valve; + valveData.state = (U32)valvesStatus[ valve ].execState; valveData.currentPosID = (U32)valvesStatus[ valve ].currentPosition; - valveData.currentPos = valvesStatus[ valve ].currentPositionInCounts; - valveData.prevPos = valvesStatus[ valve ].previousPositionInCounts; - valveData.nextPos = valvesStatus[ valve ].targetPositionInCounts; - valveData.current = valvesStatus[ valve ].valveCurrent; + valveData.currentPos = valvesStatus[ valve ].currentPositionInCounts; + valveData.nextPos = valvesStatus[ valve ].targetPositionInCounts; + valveData.current = valvesStatus[ valve ].valveCurrent; + valveData.posC = valvesStatus[ valve ].positions[ VALVE_POSITION_C_CLOSE ]; + valveData.posA = valvesStatus[ valve ].positions[ VALVE_POSITION_A_INSERT_EJECT ]; + valveData.posB = valvesStatus[ valve ].positions[ VALVE_POSITION_B_OPEN ]; broadcastHDValves(); @@ -983,8 +944,6 @@ // TODO REMOVE } -/**@}*/ - /************************************************************************* * TEST SUPPORT FUNCTIONS *************************************************************************/ @@ -1094,6 +1053,4 @@ return result; } - - - +/**@}*/