Index: firmware/App/Drivers/RotaryValve.c =================================================================== diff -u -r51f42cd88bd9c50ed9096a2d1d8ff859a95aff95 -r37294a71c0b3e3ca67ccb10b85536d124d3ef028 --- firmware/App/Drivers/RotaryValve.c (.../RotaryValve.c) (revision 51f42cd88bd9c50ed9096a2d1d8ff859a95aff95) +++ firmware/App/Drivers/RotaryValve.c (.../RotaryValve.c) (revision 37294a71c0b3e3ca67ccb10b85536d124d3ef028) @@ -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 ********** @@ -70,8 +67,7 @@ for ( i = FIRST_VALVE; i < NUM_OF_VALVES; i++ ) { - valveControl[ i ] = FPGA_PINCH_VALVES_1_8_STEP | FPGA_PINCH_VALVES_NOT_RESET | FPGA_PINCH_VALVES_NOT_SLEEP; // enable valves, configure for 1/32 step control - commandValvePosChange[ i ] = 0; + valveControl[ i ] = FPGA_PINCH_VALVES_1_8_STEP | FPGA_PINCH_VALVES_NOT_RESET | FPGA_PINCH_VALVES_NOT_SLEEP; // enable valves, configure for 1/8 step control currentValveEncPosition[ i ].data = 0; currentValveEncPosition[ i ].ovData = 0; currentValveEncPosition[ i ].ovInitData = 0; @@ -81,6 +77,8 @@ valveStatus[ i ].ovInitData = 0; valveStatus[ i ].override = OVERRIDE_RESET; } + commandValvePos[ H1_VALV ] = getH1EncoderPosition(); + commandValvePos[ H19_VALV ] = getH19EncoderPosition(); // set valve control bits once at startup setH1Control( valveControl[ H1_VALV ] ); @@ -102,17 +100,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 ); valveStatus[ H19_VALV ].data = getH19Status(); - cmdPos[ H1_VALV ] = getH1CmdPosition(); currentValveEncPosition[ H1_VALV ].data = (S32)getValveEncoderPosition( H1_VALV ); 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++ ) @@ -133,7 +128,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,29 +138,21 @@ { if ( valve < NUM_OF_VALVES ) { - S16 chg = (S16)mag * ( MOTOR_DIR_REVERSE == dir ? -1 : 1 ); - U08 ctrl; - F32 temp; + U16 mag8 = ( ( mag % 8 ) < 4 ? ( mag >> 3 ) << 3 : ( ( mag >> 3 ) << 3 ) + 1 ); // round to multiple of 8 to maximize holding torque + S16 chgSteps = (S16)mag * ( 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(); - setH1Control( ( ctrl | FPGA_PINCH_VALVES_NEW_POS_CMD ) ); - setH1Position( mag ); + setH1Position( commandValvePos[ valve ] ); } else { - ctrl = getH19Control(); - setH19Control( ( ctrl | FPGA_PINCH_VALVES_NEW_POS_CMD ) ); - setH19Position( mag ); + setH19Position( commandValvePos[ valve ] ); } } else @@ -176,35 +163,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. @@ -324,7 +282,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;