diff --git a/CHANGELOG.md b/CHANGELOG.md index e153b91b..79939ff0 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,12 +16,39 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +## Added +- `SensorProcessing` now includes an FDIR for GPS altitude. If the measured GPS altitude is out + of bounds of the range defined in the `AcsParameters`, the altitude defaults to an altitude + set in the `AcsParameters`. +- `AcsController` will now never command a RW speed larger than the maximum allowed speed. + ## Fixed - Fix for heater names: HPA heater (index 7) is now the Syrlinks heater. +- `AcsParameters` setter were previously all for scalar parameters. Now vector and matrix + parameters use their respective setters. +- Several `AcsController` components had their own implementation of `AcsParameters`. This resulted + in those parameters not being updated, while the actual ones were updated. All instances of + `AcsParameters` not belonging to `AcsController` are eiter removed or replaced by pointer + instances. +- Instead of updating the `gsTargetModeControllerParameters`, the `targetModeControllerParameters` + were updated. +- Instead of updating the `idleModeControllerParameters`, the `targetModeControllerParameters` + were updated. +- Fixed Idle Mode Controller never calling `ptgLaw` and therefore never calculating control + values. +- Fixed wrong check on wether file used for persistant boolean flag on successful still existed. +- Scaling of MTQ Cmds now scales the current values to command with the current values and not + the values of the last step, which would result in undefined behaviour. +- Solved naming collision between file used for solar array deployment and confirmation for + ACS for solar array deployment. +- Fixed that scaling of RW torque would result in a zero vector unless the maximum value was exceeded. ## Changed +- The `detumbleCounter` now does not get hard reset anymore, if the critical rate does not get + violated anymore. Instead it is incrementally reset. +- The RW antistiction now only takes the RW speeds in account. - ACS CTRL transition to DETUBMLE is now done in CTRL internally. No system level handling necessary anymore. - More fixes and improvements for SD card handling. Extend SD card setup in core controller to diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 2ae2a7bd..8d1bd369 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -6,12 +6,8 @@ AcsController::AcsController(object_id_t objectId) : ExtendedControllerBase(objectId), - sensorProcessing(&acsParameters), - navigation(&acsParameters), - actuatorCmd(&acsParameters), guidance(&acsParameters), safeCtrl(&acsParameters), - detumble(&acsParameters), ptgCtrl(&acsParameters), parameterHelper(this), mgmDataRaw(this), @@ -146,7 +142,7 @@ void AcsController::performSafe() { sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters); ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, - &susDataProcessed, &mekfData); + &susDataProcessed, &mekfData, &acsParameters); if (result != MultiplicativeKalmanFilter::MEKF_RUNNING && result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) { if (not mekfInvalidFlag) { @@ -180,7 +176,9 @@ void AcsController::performSafe() { // ToDo: this should never ever happen or we are dead. prob add an event at least } - actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs); + actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs, + *acsParameters.magnetorquerParameter.inverseAlignment, + acsParameters.magnetorquerParameter.dipolMax); // detumble check and switch if (mekfData.satRotRateMekf.isValid() && @@ -191,8 +189,8 @@ void AcsController::performSafe() { VectorOperations::norm(gyrDataProcessed.gyrVecTot.value, 3) > acsParameters.detumbleParameter.omegaDetumbleStart) { detumbleCounter++; - } else { - detumbleCounter = 0; + } else if (detumbleCounter > 0) { + detumbleCounter -= 1; } if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) { detumbleCounter = 0; @@ -203,7 +201,7 @@ void AcsController::performSafe() { updateCtrlValData(errAng); updateActuatorCmdData(cmdDipolMtqs); - // commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2]/*500, 500, 500*/, + // commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], // acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0, // acsParameters.rwHandlingParameters.rampTime); } @@ -215,7 +213,7 @@ void AcsController::performDetumble() { sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters); ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, - &susDataProcessed, &mekfData); + &susDataProcessed, &mekfData, &acsParameters); if (result != MultiplicativeKalmanFilter::MEKF_RUNNING && result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) { if (not mekfInvalidFlag) { @@ -228,8 +226,11 @@ void AcsController::performDetumble() { double magMomMtq[3] = {0, 0, 0}; detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), magMomMtq); - actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs); + mgmDataProcessed.mgmVecTot.isValid(), magMomMtq, + acsParameters.detumbleParameter.gainD); + actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs, + *acsParameters.magnetorquerParameter.inverseAlignment, + acsParameters.magnetorquerParameter.dipolMax); if (mekfData.satRotRateMekf.isValid() && VectorOperations::norm(mekfData.satRotRateMekf.value, 3) < @@ -239,8 +240,8 @@ void AcsController::performDetumble() { VectorOperations::norm(gyrDataProcessed.gyrVecTot.value, 3) < acsParameters.detumbleParameter.omegaDetumbleEnd) { detumbleCounter++; - } else { - detumbleCounter = 0; + } else if (detumbleCounter > 0) { + detumbleCounter -= 1; } if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) { detumbleCounter = 0; @@ -263,7 +264,7 @@ void AcsController::performPointingCtrl() { sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters); ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, - &susDataProcessed, &mekfData); + &susDataProcessed, &mekfData, &acsParameters); if (result != MultiplicativeKalmanFilter::MEKF_RUNNING && result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) { if (not mekfInvalidFlag) { @@ -296,24 +297,26 @@ void AcsController::performPointingCtrl() { } else { multipleRwUnavailableCounter = 0; } - double torquePtgRws[4] = {0, 0, 0, 0}, rwTrqNs[4] = {0, 0, 0, 0}; - double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0}; - double mgtDpDes[3] = {0, 0, 0}; // Variables required for guidance double targetQuat[4] = {0, 0, 0, 1}, targetSatRotRate[3] = {0, 0, 0}, errorQuat[4] = {0, 0, 0, 1}, errorAngle = 0, errorSatRotRate[3] = {0, 0, 0}; + // Variables required for setting actuators + double torquePtgRws[4] = {0, 0, 0, 0}, rwTrqNs[4] = {0, 0, 0, 0}, torqueRws[4] = {0, 0, 0, 0}, + mgtDpDes[3] = {0, 0, 0}; switch (submode) { case acs::PTG_IDLE: guidance.targetQuatPtgSun(susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate); guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, targetSatRotRate, errorQuat, errorSatRotRate, errorAngle); + ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate, + *rwPseudoInv, torquePtgRws); ptgCtrl.ptgNullspace( &acsParameters.idleModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); - actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); + actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, @@ -337,7 +340,7 @@ void AcsController::performPointingCtrl() { &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); - actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); + actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, @@ -351,20 +354,20 @@ void AcsController::performPointingCtrl() { susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate); guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, targetSatRotRate, errorQuat, errorSatRotRate, errorAngle); - ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate, + ptgCtrl.ptgLaw(&acsParameters.gsTargetModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); ptgCtrl.ptgNullspace( - &acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), + &acsParameters.gsTargetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); - actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); + actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( - &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, + &acsParameters.gsTargetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); - enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; + enableAntiStiction = acsParameters.gsTargetModeControllerParameters.enableAntiStiction; break; case acs::PTG_NADIR: @@ -382,7 +385,7 @@ void AcsController::performPointingCtrl() { &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); - actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); + actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, @@ -405,7 +408,7 @@ void AcsController::performPointingCtrl() { &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); - actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); + actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, @@ -415,18 +418,21 @@ void AcsController::performPointingCtrl() { break; } + actuatorCmd.cmdSpeedToRws( + sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, torqueRws, + cmdSpeedRws, acsParameters.onBoardParams.sampleTime, + acsParameters.rwHandlingParameters.maxRwSpeed, + acsParameters.rwHandlingParameters.inertiaWheel); if (enableAntiStiction) { - ptgCtrl.rwAntistiction(&sensorValues, torqueRwsScaled); + ptgCtrl.rwAntistiction(&sensorValues, cmdSpeedRws); } - - actuatorCmd.cmdSpeedToRws(sensorValues.rw1Set.currSpeed.value, - sensorValues.rw2Set.currSpeed.value, - sensorValues.rw3Set.currSpeed.value, - sensorValues.rw4Set.currSpeed.value, torqueRwsScaled, cmdSpeedRws); - actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs); + actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs, + *acsParameters.magnetorquerParameter.inverseAlignment, + acsParameters.magnetorquerParameter.dipolMax); updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate); - updateActuatorCmdData(rwTrqNs, cmdSpeedRws, cmdDipolMtqs); + updateActuatorCmdData(torqueRws, cmdSpeedRws, cmdDipolMtqs); // commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], // acsParameters.magnetorquesParameter.torqueDuration, cmdSpeedRws[0], // cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], @@ -584,6 +590,7 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD // GPS Processed localDataPoolMap.emplace(acsctrl::PoolIds::GC_LATITUDE, &gcLatitude); localDataPoolMap.emplace(acsctrl::PoolIds::GD_LONGITUDE, &gdLongitude); + localDataPoolMap.emplace(acsctrl::PoolIds::ALTITUDE, &altitude); localDataPoolMap.emplace(acsctrl::PoolIds::GPS_POSITION, &gpsPosition); localDataPoolMap.emplace(acsctrl::PoolIds::GPS_VELOCITY, &gpsVelocity); poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), false, 5.0}); diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 91ca32d6..c0376127 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -190,6 +190,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes acsctrl::GpsDataProcessed gpsDataProcessed; PoolEntry gcLatitude = PoolEntry(); PoolEntry gdLongitude = PoolEntry(); + PoolEntry altitude = PoolEntry(); PoolEntry gpsPosition = PoolEntry(3); PoolEntry gpsVelocity = PoolEntry(3); diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 2e5fbb1b..4abe0d7a 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -30,19 +30,19 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case 0x2: // InertiaEIVE switch (parameterId) { case 0x0: - parameterWrapper->set(inertiaEIVE.inertiaMatrix); + parameterWrapper->setMatrix(inertiaEIVE.inertiaMatrix); break; case 0x1: - parameterWrapper->set(inertiaEIVE.inertiaMatrixDeployed); + parameterWrapper->setMatrix(inertiaEIVE.inertiaMatrixDeployed); break; case 0x2: - parameterWrapper->set(inertiaEIVE.inertiaMatrixUndeployed); + parameterWrapper->setMatrix(inertiaEIVE.inertiaMatrixUndeployed); break; case 0x3: - parameterWrapper->set(inertiaEIVE.inertiaMatrixPanel1); + parameterWrapper->setMatrix(inertiaEIVE.inertiaMatrixPanel1); break; case 0x4: - parameterWrapper->set(inertiaEIVE.inertiaMatrixPanel3); + parameterWrapper->setMatrix(inertiaEIVE.inertiaMatrixPanel3); break; default: return INVALID_IDENTIFIER_ID; @@ -51,58 +51,58 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case 0x3: // MgmHandlingParameters switch (parameterId) { case 0x0: - parameterWrapper->set(mgmHandlingParameters.mgm0orientationMatrix); + parameterWrapper->setMatrix(mgmHandlingParameters.mgm0orientationMatrix); break; case 0x1: - parameterWrapper->set(mgmHandlingParameters.mgm1orientationMatrix); + parameterWrapper->setMatrix(mgmHandlingParameters.mgm1orientationMatrix); break; case 0x2: - parameterWrapper->set(mgmHandlingParameters.mgm2orientationMatrix); + parameterWrapper->setMatrix(mgmHandlingParameters.mgm2orientationMatrix); break; case 0x3: - parameterWrapper->set(mgmHandlingParameters.mgm3orientationMatrix); + parameterWrapper->setMatrix(mgmHandlingParameters.mgm3orientationMatrix); break; case 0x4: - parameterWrapper->set(mgmHandlingParameters.mgm4orientationMatrix); + parameterWrapper->setMatrix(mgmHandlingParameters.mgm4orientationMatrix); break; case 0x5: - parameterWrapper->set(mgmHandlingParameters.mgm0hardIronOffset); + parameterWrapper->setVector(mgmHandlingParameters.mgm0hardIronOffset); break; case 0x6: - parameterWrapper->set(mgmHandlingParameters.mgm1hardIronOffset); + parameterWrapper->setVector(mgmHandlingParameters.mgm1hardIronOffset); break; case 0x7: - parameterWrapper->set(mgmHandlingParameters.mgm2hardIronOffset); + parameterWrapper->setVector(mgmHandlingParameters.mgm2hardIronOffset); break; case 0x8: - parameterWrapper->set(mgmHandlingParameters.mgm3hardIronOffset); + parameterWrapper->setVector(mgmHandlingParameters.mgm3hardIronOffset); break; case 0x9: - parameterWrapper->set(mgmHandlingParameters.mgm4hardIronOffset); + parameterWrapper->setVector(mgmHandlingParameters.mgm4hardIronOffset); break; case 0xA: - parameterWrapper->set(mgmHandlingParameters.mgm0softIronInverse); + parameterWrapper->setMatrix(mgmHandlingParameters.mgm0softIronInverse); break; case 0xB: - parameterWrapper->set(mgmHandlingParameters.mgm1softIronInverse); + parameterWrapper->setMatrix(mgmHandlingParameters.mgm1softIronInverse); break; case 0xC: - parameterWrapper->set(mgmHandlingParameters.mgm2softIronInverse); + parameterWrapper->setMatrix(mgmHandlingParameters.mgm2softIronInverse); break; case 0xD: - parameterWrapper->set(mgmHandlingParameters.mgm3softIronInverse); + parameterWrapper->setMatrix(mgmHandlingParameters.mgm3softIronInverse); break; case 0xE: - parameterWrapper->set(mgmHandlingParameters.mgm4softIronInverse); + parameterWrapper->setMatrix(mgmHandlingParameters.mgm4softIronInverse); break; case 0xF: - parameterWrapper->set(mgmHandlingParameters.mgm02variance); + parameterWrapper->setVector(mgmHandlingParameters.mgm02variance); break; case 0x10: - parameterWrapper->set(mgmHandlingParameters.mgm13variance); + parameterWrapper->setVector(mgmHandlingParameters.mgm13variance); break; case 0x11: - parameterWrapper->set(mgmHandlingParameters.mgm4variance); + parameterWrapper->setVector(mgmHandlingParameters.mgm4variance); break; default: return INVALID_IDENTIFIER_ID; @@ -111,112 +111,112 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case 0x4: // SusHandlingParameters switch (parameterId) { case 0x0: - parameterWrapper->set(susHandlingParameters.sus0orientationMatrix); + parameterWrapper->setMatrix(susHandlingParameters.sus0orientationMatrix); break; case 0x1: - parameterWrapper->set(susHandlingParameters.sus1orientationMatrix); + parameterWrapper->setMatrix(susHandlingParameters.sus1orientationMatrix); break; case 0x2: - parameterWrapper->set(susHandlingParameters.sus2orientationMatrix); + parameterWrapper->setMatrix(susHandlingParameters.sus2orientationMatrix); break; case 0x3: - parameterWrapper->set(susHandlingParameters.sus3orientationMatrix); + parameterWrapper->setMatrix(susHandlingParameters.sus3orientationMatrix); break; case 0x4: - parameterWrapper->set(susHandlingParameters.sus4orientationMatrix); + parameterWrapper->setMatrix(susHandlingParameters.sus4orientationMatrix); break; case 0x5: - parameterWrapper->set(susHandlingParameters.sus5orientationMatrix); + parameterWrapper->setMatrix(susHandlingParameters.sus5orientationMatrix); break; case 0x6: - parameterWrapper->set(susHandlingParameters.sus6orientationMatrix); + parameterWrapper->setMatrix(susHandlingParameters.sus6orientationMatrix); break; case 0x7: - parameterWrapper->set(susHandlingParameters.sus7orientationMatrix); + parameterWrapper->setMatrix(susHandlingParameters.sus7orientationMatrix); break; case 0x8: - parameterWrapper->set(susHandlingParameters.sus8orientationMatrix); + parameterWrapper->setMatrix(susHandlingParameters.sus8orientationMatrix); break; case 0x9: - parameterWrapper->set(susHandlingParameters.sus9orientationMatrix); + parameterWrapper->setMatrix(susHandlingParameters.sus9orientationMatrix); break; case 0xA: - parameterWrapper->set(susHandlingParameters.sus10orientationMatrix); + parameterWrapper->setMatrix(susHandlingParameters.sus10orientationMatrix); break; case 0xB: - parameterWrapper->set(susHandlingParameters.sus11orientationMatrix); + parameterWrapper->setMatrix(susHandlingParameters.sus11orientationMatrix); break; case 0xC: - parameterWrapper->set(susHandlingParameters.sus0coeffAlpha); + parameterWrapper->setMatrix(susHandlingParameters.sus0coeffAlpha); break; case 0xD: - parameterWrapper->set(susHandlingParameters.sus0coeffBeta); + parameterWrapper->setMatrix(susHandlingParameters.sus0coeffBeta); break; case 0xE: - parameterWrapper->set(susHandlingParameters.sus1coeffAlpha); + parameterWrapper->setMatrix(susHandlingParameters.sus1coeffAlpha); break; case 0xF: - parameterWrapper->set(susHandlingParameters.sus1coeffBeta); + parameterWrapper->setMatrix(susHandlingParameters.sus1coeffBeta); break; case 0x10: - parameterWrapper->set(susHandlingParameters.sus2coeffAlpha); + parameterWrapper->setMatrix(susHandlingParameters.sus2coeffAlpha); break; case 0x11: - parameterWrapper->set(susHandlingParameters.sus2coeffBeta); + parameterWrapper->setMatrix(susHandlingParameters.sus2coeffBeta); break; case 0x12: - parameterWrapper->set(susHandlingParameters.sus3coeffAlpha); + parameterWrapper->setMatrix(susHandlingParameters.sus3coeffAlpha); break; case 0x13: - parameterWrapper->set(susHandlingParameters.sus3coeffBeta); + parameterWrapper->setMatrix(susHandlingParameters.sus3coeffBeta); break; case 0x14: - parameterWrapper->set(susHandlingParameters.sus4coeffAlpha); + parameterWrapper->setMatrix(susHandlingParameters.sus4coeffAlpha); break; case 0x15: - parameterWrapper->set(susHandlingParameters.sus4coeffBeta); + parameterWrapper->setMatrix(susHandlingParameters.sus4coeffBeta); break; case 0x16: - parameterWrapper->set(susHandlingParameters.sus5coeffAlpha); + parameterWrapper->setMatrix(susHandlingParameters.sus5coeffAlpha); break; case 0x17: - parameterWrapper->set(susHandlingParameters.sus5coeffBeta); + parameterWrapper->setMatrix(susHandlingParameters.sus5coeffBeta); break; case 0x18: - parameterWrapper->set(susHandlingParameters.sus6coeffAlpha); + parameterWrapper->setMatrix(susHandlingParameters.sus6coeffAlpha); break; case 0x19: - parameterWrapper->set(susHandlingParameters.sus6coeffBeta); + parameterWrapper->setMatrix(susHandlingParameters.sus6coeffBeta); break; case 0x1A: - parameterWrapper->set(susHandlingParameters.sus7coeffAlpha); + parameterWrapper->setMatrix(susHandlingParameters.sus7coeffAlpha); break; case 0x1B: - parameterWrapper->set(susHandlingParameters.sus7coeffBeta); + parameterWrapper->setMatrix(susHandlingParameters.sus7coeffBeta); break; case 0x1C: - parameterWrapper->set(susHandlingParameters.sus8coeffAlpha); + parameterWrapper->setMatrix(susHandlingParameters.sus8coeffAlpha); break; case 0x1D: - parameterWrapper->set(susHandlingParameters.sus8coeffBeta); + parameterWrapper->setMatrix(susHandlingParameters.sus8coeffBeta); break; case 0x1E: - parameterWrapper->set(susHandlingParameters.sus9coeffAlpha); + parameterWrapper->setMatrix(susHandlingParameters.sus9coeffAlpha); break; case 0x1F: - parameterWrapper->set(susHandlingParameters.sus9coeffBeta); + parameterWrapper->setMatrix(susHandlingParameters.sus9coeffBeta); break; case 0x20: - parameterWrapper->set(susHandlingParameters.sus10coeffAlpha); + parameterWrapper->setMatrix(susHandlingParameters.sus10coeffAlpha); break; case 0x21: - parameterWrapper->set(susHandlingParameters.sus10coeffBeta); + parameterWrapper->setMatrix(susHandlingParameters.sus10coeffBeta); break; case 0x22: - parameterWrapper->set(susHandlingParameters.sus11coeffAlpha); + parameterWrapper->setMatrix(susHandlingParameters.sus11coeffAlpha); break; case 0x23: - parameterWrapper->set(susHandlingParameters.sus11coeffBeta); + parameterWrapper->setMatrix(susHandlingParameters.sus11coeffBeta); break; default: return INVALID_IDENTIFIER_ID; @@ -225,34 +225,34 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case (0x5): // GyrHandlingParameters switch (parameterId) { case 0x0: - parameterWrapper->set(gyrHandlingParameters.gyr0orientationMatrix); + parameterWrapper->setMatrix(gyrHandlingParameters.gyr0orientationMatrix); break; case 0x1: - parameterWrapper->set(gyrHandlingParameters.gyr1orientationMatrix); + parameterWrapper->setMatrix(gyrHandlingParameters.gyr1orientationMatrix); break; case 0x2: - parameterWrapper->set(gyrHandlingParameters.gyr2orientationMatrix); + parameterWrapper->setMatrix(gyrHandlingParameters.gyr2orientationMatrix); break; case 0x3: - parameterWrapper->set(gyrHandlingParameters.gyr3orientationMatrix); + parameterWrapper->setMatrix(gyrHandlingParameters.gyr3orientationMatrix); break; case 0x4: - parameterWrapper->set(gyrHandlingParameters.gyr0bias); + parameterWrapper->setVector(gyrHandlingParameters.gyr0bias); break; case 0x5: - parameterWrapper->set(gyrHandlingParameters.gyr1bias); + parameterWrapper->setVector(gyrHandlingParameters.gyr1bias); break; case 0x6: - parameterWrapper->set(gyrHandlingParameters.gyr2bias); + parameterWrapper->setVector(gyrHandlingParameters.gyr2bias); break; case 0x7: - parameterWrapper->set(gyrHandlingParameters.gyr3bias); + parameterWrapper->setVector(gyrHandlingParameters.gyr3bias); break; case 0x8: - parameterWrapper->set(gyrHandlingParameters.gyr02variance); + parameterWrapper->setVector(gyrHandlingParameters.gyr02variance); break; case 0x9: - parameterWrapper->set(gyrHandlingParameters.gyr13variance); + parameterWrapper->setVector(gyrHandlingParameters.gyr13variance); break; case 0xA: parameterWrapper->set(gyrHandlingParameters.preferAdis); @@ -270,15 +270,18 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(rwHandlingParameters.maxTrq); break; case 0x2: - parameterWrapper->set(rwHandlingParameters.stictionSpeed); + parameterWrapper->set(rwHandlingParameters.maxRwSpeed); break; case 0x3: - parameterWrapper->set(rwHandlingParameters.stictionReleaseSpeed); + parameterWrapper->set(rwHandlingParameters.stictionSpeed); break; case 0x4: - parameterWrapper->set(rwHandlingParameters.stictionTorque); + parameterWrapper->set(rwHandlingParameters.stictionReleaseSpeed); break; case 0x5: + parameterWrapper->set(rwHandlingParameters.stictionTorque); + break; + case 0x6: parameterWrapper->set(rwHandlingParameters.rampTime); break; default: @@ -288,25 +291,25 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case (0x7): // RwMatrices switch (parameterId) { case 0x0: - parameterWrapper->set(rwMatrices.alignmentMatrix); + parameterWrapper->setMatrix(rwMatrices.alignmentMatrix); break; case 0x1: - parameterWrapper->set(rwMatrices.pseudoInverse); + parameterWrapper->setMatrix(rwMatrices.pseudoInverse); break; case 0x2: - parameterWrapper->set(rwMatrices.without1); + parameterWrapper->setMatrix(rwMatrices.without1); break; case 0x3: - parameterWrapper->set(rwMatrices.without2); + parameterWrapper->setMatrix(rwMatrices.without2); break; case 0x4: - parameterWrapper->set(rwMatrices.without3); + parameterWrapper->setMatrix(rwMatrices.without3); break; case 0x5: - parameterWrapper->set(rwMatrices.without4); + parameterWrapper->setMatrix(rwMatrices.without4); break; case 0x6: - parameterWrapper->set(rwMatrices.nullspace); + parameterWrapper->setVector(rwMatrices.nullspace); break; default: return INVALID_IDENTIFIER_ID; @@ -330,13 +333,13 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(safeModeControllerParameters.sunMagAngleMin); break; case 0x5: - parameterWrapper->set(safeModeControllerParameters.sunTargetDirLeop); + parameterWrapper->setVector(safeModeControllerParameters.sunTargetDirLeop); break; case 0x6: - parameterWrapper->set(safeModeControllerParameters.sunTargetDir); + parameterWrapper->setVector(safeModeControllerParameters.sunTargetDir); break; case 0x7: - parameterWrapper->set(safeModeControllerParameters.satRateRef); + parameterWrapper->setVector(safeModeControllerParameters.satRateRef); break; default: return INVALID_IDENTIFIER_ID; @@ -345,31 +348,31 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case (0x9): // IdleModeControllerParameters switch (parameterId) { case 0x0: - parameterWrapper->set(targetModeControllerParameters.zeta); + parameterWrapper->set(idleModeControllerParameters.zeta); break; case 0x1: - parameterWrapper->set(targetModeControllerParameters.om); + parameterWrapper->set(idleModeControllerParameters.om); break; case 0x2: - parameterWrapper->set(targetModeControllerParameters.omMax); + parameterWrapper->set(idleModeControllerParameters.omMax); break; case 0x3: - parameterWrapper->set(targetModeControllerParameters.qiMin); + parameterWrapper->set(idleModeControllerParameters.qiMin); break; case 0x4: - parameterWrapper->set(targetModeControllerParameters.gainNullspace); + parameterWrapper->set(idleModeControllerParameters.gainNullspace); break; case 0x5: - parameterWrapper->set(targetModeControllerParameters.desatMomentumRef); + parameterWrapper->setVector(idleModeControllerParameters.desatMomentumRef); break; case 0x6: - parameterWrapper->set(targetModeControllerParameters.deSatGainFactor); + parameterWrapper->set(idleModeControllerParameters.deSatGainFactor); break; case 0x7: - parameterWrapper->set(targetModeControllerParameters.desatOn); + parameterWrapper->set(idleModeControllerParameters.desatOn); break; case 0x8: - parameterWrapper->set(targetModeControllerParameters.enableAntiStiction); + parameterWrapper->set(idleModeControllerParameters.enableAntiStiction); break; default: @@ -394,7 +397,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(targetModeControllerParameters.gainNullspace); break; case 0x5: - parameterWrapper->set(targetModeControllerParameters.desatMomentumRef); + parameterWrapper->setVector(targetModeControllerParameters.desatMomentumRef); break; case 0x6: parameterWrapper->set(targetModeControllerParameters.deSatGainFactor); @@ -406,13 +409,13 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(targetModeControllerParameters.enableAntiStiction); break; case 0x9: - parameterWrapper->set(targetModeControllerParameters.refDirection); + parameterWrapper->setVector(targetModeControllerParameters.refDirection); break; case 0xA: - parameterWrapper->set(targetModeControllerParameters.refRotRate); + parameterWrapper->setVector(targetModeControllerParameters.refRotRate); break; case 0xB: - parameterWrapper->set(targetModeControllerParameters.quatRef); + parameterWrapper->setVector(targetModeControllerParameters.quatRef); break; case 0xC: parameterWrapper->set(targetModeControllerParameters.timeElapsedMax); @@ -445,52 +448,46 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case (0xB): // GsTargetModeControllerParameters switch (parameterId) { case 0x0: - parameterWrapper->set(targetModeControllerParameters.zeta); + parameterWrapper->set(gsTargetModeControllerParameters.zeta); break; case 0x1: - parameterWrapper->set(targetModeControllerParameters.om); + parameterWrapper->set(gsTargetModeControllerParameters.om); break; case 0x2: - parameterWrapper->set(targetModeControllerParameters.omMax); + parameterWrapper->set(gsTargetModeControllerParameters.omMax); break; case 0x3: - parameterWrapper->set(targetModeControllerParameters.qiMin); + parameterWrapper->set(gsTargetModeControllerParameters.qiMin); break; case 0x4: - parameterWrapper->set(targetModeControllerParameters.gainNullspace); + parameterWrapper->set(gsTargetModeControllerParameters.gainNullspace); break; case 0x5: - parameterWrapper->set(targetModeControllerParameters.desatMomentumRef); + parameterWrapper->setVector(gsTargetModeControllerParameters.desatMomentumRef); break; case 0x6: - parameterWrapper->set(targetModeControllerParameters.deSatGainFactor); + parameterWrapper->set(gsTargetModeControllerParameters.deSatGainFactor); break; case 0x7: - parameterWrapper->set(targetModeControllerParameters.desatOn); + parameterWrapper->set(gsTargetModeControllerParameters.desatOn); break; case 0x8: - parameterWrapper->set(targetModeControllerParameters.enableAntiStiction); + parameterWrapper->set(gsTargetModeControllerParameters.enableAntiStiction); break; case 0x9: - parameterWrapper->set(targetModeControllerParameters.refDirection); + parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection); break; case 0xA: - parameterWrapper->set(targetModeControllerParameters.refRotRate); + parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax); break; case 0xB: - parameterWrapper->set(targetModeControllerParameters.quatRef); + parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt); break; case 0xC: - parameterWrapper->set(targetModeControllerParameters.timeElapsedMax); + parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt); break; case 0xD: - parameterWrapper->set(targetModeControllerParameters.latitudeTgt); - break; - case 0xE: - parameterWrapper->set(targetModeControllerParameters.longitudeTgt); - break; - case 0xF: - parameterWrapper->set(targetModeControllerParameters.altitudeTgt); + parameterWrapper->set(gsTargetModeControllerParameters.altitudeTgt); break; default: return INVALID_IDENTIFIER_ID; @@ -514,7 +511,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(nadirModeControllerParameters.gainNullspace); break; case 0x5: - parameterWrapper->set(nadirModeControllerParameters.desatMomentumRef); + parameterWrapper->setVector(nadirModeControllerParameters.desatMomentumRef); break; case 0x6: parameterWrapper->set(nadirModeControllerParameters.deSatGainFactor); @@ -526,10 +523,10 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction); break; case 0x9: - parameterWrapper->set(nadirModeControllerParameters.refDirection); + parameterWrapper->setVector(nadirModeControllerParameters.refDirection); break; case 0xA: - parameterWrapper->set(nadirModeControllerParameters.quatRef); + parameterWrapper->setVector(nadirModeControllerParameters.quatRef); break; case 0xC: parameterWrapper->set(nadirModeControllerParameters.timeElapsedMax); @@ -556,7 +553,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(inertialModeControllerParameters.gainNullspace); break; case 0x5: - parameterWrapper->set(inertialModeControllerParameters.desatMomentumRef); + parameterWrapper->setVector(inertialModeControllerParameters.desatMomentumRef); break; case 0x6: parameterWrapper->set(inertialModeControllerParameters.deSatGainFactor); @@ -568,13 +565,13 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction); break; case 0x9: - parameterWrapper->set(inertialModeControllerParameters.tgtQuat); + parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat); break; case 0xA: - parameterWrapper->set(inertialModeControllerParameters.refRotRate); + parameterWrapper->setVector(inertialModeControllerParameters.refRotRate); break; case 0xC: - parameterWrapper->set(inertialModeControllerParameters.quatRef); + parameterWrapper->setVector(inertialModeControllerParameters.quatRef); break; default: return INVALID_IDENTIFIER_ID; @@ -586,7 +583,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(strParameters.exclusionAngle); break; case 0x1: - parameterWrapper->set(strParameters.boresightAxis); + parameterWrapper->setVector(strParameters.boresightAxis); break; default: return INVALID_IDENTIFIER_ID; @@ -597,6 +594,15 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case 0x0: parameterWrapper->set(gpsParameters.timeDiffVelocityMax); break; + case 0x1: + parameterWrapper->set(gpsParameters.minimumFdirAltitude); + break; + case 0x2: + parameterWrapper->set(gpsParameters.maximumFdirAltitude); + break; + case 0x3: + parameterWrapper->set(gpsParameters.fdirAltitude); + break; default: return INVALID_IDENTIFIER_ID; } @@ -658,25 +664,25 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case (0x12): // MagnetorquesParameter switch (parameterId) { case 0x0: - parameterWrapper->set(magnetorquesParameter.mtq0orientationMatrix); + parameterWrapper->setMatrix(magnetorquerParameter.mtq0orientationMatrix); break; case 0x1: - parameterWrapper->set(magnetorquesParameter.mtq1orientationMatrix); + parameterWrapper->setMatrix(magnetorquerParameter.mtq1orientationMatrix); break; case 0x2: - parameterWrapper->set(magnetorquesParameter.mtq2orientationMatrix); + parameterWrapper->setMatrix(magnetorquerParameter.mtq2orientationMatrix); break; case 0x3: - parameterWrapper->set(magnetorquesParameter.alignmentMatrixMtq); + parameterWrapper->setMatrix(magnetorquerParameter.alignmentMatrixMtq); break; case 0x4: - parameterWrapper->set(magnetorquesParameter.inverseAlignment); + parameterWrapper->setMatrix(magnetorquerParameter.inverseAlignment); break; case 0x5: - parameterWrapper->set(magnetorquesParameter.DipolMax); + parameterWrapper->set(magnetorquerParameter.dipolMax); break; case 0x6: - parameterWrapper->set(magnetorquesParameter.torqueDuration); + parameterWrapper->set(magnetorquerParameter.torqueDuration); break; default: return INVALID_IDENTIFIER_ID; diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 11de10a3..822cfabc 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -772,7 +772,7 @@ class AcsParameters : public HasParametersIF { double gyr2bias[3] = {0.15039212820512823, 0.7094475589743591, -0.22298363589743594}; double gyr3bias[3] = {0.0021730769230769217, -0.6655897435897435, 0.034096153846153845}; - /* var = sqrt(sigma), sigma = RND*sqrt(freq), following values are RND^2 and not var as freq is + /* var = sigma^2, sigma = RND*sqrt(freq), following values are RND^2 and not var as freq is * assumed to be equal for the same class of sensors */ float gyr02variance[3] = {pow(3.0e-3, 2), // RND_x = 3.0e-3 deg/s/sqrt(Hz) rms pow(3.0e-3, 2), // RND_y = 3.0e-3 deg/s/sqrt(Hz) rms @@ -783,9 +783,10 @@ class AcsParameters : public HasParametersIF { struct RwHandlingParameters { double inertiaWheel = 0.000028198; - double maxTrq = 0.0032; // 3.2 [mNm] - int32_t stictionSpeed = 100; // RPM - int32_t stictionReleaseSpeed = 120; // RPM + double maxTrq = 0.0032; // 3.2 [mNm] + int32_t maxRwSpeed = 65000; // 0.1 RPM + int32_t stictionSpeed = 1000; // 0.1 RPM + int32_t stictionReleaseSpeed = 1000; // 0.1 RPM double stictionTorque = 0.0006; uint16_t rampTime = 10; @@ -817,7 +818,7 @@ class AcsParameters : public HasParametersIF { double sunMagAngleMin = 5 * M_PI / 180; - double sunTargetDirLeop[3] = {0, .5, .5}; + double sunTargetDirLeop[3] = {0, sqrt(.5), sqrt(.5)}; double sunTargetDir[3] = {0, 0, 1}; double satRateRef[3] = {0, 0, 0}; @@ -843,7 +844,7 @@ class AcsParameters : public HasParametersIF { double refDirection[3] = {-1, 0, 0}; // Antenna double refRotRate[3] = {0, 0, 0}; double quatRef[4] = {0, 0, 0, 1}; - int8_t timeElapsedMax = 10; // rot rate calculations + uint8_t timeElapsedMax = 10; // rot rate calculations // Default is Stuttgart GS double latitudeTgt = 48.7495 * M_PI / 180.; // [rad] Latitude @@ -859,7 +860,7 @@ class AcsParameters : public HasParametersIF { struct GsTargetModeControllerParameters : PointingLawParameters { double refDirection[3] = {-1, 0, 0}; // Antenna - int8_t timeElapsedMax = 10; // rot rate calculations + uint8_t timeElapsedMax = 10; // rot rate calculations // Default is Stuttgart GS double latitudeTgt = 48.7495 * M_PI / 180.; // [rad] Latitude @@ -871,7 +872,7 @@ class AcsParameters : public HasParametersIF { double refDirection[3] = {-1, 0, 0}; // Antenna double quatRef[4] = {0, 0, 0, 1}; double refRotRate[3] = {0, 0, 0}; - int8_t timeElapsedMax = 10; // rot rate calculations + uint8_t timeElapsedMax = 10; // rot rate calculations } nadirModeControllerParameters; struct InertialModeControllerParameters : PointingLawParameters { @@ -886,7 +887,10 @@ class AcsParameters : public HasParametersIF { } strParameters; struct GpsParameters { - double timeDiffVelocityMax = 30; //[s] + double timeDiffVelocityMax = 30; // [s] + double minimumFdirAltitude = 475 * 1e3; // [m] + double maximumFdirAltitude = 575 * 1e3; // [m] + double fdirAltitude = 525 * 1e3; // [m] } gpsParameters; struct SunModelParameters { @@ -912,16 +916,16 @@ class AcsParameters : public HasParametersIF { double sensorNoiseBsGYR = 3 * M_PI / 180 / 3600; // Bias Stability } kalmanFilterParameters; - struct MagnetorquesParameter { + struct MagnetorquerParameter { double mtq0orientationMatrix[3][3] = {{1, 0, 0}, {0, 0, 1}, {0, -1, 0}}; double mtq1orientationMatrix[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; double mtq2orientationMatrix[3][3] = {{0, 0, 1}, {0, 1, 0}, {-1, 0, 0}}; double alignmentMatrixMtq[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}}; double inverseAlignment[3][3] = {{0, -1, 0}, {0, 0, 1}, {-1, 0, 0}}; - double DipolMax = 0.2; // [Am^2] + double dipolMax = 0.2; // [Am^2] uint16_t torqueDuration = 300; // [ms] - } magnetorquesParameter; + } magnetorquerParameter; struct DetumbleParameter { uint8_t detumblecounter = 75; // 30 s diff --git a/mission/controller/acs/ActuatorCmd.cpp b/mission/controller/acs/ActuatorCmd.cpp index 8cda84d6..f32ce241 100644 --- a/mission/controller/acs/ActuatorCmd.cpp +++ b/mission/controller/acs/ActuatorCmd.cpp @@ -10,64 +10,60 @@ #include "util/CholeskyDecomposition.h" #include "util/MathOperations.h" -ActuatorCmd::ActuatorCmd(AcsParameters *acsParameters_) { acsParameters = *acsParameters_; } +ActuatorCmd::ActuatorCmd() {} ActuatorCmd::~ActuatorCmd() {} -void ActuatorCmd::scalingTorqueRws(const double *rwTrq, double *rwTrqScaled) { - // Scaling the commanded torque to a maximum value - double maxTrq = acsParameters.rwHandlingParameters.maxTrq; +void ActuatorCmd::scalingTorqueRws(double *rwTrq, double maxTorque) { + uint8_t maxIdx = 0; + VectorOperations::maxAbsValue(rwTrq, 4, &maxIdx); + double maxValue = rwTrq[maxIdx]; - double maxValue = 0; - for (int i = 0; i < 4; i++) { // size of torque, always 4 ? - if (abs(rwTrq[i]) > maxValue) { - maxValue = abs(rwTrq[i]); - } - } - - if (maxValue > maxTrq) { - double scalingFactor = maxTrq / maxValue; - VectorOperations::mulScalar(rwTrq, scalingFactor, rwTrqScaled, 4); + if (maxValue > maxTorque) { + double scalingFactor = maxTorque / maxValue; + VectorOperations::mulScalar(rwTrq, scalingFactor, rwTrq, 4); } } -void ActuatorCmd::cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, - const int32_t speedRw2, const int32_t speedRw3, - const double *rwTorque, int32_t *rwCmdSpeed) { +void ActuatorCmd::cmdSpeedToRws(int32_t speedRw0, int32_t speedRw1, int32_t speedRw2, + int32_t speedRw3, const double *rwTorque, int32_t *rwCmdSpeed, + double sampleTime, int32_t maxRwSpeed, double inertiaWheel) { using namespace Math; // Calculating the commanded speed in RPM for every reaction wheel int32_t speedRws[4] = {speedRw0, speedRw1, speedRw2, speedRw3}; double deltaSpeed[4] = {0, 0, 0, 0}; - double commandTime = acsParameters.onBoardParams.sampleTime, - inertiaWheel = acsParameters.rwHandlingParameters.inertiaWheel; double radToRpm = 60 / (2 * PI); // factor for conversion to RPM // W_RW = Torque_RW / I_RW * delta t [rad/s] - double factor = commandTime / inertiaWheel * radToRpm; + double factor = sampleTime / inertiaWheel * radToRpm; int32_t deltaSpeedInt[4] = {0, 0, 0, 0}; VectorOperations::mulScalar(rwTorque, factor, deltaSpeed, 4); for (int i = 0; i < 4; i++) { deltaSpeedInt[i] = std::round(deltaSpeed[i]); } VectorOperations::add(speedRws, deltaSpeedInt, rwCmdSpeed, 4); + for (uint8_t i = 0; i < 4; i++) { + if (rwCmdSpeed[i] > maxRwSpeed) { + rwCmdSpeed[i] = maxRwSpeed; + } else if (rwCmdSpeed[i] < -maxRwSpeed) { + rwCmdSpeed[i] = -maxRwSpeed; + } + } VectorOperations::mulScalar(rwCmdSpeed, 10, rwCmdSpeed, 4); } -void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator) { +void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator, + const double *inverseAlignment, double maxDipol) { // Convert to actuator frame double dipolMomentActuatorDouble[3] = {0, 0, 0}; - MatrixOperations::multiply(*acsParameters.magnetorquesParameter.inverseAlignment, - dipolMoment, dipolMomentActuatorDouble, 3, 3, 1); + MatrixOperations::multiply(inverseAlignment, dipolMoment, dipolMomentActuatorDouble, 3, 3, + 1); // Scaling along largest element if dipol exceeds maximum - double maxDipol = acsParameters.magnetorquesParameter.DipolMax; - double maxValue = 0; - for (int i = 0; i < 3; i++) { - if (abs(dipolMomentActuator[i]) > maxDipol) { - maxValue = abs(dipolMomentActuator[i]); - } - } - if (maxValue > maxDipol) { - double scalingFactor = maxDipol / maxValue; + uint8_t maxIdx = 0; + VectorOperations::maxAbsValue(dipolMomentActuatorDouble, 3, &maxIdx); + double maxAbsValue = abs(dipolMomentActuatorDouble[maxIdx]); + if (maxAbsValue > maxDipol) { + double scalingFactor = maxDipol / maxAbsValue; VectorOperations::mulScalar(dipolMomentActuatorDouble, scalingFactor, dipolMomentActuatorDouble, 3); } diff --git a/mission/controller/acs/ActuatorCmd.h b/mission/controller/acs/ActuatorCmd.h index 969bd782..5d5d47f3 100644 --- a/mission/controller/acs/ActuatorCmd.h +++ b/mission/controller/acs/ActuatorCmd.h @@ -1,23 +1,22 @@ #ifndef ACTUATORCMD_H_ #define ACTUATORCMD_H_ -#include "AcsParameters.h" #include "MultiplicativeKalmanFilter.h" #include "SensorProcessing.h" #include "SensorValues.h" class ActuatorCmd { public: - ActuatorCmd(AcsParameters *acsParameters_); // Input mode ? + ActuatorCmd(); virtual ~ActuatorCmd(); /* * @brief: scalingTorqueRws() scales the torque via maximum part in case this part is * higher then the maximum torque - * @param: rwTrq given torque for reaction wheels - * rwTrqScaled possible scaled torque + * @param: rwTrq given torque for reaction wheels which will be + * scaled if needed to be */ - void scalingTorqueRws(const double *rwTrq, double *rwTrqScaled); + void scalingTorqueRws(double *rwTrq, double maxTorque); /* * @brief: cmdSpeedToRws() will set the maximum possible torque for the reaction @@ -28,8 +27,9 @@ class ActuatorCmd { * rwCmdSpeed output revolutions per minute for every * reaction wheel */ - void cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2, - const int32_t speedRw3, const double *rwTorque, int32_t *rwCmdSpeed); + void cmdSpeedToRws(int32_t speedRw0, int32_t speedRw1, int32_t speedRw2, int32_t speedRw3, + const double *rwTorque, int32_t *rwCmdSpeed, double sampleTime, + int32_t maxRwSpeed, double inertiaWheel); /* * @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques @@ -37,11 +37,11 @@ class ActuatorCmd { * @param: dipolMoment given dipol moment in spacecraft frame * dipolMomentActuator resulting dipol moment in actuator reference frame */ - void cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator); + void cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator, + const double *inverseAlignment, double maxDipol); protected: private: - AcsParameters acsParameters; }; #endif /* ACTUATORCMD_H_ */ diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 122e7ea2..35a2f025 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -12,7 +12,7 @@ #include "util/CholeskyDecomposition.h" #include "util/MathOperations.h" -Guidance::Guidance(AcsParameters *acsParameters_) : acsParameters(*acsParameters_) {} +Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = acsParameters_; } Guidance::~Guidance() {} @@ -26,9 +26,9 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ve double targetE[3] = {0, 0, 0}; MathOperations::cartesianFromLatLongAlt( - acsParameters.targetModeControllerParameters.latitudeTgt, - acsParameters.targetModeControllerParameters.longitudeTgt, - acsParameters.targetModeControllerParameters.altitudeTgt, targetE); + acsParameters->targetModeControllerParameters.latitudeTgt, + acsParameters->targetModeControllerParameters.longitudeTgt, + acsParameters->targetModeControllerParameters.altitudeTgt, targetE); // target direction in the ECEF frame double targetDirE[3] = {0, 0, 0}; @@ -57,9 +57,9 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ve // rotation quaternion from two vectors double refDir[3] = {0, 0, 0}; - refDir[0] = acsParameters.targetModeControllerParameters.refDirection[0]; - refDir[1] = acsParameters.targetModeControllerParameters.refDirection[1]; - refDir[2] = acsParameters.targetModeControllerParameters.refDirection[2]; + refDir[0] = acsParameters->targetModeControllerParameters.refDirection[0]; + refDir[1] = acsParameters->targetModeControllerParameters.refDirection[1]; + refDir[2] = acsParameters->targetModeControllerParameters.refDirection[2]; double noramlizedTargetDirB[3] = {0, 0, 0}; VectorOperations::normalize(targetDirB, noramlizedTargetDirB, 3); VectorOperations::normalize(refDir, refDir, 3); @@ -96,15 +96,15 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ve //------------------------------------------------------------------------------------- // Calculation of reference rotation rate in case of star tracker blinding //------------------------------------------------------------------------------------- - if (acsParameters.targetModeControllerParameters.avoidBlindStr) { + if (acsParameters->targetModeControllerParameters.avoidBlindStr) { double sunDirB[3] = {0, 0, 0}; MatrixOperations::multiply(*dcmBI, sunDirI, sunDirB, 3, 3, 1); - double exclAngle = acsParameters.strParameters.exclusionAngle, - blindStart = acsParameters.targetModeControllerParameters.blindAvoidStart, - blindEnd = acsParameters.targetModeControllerParameters.blindAvoidStop; + double exclAngle = acsParameters->strParameters.exclusionAngle, + blindStart = acsParameters->targetModeControllerParameters.blindAvoidStart, + blindEnd = acsParameters->targetModeControllerParameters.blindAvoidStop; double sightAngleSun = - VectorOperations::dot(acsParameters.strParameters.boresightAxis, sunDirB); + VectorOperations::dot(acsParameters->strParameters.boresightAxis, sunDirB); if (!(strBlindAvoidFlag)) { double critSightAngle = blindStart * exclAngle; @@ -113,7 +113,7 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ve } } else { if (sightAngleSun < blindEnd * exclAngle) { - double normBlindRefRate = acsParameters.targetModeControllerParameters.blindRotRate; + double normBlindRefRate = acsParameters->targetModeControllerParameters.blindRotRate; double blindRefRate[3] = {0, 0, 0}; if (sunDirB[1] < 0) { blindRefRate[0] = normBlindRefRate; @@ -144,9 +144,9 @@ void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double vel // transform longitude, latitude and altitude to cartesian coordiantes (ECEF) double targetE[3] = {0, 0, 0}; MathOperations::cartesianFromLatLongAlt( - acsParameters.targetModeControllerParameters.latitudeTgt, - acsParameters.targetModeControllerParameters.longitudeTgt, - acsParameters.targetModeControllerParameters.altitudeTgt, targetE); + acsParameters->targetModeControllerParameters.latitudeTgt, + acsParameters->targetModeControllerParameters.longitudeTgt, + acsParameters->targetModeControllerParameters.altitudeTgt, targetE); double targetDirE[3] = {0, 0, 0}; VectorOperations::subtract(targetE, posSatE, targetDirE, 3); @@ -198,7 +198,7 @@ void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double vel {xAxis[2], yAxis[2], zAxis[2]}}; QuaternionOperations::fromDcm(dcmIX, targetQuat); - int8_t timeElapsedMax = acsParameters.targetModeControllerParameters.timeElapsedMax; + int8_t timeElapsedMax = acsParameters->targetModeControllerParameters.timeElapsedMax; targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate); } @@ -211,9 +211,9 @@ void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3] double groundStationE[3] = {0, 0, 0}; MathOperations::cartesianFromLatLongAlt( - acsParameters.gsTargetModeControllerParameters.latitudeTgt, - acsParameters.gsTargetModeControllerParameters.longitudeTgt, - acsParameters.gsTargetModeControllerParameters.altitudeTgt, groundStationE); + acsParameters->gsTargetModeControllerParameters.latitudeTgt, + acsParameters->gsTargetModeControllerParameters.longitudeTgt, + acsParameters->gsTargetModeControllerParameters.altitudeTgt, groundStationE); double targetDirE[3] = {0, 0, 0}; VectorOperations::subtract(groundStationE, posSatE, targetDirE, 3); @@ -262,7 +262,7 @@ void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3] {xAxis[2], yAxis[2], zAxis[2]}}; QuaternionOperations::fromDcm(dcmTgt, targetQuat); - int8_t timeElapsedMax = acsParameters.gsTargetModeControllerParameters.timeElapsedMax; + int8_t timeElapsedMax = acsParameters->gsTargetModeControllerParameters.timeElapsedMax; targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate); } @@ -332,9 +332,9 @@ void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], doub // rotation quaternion from two vectors double refDir[3] = {0, 0, 0}; - refDir[0] = acsParameters.nadirModeControllerParameters.refDirection[0]; - refDir[1] = acsParameters.nadirModeControllerParameters.refDirection[1]; - refDir[2] = acsParameters.nadirModeControllerParameters.refDirection[2]; + refDir[0] = acsParameters->nadirModeControllerParameters.refDirection[0]; + refDir[1] = acsParameters->nadirModeControllerParameters.refDirection[1]; + refDir[2] = acsParameters->nadirModeControllerParameters.refDirection[2]; double noramlizedTargetDirB[3] = {0, 0, 0}; VectorOperations::normalize(targetDirB, noramlizedTargetDirB, 3); VectorOperations::normalize(refDir, refDir, 3); @@ -406,7 +406,7 @@ void Guidance::targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], doubl {xAxis[2], yAxis[2], zAxis[2]}}; QuaternionOperations::fromDcm(dcmTgt, targetQuat); - int8_t timeElapsedMax = acsParameters.nadirModeControllerParameters.timeElapsedMax; + int8_t timeElapsedMax = acsParameters->nadirModeControllerParameters.timeElapsedMax; targetRotationRate(timeElapsedMax, now, targetQuat, refSatRate); } @@ -516,19 +516,19 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, bool rw4valid = (sensorValues->rw4Set.state.value && sensorValues->rw4Set.state.isValid()); if (rw1valid && rw2valid && rw3valid && rw4valid) { - std::memcpy(rwPseudoInv, acsParameters.rwMatrices.pseudoInverse, 12 * sizeof(double)); + std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverse, 12 * sizeof(double)); return returnvalue::OK; } else if (!rw1valid && rw2valid && rw3valid && rw4valid) { - std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without1, 12 * sizeof(double)); + std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without1, 12 * sizeof(double)); return returnvalue::OK; } else if (rw1valid && !rw2valid && rw3valid && rw4valid) { - std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without2, 12 * sizeof(double)); + std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without2, 12 * sizeof(double)); return returnvalue::OK; } else if (rw1valid && rw2valid && !rw3valid && rw4valid) { - std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without3, 12 * sizeof(double)); + std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without3, 12 * sizeof(double)); return returnvalue::OK; } else if (rw1valid && rw2valid && rw3valid && !rw4valid) { - std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without4, 12 * sizeof(double)); + std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without4, 12 * sizeof(double)); return returnvalue::OK; } else { // @note: This one takes the normal pseudoInverse of all four raction wheels valid. @@ -542,15 +542,14 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3]) { std::error_code e; if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE, e) or - not std::filesystem::exists(SD_1_SKEWED_PTG_FILE, - e)) { // ToDo: if file does not exist anymore - std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir, + not std::filesystem::exists(SD_1_SKEWED_PTG_FILE, e)) { + std::memcpy(sunTargetSafe, acsParameters->safeModeControllerParameters.sunTargetDir, 3 * sizeof(double)); } else { - std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDirLeop, + std::memcpy(sunTargetSafe, acsParameters->safeModeControllerParameters.sunTargetDirLeop, 3 * sizeof(double)); } - std::memcpy(satRateSafe, acsParameters.safeModeControllerParameters.satRateRef, + std::memcpy(satRateSafe, acsParameters->safeModeControllerParameters.satRateRef, 3 * sizeof(double)); } diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index da9d429b..f3369092 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -55,14 +55,15 @@ class Guidance { ReturnValue_t getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv); private: - AcsParameters acsParameters; + const AcsParameters *acsParameters; + bool strBlindAvoidFlag = false; timeval timeSavedQuaternion; double savedQuaternion[4] = {0, 0, 0, 0}; double omegaRefSaved[3] = {0, 0, 0}; - static constexpr char SD_0_SKEWED_PTG_FILE[] = "/mnt/sd0/conf/deployment"; - static constexpr char SD_1_SKEWED_PTG_FILE[] = "/mnt/sd1/conf/deployment"; + static constexpr char SD_0_SKEWED_PTG_FILE[] = "/mnt/sd0/conf/acsDeploymentConfirm"; + static constexpr char SD_1_SKEWED_PTG_FILE[] = "/mnt/sd1/conf/acsDeploymentConfirm"; }; #endif /* ACS_GUIDANCE_H_ */ diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp index a700c6a6..77a3ef00 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp +++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp @@ -12,33 +12,22 @@ #include "util/CholeskyDecomposition.h" #include "util/MathOperations.h" -/*Initialisation of values for parameters in constructor*/ -MultiplicativeKalmanFilter::MultiplicativeKalmanFilter(AcsParameters *acsParameters_) - : initialQuaternion{0, 0, 0, 1}, - initialCovarianceMatrix{{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}} { - loadAcsParameters(acsParameters_); -} +MultiplicativeKalmanFilter::MultiplicativeKalmanFilter() {} MultiplicativeKalmanFilter::~MultiplicativeKalmanFilter() {} -void MultiplicativeKalmanFilter::loadAcsParameters(AcsParameters *acsParameters_) { - inertiaEIVE = &(acsParameters_->inertiaEIVE); - kalmanFilterParameters = &(acsParameters_->kalmanFilterParameters); -} - ReturnValue_t MultiplicativeKalmanFilter::init( const double *magneticField_, const bool validMagField_, const double *sunDir_, const bool validSS, const double *sunDirJ, const bool validSSModel, const double *magFieldJ, - const bool validMagModel, acsctrl::MekfData *mekfData) { // valids for "model measurements"? + const bool validMagModel, acsctrl::MekfData *mekfData, + AcsParameters *acsParameters) { // valids for "model measurements"? // check for valid mag/sun if (validMagField_ && validSS && validSSModel && validMagModel) { - validInit = true; // QUEST ALGO ----------------------------------------------------------------------- double sigmaSun = 0, sigmaMag = 0, sigmaGyro = 0; - sigmaSun = kalmanFilterParameters->sensorNoiseSS; - sigmaMag = kalmanFilterParameters->sensorNoiseMAG; - sigmaGyro = kalmanFilterParameters->sensorNoiseGYR; + sigmaSun = acsParameters->kalmanFilterParameters.sensorNoiseSS; + sigmaMag = acsParameters->kalmanFilterParameters.sensorNoiseMAG; + sigmaGyro = acsParameters->kalmanFilterParameters.sensorNoiseGYR; double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0}, normSunJ[3] = {0, 0, 0}; @@ -192,21 +181,18 @@ ReturnValue_t MultiplicativeKalmanFilter::init( return MEKF_INITIALIZED; } else { // no initialisation possible, no valid measurements - validInit = false; updateDataSetWithoutData(mekfData, MekfStatus::UNINITIALIZED); return MEKF_UNINITIALIZED; } } // --------------- MEKF DISCRETE TIME STEP ------------------------------- -ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, const bool validSTR_, - const double *rateGYRs_, const bool validGYRs_, - const double *magneticField_, - const bool validMagField_, const double *sunDir_, - const bool validSS, const double *sunDirJ, - const bool validSSModel, const double *magFieldJ, - const bool validMagModel, double sampleTime, - acsctrl::MekfData *mekfData) { +ReturnValue_t MultiplicativeKalmanFilter::mekfEst( + const double *quaternionSTR, const bool validSTR_, const double *rateGYRs_, + const bool validGYRs_, const double *magneticField_, const bool validMagField_, + const double *sunDir_, const bool validSS, const double *sunDirJ, const bool validSSModel, + const double *magFieldJ, const bool validMagModel, acsctrl::MekfData *mekfData, + AcsParameters *acsParameters) { // Check for GYR Measurements int MDF = 0; // Matrix Dimension Factor if (!validGYRs_) { @@ -248,9 +234,9 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c // If we are here, MEKF will perform double sigmaSun = 0, sigmaMag = 0, sigmaStr = 0; - sigmaSun = kalmanFilterParameters->sensorNoiseSS; - sigmaMag = kalmanFilterParameters->sensorNoiseMAG; - sigmaStr = kalmanFilterParameters->sensorNoiseSTR; + sigmaSun = acsParameters->kalmanFilterParameters.sensorNoiseSS; + sigmaMag = acsParameters->kalmanFilterParameters.sensorNoiseMAG; + sigmaStr = acsParameters->kalmanFilterParameters.sensorNoiseSTR; double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0}, normSunJ[3] = {0, 0, 0}; @@ -912,8 +898,8 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c biasGYR[2] = updatedGyroBias[2]; /* ----------- PROPAGATION ----------*/ - double sigmaU = kalmanFilterParameters->sensorNoiseBsGYR; - double sigmaV = kalmanFilterParameters->sensorNoiseArwGYR; + double sigmaU = acsParameters->kalmanFilterParameters.sensorNoiseBsGYR; + double sigmaV = acsParameters->kalmanFilterParameters.sensorNoiseArwGYR; double discTimeMatrix[6][6] = {{-1, 0, 0, 0, 0, 0}, {0, -1, 0, 0, 0, 0}, {0, 0, -1, 0, 0, 0}, {0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}}; @@ -931,27 +917,31 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c covQ12[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, covQ22[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, covQ12trans[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - if (normRotEst * sampleTime < M_PI / 10) { - double fact11 = pow(sigmaV, 2) * sampleTime + 1. / 3. * pow(sigmaU, 2) * pow(sampleTime, 3); + if (normRotEst * acsParameters->onBoardParams.sampleTime < M_PI / 10) { + double fact11 = pow(sigmaV, 2) * acsParameters->onBoardParams.sampleTime + + 1. / 3. * pow(sigmaU, 2) * pow(acsParameters->onBoardParams.sampleTime, 3); MatrixOperations::multiplyScalar(*identityMatrix3, fact11, *covQ11, 3, 3); - double fact12 = -(1. / 2. * pow(sigmaU, 2) * pow(sampleTime, 2)); + double fact12 = -(1. / 2. * pow(sigmaU, 2) * pow(acsParameters->onBoardParams.sampleTime, 2)); MatrixOperations::multiplyScalar(*identityMatrix3, fact12, *covQ12, 3, 3); std::memcpy(*covQ12trans, *covQ12, 3 * 3 * sizeof(double)); - double fact22 = pow(sigmaU, 2) * sampleTime; + double fact22 = pow(sigmaU, 2) * acsParameters->onBoardParams.sampleTime; MatrixOperations::multiplyScalar(*identityMatrix3, fact22, *covQ22, 3, 3); } else { - double fact22 = pow(sigmaU, 2) * sampleTime; + double fact22 = pow(sigmaU, 2) * acsParameters->onBoardParams.sampleTime; MatrixOperations::multiplyScalar(*identityMatrix3, fact22, *covQ22, 3, 3); double covQ12_0[3][3], covQ12_1[3][3], covQ12_2[3][3], covQ12_01[3][3]; - double fact12_0 = (normRotEst * sampleTime - sin(normRotEst * sampleTime) / pow(normRotEst, 3)); + double fact12_0 = + (normRotEst * acsParameters->onBoardParams.sampleTime - + sin(normRotEst * acsParameters->onBoardParams.sampleTime) / pow(normRotEst, 3)); MatrixOperations::multiplyScalar(*crossRotEst, fact12_0, *covQ12_0, 3, 3); - double fact12_1 = 1. / 2. * pow(sampleTime, 2); + double fact12_1 = 1. / 2. * pow(acsParameters->onBoardParams.sampleTime, 2); MatrixOperations::multiplyScalar(*identityMatrix3, fact12_1, *covQ12_1, 3, 3); double fact12_2 = - (1. / 2. * pow(normRotEst, 2) * pow(sampleTime, 2) + cos(normRotEst * sampleTime) - 1) / + (1. / 2. * pow(normRotEst, 2) * pow(acsParameters->onBoardParams.sampleTime, 2) + + cos(normRotEst * acsParameters->onBoardParams.sampleTime) - 1) / pow(normRotEst, 4); MatrixOperations::multiply(*crossRotEst, *crossRotEst, *covQ12_2, 3, 3, 3); MatrixOperations::multiplyScalar(*covQ12_2, fact12_2, *covQ12_2, 3, 3); @@ -961,13 +951,15 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c MatrixOperations::transpose(*covQ12, *covQ12trans, 3); double covQ11_0[3][3], covQ11_1[3][3], covQ11_2[3][3], covQ11_12[3][3]; - double fact11_0 = pow(sigmaV, 2) * sampleTime; + double fact11_0 = pow(sigmaV, 2) * acsParameters->onBoardParams.sampleTime; MatrixOperations::multiplyScalar(*identityMatrix3, fact11_0, *covQ11_0, 3, 3); - double fact11_1 = 1. / 3. * pow(sampleTime, 3); + double fact11_1 = 1. / 3. * pow(acsParameters->onBoardParams.sampleTime, 3); MatrixOperations::multiplyScalar(*identityMatrix3, fact11_1, *covQ11_1, 3, 3); - double fact11_2 = (2 * normRotEst * sampleTime - 2 * sin(normRotEst * sampleTime) - - 1. / 3. * pow(normRotEst, 3) * pow(sampleTime, 3)) / - pow(normRotEst, 5); + double fact11_2 = + (2 * normRotEst * acsParameters->onBoardParams.sampleTime - + 2 * sin(normRotEst * acsParameters->onBoardParams.sampleTime) - + 1. / 3. * pow(normRotEst, 3) * pow(acsParameters->onBoardParams.sampleTime, 3)) / + pow(normRotEst, 5); MatrixOperations::multiply(*crossRotEst, *crossRotEst, *covQ11_2, 3, 3, 3); MatrixOperations::multiplyScalar(*covQ11_2, fact11_2, *covQ11_2, 3, 3); MatrixOperations::subtract(*covQ11_1, *covQ11_2, *covQ11_12, 3, 3); @@ -1017,9 +1009,10 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c phi[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}}; double phi11_1[3][3], phi11_2[3][3], phi11_01[3][3]; - double fact11_1 = sin(normRotEst * sampleTime) / normRotEst; + double fact11_1 = sin(normRotEst * acsParameters->onBoardParams.sampleTime) / normRotEst; MatrixOperations::multiplyScalar(*crossRotEst, fact11_1, *phi11_1, 3, 3); - double fact11_2 = (1 - cos(normRotEst * sampleTime)) / pow(normRotEst, 2); + double fact11_2 = + (1 - cos(normRotEst * acsParameters->onBoardParams.sampleTime)) / pow(normRotEst, 2); MatrixOperations::multiply(*crossRotEst, *crossRotEst, *phi11_2, 3, 3, 3); MatrixOperations::multiplyScalar(*phi11_2, fact11_2, *phi11_2, 3, 3); MatrixOperations::subtract(*identityMatrix3, *phi11_1, *phi11_01, 3, 3); @@ -1028,8 +1021,11 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c double phi12_0[3][3], phi12_1[3][3], phi12_2[3][3], phi12_01[3][3]; double fact12_0 = fact11_2; MatrixOperations::multiplyScalar(*crossRotEst, fact12_0, *phi12_0, 3, 3); - MatrixOperations::multiplyScalar(*identityMatrix3, sampleTime, *phi12_1, 3, 3); - double fact12_2 = (normRotEst * sampleTime - sin(normRotEst * sampleTime) / pow(normRotEst, 3)); + MatrixOperations::multiplyScalar(*identityMatrix3, + acsParameters->onBoardParams.sampleTime, *phi12_1, 3, 3); + double fact12_2 = + (normRotEst * acsParameters->onBoardParams.sampleTime - + sin(normRotEst * acsParameters->onBoardParams.sampleTime) / pow(normRotEst, 3)); MatrixOperations::multiply(*crossRotEst, *crossRotEst, *phi12_2, 3, 3, 3); MatrixOperations::multiplyScalar(*phi12_2, fact12_2, *phi12_2, 3, 3); MatrixOperations::subtract(*phi12_0, *phi12_1, *phi12_01, 3, 3); @@ -1056,8 +1052,8 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c // Propagated Quaternion double rotSin[3] = {0, 0, 0}, rotCosMat[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double rotCos = cos(0.5 * normRotEst * sampleTime); - double sinFac = sin(0.5 * normRotEst * sampleTime) / normRotEst; + double rotCos = cos(0.5 * normRotEst * acsParameters->onBoardParams.sampleTime); + double sinFac = sin(0.5 * normRotEst * acsParameters->onBoardParams.sampleTime) / normRotEst; VectorOperations::mulScalar(rotRateEst, sinFac, rotSin, 3); double skewSin[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.h b/mission/controller/acs/MultiplicativeKalmanFilter.h index 47e1f807..3e9bfa49 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.h +++ b/mission/controller/acs/MultiplicativeKalmanFilter.h @@ -17,9 +17,8 @@ class MultiplicativeKalmanFilter { */ public: /* @brief: Constructor - * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters */ - MultiplicativeKalmanFilter(AcsParameters *acsParameters_); + MultiplicativeKalmanFilter(); virtual ~MultiplicativeKalmanFilter(); ReturnValue_t reset(acsctrl::MekfData *mekfData); @@ -33,8 +32,8 @@ class MultiplicativeKalmanFilter { */ ReturnValue_t init(const double *magneticField_, const bool validMagField_, const double *sunDir_, const bool validSS, const double *sunDirJ, const bool validSSModel, - const double *magFieldJ, const bool validMagModel, - acsctrl::MekfData *mekfData); + const double *magFieldJ, const bool validMagModel, acsctrl::MekfData *mekfData, + AcsParameters *acsParameters); /* @brief: mekfEst() - This function calculates the quaternion and gyro bias of the Kalman Filter * for the current step after the initalization @@ -54,7 +53,8 @@ class MultiplicativeKalmanFilter { const bool validGYRs_, const double *magneticField_, const bool validMagField_, const double *sunDir_, const bool validSS, const double *sunDirJ, const bool validSSModel, const double *magFieldJ, - const bool validMagModel, double sampleTime, acsctrl::MekfData *mekfData); + const bool validMagModel, acsctrl::MekfData *mekfData, + AcsParameters *acsParameters); enum MekfStatus : uint8_t { UNINITIALIZED = 0, @@ -79,23 +79,21 @@ class MultiplicativeKalmanFilter { private: /*Parameters*/ - AcsParameters::InertiaEIVE *inertiaEIVE; - AcsParameters::KalmanFilterParameters *kalmanFilterParameters; double quaternion_STR_SB[4]; - bool validInit; /*States*/ - double initialQuaternion[4]; /*after reset?QUEST*/ - double initialCovarianceMatrix[6][6]; /*after reset?QUEST*/ + double initialQuaternion[4] = {0, 0, 0, 1}; /*after reset?QUEST*/ + double initialCovarianceMatrix[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}}; double propagatedQuaternion[4]; /*Filter Quaternion for next step*/ - uint8_t sensorsAvail; + uint8_t sensorsAvail = 0; /*Outputs*/ double quatBJ[4]; /* Output Quaternion */ double biasGYR[3]; /*Between measured and estimated sat Rate*/ /*Parameter INIT*/ /*Functions*/ - void loadAcsParameters(AcsParameters *acsParameters_); void updateDataSetWithoutData(acsctrl::MekfData *mekfData, MekfStatus mekfStatus); void updateDataSet(acsctrl::MekfData *mekfData, MekfStatus mekfStatus, double quat[4], double satRotRate[3]); diff --git a/mission/controller/acs/Navigation.cpp b/mission/controller/acs/Navigation.cpp index 03446609..9e8b3719 100644 --- a/mission/controller/acs/Navigation.cpp +++ b/mission/controller/acs/Navigation.cpp @@ -8,9 +8,7 @@ #include "util/CholeskyDecomposition.h" #include "util/MathOperations.h" -Navigation::Navigation(AcsParameters *acsParameters_) : multiplicativeKalmanFilter(acsParameters_) { - acsParameters = *acsParameters_; -} +Navigation::Navigation() {} Navigation::~Navigation() {} @@ -18,7 +16,7 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues, acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::SusDataProcessed *susDataProcessed, - acsctrl::MekfData *mekfData) { + acsctrl::MekfData *mekfData, AcsParameters *acsParameters) { double quatIB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value, sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value}; bool quatIBValid = sensorValues->strSet.caliQx.isValid() && @@ -30,7 +28,8 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues, mgmDataProcessed->mgmVecTot.value, mgmDataProcessed->mgmVecTot.isValid(), susDataProcessed->susVecTot.value, susDataProcessed->susVecTot.isValid(), susDataProcessed->sunIjkModel.value, susDataProcessed->sunIjkModel.isValid(), - mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid(), mekfData); + mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid(), mekfData, + acsParameters); return mekfStatus; } else { mekfStatus = multiplicativeKalmanFilter.mekfEst( @@ -39,7 +38,7 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues, mgmDataProcessed->mgmVecTot.isValid(), susDataProcessed->susVecTot.value, susDataProcessed->susVecTot.isValid(), susDataProcessed->sunIjkModel.value, susDataProcessed->sunIjkModel.isValid(), mgmDataProcessed->magIgrfModel.value, - mgmDataProcessed->magIgrfModel.isValid(), acsParameters.onBoardParams.sampleTime, mekfData); + mgmDataProcessed->magIgrfModel.isValid(), mekfData, acsParameters); return mekfStatus; } } diff --git a/mission/controller/acs/Navigation.h b/mission/controller/acs/Navigation.h index cf9e81e3..b567fbdd 100644 --- a/mission/controller/acs/Navigation.h +++ b/mission/controller/acs/Navigation.h @@ -9,19 +9,19 @@ class Navigation { public: - Navigation(AcsParameters *acsParameters_); + Navigation(); virtual ~Navigation(); ReturnValue_t useMekf(ACS::SensorValues *sensorValues, acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, - acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MekfData *mekfData); + acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MekfData *mekfData, + AcsParameters *acsParameters); void resetMekf(acsctrl::MekfData *mekfData); protected: private: MultiplicativeKalmanFilter multiplicativeKalmanFilter; - AcsParameters acsParameters; ReturnValue_t mekfStatus = MultiplicativeKalmanFilter::MEKF_UNINITIALIZED; }; diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index e268d786..b3423367 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -14,7 +14,7 @@ using namespace Math; -SensorProcessing::SensorProcessing(AcsParameters *acsParameters_) {} +SensorProcessing::SensorProcessing() {} SensorProcessing::~SensorProcessing() {} @@ -26,7 +26,8 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const acsctrl::GpsDataProcessed *gpsDataProcessed, const double gpsAltitude, bool gpsValid, acsctrl::MgmDataProcessed *mgmDataProcessed) { - // ---------------- IGRF- 13 Implementation here ------------------------------------------------ + // ---------------- IGRF- 13 Implementation here + // ------------------------------------------------ double magIgrfModel[3] = {0.0, 0.0, 0.0}; if (gpsValid) { // Should be existing class object which will be called and modified here. @@ -549,8 +550,8 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong const bool validGps, const AcsParameters::GpsParameters *gpsParameters, acsctrl::GpsDataProcessed *gpsDataProcessed) { - // name to convert not process - double gdLongitude = 0, gcLatitude = 0, posSatE[3] = {0, 0, 0}, gpsVelocityE[3] = {0, 0, 0}; + double gdLongitude = 0, gcLatitude = 0, altitude = 0, posSatE[3] = {0, 0, 0}, + gpsVelocityE[3] = {0, 0, 0}; if (validGps) { // Transforming from Degree to Radians and calculation geocentric lattitude from geodetic gdLongitude = gpsLongitude * PI / 180.; @@ -559,9 +560,17 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong double factor = 1 - pow(eccentricityWgs84, 2); gcLatitude = atan(factor * tan(latitudeRad)); + // Altitude FDIR + if (gpsAltitude > gpsParameters->maximumFdirAltitude || + gpsAltitude < gpsParameters->maximumFdirAltitude) { + altitude = gpsParameters->fdirAltitude; + } else { + altitude = gpsAltitude; + } + // Calculation of the satellite velocity in earth fixed frame double deltaDistance[3] = {0, 0, 0}; - MathOperations::cartesianFromLatLongAlt(latitudeRad, gdLongitude, gpsAltitude, posSatE); + MathOperations::cartesianFromLatLongAlt(latitudeRad, gdLongitude, altitude, posSatE); if (validSavedPosSatE && (gpsUnixSeconds - timeOfSavedPosSatE) < (gpsParameters->timeDiffVelocityMax)) { VectorOperations::subtract(posSatE, savedPosSatE, deltaDistance, 3); @@ -580,6 +589,7 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong if (pg.getReadResult() == returnvalue::OK) { gpsDataProcessed->gdLongitude.value = gdLongitude; gpsDataProcessed->gcLatitude.value = gcLatitude; + gpsDataProcessed->altitude.value = altitude; std::memcpy(gpsDataProcessed->gpsPosition.value, posSatE, 3 * sizeof(double)); std::memcpy(gpsDataProcessed->gpsVelocity.value, gpsVelocityE, 3 * sizeof(double)); gpsDataProcessed->setValidity(validGps, true); diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index cdd29d8b..d845c2f3 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -15,7 +15,7 @@ class SensorProcessing { public: void reset(); - SensorProcessing(AcsParameters *acsParameters_); + SensorProcessing(); virtual ~SensorProcessing(); void process(timeval now, ACS::SensorValues *sensorValues, @@ -77,7 +77,6 @@ class SensorProcessing { bool validSavedPosSatE = false; SusConverter susConverter; - AcsParameters acsParameters; }; #endif /*SENSORPROCESSING_H_*/ diff --git a/mission/controller/acs/SensorValues.h b/mission/controller/acs/SensorValues.h index 79c3dfe1..25946d0b 100644 --- a/mission/controller/acs/SensorValues.h +++ b/mission/controller/acs/SensorValues.h @@ -1,17 +1,16 @@ #ifndef SENSORVALUES_H_ #define SENSORVALUES_H_ +#include +#include +#include +#include +#include #include #include #include #include -#include "fsfw_hal/devicehandlers/GyroL3GD20Handler.h" -#include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h" -#include "fsfw_hal/devicehandlers/MgmRM3100Handler.h" -#include "linux/devices/devicedefinitions/StarTrackerDefinitions.h" -#include "mission/devices/devicedefinitions/GPSDefinitions.h" - namespace ACS { class SensorValues { diff --git a/mission/controller/acs/control/Detumble.cpp b/mission/controller/acs/control/Detumble.cpp index 0e131942..893d6a4b 100644 --- a/mission/controller/acs/control/Detumble.cpp +++ b/mission/controller/acs/control/Detumble.cpp @@ -9,39 +9,32 @@ #include "../util/MathOperations.h" -Detumble::Detumble(AcsParameters *acsParameters_) { loadAcsParameters(acsParameters_); } +Detumble::Detumble() {} Detumble::~Detumble() {} -void Detumble::loadAcsParameters(AcsParameters *acsParameters_) { - detumbleParameter = &(acsParameters_->detumbleParameter); - magnetorquesParameter = &(acsParameters_->magnetorquesParameter); -} - ReturnValue_t Detumble::bDotLaw(const double *magRate, const bool magRateValid, - const double *magField, const bool magFieldValid, double *magMom) { + const double *magField, const bool magFieldValid, double *magMom, + double gain) { if (!magRateValid || !magFieldValid) { return DETUMBLE_NO_SENSORDATA; } - - // change unit from uT to T - double magFieldT[3] = {0, 0, 0}, magRateT[3] = {0, 0, 0}; + // convert uT to T + double magFieldT[3], magRateT[3]; VectorOperations::mulScalar(magField, 1e-6, magFieldT, 3); VectorOperations::mulScalar(magRate, 1e-6, magRateT, 3); - - double gain = detumbleParameter->gainD; - double factor = -gain / pow(VectorOperations::norm(magField, 3), 2); - VectorOperations::mulScalar(magRate, factor, magMom, 3); + // control law + double factor = -gain / pow(VectorOperations::norm(magFieldT, 3), 2); + VectorOperations::mulScalar(magRateT, factor, magMom, 3); return returnvalue::OK; } -ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool magRateValid, - double *magMom) { +ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool magRateValid, double *magMom, + double dipolMax) { if (!magRateValid) { return DETUMBLE_NO_SENSORDATA; } - double dipolMax = magnetorquesParameter->DipolMax; for (int i = 0; i < 3; i++) { magMom[i] = -dipolMax * sign(magRate[i]); } @@ -49,14 +42,20 @@ ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool magRateVal return returnvalue::OK; } -ReturnValue_t Detumble::bDotLawGyro(const double *satRate, const bool *satRateValid, +ReturnValue_t Detumble::bDotLawFull(const double *satRate, const bool *satRateValid, const double *magField, const bool *magFieldValid, - double *magMom) { + double *magMom, double gain) { if (!satRateValid || !magFieldValid) { return DETUMBLE_NO_SENSORDATA; } - double gain = detumbleParameter->gainD; - double factor = -gain / pow(VectorOperations::norm(magField, 3), 2); - VectorOperations::mulScalar(satRate, factor, magMom, 3); + // convert uT to T + double magFieldT[3]; + VectorOperations::mulScalar(magField, 1e-6, magFieldT, 3); + // control law + double factor = gain / pow(VectorOperations::norm(magField, 3), 2); + double magFieldNormed[3] = {0, 0, 0}, crossProduct[3] = {0, 0, 0}; + VectorOperations::normalize(magFieldT, magFieldNormed, 3); + VectorOperations::cross(satRate, magFieldNormed, crossProduct); + VectorOperations::mulScalar(crossProduct, factor, magMom, 3); return returnvalue::OK; } diff --git a/mission/controller/acs/control/Detumble.h b/mission/controller/acs/control/Detumble.h index 65e5ec28..2bf3607d 100644 --- a/mission/controller/acs/control/Detumble.h +++ b/mission/controller/acs/control/Detumble.h @@ -12,30 +12,22 @@ class Detumble { public: - Detumble(AcsParameters *acsParameters_); + Detumble(); virtual ~Detumble(); static const uint8_t INTERFACE_ID = CLASS_ID::ACS_DETUMBLE; static const ReturnValue_t DETUMBLE_NO_SENSORDATA = MAKE_RETURN_CODE(0x01); - /* @brief: Load AcsParameters for this class - * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters - */ - void loadAcsParameters(AcsParameters *acsParameters_); - ReturnValue_t bDotLaw(const double *magRate, const bool magRateValid, const double *magField, - const bool magFieldValid, double *magMom); + const bool magFieldValid, double *magMom, double gain); - ReturnValue_t bangbangLaw(const double *magRate, const bool magRateValid, double *magMom); + ReturnValue_t bangbangLaw(const double *magRate, const bool magRateValid, double *magMom, + double dipolMax); - ReturnValue_t bangbangLaw(const double *magRate, const bool *magRateValid, double *magMom); - - ReturnValue_t bDotLawGyro(const double *satRate, const bool *satRateValid, const double *magField, - const bool *magFieldValid, double *magMom); + ReturnValue_t bDotLawFull(const double *satRate, const bool *satRateValid, const double *magField, + const bool *magFieldValid, double *magMom, double gain); private: - AcsParameters::DetumbleParameter *detumbleParameter; - AcsParameters::MagnetorquesParameter *magnetorquesParameter; }; #endif /*ACS_CONTROL_DETUMBLE_H_*/ diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 74a32a4a..23f51f9e 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -1,10 +1,3 @@ -/* - * PtgCtrl.cpp - * - * Created on: 17 Jul 2022 - * Author: Robin Marquardt - */ - #include "PtgCtrl.h" #include @@ -16,16 +9,10 @@ #include "../util/MathOperations.h" -PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { loadAcsParameters(acsParameters_); } +PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_; } PtgCtrl::~PtgCtrl() {} -void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_) { - inertiaEIVE = &(acsParameters_->inertiaEIVE); - rwHandlingParameters = &(acsParameters_->rwHandlingParameters); - rwMatrices = &(acsParameters_->rwMatrices); -} - void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *errorQuat, const double *deltaRate, const double *rwPseudoInv, double *torqueRws) { @@ -62,8 +49,8 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters gainMatrixDiagonal[0][0] = gainVector[0]; gainMatrixDiagonal[1][1] = gainVector[1]; gainMatrixDiagonal[2][2] = gainVector[2]; - MatrixOperations::multiply(*gainMatrixDiagonal, *(inertiaEIVE->inertiaMatrix), - *gainMatrix, 3, 3, 3); + MatrixOperations::multiply( + *gainMatrixDiagonal, *(acsParameters->inertiaEIVE.inertiaMatrix), *gainMatrix, 3, 3, 3); // Inverse of gainMatrix double gainMatrixInverse[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; @@ -72,8 +59,8 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters gainMatrixInverse[2][2] = 1 / gainMatrix[2][2]; double pMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MatrixOperations::multiply(*gainMatrixInverse, *(inertiaEIVE->inertiaMatrix), *pMatrix, 3, - 3, 3); + MatrixOperations::multiply( + *gainMatrixInverse, *(acsParameters->inertiaEIVE.inertiaMatrix), *pMatrix, 3, 3, 3); MatrixOperations::multiplyScalar(*pMatrix, kInt, *pMatrix, 3, 3); //------------------------------------------------------------------------------------------------ @@ -91,18 +78,19 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters pErrorSign[i] = pError[i]; } } - // Torque for quaternion error + // torque for quaternion error double torqueQuat[3] = {0, 0, 0}; MatrixOperations::multiply(*gainMatrix, pErrorSign, torqueQuat, 3, 3, 1); VectorOperations::mulScalar(torqueQuat, -1, torqueQuat, 3); - // Torque for rate error + // torque for rate error double torqueRate[3] = {0, 0, 0}; - MatrixOperations::multiply(*(inertiaEIVE->inertiaMatrix), deltaRate, torqueRate, 3, 3, 1); + MatrixOperations::multiply(*(acsParameters->inertiaEIVE.inertiaMatrix), deltaRate, + torqueRate, 3, 3, 1); VectorOperations::mulScalar(torqueRate, cInt, torqueRate, 3); VectorOperations::mulScalar(torqueRate, -1, torqueRate, 3); - // Final commanded Torque for every reaction wheel + // final commanded Torque for every reaction wheel double torque[3] = {0, 0, 0}; VectorOperations::add(torqueRate, torqueQuat, torque, 3); MatrixOperations::multiply(rwPseudoInv, torque, torqueRws, 4, 3, 1); @@ -120,20 +108,22 @@ void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawP return; } - // calculating momentum of satellite and momentum of reaction wheels + // calculating momentum of satellite and momentum of reaction wheels double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3}; double momentumRwU[4] = {0, 0, 0, 0}, momentumRw[3] = {0, 0, 0}; - VectorOperations::mulScalar(speedRws, rwHandlingParameters->inertiaWheel, momentumRwU, 4); - MatrixOperations::multiply(*(rwMatrices->alignmentMatrix), momentumRwU, momentumRw, 3, 4, - 1); + VectorOperations::mulScalar(speedRws, acsParameters->rwHandlingParameters.inertiaWheel, + momentumRwU, 4); + MatrixOperations::multiply(*(acsParameters->rwMatrices.alignmentMatrix), momentumRwU, + momentumRw, 3, 4, 1); double momentumSat[3] = {0, 0, 0}, momentumTotal[3] = {0, 0, 0}; - MatrixOperations::multiply(*(inertiaEIVE->inertiaMatrix), satRate, momentumSat, 3, 3, 1); + MatrixOperations::multiply(*(acsParameters->inertiaEIVE.inertiaMatrix), satRate, + momentumSat, 3, 3, 1); VectorOperations::add(momentumSat, momentumRw, momentumTotal, 3); - // calculating momentum error + // calculating momentum error double deltaMomentum[3] = {0, 0, 0}; VectorOperations::subtract(momentumTotal, pointingLawParameters->desatMomentumRef, deltaMomentum, 3); - // resulting magnetic dipole command + // resulting magnetic dipole command double crossMomentumMagField[3] = {0, 0, 0}; VectorOperations::cross(deltaMomentum, magFieldEst, crossMomentumMagField); double normMag = VectorOperations::norm(magFieldEst, 3), factor = 0; @@ -147,53 +137,50 @@ void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawPara double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (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 + // conversion to [rad/s] for further calculations VectorOperations::mulScalar(rpmOffset, factor, rpmOffset, 4); VectorOperations::mulScalar(speedRws, 2 * Math::PI / 60, speedRws, 4); double diffRwSpeed[4] = {0, 0, 0, 0}; VectorOperations::subtract(speedRws, rpmOffset, diffRwSpeed, 4); - VectorOperations::mulScalar(diffRwSpeed, rwHandlingParameters->inertiaWheel, + VectorOperations::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::vecTransposeVecMatrix(rwMatrices->nullspace, rwMatrices->nullspace, + MathOperations::vecTransposeVecMatrix(acsParameters->rwMatrices.nullspace, + acsParameters->rwMatrices.nullspace, *nullSpaceMatrix, 4); MatrixOperations::multiply(*nullSpaceMatrix, wheelMomentum, rwTrqNs, 4, 4, 1); VectorOperations::mulScalar(rwTrqNs, gainNs, rwTrqNs, 4); VectorOperations::mulScalar(rwTrqNs, -1, rwTrqNs, 4); } -void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, double *torqueCommand) { +void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpeeds) { bool rwAvailable[4] = { (sensorValues->rw1Set.state.value && sensorValues->rw1Set.state.isValid()), (sensorValues->rw2Set.state.value && sensorValues->rw2Set.state.isValid()), (sensorValues->rw3Set.state.value && sensorValues->rw3Set.state.isValid()), (sensorValues->rw4Set.state.value && sensorValues->rw4Set.state.isValid())}; - int32_t omegaRw[4] = {sensorValues->rw1Set.currSpeed.value, sensorValues->rw2Set.currSpeed.value, - sensorValues->rw3Set.currSpeed.value, sensorValues->rw4Set.currSpeed.value}; + int32_t currRwSpeed[4] = { + sensorValues->rw1Set.currSpeed.value, sensorValues->rw2Set.currSpeed.value, + sensorValues->rw3Set.currSpeed.value, sensorValues->rw4Set.currSpeed.value}; for (uint8_t i = 0; i < 4; i++) { if (rwAvailable[i]) { - if (torqueMemory[i] != 0) { - if ((omegaRw[i] * torqueMemory[i]) > rwHandlingParameters->stictionReleaseSpeed) { - torqueMemory[i] = 0; - } else { - torqueCommand[i] = torqueMemory[i] * rwHandlingParameters->stictionTorque; - } - } else { - if ((omegaRw[i] < rwHandlingParameters->stictionSpeed) && - (omegaRw[i] > -rwHandlingParameters->stictionSpeed)) { - if (omegaRw[i] < omegaMemory[i]) { - torqueMemory[i] = -1; - } else { - torqueMemory[i] = 1; + if (rwCmdSpeeds[i] != 0) { + if (rwCmdSpeeds[i] > -acsParameters->rwHandlingParameters.stictionSpeed && + rwCmdSpeeds[i] < acsParameters->rwHandlingParameters.stictionSpeed) { + if (currRwSpeed[i] == 0) { + if (rwCmdSpeeds[i] > 0) { + rwCmdSpeeds[i] = acsParameters->rwHandlingParameters.stictionSpeed; + } else if (rwCmdSpeeds[i] < 0) { + rwCmdSpeeds[i] = -acsParameters->rwHandlingParameters.stictionSpeed; + } + } else if (currRwSpeed[i] < -acsParameters->rwHandlingParameters.stictionSpeed) { + rwCmdSpeeds[i] = acsParameters->rwHandlingParameters.stictionSpeed; + } else if (currRwSpeed[i] > acsParameters->rwHandlingParameters.stictionSpeed) { + rwCmdSpeeds[i] = -acsParameters->rwHandlingParameters.stictionSpeed; } - - torqueCommand[i] = torqueMemory[i] * rwHandlingParameters->stictionTorque; } } - } else { - torqueMemory[i] = 0; } - omegaMemory[i] = omegaRw[i]; } } diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index fb27fc6d..f6c6c345 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -1,16 +1,3 @@ -/* - * PtgCtrl.h - * - * Created on: 17 Jul 2022 - * Author: Robin Marquardt - * - * @brief: This class handles the pointing control mechanism. Calculation of an commanded - * torque for the reaction wheels, and magnetic Field strength for magnetorques for desaturation - * - * @note: A description of the used algorithms can be found in - * https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=896110 - */ - #ifndef PTGCTRL_H_ #define PTGCTRL_H_ @@ -23,6 +10,13 @@ #include "eive/resultClassIds.h" class PtgCtrl { + /* + * @brief: This class handles the pointing control mechanism. Calculation of an commanded + * torque for the reaction wheels, and magnetic Field strength for magnetorques for desaturation + * + * @note: A description of the used algorithms can be found in + * https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=896110 + */ public: /* @brief: Constructor * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters @@ -33,13 +27,7 @@ class PtgCtrl { static const uint8_t INTERFACE_ID = CLASS_ID::ACS_PTG; static const ReturnValue_t PTGCTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01); - /* @brief: Load AcsParameters for this class - * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters - */ - void loadAcsParameters(AcsParameters *acsParameters_); - /* @brief: Calculates the needed torque for the pointing control mechanism - * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters */ void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError, const double *deltaRate, const double *rwPseudoInv, double *torqueRws); @@ -54,18 +42,12 @@ class PtgCtrl { const int32_t *speedRw3, double *rwTrqNs); /* @brief: Commands the stiction torque in case wheel speed is to low - * @param: sensorValues class containing all RW related values * torqueCommand modified torque after antistiction */ - void rwAntistiction(ACS::SensorValues *sensorValues, double *torqueCommand); + void rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpeed); private: - AcsParameters::RwHandlingParameters *rwHandlingParameters; - AcsParameters::InertiaEIVE *inertiaEIVE; - AcsParameters::RwMatrices *rwMatrices; - - double torqueMemory[4] = {0, 0, 0, 0}; - double omegaMemory[4] = {0, 0, 0, 0}; + const AcsParameters *acsParameters; }; #endif /* ACS_CONTROL_PTGCTRL_H_ */ diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp index 5c665156..392e32ba 100644 --- a/mission/controller/acs/control/SafeCtrl.cpp +++ b/mission/controller/acs/control/SafeCtrl.cpp @@ -1,10 +1,3 @@ -/* - * SafeCtrl.cpp - * - * Created on: 19 Apr 2022 - * Author: Robin Marquardt - */ - #include "SafeCtrl.h" #include @@ -15,19 +8,10 @@ #include "../util/MathOperations.h" -SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { - loadAcsParameters(acsParameters_); - MatrixOperations::multiplyScalar(*(inertiaEIVE->inertiaMatrix), 10, *gainMatrixInertia, 3, - 3); -} +SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_; } SafeCtrl::~SafeCtrl() {} -void SafeCtrl::loadAcsParameters(AcsParameters *acsParameters_) { - safeModeControllerParameters = &(acsParameters_->safeModeControllerParameters); - inertiaEIVE = &(acsParameters_->inertiaEIVE); -} - ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid, double *magFieldModel, bool magFieldModelValid, double *sunDirModel, bool sunDirModelValid, double *satRateMekf, @@ -37,8 +21,8 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid, return SAFECTRL_MEKF_INPUT_INVALID; } - double kRate = safeModeControllerParameters->k_rate_mekf; - double kAlign = safeModeControllerParameters->k_align_mekf; + double kRate = acsParameters->safeModeControllerParameters.k_rate_mekf; + double kAlign = acsParameters->safeModeControllerParameters.k_align_mekf; // Calc sunDirB ,magFieldB with mekf output and model double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; @@ -71,6 +55,9 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid, VectorOperations::add(torqueRate, torqueAlign, torqueAll, 3); // Adding factor of inertia for axes + MatrixOperations::multiplyScalar(*(acsParameters->inertiaEIVE.inertiaMatrix), 10, + *gainMatrixInertia, 3, + 3); // why only for mekf one and not for no mekf MatrixOperations::multiply(*gainMatrixInertia, torqueAll, torqueCmd, 3, 3, 1); // MagMom B (orthogonal torque) @@ -126,7 +113,7 @@ ReturnValue_t SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBVal /* Only valid if angle between sun direction and magnetic field direction * is sufficiently large */ double angleSunMag = acos(cosAngleSunMag); - if (angleSunMag < safeModeControllerParameters->sunMagAngleMin) { + if (angleSunMag < acsParameters->safeModeControllerParameters.sunMagAngleMin) { return returnvalue::FAILED; } @@ -135,8 +122,8 @@ ReturnValue_t SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBVal VectorOperations::subtract(estSatRate, satRateRef, diffRate, 3); // Torque Align calculation - double kRateNoMekf = safeModeControllerParameters->k_rate_no_mekf; - double kAlignNoMekf = safeModeControllerParameters->k_align_no_mekf; + double kRateNoMekf = acsParameters->safeModeControllerParameters.k_rate_no_mekf; + double kAlignNoMekf = acsParameters->safeModeControllerParameters.k_align_no_mekf; double cosAngleAlignErr = VectorOperations::dot(sunDirRef, susDirB); double crossSusSunRef[3] = {0, 0, 0}; @@ -155,8 +142,8 @@ ReturnValue_t SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBVal // Final torque double torqueB[3] = {0, 0, 0}, torqueAlignRate[3] = {0, 0, 0}; VectorOperations::add(torqueRate, torqueAlign, torqueAlignRate, 3); - MatrixOperations::multiply(*(inertiaEIVE->inertiaMatrix), torqueAlignRate, torqueB, 3, 3, - 1); + MatrixOperations::multiply(*(acsParameters->inertiaEIVE.inertiaMatrix), torqueAlignRate, + torqueB, 3, 3, 1); // Magnetic moment double magMomB[3] = {0, 0, 0}; diff --git a/mission/controller/acs/control/SafeCtrl.h b/mission/controller/acs/control/SafeCtrl.h index 92d20313..ac5a754a 100644 --- a/mission/controller/acs/control/SafeCtrl.h +++ b/mission/controller/acs/control/SafeCtrl.h @@ -17,8 +17,6 @@ class SafeCtrl { static const uint8_t INTERFACE_ID = CLASS_ID::ACS_SAFE; static const ReturnValue_t SAFECTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01); - void loadAcsParameters(AcsParameters *acsParameters_); - ReturnValue_t safeMekf(timeval now, double *quatBJ, bool quatBJValid, double *magFieldModel, bool magFieldModelValid, double *sunDirModel, bool sunDirModelValid, double *satRateMekf, bool rateMekfValid, double *sunDirRef, @@ -32,8 +30,7 @@ class SafeCtrl { protected: private: - AcsParameters::SafeModeControllerParameters *safeModeControllerParameters; - AcsParameters::InertiaEIVE *inertiaEIVE; + AcsParameters *acsParameters; double gainMatrixInertia[3][3]; double magFieldBState[3]; diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h index c3509a04..f82e75f1 100644 --- a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h +++ b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h @@ -86,6 +86,7 @@ enum PoolIds : lp_id_t { // GPS Processed GC_LATITUDE, GD_LONGITUDE, + ALTITUDE, GPS_POSITION, GPS_VELOCITY, // MEKF @@ -109,7 +110,7 @@ static constexpr uint8_t SUS_SET_RAW_ENTRIES = 12; static constexpr uint8_t SUS_SET_PROCESSED_ENTRIES = 15; static constexpr uint8_t GYR_SET_RAW_ENTRIES = 4; static constexpr uint8_t GYR_SET_PROCESSED_ENTRIES = 5; -static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 4; +static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 5; static constexpr uint8_t MEKF_SET_ENTRIES = 3; static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 4; static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3; @@ -228,6 +229,7 @@ class GpsDataProcessed : public StaticLocalDataSet { lp_var_t gcLatitude = lp_var_t(sid.objectId, GC_LATITUDE, this); lp_var_t gdLongitude = lp_var_t(sid.objectId, GD_LONGITUDE, this); + lp_var_t altitude = lp_var_t(sid.objectId, ALTITUDE, this); lp_vec_t gpsPosition = lp_vec_t(sid.objectId, GPS_POSITION, this); lp_vec_t gpsVelocity = lp_vec_t(sid.objectId, GPS_VELOCITY, this);