added saftCtrl to acsController

mission/controller/AcsController.cpp

@@ -8,6 +8,7 @@ AcsController::AcsController(object_id_t objectId)
       navigation(&acsParameters),
       actuatorCmd(&acsParameters),
       guidance(&acsParameters),
+      safeCtrl(&acsParameters),
       detumble(&acsParameters),
       ptgCtrl(&acsParameters),
       detumbleCounter{0},
@@ -35,7 +36,7 @@ void AcsController::performControlOperation() {
       if (mode != MODE_OFF) {
         switch (submode) {
           case SUBMODE_SAFE:
-            // performSafe();
+            performSafe();
             break;
 
           case SUBMODE_DETUMBLE:
@@ -72,7 +73,66 @@ void AcsController::performControlOperation() {
   // DEBUG END
 }
 
-void AcsController::performSafe() {}
+void AcsController::performSafe() {
+  //	Concept: SAFE MODE WITH MEKF
+  //	-do the sensor processing, maybe is does make more sense do call this class function in
+  //	another place since we have to do it for every mode regardless of safe or not
+
+  ACS::SensorValues sensorValues;
+  ACS::OutputValues outputValues;
+
+  timeval now;  // We need to give the actual time here
+  sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters);
+  ReturnValue_t validMekf;
+  navigation.useMekf(&sensorValues, &outputValues, &validMekf);  // DOES THIS WORK WITH VALID?
+  // Give desired satellite rate and sun direction to align
+  double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0};
+  guidance.getTargetParamsSafe(sunTargetDir, satRateSafe);
+  //	IF MEKF is working
+  double magMomMtq[3] = {0, 0, 0};
+  bool magMomMtqValid = false;
+  if (validMekf == returnvalue::OK) {
+    safeCtrl.safeMekf(now, (outputValues.quatMekfBJ), &(outputValues.quatMekfBJValid),
+                      (outputValues.magFieldModel), &(outputValues.magFieldModelValid),
+                      (outputValues.sunDirModel), &(outputValues.sunDirModelValid),
+                      (outputValues.satRateMekf), &(outputValues.satRateMekfValid), sunTargetDir,
+                      satRateSafe, magMomMtq, &magMomMtqValid);
+  } else {
+    safeCtrl.safeNoMekf(now, outputValues.sunDirEst, &outputValues.sunDirEstValid,
+                        outputValues.sunVectorDerivative, &(outputValues.sunVectorDerivativeValid),
+                        outputValues.magFieldEst, &(outputValues.magFieldEstValid),
+                        outputValues.magneticFieldVectorDerivative,
+                        &(outputValues.magneticFieldVectorDerivativeValid), sunTargetDir,
+                        satRateSafe, magMomMtq, &magMomMtqValid);
+  }
+
+  double dipolCmdUnits[3] = {0, 0, 0};
+  actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits);
+
+  //	Detumble check and switch
+  if (outputValues.satRateMekfValid && VectorOperations<double>::norm(outputValues.satRateMekf, 3) >
+                                           acsParameters.detumbleParameter.omegaDetumbleStart) {
+    detumbleCounter++;
+  }
+
+  else if (outputValues.satRateEstValid &&
+           VectorOperations<double>::norm(outputValues.satRateEst, 3) >
+               acsParameters.detumbleParameter.omegaDetumbleStart) {
+    detumbleCounter++;
+
+  }
+
+  else {
+    detumbleCounter = 0;
+  }
+
+  if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
+    submode = SUBMODE_DETUMBLE;
+    detumbleCounter = 0;
+  }
+
+  //	commanding.magnetorquesDipol();
+}
 
 void AcsController::performDetumble() {
   ACS::SensorValues sensorValues;
@@ -293,6 +353,8 @@ void AcsController::copySusData() {
     PoolReadGuard pg(&susSets[9]);
     if (pg.getReadResult() == returnvalue::OK) {
       std::memcpy(susData.sus9.value, susSets[9].channels.value, 6 * sizeof(uint16_t));
+      sif::debug << susData.sus9.isValid() << std::endl;
+      sif::debug << susSets[9].channels.isValid() << std::endl;
     }
   }
   {