Merge pull request #118 from nasa-jpl/kwehage-interp-csp

Add interpolation for CSP commands

Author: kwehage

CMakeLists.txt

@@ -1,7 +1,7 @@
 cmake_minimum_required (VERSION 3.11)
 project(fastcat
     DESCRIPTION "C++ EtherCAT Device Command & Control Library"
-    VERSION 0.12.11
+    VERSION 0.13.0
     LANGUAGES C CXX
     )
 
@@ -43,6 +43,7 @@ endif()
 
 # @TODO(kwehage,abrinkma) Clean up missing-field-initializers warnings
 add_definitions(
+    -Werror=all
     -Wall
     -Wextra
     -Wno-missing-field-initializers

src/device_base.cc

@@ -9,7 +9,7 @@
 fastcat::DeviceBase::~DeviceBase() {}
 
 void fastcat::DeviceBase::RegisterCmdQueue(
-    std::shared_ptr<std::queue<DeviceCmd>> cmd_queue)
+    std::shared_ptr<fastcat::ThreadSafeQueue<DeviceCmd>> cmd_queue)
 {
   cmd_queue_ = cmd_queue;
 }
@@ -25,7 +25,11 @@ void fastcat::DeviceBase::SetLoopPeriod(double loop_period)
   loop_period_ = loop_period;
 }
 
-void fastcat::DeviceBase::SetTime(double time) { state_->time = time; }
+void fastcat::DeviceBase::SetTime(double time, double monotonic_time)
+{
+  state_->time           = time;
+  state_->monotonic_time = monotonic_time;
+}
 
 bool fastcat::DeviceBase::Write(fastcat::DeviceCmd& /* cmd */)
 {

src/device_base.h

@@ -12,6 +12,7 @@
 #include <yaml-cpp/yaml.h>
 
 #include "fastcat/types.h"
+#include "fastcat/thread_safe_queue.h"
 
 namespace fastcat
 {
@@ -20,36 +21,44 @@ class DeviceBase
  public:
   virtual ~DeviceBase();
   // Pure virtual methods
-  virtual bool ConfigFromYaml(YAML::Node node) = 0;
-  virtual bool Read()                          = 0;
+  virtual bool ConfigFromYaml(const YAML::Node& node) = 0;
+  virtual bool Read()                                 = 0;
 
   // Non-pure virtual methods with default implementation
   virtual FaultType Process();
   virtual bool      Write(DeviceCmd& cmd);
   virtual void      Fault();
   virtual void      Reset();
+  virtual void SetInitializationTime(double time_sec, double monotonic_time_sec)
+  {
+    initialization_time_sec_           = time_sec;
+    monotonic_initialization_time_sec_ = monotonic_time_sec;
+  }
 
   // non-virtual methods
-  void RegisterCmdQueue(std::shared_ptr<std::queue<DeviceCmd>> cmd_queue);
+  void RegisterCmdQueue(std::shared_ptr<ThreadSafeQueue<DeviceCmd>> cmd_queue);
   std::string                  GetName();
   std::shared_ptr<DeviceState> GetState();
 
-  void SetTime(double time);
+  void SetTime(double time, double monotonic_time);
   void SetLoopPeriod(double loop_period);
 
   std::vector<Signal> signals_;
 
  protected:
-  std::string name_;               ///< unique device name
-  double      loop_period_ = 0.0;  ///< only some devices need
+  std::string name_;  ///< unique device name
+
+  double loop_period_                       = 0.0;  ///< only some devices need
+  double initialization_time_sec_           = -1;   ///< only some devices need
+  double monotonic_initialization_time_sec_ = -1;   ///< only some devices need
 
   /// device-level fault, manager also has fault status flag
   bool device_fault_active_ = false;
 
   std::shared_ptr<DeviceState> state_;  ///< Fastcat state data
 
   /// for intra-device commands
-  std::shared_ptr<std::queue<DeviceCmd>> cmd_queue_;
+  std::shared_ptr<ThreadSafeQueue<DeviceCmd>> cmd_queue_;
 };
 
 }  // namespace fastcat

src/fastcat_devices/commander.h

@@ -14,7 +14,7 @@ class Commander : public DeviceBase
 {
  public:
   Commander();
-  bool ConfigFromYaml(YAML::Node node) override;
+  bool ConfigFromYaml(const YAML::Node& node) override;
   bool Read() override;
   bool Write(DeviceCmd& cmd) override;
   void Fault() override;

src/fastcat_devices/conditional.cc

@@ -41,7 +41,7 @@ fastcat::Conditional::Conditional()
   state_->type = CONDITIONAL_STATE;
 }
 
-bool fastcat::Conditional::ConfigFromYaml(YAML::Node node)
+bool fastcat::Conditional::ConfigFromYaml(const YAML::Node& node)
 {
   if (!ParseVal(node, "name", name_)) {
     return false;

src/fastcat_devices/conditional.h

@@ -26,7 +26,7 @@ class Conditional : public DeviceBase
 {
  public:
   Conditional();
-  bool ConfigFromYaml(YAML::Node node) override;
+  bool ConfigFromYaml(const YAML::Node& node) override;
   bool Read() override;
 
  protected:

src/fastcat_devices/faulter.cc

@@ -15,7 +15,7 @@ fastcat::Faulter::Faulter()
   state_->type = FAULTER_STATE;
 }
 
-bool fastcat::Faulter::ConfigFromYaml(YAML::Node node)
+bool fastcat::Faulter::ConfigFromYaml(const YAML::Node& node)
 {
   if (!ParseVal(node, "name", name_)) {
     return false;

src/fastcat_devices/faulter.h

@@ -24,7 +24,7 @@ class Faulter : public DeviceBase
    * device.
    * @return True if configuration completes without error; false otherwise.
    */
-  bool ConfigFromYaml(YAML::Node node) override;
+  bool ConfigFromYaml(const YAML::Node& node) override;
   /**
    * @brief Updates device state.
    * @return True if device state is read without error; false otherwise.

src/fastcat_devices/filter.cc

@@ -101,7 +101,7 @@ fastcat::Filter::Filter()
   state_->type = FILTER_STATE;
 }
 
-bool fastcat::Filter::ConfigFromYaml(YAML::Node node)
+bool fastcat::Filter::ConfigFromYaml(const YAML::Node& node)
 {
   if (!ParseVal(node, "name", name_)) {
     return false;

src/fastcat_devices/filter.h

@@ -43,7 +43,7 @@ class Filter : public DeviceBase
 {
  public:
   Filter();
-  bool ConfigFromYaml(YAML::Node node) override;
+  bool ConfigFromYaml(const YAML::Node& node) override;
   bool Read() override;
 
  protected: