Index: firmware/App/Controllers/Ejector.c =================================================================== diff -u -r2d40deb50ffb27667c3e57f427b1e197e163910e -re130387b9e8ed54237a6e4c3020222060c2be5a9 --- firmware/App/Controllers/Ejector.c (.../Ejector.c) (revision 2d40deb50ffb27667c3e57f427b1e197e163910e) +++ firmware/App/Controllers/Ejector.c (.../Ejector.c) (revision e130387b9e8ed54237a6e4c3020222060c2be5a9) @@ -60,18 +60,19 @@ static BOOL ejectorHomeRequested; ///< Flag indicating that ejector home operation is requested. static BOOL ejectorRetractRequested; ///< Flag indicating that ejector retract operation is requested. static BOOL ejectorExtendRequested; ///< Flag indicating that ejector extend operation is requested. +static BOOL ejectorAbortRequested; ///< Flag indicating that ejector abort operation in requested. // ********** private function prototypes ********** static EJECTOR_STATE_T handleEjectorStartState( void ); static EJECTOR_STATE_T handleEjectorHomingState( void ); -static EJECTOR_STATE_T handleEjectorRetractedState ( void ); -static EJECTOR_STATE_T handleEjectorExtendedState ( void ); -static EJECTOR_STATE_T handleEjectorRetractingState ( void ); -static EJECTOR_STATE_T handleEjectorExtendingState ( void ); +static EJECTOR_STATE_T handleEjectorRetractedState( void ); +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 setEjectorSpeed( F32 speed ); +static EJECTOR_STATE_T handleEjectorAbortState( void ); static void resetEjectorFlags( void ); static void publishEjectorData( void ); @@ -104,14 +105,15 @@ * The resetEjectorFlags function resets the ejector flags to FALSE. * @details \b Inputs: none * @details \b Outputs: ejectorHomeRequested, ejectorRetractRequested, - * ejectorExtendRequested + * ejectorExtendRequested, ejectorAbortRequested * @return none *************************************************************************/ static void resetEjectorFlags( void ) { ejectorHomeRequested = FALSE; ejectorRetractRequested = FALSE; ejectorExtendRequested = FALSE; + ejectorAbortRequested = FALSE; } /*********************************************************************//** @@ -194,6 +196,20 @@ /*********************************************************************//** * @brief + * The abortEjectorOperation function requests an ejector abort operation. + * @details \b Inputs: none + * @details \b Outputs: ejectorAbortRequested + * @return none + *************************************************************************/ +void abortEjectorOperation( void ) +{ + setEjectorMotorSpeed( EJECTOR_OFF_MOTOR_SPEED_RPM ); + // Set the flag to TRUE regardless of where the ejector is at + ejectorAbortRequested = TRUE; +} + +/*********************************************************************//** + * @brief * The getEjectorState function returns the current ejector control state machine. * @details \b Inputs: currentEjectorState * @details \b Outputs: none @@ -214,6 +230,12 @@ *************************************************************************/ void execEjectorController( void ) { + if ( TRUE == ejectorAbortRequested ) + { + // If abort has been called, set it to abort state no matter where we are at. + currentEjectorState = EJECTOR_STATE_ABORT; + } + switch( currentEjectorState ) { case EJECTOR_STATE_INIT: @@ -248,6 +270,10 @@ currentEjectorState = handleEjectorDirChangeStopState(); break; + case EJECTOR_STATE_ABORT: + currentEjectorState = handleEjectorAbortState(); + break; + default: SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_TD_EJECTOR_INVALID_STATE, (U32)currentEjectorState ) break; @@ -449,6 +475,28 @@ /*********************************************************************//** * @brief + * The handleEjectorAbortState function handles the abort state of the + * ejector control state machine. + * @details \b Inputs: ejectorOperationTimerCounter, ejectorHomeRequested + * @details \b Outputs: ejectorOperationTimerCounter + * @return next state of the ejector control state machine + *************************************************************************/ +static EJECTOR_STATE_T handleEjectorAbortState( void ) +{ + EJECTOR_STATE_T state = EJECTOR_STATE_ABORT; + + if ( TRUE == ejectorHomeRequested ) + { + ejectorOperationTimerCounter = 0; + setEjectorMotorSpeed( EJECTOR_RETRACT_MOTOR_SPEED_RPM ); + state = EJECTOR_STATE_HOMING; + } + + return state; +} + +/*********************************************************************//** + * @brief * The publishEjectorData function constructs and sends the air pump data * broadcast message. * @details \b Message \b Sent: MSG_ID_TD_EJECTOR_DATA