fixed nullspace controller

2023-04-27 13:52:41 +02:00
parent a71be232ac
commit 5a9da1a99c
4 changed files with 74 additions and 53 deletions
--- a/mission/controller/acs/AcsParameters.cpp
+++ b/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: