fixed nullspace controller

mission/controller/acs/AcsParameters.cpp

@@ -315,7 +315,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
           parameterWrapper->setMatrix(rwMatrices.without4);
           break;
         case 0x6:
-          parameterWrapper->setVector(rwMatrices.nullspace);
+          parameterWrapper->setVector(rwMatrices.nullspaceVector);
           break;
         default:
           return INVALID_IDENTIFIER_ID;
@@ -375,15 +375,18 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
           parameterWrapper->set(idleModeControllerParameters.gainNullspace);
           break;
         case 0x5:
-          parameterWrapper->setVector(idleModeControllerParameters.desatMomentumRef);
+          parameterWrapper->set(idleModeControllerParameters.nullspaceSpeed);
           break;
         case 0x6:
-          parameterWrapper->set(idleModeControllerParameters.deSatGainFactor);
+          parameterWrapper->setVector(idleModeControllerParameters.desatMomentumRef);
           break;
         case 0x7:
-          parameterWrapper->set(idleModeControllerParameters.desatOn);
+          parameterWrapper->set(idleModeControllerParameters.deSatGainFactor);
           break;
         case 0x8:
+          parameterWrapper->set(idleModeControllerParameters.desatOn);
+          break;
+        case 0x9:
           parameterWrapper->set(idleModeControllerParameters.enableAntiStiction);
           break;
         default:
@@ -408,48 +411,51 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
           parameterWrapper->set(targetModeControllerParameters.gainNullspace);
           break;
         case 0x5:
-          parameterWrapper->setVector(targetModeControllerParameters.desatMomentumRef);
+          parameterWrapper->set(targetModeControllerParameters.nullspaceSpeed);
           break;
         case 0x6:
-          parameterWrapper->set(targetModeControllerParameters.deSatGainFactor);
+          parameterWrapper->setVector(targetModeControllerParameters.desatMomentumRef);
           break;
         case 0x7:
-          parameterWrapper->set(targetModeControllerParameters.desatOn);
+          parameterWrapper->set(targetModeControllerParameters.deSatGainFactor);
           break;
         case 0x8:
-          parameterWrapper->set(targetModeControllerParameters.enableAntiStiction);
+          parameterWrapper->set(targetModeControllerParameters.desatOn);
           break;
         case 0x9:
-          parameterWrapper->setVector(targetModeControllerParameters.refDirection);
+          parameterWrapper->set(targetModeControllerParameters.enableAntiStiction);
           break;
         case 0xA:
-          parameterWrapper->setVector(targetModeControllerParameters.refRotRate);
+          parameterWrapper->setVector(targetModeControllerParameters.refDirection);
           break;
         case 0xB:
-          parameterWrapper->setVector(targetModeControllerParameters.quatRef);
+          parameterWrapper->setVector(targetModeControllerParameters.refRotRate);
           break;
         case 0xC:
-          parameterWrapper->set(targetModeControllerParameters.timeElapsedMax);
+          parameterWrapper->setVector(targetModeControllerParameters.quatRef);
           break;
         case 0xD:
-          parameterWrapper->set(targetModeControllerParameters.latitudeTgt);
+          parameterWrapper->set(targetModeControllerParameters.timeElapsedMax);
           break;
         case 0xE:
-          parameterWrapper->set(targetModeControllerParameters.longitudeTgt);
+          parameterWrapper->set(targetModeControllerParameters.latitudeTgt);
           break;
         case 0xF:
-          parameterWrapper->set(targetModeControllerParameters.altitudeTgt);
+          parameterWrapper->set(targetModeControllerParameters.longitudeTgt);
           break;
         case 0x10:
-          parameterWrapper->set(targetModeControllerParameters.avoidBlindStr);
+          parameterWrapper->set(targetModeControllerParameters.altitudeTgt);
           break;
         case 0x11:
-          parameterWrapper->set(targetModeControllerParameters.blindAvoidStart);
+          parameterWrapper->set(targetModeControllerParameters.avoidBlindStr);
           break;
         case 0x12:
-          parameterWrapper->set(targetModeControllerParameters.blindAvoidStop);
+          parameterWrapper->set(targetModeControllerParameters.blindAvoidStart);
           break;
         case 0x13:
+          parameterWrapper->set(targetModeControllerParameters.blindAvoidStop);
+          break;
+        case 0x14:
           parameterWrapper->set(targetModeControllerParameters.blindRotRate);
           break;
         default:
@@ -474,30 +480,33 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
           parameterWrapper->set(gsTargetModeControllerParameters.gainNullspace);
           break;
         case 0x5:
-          parameterWrapper->setVector(gsTargetModeControllerParameters.desatMomentumRef);
+          parameterWrapper->set(gsTargetModeControllerParameters.nullspaceSpeed);
           break;
         case 0x6:
-          parameterWrapper->set(gsTargetModeControllerParameters.deSatGainFactor);
+          parameterWrapper->setVector(gsTargetModeControllerParameters.desatMomentumRef);
           break;
         case 0x7:
-          parameterWrapper->set(gsTargetModeControllerParameters.desatOn);
+          parameterWrapper->set(gsTargetModeControllerParameters.deSatGainFactor);
           break;
         case 0x8:
-          parameterWrapper->set(gsTargetModeControllerParameters.enableAntiStiction);
+          parameterWrapper->set(gsTargetModeControllerParameters.desatOn);
           break;
         case 0x9:
-          parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection);
+          parameterWrapper->set(gsTargetModeControllerParameters.enableAntiStiction);
           break;
         case 0xA:
-          parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax);
+          parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection);
           break;
         case 0xB:
-          parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt);
+          parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax);
           break;
         case 0xC:
-          parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt);
+          parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt);
           break;
         case 0xD:
+          parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt);
+          break;
+        case 0xE:
           parameterWrapper->set(gsTargetModeControllerParameters.altitudeTgt);
           break;
         default:
@@ -522,21 +531,24 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
           parameterWrapper->set(nadirModeControllerParameters.gainNullspace);
           break;
         case 0x5:
-          parameterWrapper->setVector(nadirModeControllerParameters.desatMomentumRef);
+          parameterWrapper->set(nadirModeControllerParameters.nullspaceSpeed);
           break;
         case 0x6:
-          parameterWrapper->set(nadirModeControllerParameters.deSatGainFactor);
+          parameterWrapper->setVector(nadirModeControllerParameters.desatMomentumRef);
           break;
         case 0x7:
-          parameterWrapper->set(nadirModeControllerParameters.desatOn);
+          parameterWrapper->set(nadirModeControllerParameters.deSatGainFactor);
           break;
         case 0x8:
-          parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction);
+          parameterWrapper->set(nadirModeControllerParameters.desatOn);
           break;
         case 0x9:
-          parameterWrapper->setVector(nadirModeControllerParameters.refDirection);
+          parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction);
           break;
         case 0xA:
+          parameterWrapper->setVector(nadirModeControllerParameters.refDirection);
+          break;
+        case 0xB:
           parameterWrapper->setVector(nadirModeControllerParameters.quatRef);
           break;
         case 0xC:
@@ -564,21 +576,24 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
           parameterWrapper->set(inertialModeControllerParameters.gainNullspace);
           break;
         case 0x5:
-          parameterWrapper->setVector(inertialModeControllerParameters.desatMomentumRef);
+          parameterWrapper->set(inertialModeControllerParameters.nullspaceSpeed);
           break;
         case 0x6:
-          parameterWrapper->set(inertialModeControllerParameters.deSatGainFactor);
+          parameterWrapper->setVector(inertialModeControllerParameters.desatMomentumRef);
           break;
         case 0x7:
-          parameterWrapper->set(inertialModeControllerParameters.desatOn);
+          parameterWrapper->set(inertialModeControllerParameters.deSatGainFactor);
           break;
         case 0x8:
-          parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction);
+          parameterWrapper->set(inertialModeControllerParameters.desatOn);
           break;
         case 0x9:
-          parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat);
+          parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction);
           break;
         case 0xA:
+          parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat);
+          break;
+        case 0xB:
           parameterWrapper->setVector(inertialModeControllerParameters.refRotRate);
           break;
         case 0xC:

mission/controller/acs/AcsParameters.h

@@ -814,7 +814,7 @@ class AcsParameters : public HasParametersIF {
         {1.0864, 0, 0}, {-0.5432, -0.5432, 1.2797}, {0, 0, 0}, {-0.5432, 0.5432, 1.2797}};
     double without4[4][3] = {
         {0.5432, 0.5432, 1.2797}, {0, -1.0864, 0}, {-0.5432, 0.5432, 1.2797}, {0, 0, 0}};
-    double nullspace[4] = {-0.5000, 0.5000, -0.5000, 0.5000};
+    double nullspaceVector[4] = {-1, 1, -1, 1};
   } rwMatrices;
 
   struct SafeModeControllerParameters {
@@ -838,7 +838,9 @@ class AcsParameters : public HasParametersIF {
     double om = 0.3;
     double omMax = 1 * M_PI / 180;
     double qiMin = 0.1;
+
     double gainNullspace = 0.01;
+    double nullspaceSpeed = 32500;  // 0.1 RPM
 
     double desatMomentumRef[3] = {0, 0, 0};
     double deSatGainFactor = 1000;

mission/controller/acs/control/PtgCtrl.cpp

@@ -136,23 +136,27 @@ void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawPara
   // concentrate RW speeds as vector and convert to double
   double speedRws[4] = {static_cast<double>(*speedRw0), static_cast<double>(*speedRw1),
                         static_cast<double>(*speedRw2), static_cast<double>(*speedRw3)};
-  double wheelMomentum[4] = {0, 0, 0, 0};
-  double rpmOffset[4] = {1, 1, 1, -1}, factor = 350 * 2 * Math::PI / 60;
-  // conversion to [rad/s] for further calculations
-  VectorOperations<double>::mulScalar(rpmOffset, factor, rpmOffset, 4);
-  VectorOperations<double>::mulScalar(speedRws, 2 * Math::PI / 60, speedRws, 4);
-  double diffRwSpeed[4] = {0, 0, 0, 0};
+
+  // calculate RPM offset utilizing the nullspace
+  double rpmOffset[4] = {0, 0, 0, 0};
+  double rpmOffsetSpeed = pointingLawParameters->nullspaceSpeed / 10 * RPM_TO_RAD_PER_SEC;
+  VectorOperations<double>::mulScalar(acsParameters->rwMatrices.nullspaceVector, rpmOffsetSpeed,
+                                      rpmOffset, 4);
+
+  // calculate resulting angular momentum
+  double rwAngMomentum[4] = {0, 0, 0, 0}, diffRwSpeed[4] = {0, 0, 0, 0};
   VectorOperations<double>::subtract(speedRws, rpmOffset, diffRwSpeed, 4);
   VectorOperations<double>::mulScalar(diffRwSpeed, acsParameters->rwHandlingParameters.inertiaWheel,
-                                      wheelMomentum, 4);
-  double gainNs = pointingLawParameters->gainNullspace;
-  double nullSpaceMatrix[4][4] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  MathOperations<double>::vecTransposeVecMatrix(acsParameters->rwMatrices.nullspace,
-                                                acsParameters->rwMatrices.nullspace,
-                                                *nullSpaceMatrix, 4);
-  MatrixOperations<double>::multiply(*nullSpaceMatrix, wheelMomentum, rwTrqNs, 4, 4, 1);
-  VectorOperations<double>::mulScalar(rwTrqNs, gainNs, rwTrqNs, 4);
-  VectorOperations<double>::mulScalar(rwTrqNs, -1, rwTrqNs, 4);
+                                      rwAngMomentum, 4);
+
+  // calculate resulting torque
+  double nullspaceMatrix[4][4] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  MatrixOperations<double>::multiply(acsParameters->rwMatrices.nullspaceVector,
+                                     acsParameters->rwMatrices.nullspaceVector, *nullspaceMatrix, 4,
+                                     1, 4);
+  MatrixOperations<double>::multiply(*nullspaceMatrix, rwAngMomentum, rwTrqNs, 4, 4, 1);
+  VectorOperations<double>::mulScalar(rwTrqNs, -1 * pointingLawParameters->gainNullspace, rwTrqNs,
+                                      4);
 }
 
 void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpeeds) {

mission/controller/acs/control/PtgCtrl.h

@@ -42,7 +42,7 @@ class PtgCtrl {
 
  private:
   const AcsParameters *acsParameters;
-  static constexpr double RAD_PER_SEC_TO_RPM = 60 / (2 * M_PI);
+  static constexpr double RPM_TO_RAD_PER_SEC = (2 * M_PI) / 60;
 };
 
 #endif /* ACS_CONTROL_PTGCTRL_H_ */