Index: firmware/App/Drivers/RotaryValve.c =================================================================== diff -u -re69d7ce1c9d88695e25e8ea94529dffdd8592434 -re24a98a344ba13ceb0663b415268a7e1dd5ce99e --- firmware/App/Drivers/RotaryValve.c (.../RotaryValve.c) (revision e69d7ce1c9d88695e25e8ea94529dffdd8592434) +++ firmware/App/Drivers/RotaryValve.c (.../RotaryValve.c) (revision e24a98a344ba13ceb0663b415268a7e1dd5ce99e) @@ -1,17 +1,17 @@ /************************************************************************** * -* Copyright (c) 2024-2024 Diality Inc. - All Rights Reserved. +* Copyright (c) 2024-2025 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 RotaryValve.c * -* @author (last) Sean -* @date (last) 03-Oct-2024 +* @author (last) Dara Navaei +* @date (last) 10-Nov-2025 * -* @author (original) Sean -* @date (original) 03-Oct-2024 +* @author (original) Sean Nash +* @date (original) 24-Oct-2024 * ***************************************************************************/ @@ -27,14 +27,11 @@ // ********** private definitions ********** -#define VALVE_ENC_COUNT_2_MICRO_STEP_CONVERSION 6.25F ///< 6.25 microsteps per encoder count. +#define VALVE_MOTOR_DRIVER_FAULT_BIT_MASK 0x10 ///< Bit mask for valve motor driver fault. +#define VALVE_MOTOR_STALL_BIT_MASK 0x40 ///< Bit mask for valve stall. -#define VALVE_MOTOR_DRIVER_FAULT_BIT_MASK 0x1 ///< Bit mask for valve motor driver fault. -#define VALVE_DIRECTION_FAULT_BIT_MASK 0x2 ///< Bit mask for valve direction fault. -#define VALVE_INCORRECT_ENCODER_STATUS_BIT_MASK 0x4 ///< Bit mask for valve incorrect encoder sensor fault. - /// Bit mask for all (OR'd) valve faults. -#define VALVE_GEN_FAULT_BIT_MASK ( VALVE_MOTOR_DRIVER_FAULT_BIT_MASK | VALVE_DIRECTION_FAULT_BIT_MASK | VALVE_INCORRECT_ENCODER_STATUS_BIT_MASK ) +#define VALVE_GEN_FAULT_BIT_MASK ( VALVE_MOTOR_DRIVER_FAULT_BIT_MASK | VALVE_MOTOR_STALL_BIT_MASK ) #pragma pack(push, 1) /// Payload record structure for rotary valve set position request @@ -49,7 +46,7 @@ static U08 valveControl[ NUM_OF_VALVES ]; ///< Current control bits for each valve. static OVERRIDE_U32_T valveStatus[ NUM_OF_VALVES ]; ///< Current status bits for each valve. -static S16 commandValvePosChange[ NUM_OF_VALVES ]; ///< Current commanded valve position changes. Negative indicates CCW direction. +static S16 commandValvePos[ NUM_OF_VALVES ]; ///< Current commanded valve positions. static OVERRIDE_S32_T currentValveEncPosition[ NUM_OF_VALVES ]; ///< Current encoder valve positions (overrideable). Negative indicates CCW direction. // ********** private function prototypes ********** @@ -64,14 +61,12 @@ * @details \b Outputs: Rotary valves driver unit is initialized * @return none *************************************************************************/ -void initRotaryValvesDriver(void) +void initRotaryValvesDriver( void ) { U32 i; for ( i = FIRST_VALVE; i < NUM_OF_VALVES; i++ ) { - valveControl[ i ] = FPGA_PINCH_VALVES_1_32_STEP | FPGA_PINCH_VALVES_NOT_RESET | FPGA_PINCH_VALVES_NOT_SLEEP; // enable valves, configure for 1/32 step control - commandValvePosChange[ i ] = 0; currentValveEncPosition[ i ].data = 0; currentValveEncPosition[ i ].ovData = 0; currentValveEncPosition[ i ].ovInitData = 0; @@ -81,10 +76,8 @@ valveStatus[ i ].ovInitData = 0; valveStatus[ i ].override = OVERRIDE_RESET; } - - // set valve control bits once at startup - setH1Control( valveControl[ H1_VALV ] ); - setH19Control( valveControl[ H19_VALV ] ); + commandValvePos[ H1_VALV ] = getH1EncoderPosition(); + commandValvePos[ H19_VALV ] = getH19EncoderPosition(); } /*********************************************************************//** @@ -102,17 +95,14 @@ void readValves( void ) { U32 i; - S16 cmdPos[ NUM_OF_VALVES ]; // Get latest valve status and positions from FPGA - cmdPos[ H19_VALV ] = getH19CmdPosition(); - currentValveEncPosition[ H19_VALV ].data = (S32)getValveEncoderPosition( H19_VALV ); + currentValveEncPosition[ H19_VALV ].data = (S32)getH19EncoderPosition(); valveStatus[ H19_VALV ].data = getH19Status(); - cmdPos[ H1_VALV ] = getH1CmdPosition(); - currentValveEncPosition[ H1_VALV ].data = (S32)getValveEncoderPosition( H1_VALV ); + currentValveEncPosition[ H1_VALV ].data = (S32)getH1EncoderPosition(); valveStatus[ H1_VALV ].data = getH1Status(); - // TODO check commanded position vs. commanded position read back from FPGA - alarm different for some persistent period of time + // TODO - compare commanded vs. read position // Check valves status for faults for ( i = FIRST_VALVE; i < NUM_OF_VALVES; i++ ) @@ -121,7 +111,7 @@ if ( ( status & VALVE_GEN_FAULT_BIT_MASK ) != 0 ) { - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_PINCH_VALVE_FAULT, i, status ) + //SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_PINCH_VALVE_FAULT, i, status ) TODO - resolve issue and restore alarm } } } @@ -133,7 +123,7 @@ * @details \b Message \Sent: MSG_ID_TD_EVENT for TD_EVENT_VALVE_POS_CHANGE * @details \b Alarm: ALARM_ID_TD_SOFTWARE_FAULT if invalid valve given. * @details \b Inputs: none - * @details \b Outputs: commandValvePosChange[] + * @details \b Outputs: commandValvePos[] * @param valve Valve to set new commanded change in position for * @param mag Magnitude of position change for valve (in encoder counts) * @param dir Direction to move valve @@ -143,43 +133,21 @@ { if ( valve < NUM_OF_VALVES ) { - S16 chg = (S16)mag * ( MOTOR_DIR_REVERSE == dir ? -1 : 1 ); - U08 ctrl; - F32 temp; + U16 mag8 = ( ( mag % ROTARY_VALVE_MICROSTEP_FRACTION ) < 4 ? ( mag >> 3 ) << 3 : ( ( mag >> 3 ) << 3 ) + ROTARY_VALVE_MICROSTEP_FRACTION ); // round to multiple of 8 to maximize holding torque + S16 chgSteps = (S16)mag8 * ( MOTOR_DIR_REVERSE == dir ? -1 : 1 ); // convert to signed change in position - negative value indicates reverse direction // log event showing valve position change commanded - SEND_EVENT_WITH_2_U32_DATA( TD_EVENT_VALVE_POS_CHANGE, (U32)commandValvePosChange[ valve ], (U32)chg ); - // convert magnitude of change from encoder counts to microsteps - temp = (F32)chg * VALVE_ENC_COUNT_2_MICRO_STEP_CONVERSION; - chg = (S16)((S32)((temp) + FLOAT_TO_INT_ROUNDUP_OFFSET)); - mag = (S16)(abs(chg)); - // give FPGA valve change command - commandValvePosChange[ valve ] = chg; + SEND_EVENT_WITH_2_U32_DATA( TD_EVENT_VALVE_POS_CHANGE, (U32)commandValvePos[ valve ], (U32)chgSteps ); + // calculate and record new commanded valve position + commandValvePos[ valve ] = getValveEncoderPosition( valve ) + chgSteps; + // give FPGA new commanded valve position if ( H1_VALV == valve ) { - ctrl = getH1Control(); - if ( MOTOR_DIR_REVERSE == dir ) - { - setH1Control( ( ctrl | FPGA_PINCH_VALVES_REVERSE ) ); - } - else - { - setH1Control( ( ctrl & ~FPGA_PINCH_VALVES_REVERSE ) ); - } - setH1Position( mag ); + setH1Position( commandValvePos[ valve ] ); } else { - ctrl = getH19Control(); - if ( MOTOR_DIR_REVERSE == dir ) - { - setH19Control( ( ctrl | FPGA_PINCH_VALVES_REVERSE ) ); - } - else - { - setH19Control( ( ctrl & ~FPGA_PINCH_VALVES_REVERSE ) ); - } - setH19Position( mag ); + setH19Position( commandValvePos[ valve ] ); } } else @@ -190,35 +158,6 @@ /*********************************************************************//** * @brief - * The getValveCmdTravel function gets the most recent commanded travel (in - * encoder counts) for a given valve. - * @note A negative return value indicates travel in the reverse (CCW) direction. - * @details \b Alarm: ALARM_ID_TD_SOFTWARE_FAULT if invalid valve given. - * @details \b Inputs: commandValvePosChange[] - * @details \b Outputs: none - * @param valve ID of valve to get commanded position for - * @return Commanded position change (in encoder counts) for the given valve - *************************************************************************/ -S16 getValveCmdTravel( VALVE_T valve ) -{ - S16 result = 0; - - if ( valve < NUM_OF_VALVES ) - { - F32 temp = (F32)commandValvePosChange[ valve ] / VALVE_ENC_COUNT_2_MICRO_STEP_CONVERSION; // convert magnitude of change from microsteps to encoder counts - - result = (S16)((S32)((temp) + FLOAT_TO_INT_ROUNDUP_OFFSET)); - } - else - { - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_VALVES_INVALID_VALVE2, (U32)valve ) - } - - return result; -} - -/*********************************************************************//** - * @brief * The getValveEncoderPosition function gets the current actual position * (in encoder counts) for a given valve. * @note There are 1024 encoder counts per revolution. @@ -234,11 +173,7 @@ if ( valve < NUM_OF_VALVES ) { - result = (S16)(currentValveEncPosition[ valve ].data); - if ( OVERRIDE_KEY == currentValveEncPosition[ valve ].override ) - { - result = (S16)(currentValveEncPosition[ valve ].ovData); - } + result = (S16)getS32OverrideValue( ¤tValveEncPosition[ valve ] ); } else { @@ -268,11 +203,7 @@ if ( valve < NUM_OF_VALVES ) { - result = (U08)( valveStatus[ valve ].data & MASK_OFF_U32_MSBS ); - if ( OVERRIDE_KEY == valveStatus[ valve ].override ) - { - result = (U08)( valveStatus[ valve ].ovData & MASK_OFF_U32_MSBS ); - } + result = getU08OverrideValue( &valveStatus[ valve ] ); } else { @@ -282,7 +213,125 @@ return result; } +/*********************************************************************//** + * @brief + * The resetValve function resets 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 none + *************************************************************************/ +void resetValve( VALVE_T valve ) +{ + U08 resetValve = FPGA_PINCH_VALVES_1_8_STEP | FPGA_PINCH_VALVES_ENABLE_ENCODER | FPGA_PINCH_VALVES_DISABLE | + FPGA_PINCH_VALVES_RESET | FPGA_PINCH_VALVES_NOT_SLEEP; + switch ( valve ) + { + case H1_VALV: + setH1Control( resetValve ); + break; + + case H19_VALV: + setH19Control( resetValve ); + break; + + default: + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_VALVES_INVALID_VALVE_RESET_REQ, (U32)valve ) + break; + } +} + +/*********************************************************************//** + * @brief + * The resetValveEncoder function resets the valve's encoder. + * @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 none + *************************************************************************/ +void resetValveEncoder( VALVE_T valve ) +{ + U08 resetEnc = FPGA_PINCH_VALVES_1_8_STEP | FPGA_PINCH_VALVES_RESET_ENCODER | FPGA_PINCH_VALVES_DISABLE | + FPGA_PINCH_VALVES_RESET | FPGA_PINCH_VALVES_NOT_SLEEP; + + switch ( valve ) + { + case H1_VALV: + setH1Control( resetEnc ); + break; + + case H19_VALV: + setH19Control( resetEnc ); + break; + + default: + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_VALVES_INVALID_VALVE_RESET_ENCODER_REQ, (U32)valve ) + break; + } +} + +/*********************************************************************//** + * @brief + * The enableValve function enables 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 none + *************************************************************************/ +void enableValve( VALVE_T valve ) +{ + U08 enableValve = FPGA_PINCH_VALVES_1_8_STEP | FPGA_PINCH_VALVES_ENABLE_ENCODER | FPAG_PINCH_VALVES_ENABLE | + FPGA_PINCH_VALVES_NOT_RESET | FPGA_PINCH_VALVES_NOT_SLEEP; + + switch ( valve ) + { + case H1_VALV: + setH1Control( enableValve ); + break; + + case H19_VALV: + setH19Control( enableValve ); + break; + + default: + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_VALVES_INVALID_VALVE_ENABLE_REQ, (U32)valve ) + break; + } +} + +/*********************************************************************//** + * @brief + * The setValveZeroEncoderPosition function zeros the valve's encoders position. + * @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 none + *************************************************************************/ +void setValveZeroEncoderPosition( VALVE_T valve ) +{ + // First enable the valve and then set the position to 0 to clear the FPGA command registers + switch ( valve ) + { + case H1_VALV: + setH1Position( 0 ); + break; + + case H19_VALV: + setH19Position( 0 ); + break; + + default: + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_VALVES_INVALID_VALVE_ENCODER_REQ, (U32)valve ) + break; + } +} + + /************************************************************************* * TEST SUPPORT FUNCTIONS *************************************************************************/ @@ -346,7 +395,7 @@ { VALVE_ROTARY_SET_CMD_PAYLOAD_T payload; - memcpy( &payload, message->payload, sizeof(VALVE_ROTARY_SET_CMD_PAYLOAD_T) ); + memcpy( &payload, message->payload, sizeof( VALVE_ROTARY_SET_CMD_PAYLOAD_T ) ); if ( payload.valve < NUM_OF_VALVES ) { MOTOR_DIR_T dir = MOTOR_DIR_FORWARD;