first implementation of new laws

mission/controller/AcsController.cpp

@@ -177,20 +177,20 @@ void AcsController::performSafe() {
     case (SafeCtrl::SAFECTRL_USE_MEKF):
       safeCtrl.safeMekf(mgmDataProcessed.mgmVecTot.value, mekfData.satRotRateMekf.value,
                         susDataProcessed.sunIjkModel.value, mekfData.quatMekf.value, sunTargetDir,
-                        satRateSafe, magMomMtq, errAng);
+                        satRateSafe, inertiaEive, magMomMtq, errAng);
       safeCtrlFailureFlag = false;
       safeCtrlFailureCounter = 0;
       break;
     case (SafeCtrl::SAFECTRL_USE_NONMEKF):
       safeCtrl.safeNonMekf(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value,
-                           susDataProcessed.susVecTot.value, sunTargetDir, satRateSafe, magMomMtq,
-                           errAng);
+                           susDataProcessed.susVecTot.value, sunTargetDir, satRateSafe, inertiaEive,
+                           magMomMtq, errAng);
       safeCtrlFailureFlag = false;
       safeCtrlFailureCounter = 0;
       break;
     case (SafeCtrl::SAFECTRL_USE_DAMPING):
       safeCtrl.safeRateDamping(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value,
-                               satRateSafe, magMomMtq, errAng);
+                               satRateSafe, sunTargetDir, magMomMtq, errAng);
       safeCtrlFailureFlag = false;
       safeCtrlFailureCounter = 0;
       break;

mission/controller/acs/control/SafeCtrl.cpp

@@ -30,9 +30,9 @@ ReturnValue_t SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const ReturnV
 
 void SafeCtrl::safeMekf(const double *magFieldB, const double *satRotRateB,
                         const double *sunDirModelI, const double *quatBI, const double *sunDirRefB,
-                        const double *satRotRateRefB, double *magMomB, double &errorAngle) {
+                        const double *satRotRateRefB, const double inertiaMatrix[3][3],
+                        double *magMomB, double &errorAngle) {
   // convert magFieldB from uT to T
-  double magFieldBT[3] = {0, 0, 0};
   VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
 
   // convert sunDirModel to body rf
@@ -43,119 +43,110 @@ void SafeCtrl::safeMekf(const double *magFieldB, const double *satRotRateB,
   double dotSun = VectorOperations<double>::dot(sunDirRefB, sunDirB);
   errorAngle = acos(dotSun);
 
-  // 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);
-
-  // 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(
-        cmdParallel, acsParameters->safeModeControllerParameters.k_parallelMekf, cmdParallel, 3);
-  }
-
-  // calculate torque for orthogonal rotational rate
-  double cmdOrtho[3] = {0, 0, 0};
-  VectorOperations<double>::mulScalar(satRotRateOrthogonalB,
-                                      -acsParameters->safeModeControllerParameters.k_orthoMekf,
-                                      cmdOrtho, 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_alignMekf,
-                                           *alignFactor, 3, 3);
-  VectorOperations<double>::cross(sunDirRefB, sunDirB, crossAlign);
-  MatrixOperations<double>::multiply(*alignFactor, crossAlign, cmdAlign, 3, 3, 1);
+  splitRotationalRate(satRotRateB, sunDirB);
+  calculateRotationalRateTorque(satRotRateRefB, sunDirB, sunDirRefB, errorAngle,
+                                acsParameters->safeModeControllerParameters.k_parallelMekf,
+                                acsParameters->safeModeControllerParameters.k_orthoMekf);
+  calculateAngleErrorTorque(sunDirB, sunDirRefB,
+                            acsParameters->safeModeControllerParameters.k_alignMekf, inertiaMatrix);
 
   // sum of all torques
-  double cmdTorque[3] = {0, 0, 0};
   for (uint8_t i = 0; i < 3; i++) {
     cmdTorque[i] = cmdAlign[i] + cmdOrtho[i] + cmdParallel[i];
   }
 
-  // calculate magnetic moment to command
-  double torqueMgt[3] = {0, 0, 0};
-  VectorOperations<double>::cross(magFieldBT, cmdTorque, torqueMgt);
-  double normMag = VectorOperations<double>::norm(magFieldB, 3);
-  VectorOperations<double>::mulScalar(torqueMgt, pow(normMag, -2), magMomB, 3);
+  calculateMagneticMoment(magMomB);
 }
 
 void SafeCtrl::safeNonMekf(const double *magFieldB, const double *satRotRateB,
                            const double *sunDirB, const double *sunDirRefB,
-                           const double *satRotRateRefB, double *magMomB, double &errorAngle) {
+                           const double *satRotRateRefB, const double inertiaMatrix[3][3],
+                           double *magMomB, double &errorAngle) {
   // convert magFieldB from uT to T
   double magFieldBT[3] = {0, 0, 0};
   VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
 
-  // calculate angle alpha between sunDirRef and sunDir
+  // calculate error angle between sunDirRef and sunDir
   double dotSun = VectorOperations<double>::dot(sunDirRefB, sunDirB);
   errorAngle = acos(dotSun);
 
-  // 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);
-
-  // 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(
-        cmdParallel, acsParameters->safeModeControllerParameters.k_parallelMekf, cmdParallel, 3);
-  }
-
-  // calculate torque for orthogonal rotational rate
-  double cmdOrtho[3] = {0, 0, 0};
-  VectorOperations<double>::mulScalar(satRotRateOrthogonalB,
-                                      -acsParameters->safeModeControllerParameters.k_orthoMekf,
-                                      cmdOrtho, 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_alignMekf,
-                                           *alignFactor, 3, 3);
-  VectorOperations<double>::cross(sunDirRefB, sunDirB, crossAlign);
-  MatrixOperations<double>::multiply(*alignFactor, crossAlign, cmdAlign, 3, 3, 1);
+  splitRotationalRate(satRotRateB, sunDirB);
+  calculateRotationalRateTorque(satRotRateRefB, sunDirB, sunDirRefB, errorAngle,
+                                acsParameters->safeModeControllerParameters.k_parallelNonMekf,
+                                acsParameters->safeModeControllerParameters.k_orthoNonMekf);
+  calculateAngleErrorTorque(sunDirB, sunDirRefB,
+                            acsParameters->safeModeControllerParameters.k_alignNonMekf,
+                            inertiaMatrix);
 
   // sum of all torques
-  double cmdTorque[3] = {0, 0, 0};
   for (uint8_t i = 0; i < 3; i++) {
     cmdTorque[i] = cmdAlign[i] + cmdOrtho[i] + cmdParallel[i];
   }
 
-  // calculate magnetic moment to command
-  double torqueMgt[3] = {0, 0, 0};
-  VectorOperations<double>::cross(magFieldBT, cmdTorque, torqueMgt);
-  double normMag = VectorOperations<double>::norm(magFieldB, 3);
-  VectorOperations<double>::mulScalar(torqueMgt, pow(normMag, -2), magMomB, 3);
+  calculateMagneticMoment(magMomB);
 }
 
 void SafeCtrl::safeRateDamping(const double *magFieldB, const double *satRotRateB,
-                               const double *satRotRateRefB, double *magMomB, double &errorAngle) {
+                               const double *satRotRateRefB, const double *sunDirRefB,
+                               double *magMomB, double &errorAngle) {
   // convert magFieldB from uT to T
-  double magFieldBT[3] = {0, 0, 0};
   VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
 
-  // calculate torque for rate damping
-  double cmdTorque[3] = {0, 0, 0}, diffSatRotRate[3] = {0, 0, 0};
-  VectorOperations<double>::subtract(satRotRateRefB, satRotRateB, diffSatRotRate, 3);
-  VectorOperations<double>::mulScalar(
-      satRotRateB, acsParameters->safeModeControllerParameters.k_rateDamping, cmdTorque, 3);
+  // no error angle available for eclipse
+  errorAngle = NAN;
+
+  splitRotationalRate(satRotRateB, sunDirRefB);
+  calculateRotationalRateTorque(satRotRateRefB, sunDirRefB, sunDirRefB, errorAngle,
+                                acsParameters->safeModeControllerParameters.k_parallelNonMekf,
+                                acsParameters->safeModeControllerParameters.k_orthoNonMekf);
+
+  // sum of all torques
+  double cmdTorque[3] = {0, 0, 0};
+  VectorOperations<double>::add(cmdParallel, cmdOrtho, cmdTorque, 3);
 
   // calculate magnetic moment to command
+  calculateMagneticMoment(magMomB);
+}
+
+void SafeCtrl::splitRotationalRate(const double *satRotRateB, const double *sunDirB) {
+  // split rotational rate into parallel and orthogonal parts
+  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);
+}
+
+void SafeCtrl::calculateRotationalRateTorque(const double *satRotRateRefB, const double *sunDirB,
+                                             const double *sunDirRefB, double &errorAngle,
+                                             const double gainParallel, const double gainOrtho) {
+  // calculate torque for parallel rotational rate
+  if ((isfinite(errorAngle)) and
+      (errorAngle < (double)acsParameters->safeModeControllerParameters.angleStartSpin)) {
+    VectorOperations<double>::subtract(satRotRateRefB, satRotRateParallelB, cmdParallel, 3);
+    VectorOperations<double>::mulScalar(cmdParallel, gainParallel, cmdParallel, 3);
+  } else {
+    VectorOperations<double>::mulScalar(cmdParallel, -gainParallel, cmdParallel, 3);
+  }
+
+  // calculate torque for orthogonal rotational rate
+  VectorOperations<double>::mulScalar(satRotRateOrthogonalB, -gainOrtho, cmdOrtho, 3);
+  if (cos(VectorOperations<double>::dot(sunDirB, sunDirRefB)) < 0) {
+    VectorOperations<double>::mulScalar(cmdOrtho, -1, cmdOrtho, 3);
+  }
+}
+
+void SafeCtrl::calculateAngleErrorTorque(const double *sunDirB, const double *sunDirRefB,
+                                         const double gainAlign, const double inertiaMatrix[3][3]) {
+  // calculate torque for alignment
+  double crossAlign[3] = {0, 0, 0}, alignFactor[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  MatrixOperations<double>::multiplyScalar(*inertiaMatrix, gainAlign, *alignFactor, 3, 3);
+  VectorOperations<double>::cross(sunDirRefB, sunDirB, crossAlign);
+  MatrixOperations<double>::multiply(*alignFactor, crossAlign, cmdAlign, 3, 3, 1);
+}
+
+void SafeCtrl::calculateMagneticMoment(double *magMomB) {
   double torqueMgt[3] = {0, 0, 0};
   VectorOperations<double>::cross(magFieldBT, cmdTorque, torqueMgt);
-  double normMag = VectorOperations<double>::norm(magFieldB, 3);
+  double normMag = VectorOperations<double>::norm(magFieldBT, 3);
   VectorOperations<double>::mulScalar(torqueMgt, pow(normMag, -2), magMomB, 3);
-
-  errorAngle = NAN;
 }

mission/controller/acs/control/SafeCtrl.h

@@ -27,18 +27,37 @@ class SafeCtrl {
 
   void safeMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirModelI,
                 const double *quatBI, const double *sunDirRefB, const double *satRotRateRefB,
-                double *magMomB, double &errorAngle);
+                const double inertiaMatrix[3][3], double *magMomB, double &errorAngle);
 
   void safeNonMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirB,
-                   const double *sunDirRefB, const double *satRotRateRefB, double *magMomB,
-                   double &errorAngle);
+                   const double *sunDirRefB, const double *satRotRateRefB,
+                   const double inertiaMatrix[3][3], double *magMomB, double &errorAngle);
 
   void safeRateDamping(const double *magFieldB, const double *satRotRateB,
-                       const double *satRotRateRefB, double *magMomB, double &errorAngle);
+                       const double *satRotRateRefB, const double *sunDirRefB, double *magMomB,
+                       double &errorAngle);
+
+  void splitRotationalRate(const double *satRotRateB, const double *sunDirB);
+
+  void calculateRotationalRateTorque(const double *satRotRateRefB, const double *sunDirB,
+                                     const double *sunDirRefB, double &errorAngle,
+                                     const double gainParallel, const double gainOrtho);
+
+  void calculateAngleErrorTorque(const double *sunDirB, const double *sunDirRefB,
+                                 const double gainAlign, const double inertiaMatrix[3][3]);
+
+  void calculateMagneticMoment(double *magMomB);
 
  protected:
  private:
   AcsParameters *acsParameters;
+  double magFieldBT[3] = {0, 0, 0};
+  double satRotRateParallelB[3] = {0, 0, 0};
+  double satRotRateOrthogonalB[3] = {0, 0, 0};
+  double cmdParallel[3] = {0, 0, 0};
+  double cmdOrtho[3] = {0, 0, 0};
+  double cmdAlign[3] = {0, 0, 0};
+  double cmdTorque[3] = {0, 0, 0};
 };
 
 #endif /* ACS_CONTROL_SAFECTRL_H_ */