added event for SAFE to DETUMBLE transition. changed actuator input from double to int

mission/controller/AcsController.cpp

@@ -156,12 +156,13 @@ void AcsController::performSafe() {
   if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
     submode = SUBMODE_DETUMBLE;
     detumbleCounter = 0;
+    triggerEvent(SAFE_RATE_VIOLATION);
   }
 
   {
     PoolReadGuard pg(&actuatorCmdData);
     if (pg.getReadResult() == returnvalue::OK) {
-      double zeroVec[3] = {0, 0, 0, 0};
+      double zeroVec[4] = {0, 0, 0, 0};
       std::memcpy(actuatorCmdData.rwTargetTorque.value, zeroVec, 4 * sizeof(double));
       actuatorCmdData.rwTargetTorque.setValid(false);
       std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 4 * sizeof(double));
@@ -175,8 +176,8 @@ void AcsController::performSafe() {
   //	  PoolReadGuard pg(&dipoleSet);
   //	  MutexGuard mg(torquer::lazyLock());
   //	  torquer::NEW_ACTUATION_FLAG = true;
-  //	  dipoleSet.setDipoles(dipolCmdUnits[0], dipolCmdUnits[1], dipolCmdUnits[2],
+  //	  dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2],
-  //torqueDuration);
+  // torqueDuration);
   //  }
 }
 
@@ -215,15 +216,19 @@ void AcsController::performDetumble() {
     detumbleCounter = 0;
   }
 
+  int16_t cmdDipolUnitsInt[3] = {0, 0, 0};
+  for (int i = 0; i < 3; ++i) {
+    cmdDipolUnitsInt[i] = std::round(dipolCmdUnits[i]);
+  }
   {
     PoolReadGuard pg(&actuatorCmdData);
     if (pg.getReadResult() == returnvalue::OK) {
-      double zeroVec[3] = {0, 0, 0, 0};
+      double zeroVec[4] = {0, 0, 0, 0};
       std::memcpy(actuatorCmdData.rwTargetTorque.value, zeroVec, 4 * sizeof(double));
       actuatorCmdData.rwTargetTorque.setValid(false);
       std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 4 * sizeof(double));
       actuatorCmdData.rwTargetSpeed.setValid(false);
-      std::memcpy(actuatorCmdData.mtqTargetDipole.value, dipolCmdUnits, 3 * sizeof(double));
+      std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolUnitsInt, 3 * sizeof(double));
       actuatorCmdData.mtqTargetDipole.setValid(true);
       actuatorCmdData.setValidity(true, false);
     }
@@ -232,7 +237,7 @@ void AcsController::performDetumble() {
   //	  PoolReadGuard pg(&dipoleSet);
   //	  MutexGuard mg(torquer::lazyLock());
   //	  torquer::NEW_ACTUATION_FLAG = true;
-  //	  dipoleSet.setDipoles(dipolCmdUnits[0], dipolCmdUnits[1], dipolCmdUnits[2],
+  //	  dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2],
   // torqueDuration);
   //  }
 }
@@ -275,12 +280,21 @@ void AcsController::performPointingCtrl() {
                           &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
   actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits);
 
+  int16_t cmdDipolUnitsInt[3] = {0, 0, 0};
+  for (int i = 0; i < 3; ++i) {
+    cmdDipolUnitsInt[i] = std::round(dipolUnits[i]);
+  }
+  int32_t cmdRwSpeedInt[4] = {0, 0, 0, 0};
+  for (int i = 0; i < 4; ++i) {
+    cmdRwSpeedInt[i] = std::round(cmdSpeedRws[i]);
+  }
+
   {
     PoolReadGuard pg(&actuatorCmdData);
     if (pg.getReadResult() == returnvalue::OK) {
       std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTrqNs, 4 * sizeof(double));
-      std::memcpy(actuatorCmdData.rwTargetSpeed.value, cmdSpeedRws, 4 * sizeof(double));
+      std::memcpy(actuatorCmdData.rwTargetSpeed.value, cmdRwSpeedInt, 4 * sizeof(double));
-      std::memcpy(actuatorCmdData.mtqTargetDipole.value, dipolUnits, 3 * sizeof(double));
+      std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolUnitsInt, 3 * sizeof(double));
       actuatorCmdData.setValidity(true, true);
     }
   }
@@ -288,7 +302,7 @@ void AcsController::performPointingCtrl() {
   //	  PoolReadGuard pg(&dipoleSet);
   //	  MutexGuard mg(torquer::lazyLock());
   //	  torquer::NEW_ACTUATION_FLAG = true;
-  //	  dipoleSet.setDipoles(dipolCmdUnits[0], dipolCmdUnits[1], dipolCmdUnits[2],
+  //	  dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2],
   // torqueDuration);
   //  }
 }

mission/controller/AcsController.h

@@ -30,6 +30,10 @@ class AcsController : public ExtendedControllerBase {
   static const Submode_t SUBMODE_PTG_NADIR = 5;
   static const Submode_t SUBMODE_PTG_INERTIAL = 6;
 
+  static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM;
+  static const Event SAFE_RATE_VIOLATION =
+      MAKE_EVENT(0, severity::MEDIUM);  //!< The limits for the rotation in safe mode were violated.
+
  protected:
   void performSafe();
   void performDetumble();

mission/controller/controllerdefinitions/AcsCtrlDefinitions.h

@@ -107,7 +107,7 @@ static constexpr uint8_t GYR_SET_RAW_ENTRIES = 4;
 static constexpr uint8_t GYR_SET_PROCESSED_ENTRIES = 5;
 static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 2;
 static constexpr uint8_t MEKF_SET_ENTRIES = 2;
-static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 99;
+static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 3;
 static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3;
 
 /**
@@ -254,8 +254,9 @@ class ActuatorCmdData : public StaticLocalDataSet<ACT_CMD_SET_ENTRIES> {
   ActuatorCmdData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, ACTUATOR_CMD_DATA) {}
 
   lp_vec_t<double, 4> rwTargetTorque = lp_vec_t<double, 4>(sid.objectId, RW_TARGET_TORQUE, this);
-  lp_vec_t<double, 4> rwTargetSpeed = lp_vec_t<double, 4>(sid.objectId, RW_TARGET_SPEED, this);
+  lp_vec_t<int32_t, 4> rwTargetSpeed = lp_vec_t<int32_t, 4>(sid.objectId, RW_TARGET_SPEED, this);
-  lp_vec_t<double, 3> mtqTargetDipole = lp_vec_t<double, 3>(sid.objectId, MTQ_TARGET_DIPOLE, this);
+  lp_vec_t<int16_t, 3> mtqTargetDipole =
+      lp_vec_t<int16_t, 3>(sid.objectId, MTQ_TARGET_DIPOLE, this);
 
  private:
 };