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

Add interpolation for CSP commands
nasa-jpl · Jan 5, 2024 · 5424e2e · 5424e2e
2 parents b48af43 + d53b7b1
commit 5424e2e
Show file tree

Hide file tree

Showing 111 changed files with 1,021 additions and 383 deletions.
diff --git a/CMakeLists.txt b/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

diff --git a/src/device_base.cc b/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 */)
 {

diff --git a/src/device_base.h b/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

diff --git a/src/fastcat_devices/commander.h b/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;

diff --git a/src/fastcat_devices/conditional.cc b/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;

diff --git a/src/fastcat_devices/conditional.h b/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:

diff --git a/src/fastcat_devices/faulter.cc b/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;

diff --git a/src/fastcat_devices/faulter.h b/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.

diff --git a/src/fastcat_devices/filter.cc b/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;

diff --git a/src/fastcat_devices/filter.h b/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:

diff --git a/src/fastcat_devices/fts.cc b/src/fastcat_devices/fts.cc
@@ -15,7 +15,7 @@ fastcat::Fts::Fts()
   state_->type = FTS_STATE;
 }
 
-bool fastcat::Fts::ConfigFromYaml(YAML::Node node)
+bool fastcat::Fts::ConfigFromYaml(const YAML::Node& node)
 {
   if (!ParseVal(node, "name", name_)) {
     return false;

diff --git a/src/fastcat_devices/fts.h b/src/fastcat_devices/fts.h
@@ -24,7 +24,7 @@ class Fts : public DeviceBase
            * @param node The portion of the yaml file corresponding to this FTS device.
            * @return True if configuration completes without error; false otherwise.
            */
-  bool ConfigFromYaml(YAML::Node node) override;
+  bool ConfigFromYaml(const YAML::Node& node) override;
   /**
    * @brief Calculates wrench (forces and torques) from input signals.
    * @return True if device state is read without error; false otherwise.

diff --git a/src/fastcat_devices/function.cc b/src/fastcat_devices/function.cc
@@ -35,7 +35,7 @@ fastcat::FunctionType fastcat::FunctionTypeFromString(
   }
 }
 
-bool fastcat::Function::ConfigFromYaml(YAML::Node node)
+bool fastcat::Function::ConfigFromYaml(const YAML::Node& node)
 {
   if (!ParseVal(node, "name", name_)) {
     return false;

diff --git a/src/fastcat_devices/function.h b/src/fastcat_devices/function.h
@@ -37,7 +37,7 @@ class Function : public DeviceBase
 {
  public:
   Function();
-  bool ConfigFromYaml(YAML::Node node) override;
+  bool ConfigFromYaml(const YAML::Node& node) override;
   bool Read() override;
 
  protected:

diff --git a/src/fastcat_devices/linear_interpolation.cc b/src/fastcat_devices/linear_interpolation.cc
@@ -17,7 +17,7 @@ fastcat::LinearInterpolation::LinearInterpolation()
   state_->type = LINEAR_INTERPOLATION_STATE;
 }
 
-bool fastcat::LinearInterpolation::ConfigFromYaml(YAML::Node node)
+bool fastcat::LinearInterpolation::ConfigFromYaml(const YAML::Node& node)
 {
   if (!ParseVal(node, "name", name_)) {
     return false;

diff --git a/src/fastcat_devices/linear_interpolation.h b/src/fastcat_devices/linear_interpolation.h
@@ -15,7 +15,7 @@ class LinearInterpolation : public DeviceBase
 {
  public:
   LinearInterpolation();
-  bool ConfigFromYaml(YAML::Node node) override;
+  bool ConfigFromYaml(const YAML::Node& node) override;
   bool Read() override;
 
  protected:

diff --git a/src/fastcat_devices/pid.cc b/src/fastcat_devices/pid.cc
@@ -15,7 +15,7 @@ fastcat::Pid::Pid()
   state_->type = PID_STATE;
 }
 
-bool fastcat::Pid::ConfigFromYaml(YAML::Node node)
+bool fastcat::Pid::ConfigFromYaml(const YAML::Node& node)
 {
   if (!ParseVal(node, "name", name_)) {
     return false;

diff --git a/src/fastcat_devices/pid.h b/src/fastcat_devices/pid.h
@@ -14,7 +14,7 @@ class Pid : public DeviceBase
 {
  public:
   Pid();
-  bool ConfigFromYaml(YAML::Node node) override;
+  bool ConfigFromYaml(const YAML::Node& node) override;
   bool Read() override;
   bool Write(DeviceCmd& cmd) override;
   void Fault() override;

diff --git a/src/fastcat_devices/saturation.cc b/src/fastcat_devices/saturation.cc
@@ -15,7 +15,7 @@ fastcat::Saturation::Saturation()
   state_->type = SATURATION_STATE;
 }
 
-bool fastcat::Saturation::ConfigFromYaml(YAML::Node node)
+bool fastcat::Saturation::ConfigFromYaml(const YAML::Node& node)
 {
   if (!ParseVal(node, "name", name_)) {
     return false;

diff --git a/src/fastcat_devices/saturation.h b/src/fastcat_devices/saturation.h
@@ -14,7 +14,7 @@ class Saturation : public DeviceBase
 {
  public:
   Saturation();
-  bool ConfigFromYaml(YAML::Node node) override;
+  bool ConfigFromYaml(const YAML::Node& node) override;
   bool Read() override;
 
  protected:

diff --git a/src/fastcat_devices/schmitt_trigger.cc b/src/fastcat_devices/schmitt_trigger.cc
@@ -15,7 +15,7 @@ fastcat::SchmittTrigger::SchmittTrigger()
   state_->type = SCHMITT_TRIGGER_STATE;
 }
 
-bool fastcat::SchmittTrigger::ConfigFromYaml(YAML::Node node)
+bool fastcat::SchmittTrigger::ConfigFromYaml(const YAML::Node& node)
 {
   if (!ParseVal(node, "name", name_)) {
     return false;

diff --git a/src/fastcat_devices/schmitt_trigger.h b/src/fastcat_devices/schmitt_trigger.h
@@ -14,7 +14,7 @@ class SchmittTrigger : public DeviceBase
 {
  public:
   SchmittTrigger();
-  bool ConfigFromYaml(YAML::Node node) override;
+  bool ConfigFromYaml(const YAML::Node& node) override;
   bool Read() override;
 
  protected:

diff --git a/src/fastcat_devices/signal_generator.cc b/src/fastcat_devices/signal_generator.cc
@@ -33,7 +33,7 @@ fastcat::SignalGenerator::SignalGenerator()
   state_->type = SIGNAL_GENERATOR_STATE;
 }
 
-bool fastcat::SignalGenerator::ConfigFromYaml(YAML::Node node)
+bool fastcat::SignalGenerator::ConfigFromYaml(const YAML::Node& node)
 {
   if (!ParseVal(node, "name", name_)) {
     return false;

diff --git a/src/fastcat_devices/signal_generator.h b/src/fastcat_devices/signal_generator.h
@@ -55,7 +55,7 @@ class SignalGenerator : public DeviceBase
 {
  public:
   SignalGenerator();
-  bool ConfigFromYaml(YAML::Node node) override;
+  bool ConfigFromYaml(const YAML::Node& node) override;
   bool Read() override;
 
  protected:

diff --git a/src/fastcat_devices/three_node_thermal_model.cc b/src/fastcat_devices/three_node_thermal_model.cc
@@ -10,7 +10,7 @@ ThreeNodeThermalModel::ThreeNodeThermalModel()
   last_time_   = jsd_time_get_time_sec();  // init time
 }
 
-bool ThreeNodeThermalModel::ConfigFromYaml(YAML::Node node)
+bool ThreeNodeThermalModel::ConfigFromYaml(const YAML::Node& node)
 {
   if (!ParseVal(node, "name", name_)) {
     return false;

diff --git a/src/fastcat_devices/three_node_thermal_model.h b/src/fastcat_devices/three_node_thermal_model.h
@@ -30,7 +30,7 @@ class ThreeNodeThermalModel : public DeviceBase
    * @param node The portion of the yaml file corresponding to this device.
    * @return True if configuration completes without error; false otherwise.
    */
-  bool ConfigFromYaml(YAML::Node node) override;
+  bool ConfigFromYaml(const YAML::Node& node) override;
 
   /**
    * @brief Reads in most recent temperature and current signal values, and

diff --git a/src/fastcat_devices/virtual_fts.cc b/src/fastcat_devices/virtual_fts.cc
@@ -15,7 +15,7 @@ fastcat::VirtualFts::VirtualFts()
   state_->type = FTS_STATE;
 }
 
-bool fastcat::VirtualFts::ConfigFromYaml(YAML::Node node)
+bool fastcat::VirtualFts::ConfigFromYaml(const YAML::Node& node)
 {
   if (!ParseVal(node, "name", name_)) {
     return false;

diff --git a/src/fastcat_devices/virtual_fts.h b/src/fastcat_devices/virtual_fts.h
@@ -27,7 +27,7 @@ class VirtualFts : public Fts
    * device.
    * @return True if configuration completes without error; false otherwise.
    */
-  bool ConfigFromYaml(YAML::Node node);
+  bool ConfigFromYaml(const YAML::Node& node);
   /**
    * @brief Calculates wrench (forces and torques) from input signals.
    * @return True if device state is read without error; false otherwise.

diff --git a/src/fcgen/commander.cc.cog b/src/fcgen/commander.cc.cog
@@ -37,7 +37,7 @@ fastcat::Commander::Commander()
 
 }
 
-bool fastcat::Commander::ConfigFromYaml(YAML::Node node)
+bool fastcat::Commander::ConfigFromYaml(const YAML::Node& node)
 {
   if (!ParseVal(node, "name", name_)) {
     return false;

diff --git a/src/fcgen/fastcat_types.yaml b/src/fcgen/fastcat_types.yaml
@@ -593,7 +593,7 @@ commands:
       type: double
 
   - name: egd_csv
-    fields: 
+    fields:
     - name: target_velocity
       type: int32_t
     - name: velocity_offset
@@ -775,12 +775,20 @@ commands:
 
   - name: actuator_csp
     fields: 
+    - name: request_time
+      type: double
+    - name: receipt_stamp_time
+      type: double
     - name: target_position
       type: double
     - name: position_offset
       type: double
     - name: velocity_offset
       type: double
+    - name: acceleration_offset
+      type: double
+    - name: interpolation_mode
+      type: int16_t
     - name: torque_offset_amps
       type: double
 
@@ -964,4 +972,4 @@ commands:
     fields:
     - name: seed_temperature
       type: double
-
+
diff --git a/src/fcgen/signal_handling.cc.cog b/src/fcgen/signal_handling.cc.cog
@@ -65,7 +65,7 @@ for (native, enum) in dtype_native_enum:
   return true;
 }
 
-bool fastcat::ConfigSignalsFromYaml(YAML::Node node,
+bool fastcat::ConfigSignalsFromYaml(const YAML::Node& node,
                                     std::vector<fastcat::Signal> &signals, 
                                     bool is_commander) {
   YAML::Node signals_node;

diff --git a/src/fcgen/types.h.cog b/src/fcgen/types.h.cog
@@ -115,6 +115,7 @@ for state in data['states']:
 
   };
   double time;
+  double monotonic_time;
 } DeviceState;
-Original file line number
+Diff line change
@@ Expand Up / @@ -115,6 +115,7 @@ for state in data['states']: @@
       };
       double time;
+      double monotonic_time;
     } DeviceState;
@@ Expand Down @@