From 5a5d00e4e50bf81bcb2932386850724bad31e966 Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Thu, 9 Feb 2023 17:11:23 +0100
Subject: [PATCH] dataPool for setting RW speeds

---
 mission/devices/RwHandler.cpp                 |  2 +
 mission/devices/RwHandler.h                   |  2 +
 .../devices/devicedefinitions/RwDefinitions.h | 54 +++++++++++++++----
 3 files changed, 47 insertions(+), 11 deletions(-)

diff --git a/mission/devices/RwHandler.cpp b/mission/devices/RwHandler.cpp
index 732bb9fe..d859f9d0 100644
--- a/mission/devices/RwHandler.cpp
+++ b/mission/devices/RwHandler.cpp
@@ -243,6 +243,8 @@ uint32_t RwHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { retur
 
 ReturnValue_t RwHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
                                                  LocalDataPoolManager& poolManager) {
+  localDataPoolMap.emplace(RwDefinitions::RW_SPEED, &rwSpeed);
+
   localDataPoolMap.emplace(RwDefinitions::TEMPERATURE_C, new PoolEntry<int32_t>({0}));
 
   localDataPoolMap.emplace(RwDefinitions::CURR_SPEED, new PoolEntry<int32_t>({0}));
diff --git a/mission/devices/RwHandler.h b/mission/devices/RwHandler.h
index f9f66bd5..43deb579 100644
--- a/mission/devices/RwHandler.h
+++ b/mission/devices/RwHandler.h
@@ -96,6 +96,8 @@ class RwHandler : public DeviceHandlerBase {
 
   uint8_t commandBuffer[RwDefinitions::MAX_CMD_SIZE];
 
+  PoolEntry<int32_t> rwSpeed = PoolEntry<int32_t>(0, false);
+
   enum class InternalState { GET_RESET_STATUS, CLEAR_RESET_STATUS, READ_TEMPERATURE, GET_RW_SATUS };
 
   InternalState internalState = InternalState::GET_RESET_STATUS;
diff --git a/mission/devices/devicedefinitions/RwDefinitions.h b/mission/devices/devicedefinitions/RwDefinitions.h
index 2923b41b..4016b23c 100644
--- a/mission/devices/devicedefinitions/RwDefinitions.h
+++ b/mission/devices/devicedefinitions/RwDefinitions.h
@@ -51,7 +51,9 @@ enum PoolIds : lp_id_t {
   SPI_BYTES_WRITTEN,
   SPI_BYTES_READ,
   SPI_REG_OVERRUN_ERRORS,
-  SPI_TOTAL_ERRORS
+  SPI_TOTAL_ERRORS,
+
+  RW_SPEED,
 };
 
 enum States : uint8_t { STATE_ERROR, IDLE, COASTING, RUNNING_SPEED_STABLE, RUNNING_SPEED_CHANGING };
@@ -75,10 +77,13 @@ static const DeviceCommandId_t SET_SPEED = 6;
 static const DeviceCommandId_t GET_TEMPERATURE = 8;
 static const DeviceCommandId_t GET_TM = 9;
 
-static const uint32_t TEMPERATURE_SET_ID = GET_TEMPERATURE;
-static const uint32_t STATUS_SET_ID = GET_RW_STATUS;
-static const uint32_t LAST_RESET_ID = GET_LAST_RESET_STATUS;
-static const uint32_t TM_SET_ID = GET_TM;
+enum SetIds : uint32_t {
+  TEMPERATURE_SET_ID = GET_TEMPERATURE,
+  STATUS_SET_ID = GET_RW_STATUS,
+  LAST_RESET_ID = GET_LAST_RESET_STATUS,
+  TM_SET_ID = GET_TM,
+  RW_SPEED = 10,
+};
 
 static const size_t SIZE_GET_RESET_STATUS = 5;
 static const size_t SIZE_CLEAR_RESET_STATUS = 4;
@@ -106,9 +111,11 @@ static const uint8_t TM_SET_ENTRIES = 24;
  */
 class StatusSet : public StaticLocalDataSet<STATUS_SET_ENTRIES> {
  public:
-  StatusSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, STATUS_SET_ID) {}
+  StatusSet(HasLocalDataPoolIF* owner)
+      : StaticLocalDataSet(owner, RwDefinitions::SetIds::STATUS_SET_ID) {}
 
-  StatusSet(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, STATUS_SET_ID)) {}
+  StatusSet(object_id_t objectId)
+      : StaticLocalDataSet(sid_t(objectId, RwDefinitions::SetIds::STATUS_SET_ID)) {}
 
   lp_var_t<int32_t> temperatureCelcius =
       lp_var_t<int32_t>(sid.objectId, PoolIds::TEMPERATURE_C, this);
@@ -124,9 +131,11 @@ class StatusSet : public StaticLocalDataSet<STATUS_SET_ENTRIES> {
  */
 class LastResetSatus : public StaticLocalDataSet<LAST_RESET_ENTRIES> {
  public:
-  LastResetSatus(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, LAST_RESET_ID) {}
+  LastResetSatus(HasLocalDataPoolIF* owner)
+      : StaticLocalDataSet(owner, RwDefinitions::SetIds::LAST_RESET_ID) {}
 
-  LastResetSatus(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, LAST_RESET_ID)) {}
+  LastResetSatus(object_id_t objectId)
+      : StaticLocalDataSet(sid_t(objectId, RwDefinitions::SetIds::LAST_RESET_ID)) {}
 
   // If a reset occurs, the status code will be cached into this variable
   lp_var_t<uint8_t> lastNonClearedResetStatus =
@@ -143,9 +152,11 @@ class LastResetSatus : public StaticLocalDataSet<LAST_RESET_ENTRIES> {
  */
 class TmDataset : public StaticLocalDataSet<TM_SET_ENTRIES> {
  public:
-  TmDataset(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, TM_SET_ID) {}
+  TmDataset(HasLocalDataPoolIF* owner)
+      : StaticLocalDataSet(owner, RwDefinitions::SetIds::TM_SET_ID) {}
 
-  TmDataset(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, TM_SET_ID)) {}
+  TmDataset(object_id_t objectId)
+      : StaticLocalDataSet(sid_t(objectId, RwDefinitions::SetIds::TM_SET_ID)) {}
 
   lp_var_t<uint8_t> lastResetStatus =
       lp_var_t<uint8_t>(sid.objectId, PoolIds::TM_LAST_RESET_STATUS, this);
@@ -192,6 +203,27 @@ class TmDataset : public StaticLocalDataSet<TM_SET_ENTRIES> {
       lp_var_t<uint32_t>(sid.objectId, PoolIds::SPI_TOTAL_ERRORS, this);
 };
 
+class RwSpeedActuationSet : StaticLocalDataSet<4> {
+  friend class ::RwHandler;
+
+ public:
+  RwSpeedActuationSet(HasLocalDataPoolIF& owner)
+      : StaticLocalDataSet(&owner, RwDefinitions::SetIds::RW_SPEED) {}
+  RwSpeedActuationSet(object_id_t objectId)
+      : StaticLocalDataSet(sid_t(objectId, RwDefinitions::SetIds::RW_SPEED)) {}
+
+  void setRwSpeed(int32_t rwSpeed_) {
+    if (rwSpeed.value != rwSpeed_) {
+    }
+    rwSpeed = rwSpeed_;
+  }
+
+  void getRwSpeed(int32_t& rwSpeed_) { rwSpeed_ = rwSpeed.value; }
+
+ private:
+  lp_var_t<int32_t> rwSpeed = lp_var_t<int32_t>(sid.objectId, RW_SPEED, this);
+};
+
 }  // namespace RwDefinitions
 
 #endif /* MISSION_DEVICES_DEVICEDEFINITIONS_RWDEFINITIONS_H_ */