Index: firmware/App/Controllers/Ejector.c =================================================================== diff -u -r82bb2222f791a434d22921e2fe4f5edd7e74776d -rfdb7ee915da2741dc200fef1b624b7f383f4db85 --- firmware/App/Controllers/Ejector.c (.../Ejector.c) (revision 82bb2222f791a434d22921e2fe4f5edd7e74776d) +++ firmware/App/Controllers/Ejector.c (.../Ejector.c) (revision fdb7ee915da2741dc200fef1b624b7f383f4db85) @@ -31,7 +31,8 @@ // ********** private definitions ********** #define EJECTOR_RETRACT_OP_TIME ( ( MS_PER_SECOND * 5 ) / TASK_GENERAL_INTERVAL ) ///< Ejector retract operation interval. -#define EJECTOR_EXTEND_OP_TIME ( ( MS_PER_SECOND * 5 ) / TASK_GENERAL_INTERVAL ) ///< Ejector extend operation interval. +#define EJECTOR_EXTEND_OP_TIME ( ( MS_PER_SECOND * 4 ) / TASK_GENERAL_INTERVAL ) ///< Ejector extend operation interval. +#define EJECTOR_BACKOFF_OP_TIME ( 50 / TASK_GENERAL_INTERVAL ) ///< Ejector back-off operation interval. #define EJECTOR_DATA_PUB_INTERVAL ( MS_PER_SECOND / TASK_GENERAL_INTERVAL ) ///< Ejector data publish interval. #define DATA_PUBLISH_COUNTER_START_COUNT 13 ///< Ejector data publish start counter. @@ -68,6 +69,8 @@ static EJECTOR_STATE_T handleEjectorExtendedState ( void ); static EJECTOR_STATE_T handleEjectorRetractingState ( void ); static EJECTOR_STATE_T handleEjectorExtendingState ( void ); +static EJECTOR_STATE_T handleEjectorRetractBackoffState( void ); +static EJECTOR_STATE_T handleEjectorDirChangeStopState( void ); static void resetEjectorFlags( void ); static void publishEjectorData( void ); @@ -218,6 +221,14 @@ currentEjectorState = handleEjectorExtendingState(); break; + case EJECTOR_STATE_RETRACT_BACKOFF: + currentEjectorState = handleEjectorRetractBackoffState(); + break; + + case EJECTOR_STATE_DIR_CHANGE_STOP: + currentEjectorState = handleEjectorDirChangeStopState(); + break; + default: SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_TD_EJECTOR_INVALID_STATE, (U32)currentEjectorState ) break; @@ -267,7 +278,7 @@ { ejectorOperationTimerCounter = 0; setEjectorMotorSpeed( EJECTOR_OFF_MOTOR_SPEED_RPM ); - state = EJECTOR_STATE_RETRACTED; + state = EJECTOR_STATE_DIR_CHANGE_STOP; } return state; @@ -333,6 +344,50 @@ { ejectorOperationTimerCounter = 0; setEjectorMotorSpeed( EJECTOR_OFF_MOTOR_SPEED_RPM ); + state = EJECTOR_STATE_DIR_CHANGE_STOP; + } + + return state; +} + +/*********************************************************************//** + * @brief + * The handleEjectorDirChangeStopState function handles the direction change + * stop state of the ejector control state machine. + * @details \b Inputs: ejectorOperationTimerCounter + * @details \b Outputs: ejectorOperationTimerCounter + * @return next state of the ejector control state machine + *************************************************************************/ +static EJECTOR_STATE_T handleEjectorDirChangeStopState( void ) +{ + EJECTOR_STATE_T state = EJECTOR_STATE_DIR_CHANGE_STOP; + + if ( ++ejectorOperationTimerCounter >= EJECTOR_BACKOFF_OP_TIME ) + { + ejectorOperationTimerCounter = 0; + setEjectorMotorSpeed( EJECTOR_EXTEND_MOTOR_SPEED_RPM ); + state = EJECTOR_STATE_RETRACT_BACKOFF; + } + + return state; +} + +/*********************************************************************//** + * @brief + * The handleEjectorRetractBackoffState function handles the retract back-off + * state of the ejector control state machine. + * @details \b Inputs: ejectorOperationTimerCounter + * @details \b Outputs: ejectorOperationTimerCounter + * @return next state of the ejector control state machine + *************************************************************************/ +static EJECTOR_STATE_T handleEjectorRetractBackoffState( void ) +{ + EJECTOR_STATE_T state = EJECTOR_STATE_RETRACT_BACKOFF; + + if ( ++ejectorOperationTimerCounter >= EJECTOR_BACKOFF_OP_TIME ) + { + ejectorOperationTimerCounter = 0; + setEjectorMotorSpeed( EJECTOR_OFF_MOTOR_SPEED_RPM ); state = EJECTOR_STATE_RETRACTED; }