for now final version of controller

mission/controller/acs/control/SafeCtrl.cpp

@@ -28,67 +28,66 @@ ReturnValue_t SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const ReturnV
   }
 }
 
-ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid,
+void SafeCtrl::safeMekf(const double *magFieldB, const double *satRotRateB,
-                                 double *magFieldModel, bool magFieldModelValid,
+                        const double *sunDirModelI, const double *quatBI, const double *sunDirRefB,
-                                 double *sunDirModel, bool sunDirModelValid, double *satRateMekf,
+                        const double *satRotRateRefB, double *magMomB, double &errorAngle) {
-                                 bool rateMekfValid, double *sunDirRef, double *satRatRef,
+  // convert magFieldB from uT to T
-                                 double *outputAngle, double *outputMagMomB) {
+  double magFieldBT[3] = {0, 0, 0};
-  if (!quatBJValid || !magFieldModelValid || !sunDirModelValid || !rateMekfValid) {
+  VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
-    return SAFECTRL_MEKF_INPUT_INVALID;
+
+  // convert sunDirModel to body rf
+  double sunDirB[3] = {0, 0, 0};
+  QuaternionOperations::multiplyVector(quatBI, sunDirModelI, sunDirB);
+
+  // calculate angle alpha 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);
   }
 
-  double kRate = acsParameters->safeModeControllerParameters.k_rate_mekf;
+  // calculate torque for orthogonal rotational rate
-  double kAlign = acsParameters->safeModeControllerParameters.k_align_mekf;
+  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);
 
-  //	Calc sunDirB ,magFieldB with mekf output and model
+  // sum of all torques
-  double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  double cmdTorque[3] = {0, 0, 0};
-  MathOperations<double>::dcmFromQuat(quatBJ, *dcmBJ);
+  for (uint8_t i = 0; i < 3; i++) {
-  double sunDirB[3] = {0, 0, 0}, magFieldB[3] = {0, 0, 0};
+    cmdTorque[i] = cmdAlign[i] + cmdOrtho[i] + cmdParallel[i];
-  MatrixOperations<double>::multiply(*dcmBJ, sunDirModel, sunDirB, 3, 3, 1);
+  }
-  MatrixOperations<double>::multiply(*dcmBJ, magFieldModel, magFieldB, 3, 3, 1);
 
-  // change unit from uT to T
+  // calculate magnetic moment to command
-  VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldB, 3);
-
-  double crossSun[3] = {0, 0, 0};
-  VectorOperations<double>::cross(sunDirRef, sunDirB, crossSun);
-  double normCrossSun = VectorOperations<double>::norm(crossSun, 3);
-
-  //	calc angle alpha between sunDirRef and sunDIr
-  double dotSun = VectorOperations<double>::dot(sunDirRef, sunDirB);
-  double alpha = acos(dotSun);
-
-  //	Law Torque calculations
-  double torqueCmd[3] = {0, 0, 0}, torqueAlign[3] = {0, 0, 0}, torqueRate[3] = {0, 0, 0},
-         torqueAll[3] = {0, 0, 0};
-
-  double scalarFac = kAlign * alpha / normCrossSun;
-  VectorOperations<double>::mulScalar(crossSun, scalarFac, torqueAlign, 3);
-
-  double rateSafeMode[3] = {0, 0, 0};
-  VectorOperations<double>::subtract(satRateMekf, satRatRef, rateSafeMode, 3);
-  VectorOperations<double>::mulScalar(rateSafeMode, -kRate, torqueRate, 3);
-
-  VectorOperations<double>::add(torqueRate, torqueAlign, torqueAll, 3);
-  //	Adding factor of inertia for axes
-  MatrixOperations<double>::multiplyScalar(*(acsParameters->inertiaEIVE.inertiaMatrix), 10,
-                                           *gainMatrixInertia, 3,
-                                           3);  // why only for mekf one and not for no mekf
-  MatrixOperations<double>::multiply(*gainMatrixInertia, torqueAll, torqueCmd, 3, 3, 1);
-
-  // MagMom B (orthogonal torque)
   double torqueMgt[3] = {0, 0, 0};
-  VectorOperations<double>::cross(magFieldB, torqueCmd, torqueMgt);
+  VectorOperations<double>::cross(magFieldBT, cmdTorque, torqueMgt);
   double normMag = VectorOperations<double>::norm(magFieldB, 3);
-  VectorOperations<double>::mulScalar(torqueMgt, 1 / pow(normMag, 2), outputMagMomB, 3);
+  VectorOperations<double>::mulScalar(torqueMgt, pow(normMag, -2), magMomB, 3);
-
-  *outputAngle = alpha;
-  return returnvalue::OK;
 }
 
-void SafeCtrl::safeNoMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirB,
+void SafeCtrl::safeNonMekf(const double *magFieldB, const double *satRotRateB,
-                          const double *sunDirRefB, const double *satRotRateRefB, double *magMomB,
+                           const double *sunDirB, const double *sunDirRefB,
-                          double &errorAngle) {
+                           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);
@@ -109,23 +108,23 @@ void SafeCtrl::safeNoMekf(const double *magFieldB, const double *satRotRateB, co
   if (errorAngle < (double)acsParameters->safeModeControllerParameters.angleStartSpin) {
     VectorOperations<double>::subtract(satRotRateRefB, satRotRateParallelB, cmdParallel, 3);
     VectorOperations<double>::mulScalar(
-        cmdParallel, acsParameters->safeModeControllerParameters.k_parallel_mekf, cmdParallel, 3);
+        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_ortho_mekf,
+                                      -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_align_mekf,
+                                           acsParameters->safeModeControllerParameters.k_alignMekf,
                                            *alignFactor, 3, 3);
   VectorOperations<double>::cross(sunDirRefB, sunDirB, crossAlign);
-  MatrixOperations<double>::multiply(*alignFactor, *crossAlign, *cmdAlign, 3, 3, 1);
+  MatrixOperations<double>::multiply(*alignFactor, crossAlign, cmdAlign, 3, 3, 1);
 
   // sum of all torques
   double cmdTorque[3] = {0, 0, 0};
@@ -133,9 +132,30 @@ void SafeCtrl::safeNoMekf(const double *magFieldB, const double *satRotRateB, co
     cmdTorque[i] = cmdAlign[i] + cmdOrtho[i] + cmdParallel[i];
   }
 
-  // MagMom B (orthogonal torque)
+  // 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);
 }
+
+void SafeCtrl::safeRateDamping(const double *magFieldB, const double *satRotRateB,
+                               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);
+
+  // 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);
+
+  // 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);
+
+  errorAngle = NAN;
+}

mission/controller/acs/control/SafeCtrl.h

@@ -25,15 +25,16 @@ class SafeCtrl {
   ReturnValue_t safeCtrlStrategy(const bool magFieldValid, const ReturnValue_t mekfValid,
                                  const bool satRotRateValid, const bool sunDirValid);
 
-  ReturnValue_t safeMekf(timeval now, double *quatBJ, bool quatBJValid, double *magFieldModel,
+  void safeMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirModelI,
-                         bool magFieldModelValid, double *sunDirModel, bool sunDirModelValid,
+                const double *quatBI, const double *sunDirRefB, const double *satRotRateRefB,
-                         double *satRateMekf, bool rateMekfValid, double *sunDirRef,
+                double *magMomB, double &errorAngle);
-                         double *satRatRef,  // From Guidance (!)
-                         double *outputAngle, double *outputMagMomB);
 
-  void safeNoMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirB,
+  void safeNonMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirB,
-                  const double *sunDirRefB, const double *satRotRateRefB, double *magMomB,
+                   const double *sunDirRefB, const double *satRotRateRefB, double *magMomB,
-                  double &errorAngle);
+                   double &errorAngle);
+
+  void safeRateDamping(const double *magFieldB, const double *satRotRateB,
+                       const double *satRotRateRefB, double *magMomB, double &errorAngle);
 
  protected:
  private: