Priority in Microgrid Controller Selection

tactical-microgrid-standard/CMakeLists.txt

@@ -87,4 +87,6 @@ add_executable(Distribution
 target_include_directories(Distribution PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 target_link_libraries(Distribution PRIVATE PowerSim_Idl)
 
-add_subdirectory(tests)
+if(BUILD_TESTING)
+  add_subdirectory(tests)
+endif()

tactical-microgrid-standard/common/ControllerSelector.cpp

@@ -1,7 +1,8 @@
 #include "ControllerSelector.h"
 
 ControllerSelector::ControllerSelector(const tms::Identity& device_id)
-  : device_id_(device_id)
+  : ControllerCallbacks(lock_)
+  , device_id_(device_id)
 {
 }
 
@@ -42,6 +43,11 @@ void ControllerSelector::got_device_info(const tms::DeviceInfo& di)
   if (di.role() == tms::DeviceRole::ROLE_MICROGRID_CONTROLLER &&
       !all_controllers_.count(di.deviceId())) {
     all_controllers_[di.deviceId()] = TimePoint::min();
+    all_controllers_by_priority_.insert(Priority(di));
+    ACE_DEBUG((LM_INFO, "Got %C\n", di.deviceId().c_str()));
+    for (auto it = all_controllers_by_priority_.begin(); it != all_controllers_by_priority_.end(); ++it) {
+      ACE_DEBUG((LM_INFO, "%d %C\n", it->priority, it->id.c_str()));
+    }
   }
 }
 
@@ -65,8 +71,7 @@ void ControllerSelector::timer_fired(Timer<NewController>& timer)
   }
 
   if (now - it->second < heartbeat_deadline) {
-    selected_ = mc_id;
-    send_controller_state();
+    select(mc_id, std::chrono::duration_cast<Sec>(now - it->second));
   }
 }
 
@@ -76,6 +81,9 @@ void ControllerSelector::timer_fired(Timer<MissedHeartbeat>& timer)
   const auto& timer_id = timer.id;
   ACE_DEBUG((LM_INFO, "(%P|%t) INFO: ControllerSelector::timed_event(MissedHeartbeat): "
     "\"%C\". Timer id: %d\n", selected_.c_str(), timer_id));
+  if (missed_heartbeat_callback_) {
+    missed_heartbeat_callback_(selected_);
+  }
   schedule_once(LostController{}, lost_active_controller_delay);
 
   // Start a No MC timer if the device has missed heartbeats from all MCs
@@ -98,15 +106,19 @@ void ControllerSelector::timer_fired(Timer<LostController>&)
   Guard g(lock_);
   ACE_DEBUG((LM_INFO, "(%P|%t) INFO: ControllerSelector::timed_event(LostController): "
     "\"%C\"\n", selected_.c_str()));
+  if (lost_controller_callback_) {
+    lost_controller_callback_(selected_);
+  }
   selected_.clear();
 
   // Select a new controller if possible. If there are no recent controllers
   // and we don't hear from another, the NoControllers timer will fire.
   const TimePoint now = Clock::now();
-  for (auto it = all_controllers_.begin(); it != all_controllers_.end(); ++it) {
-    const auto last_hb = now - it->second;
+  for (auto it = all_controllers_by_priority_.begin(); it != all_controllers_by_priority_.end(); ++it) {
+    auto mc_info = all_controllers_.find(it->id);
+    const auto last_hb = now - mc_info->second;
     if (last_hb < heartbeat_deadline) {
-      select(it->first, std::chrono::duration_cast<Sec>(last_hb));
+      select(mc_info->first, std::chrono::duration_cast<Sec>(last_hb));
       break;
     }
   }
@@ -116,13 +128,19 @@ void ControllerSelector::timer_fired(Timer<NoControllers>&)
 {
   Guard g(lock_);
   ACE_DEBUG((LM_INFO, "(%P|%t) INFO: ControllerSelector::timed_event(NoControllers)\n"));
+  if (no_controllers_callback_) {
+    no_controllers_callback_();
+  }
   // TODO: CONFIG_ON_COMMS_LOSS
 }
 
 void ControllerSelector::select(const tms::Identity& id, Sec last_hb)
 {
   ACE_DEBUG((LM_INFO, "(%P|%t) INFO: ControllerSelector::select: \"%C\"\n", id.c_str()));
   selected_ = id;
+  if (new_controller_callback_) {
+    new_controller_callback_(selected_);
+  }
   send_controller_state();
   schedule_once(MissedHeartbeat{}, heartbeat_deadline - last_hb);
 }

tactical-microgrid-standard/common/ControllerSelector.h

@@ -7,6 +7,8 @@
 #include <common/OpenDDS_TMS_export.h>
 
 #include <map>
+#include <tuple>
+#include <set>
 
 struct NewController {
   tms::Identity id;
@@ -25,7 +27,46 @@ struct NoControllers {
   static const char* name() { return "NoControllers"; }
 };
 
-class PowerDevice;
+class OpenDDS_TMS_Export ControllerCallbacks {
+public:
+  using IdCallback = std::function<void(const tms::Identity&)>;
+  using Callback = std::function<void()>;
+
+  explicit ControllerCallbacks(Mutex& lock) : cb_lock_(lock) {}
+
+  void set_new_controller_callback(IdCallback cb)
+  {
+    Guard g(cb_lock_);
+    new_controller_callback_ = cb;
+  }
+
+  void set_missed_heartbeat_callback(IdCallback cb)
+  {
+    Guard g(cb_lock_);
+    missed_heartbeat_callback_ = cb;
+  }
+
+  void set_lost_controller_callback(IdCallback cb)
+  {
+    Guard g(cb_lock_);
+    lost_controller_callback_ = cb;
+  }
+
+  void set_no_controllers_callback(Callback cb)
+  {
+    Guard g(cb_lock_);
+    no_controllers_callback_ = cb;
+  }
+
+protected:
+  IdCallback new_controller_callback_;
+  IdCallback missed_heartbeat_callback_;
+  IdCallback lost_controller_callback_;
+  Callback no_controllers_callback_;
+
+private:
+  Mutex& cb_lock_;
+};
 
 /**
  * Logic for determining what controller should be used, based on TMS A.7.5.
@@ -55,6 +96,7 @@ class PowerDevice;
  * S: If there's a selectable controller with a recent heartbeat
  */
 class OpenDDS_TMS_Export ControllerSelector :
+  public ControllerCallbacks,
   public TimerHandler<NewController, MissedHeartbeat, LostController, NoControllers> {
 public:
   explicit ControllerSelector(const tms::Identity& device_id);
@@ -75,12 +117,6 @@ class OpenDDS_TMS_Export ControllerSelector :
     return selected_ == id;
   }
 
-  ACE_Reactor* get_reactor() const
-  {
-    Guard g(lock_);
-    return reactor_;
-  }
-
   void set_ActiveMicrogridControllerState_writer(tms::ActiveMicrogridControllerStateDataWriter_var amcs_dw)
   {
     amcs_dw_ = amcs_dw;
@@ -107,6 +143,22 @@ class OpenDDS_TMS_Export ControllerSelector :
 
   tms::Identity selected_;
   std::map<tms::Identity, TimePoint> all_controllers_;
+  struct Priority {
+    uint16_t priority;
+    tms::Identity id;
+
+    Priority(const tms::DeviceInfo& di)
+    : priority(di.controlService()->mc()->priorityRanking())
+    , id(di.deviceId())
+    {
+    }
+
+    bool operator<(const Priority& lhs) const
+    {
+      return std::tie(priority, id) < std::tie(lhs.priority, lhs.id);
+    }
+  };
+  std::set<Priority> all_controllers_by_priority_;
 
   // Device ID to which this controller selector belong.
   tms::Identity device_id_;

tactical-microgrid-standard/common/TimerHandler.h

@@ -146,6 +146,12 @@ class TimerHandler : public ACE_Event_Handler, protected TimerHolder<EventTypes>
     }
   }
 
+  ACE_Reactor* get_reactor() const
+  {
+    Guard g(lock_);
+    return reactor_;
+  }
+
   template <typename EventType>
   typename Timer<EventType>::Ptr get_timer(const std::string& name = "")
   {
@@ -244,6 +250,7 @@ class TimerHandler : public ACE_Event_Handler, protected TimerHolder<EventTypes>
   {
     Guard g(lock_);
     auto key = *reinterpret_cast<const std::string*>(arg);
+    ACE_DEBUG((LM_DEBUG, "(%P|%t) TimerHandler::handle_timeout: %C\n", key.c_str()));
     if (active_timers_.count(key) == 0) {
       ACE_ERROR((LM_WARNING, "(%P|%t) WARNING: TimerHandler::handle_timeout: timer key %C does NOT exist\n", key.c_str()));
     }

tactical-microgrid-standard/controller/Controller.cpp

@@ -100,7 +100,7 @@ tms::DeviceInfo Controller::populate_device_info() const
     tms::MicrogridControllerInfo mc_info;
     {
       mc_info.features().push_back(tms::MicrogridControllerFeature::MCF_GENERAL);
-      mc_info.priorityRanking(0);
+      mc_info.priorityRanking(priority_);
     }
     csi.mc() = mc_info;
   }

tactical-microgrid-standard/controller/Controller.h

@@ -6,7 +6,11 @@
 
 class Controller : public Handshaking {
 public:
-  explicit Controller(const tms::Identity& id) : Handshaking(id) {}
+  explicit Controller(const tms::Identity& id, uint16_t priority= 0)
+  : Handshaking(id)
+  , priority_(priority)
+  {
+  }
 
   DDS::ReturnCode_t init(DDS::DomainId_t domain_id, int argc = 0, char* argv[] = nullptr);
   int run();
@@ -28,6 +32,7 @@ class Controller : public Handshaking {
 private:
   mutable std::mutex mut_;
   PowerDevices power_devices_;
+  uint16_t priority_;
 
   DDS::DomainId_t tms_domain_id_ = OpenDDS::DOMAIN_UNKNOWN;;
 };

tactical-microgrid-standard/power_devices/PowerDevice.h

@@ -73,6 +73,11 @@ class PowerSim_Idl_Export PowerDevice : public Handshaking {
     return essl_;
   }
 
+  ControllerCallbacks& controller_callbacks()
+  {
+    return controller_selector_;
+  }
+
 protected:
   virtual int run_i()
   {

tactical-microgrid-standard/tests/mc-sel/CMakeLists.txt

@@ -1,15 +1,15 @@
 cmake_minimum_required(VERSION 3.27)
 
-project(opendds_tms_mc_sel_test CXX)
+project(opendds_tms_mc_sel CXX)
 enable_testing()
 
 find_package(OpenDDS REQUIRED)
 include(opendds_testing)
 
-add_executable(mc-sel-test mc-sel-test.cpp)
-target_link_libraries(mc-sel-test PRIVATE PowerSim_Idl)
+add_executable(basic-dev basic-dev.cpp)
+target_link_libraries(basic-dev PRIVATE PowerSim_Idl)
 
 add_executable(basic-mc ${CMAKE_SOURCE_DIR}/controller/Controller.cpp basic-mc.cpp)
 target_link_libraries(basic-mc PRIVATE Commands_Idl)
 
-opendds_add_test(COMMAND ./mc-sel-test)
+opendds_add_test(ARGS remove_dcs_after=0 test_verbose=1)

tactical-microgrid-standard/tests/mc-sel/basic-dev.cpp

@@ -0,0 +1,35 @@
+#include "common.h"
+
+#include <power_devices/PowerDevice.h>
+
+#include <tests/Utils/DistributedConditionSet.h>
+
+int main(int argc, char* argv[])
+{
+  const std::string name = "dev";
+  PowerDevice pd(name);
+  if (pd.init(domain, argc, argv) != DDS::RETCODE_OK) {
+    return 1;
+  }
+
+  DistributedConditionSet_rch dcs =
+    OpenDDS::DCPS::make_rch<FileBasedDistributedConditionSet>();
+
+  ControllerCallbacks& cbs = pd.controller_callbacks();
+  cbs.set_new_controller_callback([dcs, name](const tms::Identity& id) {
+    printf("new_controller\n");
+    dcs->post(name, "new controller " + id);
+  });
+  cbs.set_missed_heartbeat_callback([dcs, name](const tms::Identity& id) {
+    dcs->post(name, "missed controller " + id);
+  });
+  cbs.set_lost_controller_callback([dcs, name](const tms::Identity& id) {
+    dcs->post(name, "lost controller " + id);
+  });
+  cbs.set_no_controllers_callback([dcs, name]() {
+    dcs->post(name, "no controllers");
+  });
+
+  Exiter exiter(pd);
+  return pd.run();
+}

tactical-microgrid-standard/tests/mc-sel/basic-mc.cpp

@@ -1,46 +1,22 @@
-#include "controller/Controller.h"
+#include "common.h"
 
-struct Timeout {
-  static const char* name() { return "Timeout"; }
-};
-
-class Test : public TimerHandler<Timeout> {
-public:
-  Test()
-  {
-    reactor_->register_handler(SIGINT, this);
-    schedule(Timeout{}, Sec(0), Sec(30));
-  }
-
-private:
-  void timer_fired(Timer<Timeout>& t)
-  {
-    ACE_DEBUG((LM_INFO, "(%P|%t) Timeout\n"));
-    t.exit_after = true;
-  }
-
-  void any_timer_fired(AnyTimer timer)
-  {
-    std::visit([&](auto&& value) { this->timer_fired(*value); }, timer);
-  }
-
-  int handle_signal(int, siginfo_t*, ucontext_t*)
-  {
-    ACE_DEBUG((LM_INFO, "(%P|%t) SIGINT\n"));
-    return end_event_loop();
-  }
-};
+#include <controller/Controller.h>
 
 int main(int argc, char* argv[])
 {
-  const int domain = std::stoi(argv[1]);
-  const std::string device_id = argv[2];
+  if (argc < 4) {
+    ACE_ERROR((LM_ERROR, "(%P|%t) ERROR: requires device id, priority, and time duration arguments\n"));
+    return 1;
+  }
+  const std::string device_id = argv[1];
+  const uint16_t priority = std::stoi(argv[2]);
+  const Sec exit_after(std::stoi(argv[3]));
 
-  Controller mc(device_id);
+  Controller mc(device_id, priority);
   if (mc.init(domain, argc, argv) != DDS::RETCODE_OK) {
     return 1;
   }
 
-  Test test;
+  Exiter exiter(mc, exit_after);
   return mc.run();
 }