Adjustments to code and optimisations

include/etl/fsm.h

@@ -173,7 +173,7 @@ namespace etl
 
  // Pass this whenever no state change is desired. Specifically cast to
  // Highest unsigned value of fsm_state_id_t.
- static const fsm_state_id_t No_State_Change = static_cast<fsm_state_id_t>(-1);
+ static ETL_CONSTANT fsm_state_id_t No_State_Change = etl::integral_limits<fsm_state_id_t>::max;
 
  /// Allows ifsm_state functions to be private.
  friend class etl::fsm;
@@ -389,27 +389,24 @@ namespace etl
  {
  etl::fsm_state_id_t next_state_id = p_state->process_event(message);
 
- if(next_state_id != ifsm_state::No_State_Change)
+ if (have_changed_state(next_state_id))
  {
  ETL_ASSERT(next_state_id < number_of_states, ETL_ERROR(etl::fsm_state_id_exception));
  etl::ifsm_state* p_next_state = state_list[next_state_id];
 
- // Have we changed state?
- if (p_next_state != p_state)
+ do
  {
- do
- {
- p_state->on_exit_state();
- p_state = p_next_state;
+ p_state->on_exit_state();
+ p_state = p_next_state;
 
-  next_state_id = p_state->on_enter_state();
- if(next_state_id != ifsm_state::No_State_Change)
-  {
-  ETL_ASSERT(next_state_id < number_of_states, ETL_ERROR(etl::fsm_state_id_exception));
-  p_next_state = state_list[next_state_id];
- }
- } while (p_next_state != p_state); // Have we changed state again?
- }
+ next_state_id = p_state->on_enter_state();
+
+ if (have_changed_state(next_state_id))
+ {
+ ETL_ASSERT(next_state_id < number_of_states, ETL_ERROR(etl::fsm_state_id_exception));
+ p_next_state = state_list[next_state_id];
+ }
+ } while (p_next_state != p_state); // Have we changed state again?
  }
  }
 
@@ -493,6 +490,13 @@ namespace etl
 
  private:
 
+ //********************************************
+ bool have_changed_state(etl::fsm_state_id_t next_state_id) const
+ {
+ return (next_state_id != p_state->get_state_id()) &&
+ (next_state_id != ifsm_state::No_State_Change);
+ }
+
  etl::ifsm_state* p_state; ///< A pointer to the current state.
  etl::ifsm_state** state_list; ///< The list of added states.
  etl::fsm_state_id_t number_of_states; ///< The number of states.

include/etl/hfsm.h

@@ -33,7 +33,6 @@ SOFTWARE.
 
 namespace etl
 {
-
  //***************************************************************************
  /// The HFSM class.
  /// Builds on the FSM class by overriding the receive function and adding
@@ -87,54 +86,61 @@ namespace etl
  private:
 
  //*******************************************
- /// Return the common ancester of the two states.
+ /// Return the first common ancester of the two states.
  //*******************************************
- static etl::ifsm_state* common_ancestor(etl::ifsm_state* p_one, etl::ifsm_state* p_two)
+ static etl::ifsm_state* common_ancestor(etl::ifsm_state* p1, etl::ifsm_state* p2)
  {
- if (p_one == p_two)
+ size_t depth1 = get_depth(p1);
+ size_t depth2 = get_depth(p2);
+
+ // Align p1 and p2 to the same depth.
+ if (depth1 > depth2)
  {
- return p_one;
+ p1 = align_depth(p1, depth1 - depth2);
  }
-
- etl::ifsm_state* p_current_state = p_one;
-
- while (p_current_state->p_parent != ETL_NULLPTR)
+ else
  {
- if (p_current_state->p_parent == p_two)
- {
- return p_current_state->p_parent;
- }
-
- p_current_state = p_current_state->p_parent;
+ p2 = align_depth(p2, depth2 - depth1);
  }
 
- if (p_two->p_parent != ETL_NULLPTR)
+ // Now they're aligned they can step towards the root together.
+ while (p1 != p2)
  {
- return common_ancestor(p_one, p_two->p_parent);
+ p1 = p1->p_parent;
+ p2 = p2->p_parent;
  }
 
- return ETL_NULLPTR;
+ return p1;
  }
 
  //*******************************************
- /// Exiting the state.
+ /// Find the depth of the state.
  //*******************************************
- static void do_exits(const etl::ifsm_state* p_root, etl::ifsm_state* p_source)
+ static size_t get_depth(etl::ifsm_state* p)
  {
- etl::ifsm_state* p_current = p_source;
+ size_t depth = 0;
 
- // Iterate down to the lowest child
- while(p_current->p_active_child != ETL_NULLPTR)
+ while (p != ETL_NULLPTR)
  {
- p_current = p_current->p_active_child;
+ p = p->p_parent;
+ ++depth;
  }
 
- // Run exit state on all states up to the root
- while (p_current != p_root)
+ return depth;
+ }
+
+ //*******************************************
+ /// Align the depths of the states.
+ //*******************************************
+ static etl::ifsm_state* align_depth(etl::ifsm_state* p, size_t offset)
+ {
+ while (offset != 0U)
  {
- p_current->on_exit_state();
- p_current = p_current->p_parent;
+ p = p->p_parent;
+ --offset;
  }
+
+ return p;
  }
 
  //*******************************************
@@ -145,7 +151,8 @@ namespace etl
  ETL_ASSERT(p_target != ETL_NULLPTR, ETL_ERROR(etl::fsm_null_state_exception));
 
  // We need to go recursively up the tree if the target and root don't match
- if (p_root != p_target && p_target->p_parent != ETL_NULLPTR)
+ if ((p_root != p_target) && 
+ (p_target->p_parent != ETL_NULLPTR))
  {
  if (p_target->p_parent != p_root)
  {
@@ -176,6 +183,27 @@ namespace etl
 
  return next_state;
  }
+
+ //*******************************************
+ /// Exiting the state.
+ //*******************************************
+ static void do_exits(const etl::ifsm_state* p_root, etl::ifsm_state* p_source)
+ {
+ etl::ifsm_state* p_current = p_source;
+
+ // Iterate down to the lowest child
+ while (p_current->p_active_child != ETL_NULLPTR)
+ {
+ p_current = p_current->p_active_child;
+ }
+
+ // Run exit state on all states up to the root
+ while (p_current != p_root)
+ {
+ p_current->on_exit_state();
+ p_current = p_current->p_parent;
+ }
+ }
  };
 }
 #endif

test/test_fsm.cpp

@@ -224,7 +224,7 @@ namespace
  etl::fsm_state_id_t on_event_unknown(const etl::imessage&)
  {
  ++get_fsm_context().unknownCount;
- return No_State_Change;
+ return StateId::IDLE; //No_State_Change;
  }
 
  //***********************************

test/test_hfsm.cpp

@@ -327,6 +327,7 @@ namespace
  etl::fsm_state_id_t on_event_unknown(const etl::imessage&)
  {
  ++get_fsm_context().unknownCount;
+
  return No_State_Change;
  }
 
@@ -355,7 +356,8 @@ namespace
  {
  ++get_fsm_context().setSpeedCount;
  get_fsm_context().SetSpeedValue(event.speed);
- return No_State_Change;
+ //return No_State_Change;
+ return this->get_state_id();
  }
 
  //***********************************