Index: firmware/App/Modes/OperationModes.c =================================================================== diff -u -r533272e6ef2873fcfe7a41338a6c88c7a601605d -rbf3ba65cafc8cbfb34e03d9fb2fc248069a8addb --- firmware/App/Modes/OperationModes.c (.../OperationModes.c) (revision 533272e6ef2873fcfe7a41338a6c88c7a601605d) +++ firmware/App/Modes/OperationModes.c (.../OperationModes.c) (revision bf3ba65cafc8cbfb34e03d9fb2fc248069a8addb) @@ -1,33 +1,350 @@ +/************************************************************************** +* +* Copyright (c) 2024-2024 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 OperationModes.c +* +* @author (last) Sean +* @date (last) 15-Nov-2024 +* +* @author (original) Sean +* @date (original) 15-Nov-2024 +* +***************************************************************************/ - +#include "Messaging.h" +#include "ModeInitPOST.h" +#include "ModeFault.h" +#include "ModeStandby.h" #include "OperationModes.h" +#include "TaskGeneral.h" +/** + * @addtogroup ROOperationModes + * @{ + */ + +// ********** private definitions ********** + +#define BROADCAST_TD_OP_MODE_INTERVAL ( 250 / TASK_GENERAL_INTERVAL ) ///< RO operation mode broadcast interval (in task interval/sec). +#define DATA_PUBLISH_COUNTER_START_COUNT 11 ///< Data publish counter start count. + +// ********** private data ********** + +static volatile BOOL modeRequest[ NUM_OF_RO_MODES - 1 ]; ///< Pending operation mode change requests. +static RO_OP_MODE_T lastMode; ///< Last operation mode prior to current mode. +static RO_OP_MODE_T currentMode; ///< Current operation mode. +static U32 currentSubMode; ///< The currently active state of the active mode. +static U32 broadcastModeIntervalCtr; ///< Interval counter used to determine when to broadcast operation mode. Initialize to 11 to stagger broadcast. +static U32 currentSubState; ///< current sub state. +static U32 current4thLevelState; ///< current 4th level state. + +/// Interval (in task intervals) at which to publish operation mode data to CAN bus. +static OVERRIDE_U32_T opModePublishInterval = { BROADCAST_TD_OP_MODE_INTERVAL, BROADCAST_TD_OP_MODE_INTERVAL, BROADCAST_TD_OP_MODE_INTERVAL, 0 }; + +/// This matrix determines legal transitions from one mode to another +static const RO_OP_MODE_T MODE_TRANSITION_TABLE[ NUM_OF_RO_MODES - 1 ][ NUM_OF_RO_MODES - 1 ] = { +// From to-> FAULT SERVICE INIT STANBY +/* FAUL */{ RO_MODE_FAUL, RO_MODE_SERV, RO_MODE_NLEG, RO_MODE_NLEG }, +/* SERV */{ RO_MODE_FAUL, RO_MODE_SERV, RO_MODE_NLEG, RO_MODE_NLEG }, +/* INIT */{ RO_MODE_FAUL, RO_MODE_NLEG, RO_MODE_INIT, RO_MODE_STAN }, +/* STAN */{ RO_MODE_FAUL, RO_MODE_SERV, RO_MODE_NLEG, RO_MODE_STAN } }; + +// ********** private function prototypes ********** + +static RO_OP_MODE_T arbitrateModeRequest( void ); +static void transitionToNewOperationMode( RO_OP_MODE_T newMode ); +static void broadcastOperationMode( void ); + +/*********************************************************************//** + * @brief + * The initOperationModes function initializes the Operation Modes unit. + * @details \b Inputs: none + * @details \b Outputs: Operation Modes unit initialized. + * @return none + *************************************************************************/ void initOperationModes( void ) { + U32 i; + // Initialize mode requests to none pending + for ( i = 0; i < ( NUM_OF_RO_MODES - 1 ); i++ ) + { + modeRequest[ i ] = FALSE; + } + + // Start in init mode + lastMode = RO_MODE_INIT; + currentMode = RO_MODE_INIT; + currentSubMode = 0; + currentSubState = NO_SUB_STATE; + current4thLevelState = NO_SUB_STATE; + broadcastModeIntervalCtr = DATA_PUBLISH_COUNTER_START_COUNT; + + transitionToNewOperationMode( RO_MODE_INIT ); + + // Call initializers for the individual modes + initFaultMode(); +// initServiceMode(); + initInitAndPOSTMode(); + initStandbyMode(); } +/*********************************************************************//** + * @brief + * The execOperationModes function executes the Operation Modes state machine. + * @details \b Alarm: ALARM_ID_RO_SOFTWARE_FAULT if current mode is invalid. + * @details \b Inputs: none + * @details \b Outputs: currentMode is set by state machine. + * @return none + *************************************************************************/ void execOperationModes( void ) { + RO_OP_MODE_T newMode; + U32 priorSubMode = currentSubMode; + U32 priorSubState = currentSubState; + U32 prior4thLevelState = current4thLevelState; + // Any new mode requests? + newMode = arbitrateModeRequest(); // Will return current mode if no pending requests + newMode = MODE_TRANSITION_TABLE[ currentMode ][ newMode ]; + + // Is requested new mode valid and legal at this time? + if ( newMode >= RO_MODE_NLEG ) + { + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_OP_MODES_ILLEGAL_MODE_TRANSITION_REQUESTED, newMode ) + newMode = currentMode; + } + + // Has mode changed? + if ( currentMode != newMode ) + { + lastMode = currentMode; + // Handle transition to new mode + transitionToNewOperationMode( newMode ); + currentMode = newMode; +// sendOperationStatusEvent(); + } + + // Mode specific processing to be done continuously + switch ( currentMode ) + { + case RO_MODE_FAUL: + currentSubMode = execFaultMode(); + break; + + case RO_MODE_SERV: +// currentSubMode = execServiceMode(); + break; + + case RO_MODE_INIT: + currentSubMode = execInitAndPOSTMode(); + break; + + case RO_MODE_STAN: + currentSubMode = execStandbyMode(); + break; + + default: + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_RO_SOFTWARE_FAULT, SW_FAULT_ID_OP_MODES_INVALID_MODE_STATE, currentMode ) + currentMode = RO_MODE_FAUL; + currentSubMode = 0; + break; + } // End switch + +#ifndef _VECTORCAST_ + // Send operation status event when appropriate + if ( ( priorSubMode != currentSubMode ) || ( priorSubState != currentSubState ) || ( prior4thLevelState != current4thLevelState ) ) +#endif + { +// sendOperationStatusEvent(); + SEND_EVENT_WITH_2_U32_DATA( RO_EVENT_SUB_MODE_CHANGE, priorSubMode, currentSubMode ) + } + + // Broadcast current operation mode on interval + broadcastOperationMode(); } +/*********************************************************************//** + * @brief + * The requestNewOperationMode function requests transition to a new + * operation mode. The request will be arbitrated when the state machine + * is next executed. + * @details \b Alarm: ALARM_ID_RO_SOFTWARE_FAULT if given new mode is invalid. + * @details \b Inputs: none + * @details \b Outputs: modeRequest[] + * @return none + *************************************************************************/ void requestNewOperationMode( RO_OP_MODE_T newMode ) { - + // Validate requested mode + if ( newMode < RO_MODE_NLEG ) + { + // Make request + modeRequest[ newMode ] = TRUE; + } + else + { // Invalid mode requested + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_RO_SOFTWARE_FAULT, SW_FAULT_ID_OP_MODES_INVALID_MODE_REQUESTED, newMode ) + } } +/*********************************************************************//** + * @brief + * The getCurrentOperationMode function gets the current operation mode. + * @details \b Inputs: currentMode + * @details \b Outputs: none + * @return the current operation mode + *************************************************************************/ RO_OP_MODE_T getCurrentOperationMode( void ) { - return 0; + return currentMode; } +/*********************************************************************//** + * @brief + * The getPreviousOperationMode function gets the previous operation mode. + * @details \b Inputs: lastMode + * @details \b Outputs: none + * @return the previous operation mode + *************************************************************************/ RO_OP_MODE_T getPreviousOperationMode( void ) { - return 0; + return lastMode; } +/*********************************************************************//** + * @brief + * The getCurrentSubMode function gets the current operation sub-mode. + * @details \b Inputs: currentSubMode + * @details \b Outputs: none + * @return the current operation sub-mode + *************************************************************************/ U32 getCurrentSubMode( void ) { - return 0; + return currentSubMode; } + +/*********************************************************************//** + * @brief + * The arbitrateModeRequest function arbitrates any pending mode transition + * requests. + * @note Thread protection prevents new mode requests from being made while + * arbitration is in progress. + * @details \b Inputs: modeRequest[] + * @details \b Outputs: modeRequest[] is reset + * @return the next operation mode (current mode if no requests pending) + *************************************************************************/ +static RO_OP_MODE_T arbitrateModeRequest( void ) +{ + RO_OP_MODE_T reqMode = currentMode; + U32 i; + + // Block additional requests until after mode arbitration + _disable_IRQ(); + + // Select highest priority mode request -or- current mode if no requests pending + for ( i = 0; i < RO_MODE_NLEG; i++ ) + { + if ( modeRequest[ i ] != FALSE ) + { + reqMode = (RO_OP_MODE_T)i; + break; + } + } + + // Clear all requests now that an arbitration winner is selected + for ( i = 0; i < RO_MODE_NLEG; i++ ) + { + modeRequest[ i ] = FALSE; + } + + // Un-block requests + _enable_IRQ(); + + return reqMode; +} + +/*********************************************************************//** + * @brief + * The transitionToNewOperationMode function calls the transition to function + * for a new operation mode that we are transitioning to. + * @details \b Alarm: ALARM_ID_RO_SOFTWARE_FAULT if given new mode is invalid. + * @details \b Inputs: none + * @details \b Outputs: transition function called for new mode + * @return none + *************************************************************************/ +static void transitionToNewOperationMode( RO_OP_MODE_T newMode ) +{ + // Setup for new operating mode + switch ( newMode ) + { + case RO_MODE_FAUL: + currentSubMode = transitionToFaultMode(); + break; + case RO_MODE_SERV: +// currentSubMode = transitionToServiceMode(); + break; + case RO_MODE_INIT: + currentSubMode = transitionToInitAndPOSTMode(); + break; + case RO_MODE_STAN: + currentSubMode = transitionToStandbyMode(); + break; + default: + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_RO_SOFTWARE_FAULT, SW_FAULT_ID_OP_MODES_INVALID_MODE_TO_TRANSITION_TO, newMode ) + break; + } +} + +/*********************************************************************//** + * @brief + * The broadcastOperationMode function sends the current operation mode at + * the prescribed interval. + * @details \b Inputs: broadcastModeIntervalCtr + * @details \b Outputs: RO operation mode broadcast message sent. + * @return none + *************************************************************************/ +static void broadcastOperationMode( void ) +{ + if ( ++broadcastModeIntervalCtr >= getU32OverrideValue( &opModePublishInterval ) ) + { + OP_MODE_PAYLOAD_T data; + + broadcastModeIntervalCtr = 0; + data.opMode = (U32)currentMode; + data.subMode = currentSubMode; + broadcastData( MSG_ID_RO_OP_MODE_DATA, COMM_BUFFER_OUT_CAN_RO_BROADCAST, (U08*)&data, sizeof( OP_MODE_PAYLOAD_T ) ); + } +} + +/*********************************************************************//** + * @brief + * The setCurrentSubState function sets the current subState. + * @details \b Inputs: none + * @details \b Outputs: currentSubState + * @param subState the enumerated sub state. + * @return none + *************************************************************************/ +void setCurrentSubState( U32 subState ) +{ + currentSubState = subState; +} + +/*********************************************************************//** + * @brief + * The setCurrent4thLevelState function sets the current 4th level state. + * @details \b Inputs: none + * @details \b Outputs: current4thLevelState + * @param subState the enumerated sub state. + * @return none + *************************************************************************/ +void setCurrent4thLevelState( U32 state ) +{ + current4thLevelState = state; +} + +/**@}*/