State Machine Implementation Details

The HYPED code base consists of modules which are essentially threads that interact through shared memory, or practically speaking by modifying the central data structure (CDS) that is defined in src/data/data.hpp. One of those modules has the sole purpose of managing the high level state of the pod and thereby guide the decision making of all the components. This is what we call the state machine, because it is based on the mathematical concept of finite-state machines (FSM).

Communication with other modules

The state machine module does not have any hardware interfaces and therefore its only job is to read and write to the CDS. In particular, the state machine section in the CDS contains a field current_state of type data::State. This is an enum value which corresponds to the state the pod is currently in, we will refer to this as the pod state. All other modules are required to take this value into account when making internal decisions.

// src/data/data.hpp

enum State {
  kIdle,
  kCalibrating,
  kReady,
  kAccelerating,
  kCruising,
  kNominalBraking,
  kEmergencyBraking,
  kFailureStopped,
  kFinished,
  kInvalid,
  num_states
};

Representing state internally

On the other hand, the state machine module (STM) itself has a state. This internal state is reflected by the current_state_ member of the state_machine::Main class. As opposed to data::StateMachine::current_state, this field does not simply contain a numerical value representing a state, but it actually represents specific functionality. All the internal state objects follow the same interface, as can be found in state.hpp.

// src/state_machine/state.hpp

class State {
 public:
  virtual void enter(Logger &log) = 0;
  virtual void exit(Logger &log)  = 0;

  virtual State *checkTransition(Logger &log) = 0;
};

The State::enter and State::exit methods are called whenever we enter or exit a particular state. This involves changing the pod state in the CDS and logging the state changes. Because these operations are essentially identical for all states, we use a macro to define them.

// src/state_machine/state.hpp

#define MAKE_STATE(S)                                                                              \
  class S : public State {                                                                         \
   public:                                                                                         \
    S() {}                                                                                         \
    static S *getInstance() { return &S::instance_; }                                              \
                                                                                                   \
    State *checkTransition(Logger &log);                                                           \
    void enter(Logger &log)                                                                        \
    {                                                                                              \
      log.INFO(Messages::kStmLoggingIdentifier, Messages::kEnteringStateFormat,                    \
               S::string_representation_);                                                         \
      data::StateMachine sm_data = data_.getStateMachineData();                                    \
      sm_data.current_state      = S::enum_value_;                                                 \
      data_.setStateMachineData(sm_data);                                                          \
    }                                                                                              \
    void exit(Logger &log)                                                                         \
    {                                                                                              \
      log.INFO(Messages::kStmLoggingIdentifier, Messages::kExitingStateFormat,                     \
               S::string_representation_);                                                         \
    }                                                                                              \
                                                                                                   \
   private:                                                                                        \
    static S instance_;                                                                            \
    static char string_representation_[];                                                          \
    static data::State enum_value_;                                                                \
  };

You are not required to understand how this works in detail, all you need to know is that this allows us to declare a new state and implement the enter and exit methods automatically on a single line of code:

MAKE_STATE(Idle)

Making decisions

Thereby, we can focus on the crucial aspects of each state which is the checkTransition method. This is where decisions about the pod's behavoiur are being made. In particular, we check all the possible transition conditions and return a new state if any of them are met. These methods are implemented in state.cpp, for each state individually.

All of the implementations follow a similar pattern: retrieve all the newest data points, then for each possible transition check whether the preconditions are met and return the resulting state if that is the case. If none of them are met, don't return a new state.

// src/state_machine/state.cpp

State *Idle::checkTransition(Logger &log)
{
  updateModuleData();

  bool emergency = checkEmergency(log, embrakes_data_, nav_data_, batteries_data_, telemetry_data_,
                                  sensors_data_, motors_data_);
  if (emergency) { return FailureStopped::getInstance(); }

  bool calibrate_command = checkCalibrateCommand(log, telemetry_data_);
  if (!calibrate_command) { return nullptr; }

  bool all_initialised = checkModulesInitialised(log, embrakes_data_, nav_data_, batteries_data_,
                                                 telemetry_data_, sensors_data_, motors_data_);
  if (all_initialised) { return Calibrating::getInstance(); }

  return nullptr;
}

Checking transition conditions

Since there is a great overlap between the different transitions, for example consider the emergency checks, it makes sense to handle the concrete logic elsewhere. This is why we have a collection of (for all intents and purposes) pure boolean functions in transitions.cpp. Another significant advantage of doing this is that we are able to verify this logic easily through unit tests because no side effects occur and no state has to be taken into account.

The functions to check transition conditions take into account all of the entries in the CDS that are relevant and leaves out all the ones that do not affect the outcome. Generally speaking, it is assumed that each of the functions will only return true once because this will trigger a transition. Therefore, it is safe to print a log message in that case.

// src/state_machine/transitions.cpp

bool checkModulesReady(Logger &log, EmergencyBrakes &embrakes_data, Navigation &nav_data,
                       Batteries &batteries_data, Telemetry &telemetry_data, Sensors &sensors_data,
                       Motors &motors_data)
{
  if (embrakes_data.module_status != ModuleStatus::kReady) return false;
  if (nav_data.module_status != ModuleStatus::kReady) return false;
  if (batteries_data.module_status != ModuleStatus::kReady) return false;
  if (telemetry_data.module_status != ModuleStatus::kReady) return false;
  if (sensors_data.module_status != ModuleStatus::kReady) return false;
  if (motors_data.module_status != ModuleStatus::kReady) return false;

  log.INFO(Messages::kStmLoggingIdentifier, Messages::kModulesCalibratedLog);
  return true;
}