Index: firmware/App/Controllers/Valves.c =================================================================== diff -u -r9d11c51da60da3f8d2917433938a583f8a105318 -r5eda216f634a74512a5bc11b0984676567512d80 --- firmware/App/Controllers/Valves.c (.../Valves.c) (revision 9d11c51da60da3f8d2917433938a583f8a105318) +++ firmware/App/Controllers/Valves.c (.../Valves.c) (revision 5eda216f634a74512a5bc11b0984676567512d80) @@ -43,7 +43,9 @@ #define VALVE_FORCE_HOME TRUE ///< Force valve to home even if already homed. #define ZERO_ENC_DEBOUNCE_THRESHOLD_CNT 5 ///< Valves zero encoder debounce threshold count. #define ZERO_ENC_DEBOUNCE_TIMEOUT_MS ( 0.25 * MS_PER_SECOND ) ///< Valves zero encoder debounce timeout in milliseconds. -#define POS_D_PARTIAL_CLOSE_FROM_ZERO_CNT 32 ///< Position D partial close from zero position in counts. +#define VALVE_OFFEST_FROM_EDG_CNT 32 ///< Valves offset from the edge. +#define POS_C_FROM_ZERO_CNT VALVE_OFFEST_FROM_EDG_CNT ///< Position C from zero position in counts. +#define POS_D_PARTIAL_CLOSE_FROM_ZERO_CNT ( POS_C_FROM_ZERO_CNT + 32 ) ///< Position D partial close from zero position in counts. /// Valve controller states typedef enum Valve_Control_States @@ -59,16 +61,6 @@ NUM_OF_VALVE_STATES, ///< Number of valve exec states. } VALVE_STATE_T; -/// Valve homing status -typedef enum Valve_Homing_Status -{ - VALVE_HOMING_INIT = 0, ///< Valve homing init. - VALVE_HOMING_FORWARD, ///< Valve homing forward. - VALVE_HOMING_REVERSE, ///< Valve homing reverse. - VALVE_HOMING_COMPLETE, ///< Valve homing complete. - NUM_OF_VALVE_HOMING_STATE, ///< Number of valve homing. -} VALVE_HOMING_STATUS_T; - /// Valve status structure typedef struct { @@ -89,7 +81,6 @@ BOOL hasHomingFailed; ///< Valve homing failed flag S16 proposedEnergizedPos; ///< Valve homing proposed energized position. BOOL hasValveBeenReset; ///< Valve homing has valve been reset. - VALVE_HOMING_STATUS_T valveHomingStatus; ///< Valve homing status. } VALVE_STATUS_T; /// Payload record structure for pinch valve set position request @@ -411,14 +402,16 @@ if ( valveEncPosistion >= ZERO_ENC_DEBOUNCE_THRESHOLD_CNT ) { - // Keep reseting the debouce timer until the encoder count is less than the encoder count is less than the number + // Keep reseting the debounce timer until the encoder count is less than the encoder count is less than the number currentValveStates[ valve ].valveOpsStartTime = getMSTimerCount(); } else if ( TRUE == didTimeout( currentValveStates[ valve ].valveOpsStartTime, ZERO_ENC_DEBOUNCE_TIMEOUT_MS ) ) { + // Getting ready for homing enable the homing bit upon transitioning to homing not started + setValveHomingEnableDisable( valve, TRUE ); // Position C is hard coded as count 8. // Position D is hard coded as an offset form 0. - currentValveStates[ valve ].positionsABC[ VALVE_POSITION_C_CLOSE ] = ROTARY_VALVE_MICROSTEP_FRACTION; + currentValveStates[ valve ].positionsABC[ VALVE_POSITION_C_CLOSE ] = POS_C_FROM_ZERO_CNT; currentValveStates[ valve ].positionsABC[ VALVE_POSITION_D_PARTIAL_CLOSE ] = POS_D_PARTIAL_CLOSE_FROM_ZERO_CNT; nextState = VALVE_STATE_HOMING_NOT_STARTED; } @@ -445,25 +438,7 @@ { currentValveStates[ valve ].proposedEnergizedPos = ROTARY_VALVE_INIT_FULL_SWING_COUNTS; - //switch ( currentValveStates[ valve ].valveHomingStatus ) - //{ - // case VALVE_HOMING_INIT: - // currentValveStates[ valve ].proposedEnergizedPos = 1880; //ROTARY_VALVE_INIT_FULL_SWING_COUNTS; - // break; - // - // case VALVE_HOMING_FORWARD: - // currentValveStates[ valve ].proposedEnergizedPos += ROTARY_VALVE_MICROSTEP_FRACTION; - // break; - // - // case VALVE_HOMING_REVERSE: - // currentValveStates[ valve ].proposedEnergizedPos -= ROTARY_VALVE_MICROSTEP_FRACTION; - // break; - // - // default: - // // Do nothing as complete has nothing to do - // break; - //} - + // TODO remove this? What is the criteria for the homing failure? if ( ( currentValveStates[ valve ].proposedEnergizedPos <= ROTARY_VALVE_ENERGIZED_EDGE_MIN_COUNTS ) || ( currentValveStates[ valve ].proposedEnergizedPos >= ROTARY_VALVE_ENERGIZED_EDGE_MAX_COUNTS ) ) { @@ -497,69 +472,51 @@ static VALVE_STATE_T handleValvesFindEnergizedEdgeState( VALVE_T valve ) { VALVE_STATE_T nextState = VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE; - S16 curPos = currentValveStates[ valve ].currentEncPosition; + S16 currPos = currentValveStates[ valve ].currentEncPosition; + BOOL hasEdgeBeenReached = FALSE; checkValveInRange( valve, SW_FAULT_ID_TD_VALVES_INVALID_FIND_ENERGIZED_EDGE ); // have we found forward edge of travel? - if ( abs( curPos - currentValveStates[ valve ].priorEncPosition ) < VALVE_HOME_MIN_POS_CHG ) + if ( abs( currPos - currentValveStates[ valve ].priorEncPosition ) < VALVE_HOME_MIN_POS_CHG ) { if ( 0 == currentValveStates[ valve ].valveOpsStartTime ) { currentValveStates[ valve ].valveOpsStartTime = getMSTimerCount(); } else if ( TRUE == didTimeout( currentValveStates[ valve ].valveOpsStartTime, HOMING_EDGE_DETECTION_TIMEOUT_MS ) ) { - //if ( VALVE_HOMING_INIT == currentValveStates[ valve ].valveHomingStatus ) - //{ - // S16 tgtPos = currentValveStates[ valve ].positionsABC[ VALVE_POSITION_C_CLOSE ]; - // S16 mag = curPos - tgtPos; // calculate magnitude of position change - // - // // Set commanded position and transition start time stamp - // // Transition back to position C to try the next energized position. It has to start from position C - // currentValveStates[ valve ].commandedPosition = VALVE_POSITION_C_CLOSE; - // currentValveStates[ valve ].valveOpsStartTime = getMSTimerCount(); - // currentValveStates[ valve ].valveHomingStatus = VALVE_HOMING_FORWARD; - // nextState = VALVE_STATE_IN_TRANSITION; - // // Command FPGA to move valve to commanded position - // setValveCmdChangePosition( valve, (U16)(mag), MOTOR_DIR_REVERSE ); - //} - //else - { - S16 posC = currentValveStates[ valve ].positionsABC[ VALVE_POSITION_C_CLOSE ]; - S16 posA = posC + ( ( curPos - posC ) / 2 ); - - // If position A was calculated to not be a division of 8 it is rounded down to be a division of 8 - posA = ( posA % ROTARY_VALVE_MICROSTEP_FRACTION ) < 4 ? ( posA >> 3 ) << 3 : posA; - - currentValveStates[ valve ].positionsABC[ VALVE_POSITION_B_OPEN ] = curPos; - currentValveStates[ valve ].positionsABC[ VALVE_POSITION_A_INSERT_EJECT ] = posA; - currentValveStates[ valve ].hasValveBeenHomed = TRUE; - currentValveStates[ valve ].currentPosition = VALVE_POSITION_B_OPEN; - currentValveStates[ valve ].pendingCommandedPosition = VALVE_POSITION_A_INSERT_EJECT; - currentValveStates[ valve ].valveHomingStatus = VALVE_HOMING_COMPLETE; - currentValveStates[ valve ].hasTransitionBeenRequested = TRUE; - currentValveStates[ valve ].hasHomingBeenRequested = FALSE; - nextState = VALVE_STATE_IDLE; - } + hasEdgeBeenReached = TRUE; } } - else if ( curPos <= 0 ) + else if ( currPos <= 0 ) { - switch ( currentValveStates[ valve ].valveHomingStatus ) - { - case VALVE_HOMING_INIT: - case VALVE_HOMING_FORWARD: - currentValveStates[ valve ].valveHomingStatus = VALVE_HOMING_REVERSE; - break; + // Lost the shaft. Get the maximum value in the special register as the maximum. + S16 maxEncPosition = getValveMaximumEncoderPosition( valve ); - default: - // If init or forward modes, then reverse again. - break; - } + hasEdgeBeenReached = TRUE; + currPos = maxEncPosition - VALVE_OFFEST_FROM_EDG_CNT; + } - currentValveStates[ valve ].hasValveBeenReset = FALSE; - nextState = VALVE_STATE_RESET_VALVE; + if ( TRUE == hasEdgeBeenReached ) + { + // Done with homing disable the homing bit + setValveHomingEnableDisable( valve, FALSE ); + + S16 posC = currentValveStates[ valve ].positionsABC[ VALVE_POSITION_C_CLOSE ]; + S16 posA = posC + ( ( currPos - posC ) / 2 ); + + // If position A was calculated to not be a division of 8 it is rounded down to be a division of 8 + posA = ( posA % ROTARY_VALVE_MICROSTEP_FRACTION ) < 4 ? ( posA >> 3 ) << 3 : posA; + + currentValveStates[ valve ].positionsABC[ VALVE_POSITION_B_OPEN ] = currPos; + currentValveStates[ valve ].positionsABC[ VALVE_POSITION_A_INSERT_EJECT ] = posA; + currentValveStates[ valve ].hasValveBeenHomed = TRUE; + currentValveStates[ valve ].currentPosition = VALVE_POSITION_B_OPEN; + currentValveStates[ valve ].pendingCommandedPosition = VALVE_POSITION_A_INSERT_EJECT; + currentValveStates[ valve ].hasTransitionBeenRequested = TRUE; + currentValveStates[ valve ].hasHomingBeenRequested = FALSE; + nextState = VALVE_STATE_IDLE; } return nextState; @@ -583,26 +540,17 @@ // handle a home request if ( TRUE == currentValveStates[ valve ].hasHomingBeenRequested ) { - if ( VALVE_HOMING_COMPLETE == currentValveStates[ valve ].valveHomingStatus ) - { - S16 tgtPos = currentValveStates[ valve ].positionsABC[ VALVE_POSITION_C_CLOSE ]; - S16 mag = currentValveStates[ valve ].currentEncPosition - tgtPos; // calculate magnitude of position change + S16 tgtPos = currentValveStates[ valve ].positionsABC[ VALVE_POSITION_C_CLOSE ]; + S16 mag = currentValveStates[ valve ].currentEncPosition - tgtPos; // calculate magnitude of position change - // Set commanded position and transition start time stamp - // Transition back to position C to try the next energized position. It has to start from position C - currentValveStates[ valve ].commandedPosition = VALVE_POSITION_C_CLOSE; - currentValveStates[ valve ].valveOpsStartTime = getMSTimerCount(); - currentValveStates[ valve ].valveHomingStatus = VALVE_HOMING_INIT; - currentValveStates[ valve ].hasValveBeenReset = FALSE; - nextState = VALVE_STATE_IN_TRANSITION; - // Command FPGA to move valve to commanded position - setValveCmdChangePosition( valve, (U16)(mag), MOTOR_DIR_REVERSE ); - } - else if ( VALVE_HOMING_INIT == currentValveStates[ valve ].valveHomingStatus ) - { - currentValveStates[ valve ].hasValveBeenReset = FALSE; - nextState = VALVE_STATE_HOMING_NOT_STARTED; - } + // Set commanded position and transition start time stamp + // Transition back to position C to try the next energized position. It has to start from position C + currentValveStates[ valve ].commandedPosition = VALVE_POSITION_C_CLOSE; + currentValveStates[ valve ].valveOpsStartTime = getMSTimerCount(); + currentValveStates[ valve ].hasValveBeenReset = FALSE; + nextState = VALVE_STATE_IN_TRANSITION; + // Command FPGA to move valve to commanded position + setValveCmdChangePosition( valve, (U16)(mag), MOTOR_DIR_REVERSE ); } // handle a position change request else if ( TRUE == currentValveStates[ valve ].hasTransitionBeenRequested ) @@ -650,7 +598,6 @@ if ( abs( delta ) < VALVE_TRANSITION_MIN_TGT_DELTA ) { currentValveStates[ valve ].currentPosition = currentValveStates[ valve ].commandedPosition; - nextState = ( VALVE_HOMING_COMPLETE != currentValveStates[ valve ].valveHomingStatus ? VALVE_STATE_HOMING_NOT_STARTED : VALVE_STATE_IDLE ); } // is transition taking too long? else if ( ( TRUE == didTimeout( currentValveStates[ valve ].valveOpsStartTime, VALVE_TRANSITION_TIMEOUT_MS ) ) && Index: firmware/App/Drivers/RotaryValve.c =================================================================== diff -u -r63f70637ccd17ac53cb3664332a670b01f28c744 -r5eda216f634a74512a5bc11b0984676567512d80 --- firmware/App/Drivers/RotaryValve.c (.../RotaryValve.c) (revision 63f70637ccd17ac53cb3664332a670b01f28c744) +++ firmware/App/Drivers/RotaryValve.c (.../RotaryValve.c) (revision 5eda216f634a74512a5bc11b0984676567512d80) @@ -331,7 +331,87 @@ } } +/*********************************************************************//** + * @brief + * The setValveHomingEnableDisable function enables or disables the homing + * bit in the valve's control set. + * @details \b Alarm: ALARM_ID_TD_SOFTWARE_FAULT if invalid valve given. + * @details \b Inputs: none + * @details \b Outputs: none + * @param valve ID of valve to get status for + * @param enableHoming TRUE if enable homing needed otherwise, FALSE + * @return none + *************************************************************************/ +void setValveHomingEnableDisable( VALVE_T valve, BOOL enableHoming ) +{ + U08 valveControlSet; + switch ( valve ) + { + case H1_VALV: + valveControlSet = getH1Control(); + if ( TRUE == enableHoming ) + { + valveControlSet |= FPGA_PINCH_VALVES_ENABLE_HOMING; + } + else + { + valveControlSet &= ~FPGA_PINCH_VALVES_ENABLE_HOMING; + } + setH1Control( valveControlSet ); + break; + + case H19_VALV: + valveControlSet = getH19Control(); + if ( TRUE == enableHoming ) + { + valveControlSet |= FPGA_PINCH_VALVES_ENABLE_HOMING; + } + else + { + valveControlSet &= ~FPGA_PINCH_VALVES_ENABLE_HOMING; + } + setH19Control( valveControlSet ); + break; + + default: + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_VALVES_INVALID_VALVE_HOMING_SET, (U32)valve ) + break; + } +} + +/*********************************************************************//** + * @brief + * The getValveMaximumEncoderPosition function gets the maximum encoder + * position of the valve. + * @details \b Alarm: ALARM_ID_TD_SOFTWARE_FAULT if invalid valve given. + * @details \b Inputs: none + * @details \b Outputs: none + * @param valve ID of valve to get status for + * @return gets the valves maximum encoder position + *************************************************************************/ +S16 getValveMaximumEncoderPosition( VALVE_T valve ) +{ + S16 maxEncoderValue; + + switch ( valve ) + { + case H1_VALV: + maxEncoderValue = getH1MaxEncPosition(); + break; + + case H19_VALV: + maxEncoderValue = getH19MaxEncPosition(); + break; + + default: + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_VALVES_INVALID_VALVE_MAX_ENC_POS_REQ, (U32)valve ) + break; + } + + return maxEncoderValue; +} + /************************************************************************* * TEST SUPPORT FUNCTIONS *************************************************************************/ Index: firmware/App/Drivers/RotaryValve.h =================================================================== diff -u -r9d11c51da60da3f8d2917433938a583f8a105318 -r5eda216f634a74512a5bc11b0984676567512d80 --- firmware/App/Drivers/RotaryValve.h (.../RotaryValve.h) (revision 9d11c51da60da3f8d2917433938a583f8a105318) +++ firmware/App/Drivers/RotaryValve.h (.../RotaryValve.h) (revision 5eda216f634a74512a5bc11b0984676567512d80) @@ -59,6 +59,8 @@ void resetValveEncoder( VALVE_T valve ); void enableValve( VALVE_T valve ); void setValveZeroEncoderPosition( VALVE_T valve ); +void setValveHomingEnableDisable( VALVE_T valve, BOOL enableHoming ); +S16 getValveMaximumEncoderPosition( VALVE_T valve ); BOOL testValveEncoderPositionOverride( MESSAGE_T *message ); BOOL testValveStatusOverride( MESSAGE_T *message ); Index: firmware/App/Services/AlarmMgmtSWFaults.h =================================================================== diff -u -rcf22584fb68bb29311d81d1e5ad79f8cf064b8fb -r5eda216f634a74512a5bc11b0984676567512d80 --- firmware/App/Services/AlarmMgmtSWFaults.h (.../AlarmMgmtSWFaults.h) (revision cf22584fb68bb29311d81d1e5ad79f8cf064b8fb) +++ firmware/App/Services/AlarmMgmtSWFaults.h (.../AlarmMgmtSWFaults.h) (revision 5eda216f634a74512a5bc11b0984676567512d80) @@ -177,6 +177,8 @@ SW_FAULT_ID_TD_VALVES_INVALID_IDLE = 146, SW_FAULT_ID_TD_VALVES_INVALID_IN_TRANSITION = 147, SW_FAULT_ID_TD_EJECTOR_OPTICAL_SENSOR_INVALID_REQ = 148, + SW_FAULT_ID_VALVES_INVALID_VALVE_HOMING_SET = 149, + SW_FAULT_ID_VALVES_INVALID_VALVE_MAX_ENC_POS_REQ = 150, NUM_OF_SW_FAULT_IDS } SW_FAULT_ID_T; Index: firmware/App/Services/FpgaTD.c =================================================================== diff -u -rcf22584fb68bb29311d81d1e5ad79f8cf064b8fb -r5eda216f634a74512a5bc11b0984676567512d80 --- firmware/App/Services/FpgaTD.c (.../FpgaTD.c) (revision cf22584fb68bb29311d81d1e5ad79f8cf064b8fb) +++ firmware/App/Services/FpgaTD.c (.../FpgaTD.c) (revision 5eda216f634a74512a5bc11b0984676567512d80) @@ -114,7 +114,7 @@ S16 h2Temperature; ///< Reg 274. H2 raw temperature data. U08 h2ReadCount; ///< Reg 276. H2 read count. U08 h2ErrorCount; ///< Reg 277. H2 error count. - U16 reserved1; ///< Reg 278. Reserved and available for future use. + S16 h1MaxEncPosition; ///< Reg 278. H1 max encoder position. S16 h1EncPosition; ///< Reg 280. H1 encoder position (1000 steps/rev). U16 h1Status; ///< Reg 282. H1 status. U32 reserved2; ///< Reg 284. Reserved and available for future use. @@ -124,8 +124,7 @@ S16 h4SpeedFromHall; ///< Reg 294. H4 measured speed from hall sensor(s). U16 h19Status; ///< Reg 296. H19 status. S16 h19EncPosition; ///< Reg 298. H19 encoder position (1000 steps/rev). - U08 reserved4; ///< Reg 300. H19 status. - U08 h4Status; ///< Reg 301. H4 status. + S16 h19MaxEncPosition; ///< Reg 300. H19 max encoder position. U16 GPIOReg; ///< Reg 302. GPIO register. U08 HEPStatus; ///< Reg 304. HEP status register. U08 HEPAdcReadCount; ///< Reg 305. HEP ADC read counter. @@ -151,7 +150,7 @@ S16 ACPower3Voltage; ///< Reg 342. AC power voltage - 3. S16 h4RotorHallCount; ///< Reg 344. H4 rotor count from hall sensor. U08 h6RotorStatus; ///< Reg 346. H6 rotor status. - U08 reserved5; ///< + U08 reserved5; ///< Reg 347. Reserved register. U16 h12Speed; ///< Reg 348. H12 speed (air pump in RPM). U16 rotorRevsCounter; ///< Reg 350. Rotor revs counter U16 baroManufactInfo; ///< Reg 352. Baro sensor manufacturing information. @@ -1003,7 +1002,9 @@ * 0=reset (set to 1 after power up) * Sleep: bit 6 (active low) * 0=sleep mode - * Reserved: bit 7 + * Homing: bit 7 (set 1 to enable homing) + * 0=disable homing + * 1=enable homing * @details \b Inputs: none * @details \b Outputs: fpgaActuatorSetPoints.h19Control * @param controlBits The control bits to apply to the venous pinch valve. @@ -1069,6 +1070,19 @@ /*********************************************************************//** * @brief + * The getH19MaxEncPosition function reads max encoder position of the venous + * pinch valve prior to losing the shaft. + * @details \b Inputs: fpgaSensorReadings.h19MaxEncPosition + * @details \b Outputs: none + * @return Max encoder position of the venous pinch valve + *************************************************************************/ +S16 getH19MaxEncPosition( void ) +{ + return fpgaSensorReadings.h19MaxEncPosition; +} + +/*********************************************************************//** + * @brief * The setH1Control function sets the control bits for the arterial pinch valve. * Microstep setting: bits 0..2: * 0=full step (100% torque) @@ -1089,7 +1103,9 @@ * 0=reset (set to 1 after power up) * Sleep: bit 6 (active low) * 0=sleep mode - * Reserved: bit 7 + * Homing: bit 7 (set 1 to enable homing) + * 0=disable homing + * 1=enable homing * @details \b Inputs: none * @details \b Outputs: fpgaActuatorSetPoints.h1Control * @param controlBits The control bits to apply to the arterial pinch valve. @@ -1154,6 +1170,19 @@ /*********************************************************************//** * @brief + * The getH1MaxEncPosition function reads max encoder position of the arterial + * pinch valve prior to losing its shaft. + * @details \b Inputs: fpgaSensorReadings.h1MaxEncPosition + * @details \b Outputs: none + * @return Max encoder value of the arterial pinch valve + *************************************************************************/ +S16 getH1MaxEncPosition( void ) +{ + return fpgaSensorReadings.h1MaxEncPosition; +} + +/*********************************************************************//** + * @brief * The getFPGABoardTemperature function reads the FPGA board temperature. * @details \b Inputs: fpgaSensorReadings * @details \b Outputs: none Index: firmware/App/Services/FpgaTD.h =================================================================== diff -u -rcf22584fb68bb29311d81d1e5ad79f8cf064b8fb -r5eda216f634a74512a5bc11b0984676567512d80 --- firmware/App/Services/FpgaTD.h (.../FpgaTD.h) (revision cf22584fb68bb29311d81d1e5ad79f8cf064b8fb) +++ firmware/App/Services/FpgaTD.h (.../FpgaTD.h) (revision 5eda216f634a74512a5bc11b0984676567512d80) @@ -40,6 +40,7 @@ #define FPGA_PINCH_VALVES_RESET 0x00 ///< Bit mask for configuring pinch valve to be in reset. #define FPGA_PINCH_VALVES_NOT_RESET 0x20 ///< Bit mask for configuring pinch valve to not be in reset. #define FPGA_PINCH_VALVES_NOT_SLEEP 0x40 ///< Bit mask for configuring pinch valve to not be in sleep mode. +#define FPGA_PINCH_VALVES_ENABLE_HOMING 0x80 ///< Bit mask for configuring pinch valve to enable homing sequence. Bit 7 of control register. #define FPGA_PINCH_VALVES_NEW_POS_CMD 0x80 ///< Bit mask for configuring pinch valve to accept a new position command. // Bit definitions for syringe pump control register @@ -132,12 +133,14 @@ void setH19Position( S16 setPoint ); S16 getH19EncoderPosition( void ); U16 getH19Status( void ); +S16 getH19MaxEncPosition( void ); void setH1Control( U08 controlBits ); U08 getH1Control( void ); void setH1Position( S16 setPoint ); S16 getH1EncoderPosition( void ); U16 getH1Status( void ); +S16 getH1MaxEncPosition( void ); U16 getFPGABoardTemperature( void ); U32 getFPGAPBAADCTemperature( void );