nearly there

mission/controller/acs/control/SafeCtrl.cpp

@@ -13,6 +13,21 @@ SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameter
 
 SafeCtrl::~SafeCtrl() {}
 
+ReturnValue_t SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const ReturnValue_t mekfValid,
+                                         const bool satRotRateValid, const bool sunDirValid) {
+  if (not magFieldValid) {
+    return SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
+  } else if (mekfValid) {
+    return SAFECTRL_USE_MEKF;
+  } else if (satRotRateValid and sunDirValid) {
+    return SAFECTRL_USE_NONMEKF;
+  } else if (satRotRateValid and not sunDirValid) {
+    return SAFECTRL_USE_DAMPING;
+  } else {
+    return SAFECTRL_NO_SENSORS_FOR_CONTROL;
+  }
+}
+
 ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid,
                                  double *magFieldModel, bool magFieldModelValid,
                                  double *sunDirModel, bool sunDirModelValid, double *satRateMekf,
@@ -71,81 +86,56 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid,
   return returnvalue::OK;
 }
 
-// Will be the version in worst case scenario in event of no working MEKF
-ReturnValue_t SafeCtrl::safeNoMekf(const double *magFieldB,
-                                   const double *magneticFieldVectorDerivative,
-                                   const double *sunVector, const double *sunvectorDerivative,
-                                   double omegaRef, double *magMomB, double *spinAxis) {
-  if (0) {
-    return returnvalue::FAILED;
-  }
-
+void SafeCtrl::safeNoMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirB,
+                          const double *sunDirRefB, const double *satRotRateRefB, double *magMomB,
+                          double &errorAngle) {
+  // convert magFieldB from uT to T
   double magFieldBT[3] = {0, 0, 0};
   VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
 
-  double cmdParallel[3] = {0, 0, 0}, cmdAlign[3] = {0, 0, 0}, cmdOrtho[3] = {0, 0, 0},
-         torqueCmd[3] = {0, 0, 0};
+  // calculate angle alpha between sunDirRef and sunDir
+  double dotSun = VectorOperations<double>::dot(sunDirRefB, sunDirB);
+  errorAngle = acos(dotSun);
 
-  bool valid;
-  double omega =
-      estimateRotationAroundSun(magFieldB, magneticFieldVectorDerivative, sunVector, &valid);
+  // split rotational rate into parallel and orthogonal parts
+  double satRotRateParallelB[3] = {0, 0, 0}, satRotRateOrthogonalB[3] = {0, 0, 0};
+  double parallelLength = VectorOperations<double>::dot(satRotRateB, sunDirB) *
+                          pow(VectorOperations<double>::norm(sunDirB, 3), -2);
+  VectorOperations<double>::mulScalar(sunDirB, parallelLength, satRotRateParallelB, 3);
+  VectorOperations<double>::subtract(satRotRateB, satRotRateParallelB, satRotRateOrthogonalB, 3);
 
-  if (valid) {
+  // calculate torque for parallel rotational rate
+  double cmdParallel[3] = {0, 0, 0};
+  if (errorAngle < (double)acsParameters->safeModeControllerParameters.angleStartSpin) {
+    VectorOperations<double>::subtract(satRotRateRefB, satRotRateParallelB, cmdParallel, 3);
     VectorOperations<double>::mulScalar(
-        sunVector,
-        acsParameters->safeModeControllerParameters.k_parallel_no_mekf * (omegaRef - omega),
-        cmdParallel, 3);
-    omega = omega * sign<double>(omega);
+        cmdParallel, acsParameters->safeModeControllerParameters.k_parallel_mekf, cmdParallel, 3);
   }
 
-  VectorOperations<double>::cross(spinAxis, sunVector, cmdAlign);
+  // calculate torque for orthogonal rotational rate
+  double cmdOrtho[3] = {0, 0, 0};
+  VectorOperations<double>::mulScalar(satRotRateOrthogonalB,
+                                      -acsParameters->safeModeControllerParameters.k_ortho_mekf,
+                                      cmdOrtho, 3);
 
-  VectorOperations<double>::mulScalar(
-      cmdAlign, acsParameters->safeModeControllerParameters.k_align_no_mekf, cmdAlign, 3);
-
-  VectorOperations<double>::cross(sunvectorDerivative, sunVector, cmdOrtho);
-  VectorOperations<double>::mulScalar(
-      cmdOrtho, -acsParameters->safeModeControllerParameters.k_ortho_no_mekf, cmdOrtho, 3);
-
-  // only spin up when the angle to the sun is less than a certain angle.
-  // note that we check the cosin, thus "<"
-  if (VectorOperations<double>::dot(spinAxis, sunVector) <
-      acsParameters->safeModeControllerParameters.cosineStartSpin) {
-    VectorOperations<double>::mulScalar(cmdParallel, 0, cmdParallel, 3);
-  }
+  // calculate torque for alignment
+  double cmdAlign[3] = {0, 0, 0}, crossAlign[3] = {0, 0, 0},
+         alignFactor[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  MatrixOperations<double>::multiplyScalar(*acsParameters->inertiaEIVE.inertiaMatrix,
+                                           acsParameters->safeModeControllerParameters.k_align_mekf,
+                                           *alignFactor, 3, 3);
+  VectorOperations<double>::cross(sunDirRefB, sunDirB, crossAlign);
+  MatrixOperations<double>::multiply(*alignFactor, *crossAlign, *cmdAlign, 3, 3, 1);
 
+  // sum of all torques
+  double cmdTorque[3] = {0, 0, 0};
   for (uint8_t i = 0; i < 3; i++) {
-    torqueCmd[i] = cmdAlign[i] + cmdOrtho[i] + cmdParallel[i];
+    cmdTorque[i] = cmdAlign[i] + cmdOrtho[i] + cmdParallel[i];
   }
 
   // MagMom B (orthogonal torque)
   double torqueMgt[3] = {0, 0, 0};
-  VectorOperations<double>::cross(magFieldBT, torqueCmd, torqueMgt);
+  VectorOperations<double>::cross(magFieldBT, cmdTorque, torqueMgt);
   double normMag = VectorOperations<double>::norm(magFieldB, 3);
-  VectorOperations<double>::mulScalar(torqueMgt, 1 / pow(normMag, 2), magMomB, 3);
-
-  *outputAngle = angleAlignErr;
-  return returnvalue::OK;
-}
-
-double SafeCtrl::estimateRotationAroundSun(const double *magneticFieldVector,
-                                           const double *magneticFieldVectorDerivative,
-                                           const double *sunVector, bool *updated) {
-  *updated = true;
-  double vector[3];
-  VectorOperations<double>::cross(magneticFieldVector, sunVector, vector);
-
-  float magLength = VectorOperations<double>::norm(magneticFieldVector, 3);
-  float length = VectorOperations<double>::norm(vector, 3);
-
-  // only check if angle between B and sun is large enough
-  if (length > (acsParameters->safeModeControllerParameters.sineCalculateOmegaSun * magLength)) {
-    float omega = VectorOperations<double>::dot(magneticFieldVectorDerivative, vector);
-    omega = omega / (length * length);
-    lastCalculatedOmega = omega;
-    return omega;
-  } else {
-    *updated = false;
-    return lastCalculatedOmega;
-  }
+  VectorOperations<double>::mulScalar(torqueMgt, pow(normMag, -2), magMomB, 3);
 }