/************************************************************************** * * Copyright (c) 2025-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 EjectorMotor.c * * @author (last) Sean * @date (last) 13-May-2025 * * @author (original) Sean * @date (original) 13-May-2025 * ***************************************************************************/ #include "AlarmMgmtTD.h" #include "EjectorMotor.h" #include "FpgaTD.h" #include "Messaging.h" /** * @addtogroup EjectorMotor * @{ */ // ********** private definitions ********** #define EJECTOR_MOTOR_MICROSTEP_TOGGLE_TIME_FOR_STOP 0xFFFFFFFF ///< Ejector motor microstep toggle time setting to indicate zero speed (stopped). #define EJECTOR_STEPS_PER_REV 48.0F ///< Number of steps per revolution (7.5 degrees per step). #define EJECTOR_MICRO_STEPS_PER_STEP 32.0F ///< Number of micro-steps per step. #define EJECTOR_TOGGLES_PER_STEP 2.0F ///< Stepper motor driver toggles per step or microstep. #define EJECTOR_MOTOR_START_RAMP_SPEED 300000 ///< Starting speed for all ejector operations to ramp up from. #define EJECTOR_MOTOR_RAMP_DIVISOR 5 ///< Used for ramping profile. /// Number of ejector motor micro steps per revolution. #define EJECTOR_MICRO_STEPS_PER_REV ( EJECTOR_STEPS_PER_REV * EJECTOR_MICRO_STEPS_PER_STEP ) // Bit definitions for ejector motor control register #define EJECTOR_MOTOR_CONTROL_SLEEP_OFF 0x40 ///< Ejector motor control register bit for sleep mode (active low). #define EJECTOR_MOTOR_CONTROL_NOT_RESET 0x20 ///< Ejector motor control register bit for resetting stepper motor (active low). #define EJECTOR_MOTOR_CONTROL_ENABLE 0x00 ///< Ejector motor control register bit mask for enable. #define EJECTOR_MOTOR_CONTROL_DISABLE 0x10 ///< Ejector motor control register bit for enable (active low). #define EJECTOR_MOTOR_CONTROL_REVERSE_DIR 0x00 ///< Ejector motor control register bit for direction (1=fwd, 0=rev). #define EJECTOR_MOTOR_CONTROL_FORWARD_DIR 0x08 ///< Ejector motor control register bit mask for forward direction. #define EJECTOR_MOTOR_CONTROL_32TH_STEP 0x03 ///< Ejector motor control register bits for 1/32 micro-stepping mode. #define EJECTOR_MOTOR_CONTROL_FULL_STEP 0x00 ///< Ejector motor control register bits for full stepping mode. #define EJECTOR_OPT_SENSOR_ACTIVE 0 ///< Ejector optical sensor active. #define EJECTOR_OPT_SENSOR_INACTIVE 1 ///< Ejector optical sensor inactive. /// Control bits to run ejector motor in reverse direction static const U08 EJECTOR_MOTOR_CONTROL_RUN_REVERSE = EJECTOR_MOTOR_CONTROL_SLEEP_OFF | EJECTOR_MOTOR_CONTROL_NOT_RESET | EJECTOR_MOTOR_CONTROL_ENABLE | EJECTOR_MOTOR_CONTROL_REVERSE_DIR | EJECTOR_MOTOR_CONTROL_32TH_STEP; /// Control bits to run ejector motor in forward direction static const U08 EJECTOR_MOTOR_CONTROL_RUN_FORWARD = EJECTOR_MOTOR_CONTROL_SLEEP_OFF | EJECTOR_MOTOR_CONTROL_NOT_RESET | EJECTOR_MOTOR_CONTROL_ENABLE | EJECTOR_MOTOR_CONTROL_FORWARD_DIR | EJECTOR_MOTOR_CONTROL_32TH_STEP; /// Control bits to disable ejector motor static const U08 EJECTOR_MOTOR_CONTROL_DISABLED = EJECTOR_MOTOR_CONTROL_SLEEP_OFF | EJECTOR_MOTOR_CONTROL_NOT_RESET | EJECTOR_MOTOR_CONTROL_FORWARD_DIR | EJECTOR_MOTOR_CONTROL_32TH_STEP; /// Payload record structure for ejector motor set request typedef struct { F32 setSpeed; ///< Set speed for ejector motor (in RPM). Negative RPM indicates reverse direction. } EJECTOR_MOTOR_SET_CMD_PAYLOAD_T; // ********** private data ********** static F32 currentEjectorMotorSetSpeed; ///< Current ejector motor set speed (in RPM). static U32 ejectorMotorSetToggleTime; ///< Time (in uSec) between microstep toggles. static U32 ejectorMotorRampUpToggleTime; ///< Current ramp time (in uSec) between microstep toggles. static U32 ejectorMotorRampTimerCtr; ///< Used to track ramp up time. static BOOL ejectorMotorRampUpInProgress; ///< Flag indicating whether a ramp up is in progress. static OVERRIDE_U32_T ejectorOpticalSensors[ NUM_OF_EJECTOR_OPT_SENSORS ]; ///< Ejector retract optical sensor. // ********** private function prototypes ********** static U32 calcEjectorStepToggleTimeFromSetSpeed( F32 rpm ); /*********************************************************************//** * @brief * The initEjectorMotor function initializes the ejector motor driver unit. * @details \b Inputs: none * @details \b Outputs: EjectorMotor unit initialized. * @return none *************************************************************************/ void initEjectorMotor(void) { U32 optSensor; currentEjectorMotorSetSpeed = 0.0F; ejectorMotorSetToggleTime = EJECTOR_MOTOR_MICROSTEP_TOGGLE_TIME_FOR_STOP; ejectorMotorRampUpToggleTime = EJECTOR_MOTOR_MICROSTEP_TOGGLE_TIME_FOR_STOP; ejectorMotorRampTimerCtr = 0; ejectorMotorRampUpInProgress = FALSE; for ( optSensor = EJECTOR_OPT_SENSOR_RETRACT; optSensor < NUM_OF_EJECTOR_OPT_SENSORS; optSensor++ ) { ejectorOpticalSensors[ optSensor ].data = EJECTOR_OPT_SENSOR_INACTIVE; ejectorOpticalSensors[ optSensor ].ovData = 0; ejectorOpticalSensors[ optSensor ].ovInitData = 0; ejectorOpticalSensors[ optSensor ].override = OVERRIDE_RESET; } setH5StepToggleTime( ejectorMotorSetToggleTime ); setH5ControlFlags( EJECTOR_MOTOR_CONTROL_DISABLED ); } /*********************************************************************//** * @brief * The disableEjectorMotor function sets the ejector motor to be disabled * when not in motion to prevent over-heating. * @note Ejector motor must be off first. * @details \b Inputs: ejectorMotorSetToggleTime * @details \b Outputs: none * @return none *************************************************************************/ void disableEjectorMotor( void ) { if ( EJECTOR_MOTOR_MICROSTEP_TOGGLE_TIME_FOR_STOP == ejectorMotorSetToggleTime ) { setH5ControlFlags( EJECTOR_MOTOR_CONTROL_DISABLED ); } } /*********************************************************************//** * @brief * The setEjectorMotorSpeed function sets the ejector motor to the given * set speed and direction. * @details \b Inputs: currentEjectorMotorSetSpeed * @details \b Outputs: currentEjectorMotorSetSpeed, ejectorMotorSetToggleTime * ejectorMotorRampUpToggleTime, ejectorMotorRampUpInProgress * @param rpm Speed (rpm) to set the ejector motor to (negative value indicates reverse direction) * @return TRUE if ejector motor speed set successfully, FALSE if not. *************************************************************************/ BOOL setEjectorMotorSpeed( F32 rpm ) { BOOL result = FALSE; // if starting motor, motor must be stopped first if ( ( fabs(rpm) > 0.0F ) && ( fabs( currentEjectorMotorSetSpeed ) < NEARLY_ZERO ) ) { U32 stepTime = calcEjectorStepToggleTimeFromSetSpeed( rpm ); U08 ctrl = EJECTOR_MOTOR_CONTROL_RUN_FORWARD; currentEjectorMotorSetSpeed = rpm; if ( rpm < 0.0F ) { ctrl = EJECTOR_MOTOR_CONTROL_RUN_REVERSE; } ejectorMotorSetToggleTime = stepTime; ejectorMotorRampUpToggleTime = EJECTOR_MOTOR_START_RAMP_SPEED; setH5ControlFlags( ctrl ); setH5StepToggleTime( ejectorMotorRampUpToggleTime ); ejectorMotorRampUpInProgress = TRUE; result = TRUE; } // if stopping motor, motor must be running first else if ( ( fabs(rpm) < NEARLY_ZERO ) && ( fabs( currentEjectorMotorSetSpeed ) > 0.0F ) ) { ejectorMotorRampUpInProgress = FALSE; ejectorMotorSetToggleTime = EJECTOR_MOTOR_MICROSTEP_TOGGLE_TIME_FOR_STOP; ejectorMotorRampUpToggleTime = EJECTOR_MOTOR_MICROSTEP_TOGGLE_TIME_FOR_STOP; currentEjectorMotorSetSpeed = 0.0F; setH5StepToggleTime( ejectorMotorSetToggleTime ); result = TRUE; } return result; } /*********************************************************************//** * @brief * The getEjectorMotorSetSpeed function gets the current set ejector motor * speed (in RPM). If the direction is reverse, the reported speed will be * negative. * @details \b Inputs: currentEjectorMotorSetSpeed * @details \b Outputs: none * @return currentEjectorMotorSetSpeed *************************************************************************/ F32 getEjectorMotorSetSpeed( void ) { return currentEjectorMotorSetSpeed; } /*********************************************************************//** * @brief * The execEjectorMotorRamping function manages the ejector motor ramping. * @note This ramping function is executed within the priority task while * other ejector related functions occur within the general task. Therefore * care should be taken when setting the ramp in progress flag. * @details \b Inputs: ejectorMotorRampUpInProgress * @details \b Outputs: none * @return none *************************************************************************/ void execEjectorMotorRamping( void ) { // Update the optical sensor values // TODO is debouncing needed?? ejectorOpticalSensors[ EJECTOR_OPT_SENSOR_RETRACT ].data = getFPGAEjectorRetractOpticalSensor(); ejectorOpticalSensors[ EJECTOR_OPT_SENSOR_ENGAGE ].data = getFPGAEjectorEngageOpticalSensor(); if ( TRUE == ejectorMotorRampUpInProgress ) { ejectorMotorRampTimerCtr++; if ( ejectorMotorRampUpToggleTime > ejectorMotorSetToggleTime ) { ejectorMotorRampUpToggleTime = (U32)( (F32)EJECTOR_MOTOR_START_RAMP_SPEED / ( (F32)( ejectorMotorRampTimerCtr * ejectorMotorRampTimerCtr * ejectorMotorRampTimerCtr ) / (F32)EJECTOR_MOTOR_RAMP_DIVISOR ) ); if ( ejectorMotorRampUpToggleTime > ejectorMotorSetToggleTime ) { setH5StepToggleTime( ejectorMotorRampUpToggleTime ); } else { setH5StepToggleTime( ejectorMotorSetToggleTime ); ejectorMotorRampUpInProgress = FALSE; } } } } /*********************************************************************//** * @brief * The isEjectorOpticalSensorActive function checks whether either of the * ejector optical sensors are active or inactive. * @details \b Alarm: ALARM_ID_TD_SOFTWARE_FAULT if the ejector optical sensor * is not in range. * @details Inputs: ejectorOpticalSensors * @details Outputs: none * @param rsensorID the optical sensor to check * @return TRUE if the sensor is active otherwise, FALSE *************************************************************************/ BOOL isEjectorOpticalSensorActive( EJECTOR_OPT_SENSOR_T sensorID ) { BOOL status = FALSE; switch ( sensorID ) { case EJECTOR_OPT_SENSOR_RETRACT: status = ( EJECTOR_OPT_SENSOR_ACTIVE == getU32OverrideValue( &ejectorOpticalSensors[ EJECTOR_OPT_SENSOR_RETRACT ] ) ? TRUE : FALSE ); break; case EJECTOR_OPT_SENSOR_ENGAGE: status = ( EJECTOR_OPT_SENSOR_ACTIVE == getU32OverrideValue( &ejectorOpticalSensors[ EJECTOR_OPT_SENSOR_ENGAGE ] ) ? TRUE : FALSE ); break; default: SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_TD_EJECTOR_OPTICAL_SENSOR_INVALID_REQ, (U32)sensorID ) break; } return status; } /*********************************************************************//** * @brief * The calcEjectorStepToggleTimeFromSetSpeed function calculates the stepper * toggle period for a given set speed (in RPM). * @details Inputs: none * @details Outputs: none * @param rpm Speed to convert to step toggle time (in uSec) * @return toggle time for set speed *************************************************************************/ static U32 calcEjectorStepToggleTimeFromSetSpeed( F32 rpm ) { U32 result = EJECTOR_MOTOR_MICROSTEP_TOGGLE_TIME_FOR_STOP; F64 temp; F32 conv; // Toggle time is direction agnostic, so ignore direction implied by sign. rpm = fabs( rpm ); // Convert given speed to stepper toggle period temp = (F64)EJECTOR_MICRO_STEPS_PER_REV * (F64)rpm; // = uSteps/min temp /= (F64)(SEC_PER_MIN); // = uSteps/sec temp /= (F64)US_PER_SECOND; // = uSteps/uSec conv = (F32)temp * EJECTOR_TOGGLES_PER_STEP; // = toggles/uSec conv = 1.0F / conv; // = uSec/toggle result = (U32)FLOAT_TO_INT_WITH_ROUND( conv ); ejectorMotorRampUpToggleTime = EJECTOR_MOTOR_START_RAMP_SPEED; return result; } /************************************************************************* * TEST SUPPORT FUNCTIONS *************************************************************************/ /*********************************************************************//** * @brief * The testSetEjectorMotorSpeed function sets the ejector motor to a given * set speed. * @details \b Inputs: none * @details \b Outputs: none * @param message set message from Dialin which includes the set speed being * requested. * @return TRUE if set request is successful, FALSE if not *************************************************************************/ BOOL testSetEjectorMotorSpeed( MESSAGE_T *message ) { BOOL result = FALSE; // Verify tester has logged in with TD and override type is valid if ( TRUE == isTestingActivated() ) { // Verify payload length is valid if ( sizeof( F32 ) == message->hdr.payloadLen ) { EJECTOR_MOTOR_SET_CMD_PAYLOAD_T payload; memcpy( &payload, message->payload, sizeof(EJECTOR_MOTOR_SET_CMD_PAYLOAD_T) ); result = setEjectorMotorSpeed( payload.setSpeed ); } } return result; } /*********************************************************************//** * @brief * The testEjectorOpticalSensorOverride function overrides the optical sensors. * @details \b Inputs: none * @details \b Outputs: ejectorOpticalSensors * @param message Override message from Dialin which includes the sensor to * override its value. * @return TRUE if override request is successful, FALSE if not *************************************************************************/ BOOL testEjectorOpticalSensorOverride( MESSAGE_T *message ) { BOOL result = u32ArrayOverride( message, &ejectorOpticalSensors[ EJECTOR_OPT_SENSOR_RETRACT ], NUM_OF_EJECTOR_OPT_SENSORS - 1, EJECTOR_OPT_SENSOR_ACTIVE, EJECTOR_OPT_SENSOR_INACTIVE ); return result; } /**@}*/