Update support for start/stop power devices

tactical-microgrid-standard/CMakeLists.txt

@@ -37,6 +37,7 @@ target_link_libraries(Commands_Idl PUBLIC TMS_Common)
 add_library(PowerSim_Idl
   power_devices/PowerDevice.cpp
   power_devices/PowerConnectionDataReaderListenerImpl.cpp
+  power_devices/EnergyStartStopRequestDataReaderListenerImpl.cpp
 )
 opendds_export_header(PowerSim_Idl)
 opendds_target_sources(PowerSim_Idl

tactical-microgrid-standard/power_devices/Distribution.cpp

@@ -152,6 +152,11 @@ void ElectricCurrentDataReaderListenerImpl::on_data_available(DDS::DataReader_pt
     return;
   }
 
+  // Simulate the non-operational mode by ignoring the simulated current messages
+  if (dist_dev_.energy_level() != tms::EnergyStartStopLevel::ESSL_OPERATIONAL) {
+    return;
+  }
+
   const powersim::ConnectedDeviceSeq& connected_devices_in = dist_dev_.connected_devices_in();
   const powersim::ConnectedDeviceSeq& connected_devices_out = dist_dev_.connected_devices_out();
 

tactical-microgrid-standard/power_devices/EnergyStartStopRequestDataReaderListenerImpl.cpp

@@ -0,0 +1,53 @@
+#include "EnergyStartStopRequestDataReaderListenerImpl.h"
+#include "PowerDevice.h"
+
+#include <common/mil-std-3071_data_modelTypeSupportImpl.h>
+
+
+void EnergyStartStopRequestDataReaderListenerImpl::on_data_available(DDS::DataReader_ptr reader)
+{
+  tms::EnergyStartStopRequestSeq data;
+  DDS::SampleInfoSeq info_seq;
+  tms::EnergyStartStopRequestDataReader_var typed_reader = tms::EnergyStartStopRequestDataReader::_narrow(reader);
+  DDS::ReturnCode_t rc = typed_reader->take(data, info_seq, DDS::LENGTH_UNLIMITED,
+                                            DDS::ANY_SAMPLE_STATE, DDS::ANY_VIEW_STATE, DDS::ANY_INSTANCE_STATE);
+  if (rc != DDS::RETCODE_OK) {
+    ACE_ERROR((LM_WARNING, "(%P|%t) WARNING: EnergyStartStopRequestDataReaderListenerImpl::on_data_available: "
+               "take data failed: %C\n", OpenDDS::DCPS::retcode_to_string(rc)));
+    return;
+  }
+
+  for (CORBA::ULong i = 0; i < data.length(); ++i) {
+    if (info_seq[i].valid_data) {
+      const tms::EnergyStartStopRequest& essr = data[i];
+      const tms::Identity& sending_mc_id = essr.requestId().requestingDeviceId();
+      if (sending_mc_id != power_device_.selected()) {
+        // Ignore request from non-selected controller
+        continue;
+      }
+
+      const tms::Identity& target_id = essr.requestId().targetDeviceId();
+      if (target_id == power_device_.get_device_id()) {
+        // Always set to the requested level and send an OK reply
+        const tms::EnergyStartStopLevel essl = essr.toLevel();
+        power_device_.energy_level(essl);
+
+        tms::Reply reply;
+        reply.requestingDeviceId() = sending_mc_id;
+        reply.targetDeviceId() = power_device_.get_device_id();
+        reply.config() = essr.requestId().config();
+        reply.portNumber() = tms::INVALID_PORT_NUMBER;
+        reply.requestSequenceId() = essr.sequenceId();
+        reply.status().code() = tms::ReplyCode::REPLY_OK;
+        reply.status().reason() = "OK";
+
+        const DDS::ReturnCode_t rc = power_device_.reply_dw()->write(reply, DDS::HANDLE_NIL);
+        if (rc != DDS::RETCODE_OK) {
+          ACE_ERROR((LM_WARNING, "(%P|%t) WARNING: EnergyStartStopRequestDataReaderListenerImpl::on_data_available: "
+                     "write reply failed: %C\n", OpenDDS::DCPS::retcode_to_string(rc)));
+        }
+        break;
+      }
+    }
+  }
+}

tactical-microgrid-standard/power_devices/EnergyStartStopRequestDataReaderListenerImpl.h

@@ -0,0 +1,22 @@
+#ifndef ENERGY_START_STOP_REQUEST_DATA_READER_LISTENER_IMPL_H
+#define ENERGY_START_STOP_REQUEST_DATA_READER_LISTENER_IMPL_H
+
+#include "common/DataReaderListenerBase.h"
+
+class PowerDevice;
+
+class EnergyStartStopRequestDataReaderListenerImpl : public DataReaderListenerBase {
+public:
+  explicit EnergyStartStopRequestDataReaderListenerImpl(PowerDevice& pwr_dev)
+    : DataReaderListenerBase("tms::EnergyStartStopRequest - DataReaderListenerImpl")
+    , power_device_(pwr_dev) {}
+
+  virtual ~EnergyStartStopRequestDataReaderListenerImpl() = default;
+
+  void on_data_available(DDS::DataReader_ptr reader) final;
+
+private:
+  PowerDevice& power_device_;
+};
+
+#endif

tactical-microgrid-standard/power_devices/Load.cpp

@@ -109,6 +109,8 @@ class LoadDevice : public PowerDevice {
     device_info.powerDevice() = pdi;
     return device_info;
   }
+
+  tms::EnergyStartStopLevel essl_ = tms::EnergyStartStopLevel::ESSL_OPERATIONAL;
 };
 
 void ElectricCurrentDataReaderListenerImpl::on_data_available(DDS::DataReader_ptr reader)
@@ -124,6 +126,11 @@ void ElectricCurrentDataReaderListenerImpl::on_data_available(DDS::DataReader_pt
     return;
   }
 
+  // Simulate the non-operational mode by ignoring the simulated current messages
+  if (load_dev_.energy_level() != tms::EnergyStartStopLevel::ESSL_OPERATIONAL) {
+    return;
+  }
+
   for (CORBA::ULong i = 0; i < data.length(); ++i) {
     if (info_seq[i].valid_data) {
       const powersim::ElectricCurrent& ec = data[i];

tactical-microgrid-standard/power_devices/PowerDevice.cpp

@@ -1,5 +1,6 @@
 #include "PowerDevice.h"
 #include "PowerConnectionDataReaderListenerImpl.h"
+#include "EnergyStartStopRequestDataReaderListenerImpl.h"
 #include "common/Utils.h"
 #include "common/QosHelper.h"
 
@@ -26,6 +27,91 @@ DDS::ReturnCode_t PowerDevice::init(DDS::DomainId_t domain, int argc, char* argv
 
   DDS::DomainParticipant_var dp = get_domain_participant();
 
+  // Subscribe to tms::EnergyStartStopRequest topic
+  tms::EnergyStartStopRequestTypeSupport_var essr_ts = new tms::EnergyStartStopRequestTypeSupportImpl;
+  if (DDS::RETCODE_OK != essr_ts->register_type(dp, "")) {
+    ACE_ERROR((LM_ERROR, "(%P|%t) ERROR: SourceDevice::init: register_type EnergyStartStopRequest failed\n"));
+    return DDS::RETCODE_ERROR;
+  }
+
+  CORBA::String_var essr_type_name = essr_ts->get_type_name();
+  DDS::Topic_var essr_topic = dp->create_topic(tms::topic::TOPIC_ENERGY_START_STOP_REQUEST.c_str(),
+                                               essr_type_name,
+                                               TOPIC_QOS_DEFAULT,
+                                               nullptr,
+                                               ::OpenDDS::DCPS::DEFAULT_STATUS_MASK);
+  if (!essr_topic) {
+    ACE_ERROR((LM_ERROR, "(%P|%t) ERROR: SourceDevice::init: create_topic \"%C\" failed\n",
+               tms::topic::TOPIC_ENERGY_START_STOP_REQUEST.c_str()));
+    return DDS::RETCODE_ERROR;
+  }
+
+  const DDS::SubscriberQos tms_sub_qos = Qos::Subscriber::get_qos();
+  DDS::Subscriber_var tms_sub = dp->create_subscriber(tms_sub_qos,
+                                                      nullptr,
+                                                      ::OpenDDS::DCPS::DEFAULT_STATUS_MASK);
+  if (!tms_sub) {
+    ACE_ERROR((LM_ERROR, "(%P|%t) ERROR: SourceDevice::init: create_subscriber with TMS QoS failed\n"));
+    return DDS::RETCODE_ERROR;
+  }
+
+  const DDS::DataReaderQos& essr_dr_qos = Qos::DataReader::fn_map.at(tms::topic::TOPIC_ENERGY_START_STOP_REQUEST)(device_id_);
+  DDS::DataReaderListener_var essr_dr_listener(new EnergyStartStopRequestDataReaderListenerImpl(*this));
+  DDS::DataReader_var essr_dr_base = tms_sub->create_datareader(essr_topic,
+                                                                essr_dr_qos,
+                                                                essr_dr_listener,
+                                                                ::OpenDDS::DCPS::DEFAULT_STATUS_MASK);
+  if (!essr_dr_base) {
+    ACE_ERROR((LM_ERROR, "(%P|%t) ERROR: SourceDevice::init: create_datareader for topic \"%C\" failed\n",
+               tms::topic::TOPIC_ENERGY_START_STOP_REQUEST.c_str()));
+    return DDS::RETCODE_ERROR;
+  }
+
+  // Publish to tms::Reply topic
+  tms::ReplyTypeSupport_var reply_ts = new tms::ReplyTypeSupportImpl;
+  if (DDS::RETCODE_OK != reply_ts->register_type(dp, "")) {
+    ACE_ERROR((LM_ERROR, "(%P|%t) ERROR: SourceDevice::init: register_type tms::Reply failed\n"));
+    return DDS::RETCODE_ERROR;
+  }
+
+  CORBA::String_var reply_type_name = reply_ts->get_type_name();
+  DDS::Topic_var reply_topic = dp->create_topic(tms::topic::TOPIC_REPLY.c_str(),
+                                                reply_type_name,
+                                                TOPIC_QOS_DEFAULT,
+                                                nullptr,
+                                                ::OpenDDS::DCPS::DEFAULT_STATUS_MASK);
+  if (!reply_topic) {
+    ACE_ERROR((LM_ERROR, "(%P|%t) ERROR: SourceDevice::init: create_topic \"%C\" failed\n",
+               tms::topic::TOPIC_REPLY.c_str()));
+    return DDS::RETCODE_ERROR;
+  }
+
+  const DDS::PublisherQos tms_pub_qos = Qos::Publisher::get_qos();
+  DDS::Publisher_var tms_pub = dp->create_publisher(tms_pub_qos,
+                                                    nullptr,
+                                                    ::OpenDDS::DCPS::DEFAULT_STATUS_MASK);
+  if (!tms_pub) {
+    ACE_ERROR((LM_ERROR, "(%P|%t) ERROR: SourceDevice::init: create_publisher with TMS QoS failed\n"));
+    return DDS::RETCODE_ERROR;
+  }
+
+  const DDS::DataWriterQos& reply_dw_qos = Qos::DataWriter::fn_map.at(tms::topic::TOPIC_REPLY)(device_id_);
+  DDS::DataWriter_var reply_dw_base = tms_pub->create_datawriter(reply_topic,
+                                                                 reply_dw_qos,
+                                                                 nullptr,
+                                                                 ::OpenDDS::DCPS::DEFAULT_STATUS_MASK);
+  if (!reply_dw_base) {
+    ACE_ERROR((LM_ERROR, "(%P|%t) ERROR: SourceDevice::init: create_datawriter for topic \"%C\" failed\n",
+               tms::topic::TOPIC_REPLY.c_str()));
+    return DDS::RETCODE_ERROR;
+  }
+
+  reply_dw_ = tms::ReplyDataWriter::_narrow(reply_dw_base);
+  if (!reply_dw_) {
+    ACE_ERROR((LM_ERROR, "(%P|%t) ERROR: SourceDevice::init: ReplyDataWriter narrow failed\n"));
+    return DDS::RETCODE_ERROR;
+  }
+
   // Publish to the tms::ActiveMicrogridControllerState topic
   tms::ActiveMicrogridControllerStateTypeSupport_var amcs_ts = new tms::ActiveMicrogridControllerStateTypeSupportImpl();
   rc = amcs_ts->register_type(participant_, "");
@@ -46,15 +132,6 @@ DDS::ReturnCode_t PowerDevice::init(DDS::DomainId_t domain, int argc, char* argv
     return DDS::RETCODE_ERROR;
   }
 
-  const DDS::PublisherQos tms_pub_qos = Qos::Publisher::get_qos();
-  DDS::Publisher_var tms_pub = dp->create_publisher(tms_pub_qos,
-                                                    nullptr,
-                                                    ::OpenDDS::DCPS::DEFAULT_STATUS_MASK);
-  if (!tms_pub) {
-    ACE_ERROR((LM_ERROR, "(%P|%t) ERROR: PowerDevice::init: create_publisher with TMS QoS failed\n"));
-    return DDS::RETCODE_ERROR;
-  }
-
   const DDS::DataWriterQos& amcs_dw_qos = Qos::DataWriter::fn_map.at(tms::topic::TOPIC_ACTIVE_MICROGRID_CONTROLLER_STATE)(device_id_);
   DDS::DataWriter_var amcs_dw_base = tms_pub->create_datawriter(amcs_topic,
                                                              amcs_dw_qos,
@@ -74,7 +151,7 @@ DDS::ReturnCode_t PowerDevice::init(DDS::DomainId_t domain, int argc, char* argv
 
   controller_selector_.set_ActiveMicrogridControllerState_writer(amcs_dw);
 
-  // Subscribe to the PowerConnection topic
+  // Subscribe to the powersim::PowerConnection topic
   const DDS::DomainId_t sim_domain_id = Utils::get_sim_domain_id(domain);
   sim_participant_ = get_participant_factory()->create_participant(sim_domain_id,
                                                                    PARTICIPANT_QOS_DEFAULT,

tactical-microgrid-standard/power_devices/PowerDevice.h

@@ -54,6 +54,25 @@ class PowerSim_Idl_Export PowerDevice : public Handshaking {
     return verbose_;
   }
 
+  tms::ReplyDataWriter_var reply_dw() const
+  {
+    return reply_dw_;
+  }
+
+  // Each concrete power device overrides this method, if necessary, to handle
+  // the change in energy level for that particular power device.
+  virtual void energy_level(tms::EnergyStartStopLevel essl)
+  {
+    std::lock_guard<std::mutex> guard(essl_m_);
+    essl_ = essl;
+  }
+
+  tms::EnergyStartStopLevel energy_level() const
+  {
+    std::lock_guard<std::mutex> guard(essl_m_);
+    return essl_;
+  }
+
 protected:
   virtual int run_i()
   {
@@ -85,6 +104,12 @@ class PowerSim_Idl_Export PowerDevice : public Handshaking {
   // Participant containing entities for simulation topics
   DDS::DomainParticipant_var sim_participant_;
 
+  // Data writer for sending tms::Reply. Used for tms::EnergyStartStopRequest, for example.
+  tms::ReplyDataWriter_var reply_dw_;
+
+  mutable std::mutex essl_m_;
+  tms::EnergyStartStopLevel essl_ = tms::EnergyStartStopLevel::ESSL_OPERATIONAL;
+
   // Whether to print power simulation messages
   bool verbose_;