diff --git a/CHANGELOG.md b/CHANGELOG.md index 02840492..3a29477a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -12,10 +12,34 @@ Starting at v2.0.0, this project will adhere to semantic versioning and the the will consitute of a breaking change warranting a new major release: - The TMTC interface changes in any shape of form. -- The behavour of the OBSW changes in a major shape or form relevant for operations +- The behaviour of the OBSW changes in a major shape or form relevant for operations # [unreleased] +## Changed + +- All `targetQuat` functions in `Guidance` now return the target quaternion (target + in ECI frame), which is passed on to `CtrlValData`. + +## Added + +- `MEKF` now returns an unique returnvalue depending on why the function terminates. These + returnvalues are used in the `AcsController` to determine on how to procede with its + perform functions. In case the `MEKF` did terminate before estimating the quaternion + and rotational rate, an info event will be triggered. Another info event can only be + triggered after the `MEKF` has run successfully again. If the `AcsController` tries to + perform any pointing mode and the `MEKF` fails, the `performPointingCtrl` function will + set the RWs to the last RW speeds and set a zero dipole vector. If the `MEKF` does not + recover within 5 cycles (2 mins) the `AcsController` triggers another event, resulting in + the `AcsSubsystem` being commanded to `SAFE`. +- `MekfData` now includes `mekfStatus` +- `CtrlValData` now includes `tgtRotRate` + +## Fixed + +- Usage of floats as iterators and using them to calculate a uint8_t index in `SusConverter` +- Removed unused variables in the `AcsController` + # [v1.30.0] eive-tmtc: v2.14.0 diff --git a/mission/acsDefs.h b/mission/acsDefs.h index f68ff24a..61a3c644 100644 --- a/mission/acsDefs.h +++ b/mission/acsDefs.h @@ -25,6 +25,10 @@ static const Event SAFE_RATE_VIOLATION = MAKE_EVENT(0, severity::MEDIUM); static constexpr Event SAFE_RATE_RECOVERY = MAKE_EVENT(1, severity::MEDIUM); //!< Multiple RWs are invalid, not commandable and therefore higher ACS modes cannot be maintained. static constexpr Event MULTIPLE_RW_INVALID = MAKE_EVENT(2, severity::HIGH); +//!< MEKF was not able to compute a solution. +static constexpr Event MEKF_INVALID_INFO = MAKE_EVENT(3, severity::INFO); +//!< MEKF was not able to compute a solution during any pointing ACS mode for a prolonged time. +static constexpr Event MEKF_INVALID_MODE_VIOLATION = MAKE_EVENT(4, severity::HIGH); extern const char* getModeStr(AcsMode mode); diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index e331f995..9bbd6fe4 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -14,8 +14,6 @@ AcsController::AcsController(object_id_t objectId) safeCtrl(&acsParameters), detumble(&acsParameters), ptgCtrl(&acsParameters), - detumbleCounter{0}, - multipleRwUnavailableCounter{0}, parameterHelper(this), mgmDataRaw(this), mgmDataProcessed(this), @@ -28,6 +26,14 @@ AcsController::AcsController(object_id_t objectId) ctrlValData(this), actuatorCmdData(this) {} +ReturnValue_t AcsController::initialize() { + ReturnValue_t result = parameterHelper.initialize(); + if (result != returnvalue::OK) { + return result; + } + return ExtendedControllerBase::initialize(); +} + ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) { ReturnValue_t result = actionHelper.handleActionMessage(message); if (result == returnvalue::OK) { @@ -133,17 +139,24 @@ void AcsController::performSafe() { sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters); - ReturnValue_t validMekf; - navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, - &mekfData, &validMekf); - - // give desired satellite rate and sun direction to align + ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, + &susDataProcessed, &mekfData); + if (result != MultiplicativeKalmanFilter::KALMAN_RUNNING && + result != MultiplicativeKalmanFilter::KALMAN_INITIALIZED) { + if (not mekfInvalidFlag) { + triggerEvent(acs::MEKF_INVALID_INFO); + mekfInvalidFlag = true; + } + } else { + mekfInvalidFlag = false; + } + // get desired satellite rate and sun direction to align double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0}; guidance.getTargetParamsSafe(sunTargetDir, satRateSafe); // if MEKF is working double magMomMtq[3] = {0, 0, 0}, errAng = 0.0; bool magMomMtqValid = false; - if (validMekf == returnvalue::OK) { + if (result == MultiplicativeKalmanFilter::KALMAN_RUNNING) { safeCtrl.safeMekf(now, mekfData.quatMekf.value, mekfData.quatMekf.isValid(), mgmDataProcessed.magIgrfModel.value, mgmDataProcessed.magIgrfModel.isValid(), susDataProcessed.sunIjkModel.value, susDataProcessed.isValid(), @@ -158,24 +171,9 @@ void AcsController::performSafe() { sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid); } - int16_t cmdDipolMtqs[3] = {0, 0, 0}; actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs); - { - PoolReadGuard pg(&ctrlValData); - if (pg.getReadResult() == returnvalue::OK) { - double unitQuat[4] = {0, 0, 0, 1}; - std::memcpy(ctrlValData.tgtQuat.value, unitQuat, 4 * sizeof(double)); - ctrlValData.tgtQuat.setValid(false); - std::memcpy(ctrlValData.errQuat.value, unitQuat, 4 * sizeof(double)); - ctrlValData.errQuat.setValid(false); - ctrlValData.errAng.value = errAng; - ctrlValData.errAng.setValid(true); - ctrlValData.setValidity(true, false); - } - } - - // Detumble check and switch + // detumble check and switch if (mekfData.satRotRateMekf.isValid() && VectorOperations::norm(mekfData.satRotRateMekf.value, 3) > acsParameters.detumbleParameter.omegaDetumbleStart) { @@ -193,20 +191,8 @@ void AcsController::performSafe() { triggerEvent(acs::SAFE_RATE_VIOLATION); } - { - PoolReadGuard pg(&actuatorCmdData); - if (pg.getReadResult() == returnvalue::OK) { - int32_t zeroVec[4] = {0, 0, 0, 0}; - std::memcpy(actuatorCmdData.rwTargetTorque.value, zeroVec, 4 * sizeof(int32_t)); - actuatorCmdData.rwTargetTorque.setValid(false); - std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 4 * sizeof(int32_t)); - actuatorCmdData.rwTargetSpeed.setValid(false); - std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolMtqs, 3 * sizeof(int16_t)); - actuatorCmdData.mtqTargetDipole.setValid(true); - actuatorCmdData.setValidity(true, false); - } - } - + updateCtrlValData(errAng); + updateActuatorCmdData(cmdDipolMtqs); // commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], // acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0, // acsParameters.rwHandlingParameters.rampTime); @@ -218,15 +204,21 @@ void AcsController::performDetumble() { sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters); - ReturnValue_t validMekf; - navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, - &mekfData, &validMekf); - + ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, + &susDataProcessed, &mekfData); + if (result != MultiplicativeKalmanFilter::KALMAN_RUNNING && + result != MultiplicativeKalmanFilter::KALMAN_INITIALIZED) { + if (not mekfInvalidFlag) { + triggerEvent(acs::MEKF_INVALID_INFO); + mekfInvalidFlag = true; + } + } else { + mekfInvalidFlag = false; + } double magMomMtq[3] = {0, 0, 0}; detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), magMomMtq); - int16_t cmdDipolMtqs[3] = {0, 0, 0}; actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs); if (mekfData.satRotRateMekf.isValid() && @@ -246,19 +238,8 @@ void AcsController::performDetumble() { triggerEvent(acs::SAFE_RATE_RECOVERY); } - { - PoolReadGuard pg(&actuatorCmdData); - if (pg.getReadResult() == returnvalue::OK) { - std::memset(actuatorCmdData.rwTargetTorque.value, 0, 4 * sizeof(double)); - actuatorCmdData.rwTargetTorque.setValid(false); - std::memset(actuatorCmdData.rwTargetSpeed.value, 0, 4 * sizeof(int32_t)); - actuatorCmdData.rwTargetSpeed.setValid(false); - std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolMtqs, 3 * sizeof(int16_t)); - actuatorCmdData.mtqTargetDipole.setValid(true); - actuatorCmdData.setValidity(true, false); - } - } - + disableCtrlValData(); + updateActuatorCmdData(cmdDipolMtqs); // commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], // acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0, // acsParameters.rwHandlingParameters.rampTime); @@ -270,22 +251,34 @@ void AcsController::performPointingCtrl() { sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters); - ReturnValue_t validMekf; - navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, - &mekfData, &validMekf); - - double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0}; - double quatRef[4] = {0, 0, 0, 0}; + ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, + &susDataProcessed, &mekfData); + if (result != MultiplicativeKalmanFilter::KALMAN_RUNNING && + result != MultiplicativeKalmanFilter::KALMAN_INITIALIZED) { + if (not mekfInvalidFlag) { + triggerEvent(acs::MEKF_INVALID_INFO); + mekfInvalidFlag = true; + } + if (mekfInvalidCounter > 4) { + triggerEvent(acs::MEKF_INVALID_MODE_VIOLATION); + } + mekfInvalidCounter++; + commandActuators(0, 0, 0, acsParameters.magnetorquesParameter.torqueDuration, cmdSpeedRws[0], + cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], + acsParameters.rwHandlingParameters.rampTime); + return; + } else { + mekfInvalidFlag = false; + mekfInvalidCounter = 0; + } uint8_t enableAntiStiction = true; - - double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0}, deltaRate[3] = {0, 0, 0}; double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); + result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); if (result == returnvalue::FAILED) { - multipleRwUnavailableCounter++; if (multipleRwUnavailableCounter > 4) { triggerEvent(acs::MULTIPLE_RW_INVALID); } + multipleRwUnavailableCounter++; return; } else { multipleRwUnavailableCounter = 0; @@ -294,40 +287,37 @@ void AcsController::performPointingCtrl() { 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}; switch (submode) { case acs::PTG_IDLE: - guidance.sunQuatPtg(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now, - targetQuat, refSatRate); - std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef, - 4 * sizeof(double)); - enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; - guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, - deltaRate); - ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate, - *rwPseudoInv, torquePtgRws); + guidance.targetQuatPtgSun(susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate); + guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, + targetSatRotRate, errorQuat, errorSatRotRate, errorAngle); ptgCtrl.ptgNullspace( - &acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), + &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); ptgCtrl.ptgDesaturation( - &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, + &acsParameters.idleModeControllerParameters, 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.idleModeControllerParameters.enableAntiStiction; break; case acs::PTG_TARGET: - guidance.targetQuatPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat, - refSatRate); - std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef, - 4 * sizeof(double)); - enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; - guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, - deltaRate); - ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate, + guidance.targetQuatPtgThreeAxes(now, gpsDataProcessed.gpsPosition.value, + gpsDataProcessed.gpsVelocity.value, targetQuat, + targetSatRotRate); + guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, + targetSatRotRate, acsParameters.targetModeControllerParameters.quatRef, + acsParameters.targetModeControllerParameters.refRotRate, errorQuat, + errorSatRotRate, errorAngle); + ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); ptgCtrl.ptgNullspace( &acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), @@ -340,17 +330,15 @@ void AcsController::performPointingCtrl() { 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; break; case acs::PTG_TARGET_GS: - guidance.targetQuatPtgGs(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now, - targetQuat, refSatRate); - std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef, - 4 * sizeof(double)); - enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; - guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, - deltaRate); - ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate, + guidance.targetQuatPtgGs(now, gpsDataProcessed.gpsPosition.value, + susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate); + guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, + targetSatRotRate, errorQuat, errorSatRotRate, errorAngle); + ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); ptgCtrl.ptgNullspace( &acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), @@ -363,16 +351,18 @@ void AcsController::performPointingCtrl() { 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; break; case acs::PTG_NADIR: - guidance.quatNadirPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat, - refSatRate); - std::memcpy(quatRef, acsParameters.nadirModeControllerParameters.quatRef, 4 * sizeof(double)); - enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction; - guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, - deltaRate); - ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, quatError, deltaRate, + guidance.targetQuatPtgNadirThreeAxes(now, gpsDataProcessed.gpsPosition.value, + gpsDataProcessed.gpsVelocity.value, targetQuat, + targetSatRotRate); + guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, + targetSatRotRate, acsParameters.nadirModeControllerParameters.quatRef, + acsParameters.nadirModeControllerParameters.refRotRate, errorQuat, + errorSatRotRate, errorAngle); + ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); ptgCtrl.ptgNullspace( &acsParameters.nadirModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), @@ -385,16 +375,17 @@ void AcsController::performPointingCtrl() { 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.nadirModeControllerParameters.enableAntiStiction; break; case acs::PTG_INERTIAL: - guidance.inertialQuatPtg(targetQuat, refSatRate); - std::memcpy(quatRef, acsParameters.inertialModeControllerParameters.quatRef, + std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat, 4 * sizeof(double)); - enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction; - guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, - deltaRate); - ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, quatError, deltaRate, + guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, + targetSatRotRate, acsParameters.inertialModeControllerParameters.quatRef, + acsParameters.inertialModeControllerParameters.refRotRate, errorQuat, + errorSatRotRate, errorAngle); + ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); ptgCtrl.ptgNullspace( &acsParameters.inertialModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), @@ -407,6 +398,7 @@ void AcsController::performPointingCtrl() { 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.inertialModeControllerParameters.enableAntiStiction; break; } @@ -414,24 +406,14 @@ void AcsController::performPointingCtrl() { ptgCtrl.rwAntistiction(&sensorValues, torqueRwsScaled); } - int32_t cmdSpeedRws[4] = {0, 0, 0, 0}; actuatorCmd.cmdSpeedToRws(sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, torqueRwsScaled, cmdSpeedRws); - int16_t cmdDipolMtqs[3] = {0, 0, 0}; actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs); - { - PoolReadGuard pg(&actuatorCmdData); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTrqNs, 4 * sizeof(double)); - std::memcpy(actuatorCmdData.rwTargetSpeed.value, cmdSpeedRws, 4 * sizeof(int32_t)); - std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolMtqs, 3 * sizeof(int16_t)); - actuatorCmdData.setValidity(true, true); - } - } - + updateCtrlValData(targetQuat, errorQuat, errorAngle); + updateActuatorCmdData(rwTrqNs, cmdSpeedRws, cmdDipolMtqs); // commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], // acsParameters.magnetorquesParameter.torqueDuration, cmdSpeedRws[0], // cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], @@ -467,6 +449,67 @@ ReturnValue_t AcsController::commandActuators(int16_t xDipole, int16_t yDipole, return returnvalue::OK; } +void AcsController::updateActuatorCmdData(int16_t mtqTargetDipole[3]) { + double rwTargetTorque[4] = {0.0, 0.0, 0.0, 0.0}; + int32_t rwTargetSpeed[4] = {0, 0, 0, 0}; + updateActuatorCmdData(rwTargetTorque, rwTargetSpeed, mtqTargetDipole); +} + +void AcsController::updateActuatorCmdData(double rwTargetTorque[4], int32_t rwTargetSpeed[4], + int16_t mtqTargetDipole[3]) { + { + PoolReadGuard pg(&actuatorCmdData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTargetTorque, 4 * sizeof(double)); + std::memcpy(actuatorCmdData.rwTargetSpeed.value, rwTargetSpeed, 4 * sizeof(int32_t)); + std::memcpy(actuatorCmdData.mtqTargetDipole.value, mtqTargetDipole, 3 * sizeof(int16_t)); + actuatorCmdData.setValidity(true, true); + } + } +} + +void AcsController::updateCtrlValData(double errAng) { + double unitQuat[4] = {0, 0, 0, 1}; + { + PoolReadGuard pg(&ctrlValData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(ctrlValData.tgtQuat.value, unitQuat, 4 * sizeof(double)); + ctrlValData.tgtQuat.setValid(false); + std::memcpy(ctrlValData.errQuat.value, unitQuat, 4 * sizeof(double)); + ctrlValData.errQuat.setValid(false); + ctrlValData.errAng.value = errAng; + ctrlValData.errAng.setValid(true); + ctrlValData.setValidity(true, false); + } + } +} + +void AcsController::updateCtrlValData(double tgtQuat[4], double errQuat[4], double errAng) { + { + PoolReadGuard pg(&ctrlValData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(ctrlValData.tgtQuat.value, tgtQuat, 4 * sizeof(double)); + std::memcpy(ctrlValData.errQuat.value, errQuat, 4 * sizeof(double)); + ctrlValData.errAng.value = errAng; + ctrlValData.setValidity(true, true); + } + } +} + +void AcsController::disableCtrlValData() { + double unitQuat[4] = {0, 0, 0, 1}; + double errAng = 0; + { + PoolReadGuard pg(&ctrlValData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(ctrlValData.tgtQuat.value, unitQuat, 4 * sizeof(double)); + std::memcpy(ctrlValData.errQuat.value, unitQuat, 4 * sizeof(double)); + ctrlValData.errAng.value = errAng; + ctrlValData.setValidity(false, true); + } + } +} + ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { // MGM Raw @@ -540,11 +583,13 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD // MEKF localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf); localDataPoolMap.emplace(acsctrl::PoolIds::SAT_ROT_RATE_MEKF, &satRotRateMekf); + localDataPoolMap.emplace(acsctrl::PoolIds::MEKF_STATUS, &mekfStatus); poolManager.subscribeForDiagPeriodicPacket({mekfData.getSid(), false, 5.0}); // Ctrl Values localDataPoolMap.emplace(acsctrl::PoolIds::TGT_QUAT, &tgtQuat); localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_QUAT, &errQuat); localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_ANG, &errAng); + localDataPoolMap.emplace(acsctrl::PoolIds::TGT_ROT_RATE, &tgtRotRate); poolManager.subscribeForRegularPeriodicPacket({ctrlValData.getSid(), false, 5.0}); // Actuator CMD localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_TORQUE, &rwTargetTorque); @@ -811,11 +856,3 @@ void AcsController::copyGyrData() { } } } - -ReturnValue_t AcsController::initialize() { - ReturnValue_t result = parameterHelper.initialize(); - if (result != returnvalue::OK) { - return result; - } - return ExtendedControllerBase::initialize(); -} diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index cf469f4c..4bec8b96 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -10,6 +10,7 @@ #include "acs/ActuatorCmd.h" #include "acs/Guidance.h" +#include "acs/MultiplicativeKalmanFilter.h" #include "acs/Navigation.h" #include "acs/SensorProcessing.h" #include "acs/control/Detumble.h" @@ -49,11 +50,15 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes Detumble detumble; PtgCtrl ptgCtrl; - uint8_t detumbleCounter; - uint8_t multipleRwUnavailableCounter; - ParameterHelper parameterHelper; + uint8_t detumbleCounter = 0; + uint8_t multipleRwUnavailableCounter = 0; + bool mekfInvalidFlag = true; + uint8_t mekfInvalidCounter = 0; + int32_t cmdSpeedRws[4] = {0, 0, 0, 0}; + int16_t cmdDipolMtqs[3] = {0, 0, 0}; + #if OBSW_THREAD_TRACING == 1 uint32_t opCounter = 0; #endif @@ -86,6 +91,12 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes ReturnValue_t commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole, uint16_t dipoleTorqueDuration, int32_t rw1Speed, int32_t rw2Speed, int32_t rw3Speed, int32_t rw4Speed, uint16_t rampTime); + void updateActuatorCmdData(int16_t mtqTargetDipole[3]); + void updateActuatorCmdData(double rwTargetTorque[4], int32_t rwTargetSpeed[4], + int16_t mtqTargetDipole[3]); + void updateCtrlValData(double errAng); + void updateCtrlValData(double tgtQuat[4], double errQuat[4], double errAng); + void disableCtrlValData(); /* ACS Sensor Values */ ACS::SensorValues sensorValues; @@ -176,12 +187,14 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes acsctrl::MekfData mekfData; PoolEntry quatMekf = PoolEntry(4); PoolEntry satRotRateMekf = PoolEntry(3); + PoolEntry mekfStatus = PoolEntry(); // Ctrl Values acsctrl::CtrlValData ctrlValData; PoolEntry tgtQuat = PoolEntry(4); PoolEntry errQuat = PoolEntry(4); PoolEntry errAng = PoolEntry(); + PoolEntry tgtRotRate = PoolEntry(4); // Actuator CMD acsctrl::ActuatorCmdData actuatorCmdData; diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index a06d6a58..2e5fbb1b 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -342,7 +342,41 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, return INVALID_IDENTIFIER_ID; } break; - case (0x9): // TargetModeControllerParameters + case (0x9): // IdleModeControllerParameters + switch (parameterId) { + case 0x0: + parameterWrapper->set(targetModeControllerParameters.zeta); + break; + case 0x1: + parameterWrapper->set(targetModeControllerParameters.om); + break; + case 0x2: + parameterWrapper->set(targetModeControllerParameters.omMax); + break; + case 0x3: + parameterWrapper->set(targetModeControllerParameters.qiMin); + break; + case 0x4: + parameterWrapper->set(targetModeControllerParameters.gainNullspace); + break; + case 0x5: + parameterWrapper->set(targetModeControllerParameters.desatMomentumRef); + break; + case 0x6: + parameterWrapper->set(targetModeControllerParameters.deSatGainFactor); + break; + case 0x7: + parameterWrapper->set(targetModeControllerParameters.desatOn); + break; + case 0x8: + parameterWrapper->set(targetModeControllerParameters.enableAntiStiction); + break; + + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0xA): // TargetModeControllerParameters switch (parameterId) { case 0x0: parameterWrapper->set(targetModeControllerParameters.zeta); @@ -408,7 +442,61 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, return INVALID_IDENTIFIER_ID; } break; - case (0xA): // NadirModeControllerParameters + case (0xB): // GsTargetModeControllerParameters + switch (parameterId) { + case 0x0: + parameterWrapper->set(targetModeControllerParameters.zeta); + break; + case 0x1: + parameterWrapper->set(targetModeControllerParameters.om); + break; + case 0x2: + parameterWrapper->set(targetModeControllerParameters.omMax); + break; + case 0x3: + parameterWrapper->set(targetModeControllerParameters.qiMin); + break; + case 0x4: + parameterWrapper->set(targetModeControllerParameters.gainNullspace); + break; + case 0x5: + parameterWrapper->set(targetModeControllerParameters.desatMomentumRef); + break; + case 0x6: + parameterWrapper->set(targetModeControllerParameters.deSatGainFactor); + break; + case 0x7: + parameterWrapper->set(targetModeControllerParameters.desatOn); + break; + case 0x8: + parameterWrapper->set(targetModeControllerParameters.enableAntiStiction); + break; + case 0x9: + parameterWrapper->set(targetModeControllerParameters.refDirection); + break; + case 0xA: + parameterWrapper->set(targetModeControllerParameters.refRotRate); + break; + case 0xB: + parameterWrapper->set(targetModeControllerParameters.quatRef); + break; + case 0xC: + parameterWrapper->set(targetModeControllerParameters.timeElapsedMax); + break; + case 0xD: + parameterWrapper->set(targetModeControllerParameters.latitudeTgt); + break; + case 0xE: + parameterWrapper->set(targetModeControllerParameters.longitudeTgt); + break; + case 0xF: + parameterWrapper->set(targetModeControllerParameters.altitudeTgt); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0xC): // NadirModeControllerParameters switch (parameterId) { case 0x0: parameterWrapper->set(nadirModeControllerParameters.zeta); @@ -450,7 +538,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, return INVALID_IDENTIFIER_ID; } break; - case (0xB): // InertialModeControllerParameters + case (0xD): // InertialModeControllerParameters switch (parameterId) { case 0x0: parameterWrapper->set(inertialModeControllerParameters.zeta); @@ -492,7 +580,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, return INVALID_IDENTIFIER_ID; } break; - case (0xC): // StrParameters + case (0xE): // StrParameters switch (parameterId) { case 0x0: parameterWrapper->set(strParameters.exclusionAngle); @@ -504,7 +592,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, return INVALID_IDENTIFIER_ID; } break; - case (0xD): // GpsParameters + case (0xF): // GpsParameters switch (parameterId) { case 0x0: parameterWrapper->set(gpsParameters.timeDiffVelocityMax); @@ -513,7 +601,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, return INVALID_IDENTIFIER_ID; } break; - case (0xE): // SunModelParameters + case (0x10): // SunModelParameters switch (parameterId) { case 0x0: parameterWrapper->set(sunModelParameters.domega); @@ -543,7 +631,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, return INVALID_IDENTIFIER_ID; } break; - case (0xF): // KalmanFilterParameters + case (0x11): // KalmanFilterParameters switch (parameterId) { case 0x0: parameterWrapper->set(kalmanFilterParameters.sensorNoiseSTR); @@ -567,7 +655,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, return INVALID_IDENTIFIER_ID; } break; - case (0x10): // MagnetorquesParameter + case (0x12): // MagnetorquesParameter switch (parameterId) { case 0x0: parameterWrapper->set(magnetorquesParameter.mtq0orientationMatrix); @@ -594,7 +682,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, return INVALID_IDENTIFIER_ID; } break; - case (0x11): // DetumbleParameter + case (0x13): // DetumbleParameter switch (parameterId) { case 0x0: parameterWrapper->set(detumbleParameter.detumblecounter); diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 365a04bd..f1b538c8 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -841,10 +841,12 @@ class AcsParameters : public HasParametersIF { uint8_t enableAntiStiction = true; } pointingLawParameters; + struct IdleModeControllerParameters : PointingLawParameters { + } idleModeControllerParameters; + struct TargetModeControllerParameters : PointingLawParameters { double refDirection[3] = {-1, 0, 0}; // Antenna - double refRotRate[3] = {0, 0, 0}; // Not used atm, do we want an option to - // give this as an input- currently en calculation is done + double refRotRate[3] = {0, 0, 0}; double quatRef[4] = {0, 0, 0, 1}; int8_t timeElapsedMax = 10; // rot rate calculations @@ -860,9 +862,20 @@ class AcsParameters : public HasParametersIF { double blindRotRate = 1 * M_PI / 180; } targetModeControllerParameters; + struct GsTargetModeControllerParameters : PointingLawParameters { + double refDirection[3] = {-1, 0, 0}; // Antenna + int8_t timeElapsedMax = 10; // rot rate calculations + + // Default is Stuttgart GS + double latitudeTgt = 48.7495 * M_PI / 180.; // [rad] Latitude + double longitudeTgt = 9.10384 * M_PI / 180.; // [rad] Longitude + double altitudeTgt = 500; // [m] + } gsTargetModeControllerParameters; + struct NadirModeControllerParameters : PointingLawParameters { 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 } nadirModeControllerParameters; diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index c4f05270..9b641c27 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -1,10 +1,3 @@ -/* - * Guidance.cpp - * - * Created on: 6 Jun 2022 - * Author: Robin Marquardt - */ - #include "Guidance.h" #include @@ -19,74 +12,50 @@ #include "util/CholeskyDecomposition.h" #include "util/MathOperations.h" -Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = *acsParameters_; } +Guidance::Guidance(AcsParameters *acsParameters_) : acsParameters(*acsParameters_) {} Guidance::~Guidance() {} -void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3]) { - if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE) or - not std::filesystem::exists(SD_1_SKEWED_PTG_FILE)) { // ToDo: if file does not exist anymore - std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir, - 3 * sizeof(double)); - } else { - std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDirLeop, - 3 * sizeof(double)); - } - std::memcpy(satRateSafe, acsParameters.safeModeControllerParameters.satRateRef, - 3 * sizeof(double)); -} - -void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, - acsctrl::SusDataProcessed *susDataProcessed, - acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now, - double targetQuat[4], double refSatRate[3]) { +void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double velSatE[3], + double sunDirI[3], double refDirB[3], double quatBI[4], + double targetQuat[4], double targetSatRotRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion to groundstation or given latitude, longitude and altitude //------------------------------------------------------------------------------------- - // Transform longitude, latitude and altitude to cartesian coordiantes (earth - // fixed/centered frame) - double targetCart[3] = {0, 0, 0}; + // transform longitude, latitude and altitude to ECEF + double targetE[3] = {0, 0, 0}; MathOperations::cartesianFromLatLongAlt( acsParameters.targetModeControllerParameters.latitudeTgt, acsParameters.targetModeControllerParameters.longitudeTgt, - acsParameters.targetModeControllerParameters.altitudeTgt, targetCart); + acsParameters.targetModeControllerParameters.altitudeTgt, targetE); - // Position of the satellite in the earth/fixed frame via GPS - double posSatE[3] = {0, 0, 0}; - double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180; - double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180; - MathOperations::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad, - sensorValues->gpsSet.altitude.value, posSatE); - - // Target direction in the ECEF frame + // target direction in the ECEF frame double targetDirE[3] = {0, 0, 0}; - VectorOperations::subtract(targetCart, posSatE, targetDirE, 3); + VectorOperations::subtract(targetE, posSatE, targetDirE, 3); - // Transformation between ECEF and IJK frame - double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot); - MathOperations::inverseMatrixDimThree(*dcmEJ, *dcmJE); + // transformation between ECEF and ECI frame + double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot); + MathOperations::inverseMatrixDimThree(*dcmEI, *dcmIE); - double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot); + double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot); - // Transformation between ECEF and Body frame - double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + // transformation between ECEF and Body frame + double dcmBI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double quatBJ[4] = {0, 0, 0, 0}; - std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double)); - QuaternionOperations::toDcm(quatBJ, dcmBJ); - MatrixOperations::multiply(*dcmBJ, *dcmJE, *dcmBE, 3, 3, 3); + QuaternionOperations::toDcm(quatBI, dcmBI); + MatrixOperations::multiply(*dcmBI, *dcmIE, *dcmBE, 3, 3, 3); - // Target Direction in the body frame + // target Direction in the body frame double targetDirB[3] = {0, 0, 0}; MatrixOperations::multiply(*dcmBE, targetDirE, targetDirB, 3, 3, 1); - // rotation quaternion from two vectors + // rotation quaternion from two vectors double refDir[3] = {0, 0, 0}; refDir[0] = acsParameters.targetModeControllerParameters.refDirection[0]; refDir[1] = acsParameters.targetModeControllerParameters.refDirection[1]; @@ -106,15 +75,13 @@ void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl: VectorOperations::normalize(targetQuat, targetQuat, 4); //------------------------------------------------------------------------------------- - // Calculation of reference rotation rate + // calculation of reference rotation rate //------------------------------------------------------------------------------------- - double velSatE[3] = {0, 0, 0}; - std::memcpy(velSatE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double)); double velSatB[3] = {0, 0, 0}, velSatBPart1[3] = {0, 0, 0}, velSatBPart2[3] = {0, 0, 0}; - // Velocity: v_B = dcm_BI * dcmIE * v_E + dcm_BI * DotDcm_IE * v_E + // velocity: v_B = dcm_BI * dcmIE * v_E + dcm_BI * DotDcm_IE * v_E MatrixOperations::multiply(*dcmBE, velSatE, velSatBPart1, 3, 3, 1); double dcmBEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MatrixOperations::multiply(*dcmBJ, *dcmJEDot, *dcmBEDot, 3, 3, 3); + MatrixOperations::multiply(*dcmBI, *dcmIEDot, *dcmBEDot, 3, 3, 3); MatrixOperations::multiply(*dcmBEDot, posSatE, velSatBPart2, 3, 3, 1); VectorOperations::add(velSatBPart1, velSatBPart2, velSatB, 3); @@ -124,21 +91,14 @@ void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl: double satRateDir[3] = {0, 0, 0}; VectorOperations::cross(velSatB, targetDirB, satRateDir); VectorOperations::normalize(satRateDir, satRateDir, 3); - VectorOperations::mulScalar(satRateDir, normRefSatRate, refSatRate, 3); + VectorOperations::mulScalar(satRateDir, normRefSatRate, targetSatRotRate, 3); //------------------------------------------------------------------------------------- // Calculation of reference rotation rate in case of star tracker blinding //------------------------------------------------------------------------------------- if (acsParameters.targetModeControllerParameters.avoidBlindStr) { double sunDirB[3] = {0, 0, 0}; - - if (susDataProcessed->sunIjkModel.isValid()) { - double sunDirJ[3] = {0, 0, 0}; - std::memcpy(sunDirJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double)); - MatrixOperations::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1); - } else { - std::memcpy(sunDirB, susDataProcessed->susVecTot.value, 3 * sizeof(double)); - } + MatrixOperations::multiply(*dcmBI, sunDirI, sunDirB, 3, 3, 1); double exclAngle = acsParameters.strParameters.exclusionAngle, blindStart = acsParameters.targetModeControllerParameters.blindAvoidStart, @@ -148,18 +108,14 @@ void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl: if (!(strBlindAvoidFlag)) { double critSightAngle = blindStart * exclAngle; - if (sightAngleSun < critSightAngle) { strBlindAvoidFlag = true; } - } - else { if (sightAngleSun < blindEnd * exclAngle) { double normBlindRefRate = acsParameters.targetModeControllerParameters.blindRotRate; double blindRefRate[3] = {0, 0, 0}; - if (sunDirB[1] < 0) { blindRefRate[0] = normBlindRefRate; blindRefRate[1] = 0; @@ -169,21 +125,353 @@ void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl: blindRefRate[1] = 0; blindRefRate[2] = 0; } - - VectorOperations::add(blindRefRate, refSatRate, refSatRate, 3); - + VectorOperations::add(blindRefRate, targetSatRotRate, targetSatRotRate, 3); } else { strBlindAvoidFlag = false; } } } + // revert calculated quaternion from qBX to qIX + double quatIB[4] = {0, 0, 0, 1}; + QuaternionOperations::inverse(quatBI, quatIB); + QuaternionOperations::multiply(quatIB, targetQuat, targetQuat); } -void Guidance::refRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4], - double *refSatRate) { +void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double velSatE[3], + double targetQuat[4], double targetSatRotRate[3]) { + //------------------------------------------------------------------------------------- + // Calculation of target quaternion for target pointing + //------------------------------------------------------------------------------------- + // 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); + double targetDirE[3] = {0, 0, 0}; + VectorOperations::subtract(targetE, posSatE, targetDirE, 3); + + // transformation between ECEF and ECI frame + double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot); + MathOperations::inverseMatrixDimThree(*dcmEI, *dcmIE); + + double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot); + + // target direction in the ECI frame + double posSatI[3] = {0, 0, 0}, targetI[3] = {0, 0, 0}, targetDirI[3] = {0, 0, 0}; + MatrixOperations::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1); + MatrixOperations::multiply(*dcmIE, targetE, targetI, 3, 3, 1); + VectorOperations::subtract(targetI, posSatI, targetDirI, 3); + + // x-axis aligned with target direction + // this aligns with the camera, E- and S-band antennas + double xAxis[3] = {0, 0, 0}; + VectorOperations::normalize(targetDirI, xAxis, 3); + + // transform velocity into inertial frame + double velocityI[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0}; + MatrixOperations::multiply(*dcmIE, velSatE, velPart1, 3, 3, 1); + MatrixOperations::multiply(*dcmIEDot, posSatE, velPart2, 3, 3, 1); + VectorOperations::add(velPart1, velPart2, velocityI, 3); + + // orbital normal vector of target and velocity vector + double orbitalNormalI[3] = {0, 0, 0}; + VectorOperations::cross(posSatI, velocityI, orbitalNormalI); + VectorOperations::normalize(orbitalNormalI, orbitalNormalI, 3); + + // y-axis of satellite in orbit plane so that z-axis is parallel to long side of picture + // resolution + double yAxis[3] = {0, 0, 0}; + VectorOperations::cross(orbitalNormalI, xAxis, yAxis); + VectorOperations::normalize(yAxis, yAxis, 3); + + // z-axis completes RHS + double zAxis[3] = {0, 0, 0}; + VectorOperations::cross(xAxis, yAxis, zAxis); + + // join transformation matrix + double dcmIX[3][3] = {{xAxis[0], yAxis[0], zAxis[0]}, + {xAxis[1], yAxis[1], zAxis[1]}, + {xAxis[2], yAxis[2], zAxis[2]}}; + QuaternionOperations::fromDcm(dcmIX, targetQuat); + + int8_t timeElapsedMax = acsParameters.targetModeControllerParameters.timeElapsedMax; + targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate); +} + +void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3], + double targetQuat[4], double targetSatRotRate[3]) { + //------------------------------------------------------------------------------------- + // Calculation of target quaternion for ground station pointing + //------------------------------------------------------------------------------------- + // transform longitude, latitude and altitude to cartesian coordiantes (ECEF) + double groundStationE[3] = {0, 0, 0}; + + MathOperations::cartesianFromLatLongAlt( + acsParameters.gsTargetModeControllerParameters.latitudeTgt, + acsParameters.gsTargetModeControllerParameters.longitudeTgt, + acsParameters.gsTargetModeControllerParameters.altitudeTgt, groundStationE); + double targetDirE[3] = {0, 0, 0}; + VectorOperations::subtract(groundStationE, posSatE, targetDirE, 3); + + // transformation between ECEF and ECI frame + double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot); + MathOperations::inverseMatrixDimThree(*dcmEI, *dcmIE); + + double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot); + + // target direction in the ECI frame + double posSatI[3] = {0, 0, 0}, groundStationI[3] = {0, 0, 0}, groundStationDirI[3] = {0, 0, 0}; + MatrixOperations::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1); + MatrixOperations::multiply(*dcmIE, groundStationE, groundStationI, 3, 3, 1); + VectorOperations::subtract(groundStationI, posSatI, groundStationDirI, 3); + + // negative x-axis aligned with target direction + // this aligns with the camera, E- and S-band antennas + double xAxis[3] = {0, 0, 0}; + VectorOperations::normalize(groundStationDirI, xAxis, 3); + VectorOperations::mulScalar(xAxis, -1, xAxis, 3); + + // get sun vector model in ECI + VectorOperations::normalize(sunDirI, sunDirI, 3); + + // calculate z-axis as projection of sun vector into plane defined by x-axis as normal vector + // z = sPerpenticular = s - sParallel = s - (x*s)/norm(x)^2 * x + double xDotS = VectorOperations::dot(xAxis, sunDirI); + xDotS /= pow(VectorOperations::norm(xAxis, 3), 2); + double sunParallel[3], zAxis[3]; + VectorOperations::mulScalar(xAxis, xDotS, sunParallel, 3); + VectorOperations::subtract(sunDirI, sunParallel, zAxis, 3); + VectorOperations::normalize(zAxis, zAxis, 3); + + // y-axis completes RHS + double yAxis[3]; + VectorOperations::cross(zAxis, xAxis, yAxis); + VectorOperations::normalize(yAxis, yAxis, 3); + + // join transformation matrix + double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]}, + {xAxis[1], yAxis[1], zAxis[1]}, + {xAxis[2], yAxis[2], zAxis[2]}}; + QuaternionOperations::fromDcm(dcmTgt, targetQuat); + + int8_t timeElapsedMax = acsParameters.gsTargetModeControllerParameters.timeElapsedMax; + targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate); +} + +void Guidance::targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double refSatRate[3]) { + //------------------------------------------------------------------------------------- + // Calculation of target quaternion to sun + //------------------------------------------------------------------------------------- + // positive z-Axis of EIVE in direction of sun + double zAxis[3] = {0, 0, 0}; + VectorOperations::normalize(sunDirI, zAxis, 3); + + // assign helper vector (north pole inertial) + double helperVec[3] = {0, 0, 1}; + + // construct y-axis from helper vector and z-axis + double yAxis[3] = {0, 0, 0}; + VectorOperations::cross(zAxis, helperVec, yAxis); + VectorOperations::normalize(yAxis, yAxis, 3); + + // x-axis completes RHS + double xAxis[3] = {0, 0, 0}; + VectorOperations::cross(yAxis, zAxis, xAxis); + VectorOperations::normalize(xAxis, xAxis, 3); + + // join transformation matrix + double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]}, + {xAxis[1], yAxis[1], zAxis[1]}, + {xAxis[2], yAxis[2], zAxis[2]}}; + QuaternionOperations::fromDcm(dcmTgt, targetQuat); + + //---------------------------------------------------------------------------- + // Calculation of reference rotation rate + //---------------------------------------------------------------------------- + refSatRate[0] = 0; + refSatRate[1] = 0; + refSatRate[2] = 0; +} + +void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], double quatBI[4], + double targetQuat[4], double refDirB[3], + double refSatRate[3]) { + //------------------------------------------------------------------------------------- + // Calculation of target quaternion for Nadir pointing + //------------------------------------------------------------------------------------- + double targetDirE[3] = {0, 0, 0}; + VectorOperations::mulScalar(posSatE, -1, targetDirE, 3); + + // transformation between ECEF and ECI frame + double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot); + MathOperations::inverseMatrixDimThree(*dcmEI, *dcmIE); + + double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot); + + // transformation between ECEF and Body frame + double dcmBI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + QuaternionOperations::toDcm(quatBI, dcmBI); + MatrixOperations::multiply(*dcmBI, *dcmIE, *dcmBE, 3, 3, 3); + + // target Direction in the body frame + double targetDirB[3] = {0, 0, 0}; + MatrixOperations::multiply(*dcmBE, targetDirE, targetDirB, 3, 3, 1); + + // 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]; + double noramlizedTargetDirB[3] = {0, 0, 0}; + VectorOperations::normalize(targetDirB, noramlizedTargetDirB, 3); + VectorOperations::normalize(refDir, refDir, 3); + double normTargetDirB = VectorOperations::norm(noramlizedTargetDirB, 3); + double normRefDir = VectorOperations::norm(refDir, 3); + double crossDir[3] = {0, 0, 0}; + double dotDirections = VectorOperations::dot(noramlizedTargetDirB, refDir); + VectorOperations::cross(noramlizedTargetDirB, refDir, crossDir); + targetQuat[0] = crossDir[0]; + targetQuat[1] = crossDir[1]; + targetQuat[2] = crossDir[2]; + targetQuat[3] = sqrt(pow(normTargetDirB, 2) * pow(normRefDir, 2) + dotDirections); + VectorOperations::normalize(targetQuat, targetQuat, 4); + //------------------------------------------------------------------------------------- // Calculation of reference rotation rate //------------------------------------------------------------------------------------- + refSatRate[0] = 0; + refSatRate[1] = 0; + refSatRate[2] = 0; + + // revert calculated quaternion from qBX to qIX + double quatIB[4] = {0, 0, 0, 1}; + QuaternionOperations::inverse(quatBI, quatIB); + QuaternionOperations::multiply(quatIB, targetQuat, targetQuat); +} + +void Guidance::targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], double velSatE[3], + double targetQuat[4], double refSatRate[3]) { + //------------------------------------------------------------------------------------- + // Calculation of target quaternion for Nadir pointing + //------------------------------------------------------------------------------------- + // transformation between ECEF and ECI frame + double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot); + MathOperations::inverseMatrixDimThree(*dcmEI, *dcmIE); + + double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot); + + // satellite position in inertial reference frame + double posSatI[3] = {0, 0, 0}; + MatrixOperations::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1); + + // negative x-axis aligned with position vector + // this aligns with the camera, E- and S-band antennas + double xAxis[3] = {0, 0, 0}; + VectorOperations::normalize(posSatI, xAxis, 3); + VectorOperations::mulScalar(xAxis, -1, xAxis, 3); + + // make z-Axis parallel to major part of camera resolution + double zAxis[3] = {0, 0, 0}; + double velocityI[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0}; + MatrixOperations::multiply(*dcmIE, velSatE, velPart1, 3, 3, 1); + MatrixOperations::multiply(*dcmIEDot, posSatE, velPart2, 3, 3, 1); + VectorOperations::add(velPart1, velPart2, velocityI, 3); + VectorOperations::cross(xAxis, velocityI, zAxis); + VectorOperations::normalize(zAxis, zAxis, 3); + + // y-Axis completes RHS + double yAxis[3] = {0, 0, 0}; + VectorOperations::cross(zAxis, xAxis, yAxis); + + // join transformation matrix + double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]}, + {xAxis[1], yAxis[1], zAxis[1]}, + {xAxis[2], yAxis[2], zAxis[2]}}; + QuaternionOperations::fromDcm(dcmTgt, targetQuat); + + int8_t timeElapsedMax = acsParameters.nadirModeControllerParameters.timeElapsedMax; + targetRotationRate(timeElapsedMax, now, targetQuat, refSatRate); +} + +void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4], + double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3], + double errorQuat[4], double errorSatRotRate[3], double errorAngle) { + // First calculate error quaternion between current and target orientation + QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat); + // Last calculate add rotation from reference quaternion + QuaternionOperations::multiply(refQuat, errorQuat, errorQuat); + // Keep scalar part of quaternion positive + if (errorQuat[3] < 0) { + VectorOperations::mulScalar(errorQuat, -1, errorQuat, 4); + } + // Calculate error angle + errorAngle = QuaternionOperations::getAngle(errorQuat, true); + + // Only give back error satellite rotational rate if orientation has already been aquired + if (errorAngle < 2. / 180. * M_PI) { + // First combine the target and reference satellite rotational rates + double combinedRefSatRotRate[3] = {0, 0, 0}; + VectorOperations::add(targetSatRotRate, refSatRotRate, combinedRefSatRotRate, 3); + // Then substract the combined required satellite rotational rates from the actual rate + VectorOperations::subtract(currentSatRotRate, combinedRefSatRotRate, errorSatRotRate, + 3); + } else { + // If orientation has not been aquired yet set satellite rotational rate to zero + errorSatRotRate = 0; + } + + // target flag in matlab, importance, does look like it only gives feedback if pointing control is + // under 150 arcsec ?? +} + +void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4], + double targetSatRotRate[3], double errorQuat[4], + double errorSatRotRate[3], double errorAngle) { + // First calculate error quaternion between current and target orientation + QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat); + // Keep scalar part of quaternion positive + if (errorQuat[3] < 0) { + VectorOperations::mulScalar(errorQuat, -1, errorQuat, 4); + } + // Calculate error angle + errorAngle = QuaternionOperations::getAngle(errorQuat, true); + + // Only give back error satellite rotational rate if orientation has already been aquired + if (errorAngle < 2. / 180. * M_PI) { + // Then substract the combined required satellite rotational rates from the actual rate + VectorOperations::subtract(currentSatRotRate, targetSatRotRate, errorSatRotRate, 3); + } else { + // If orientation has not been aquired yet set satellite rotational rate to zero + errorSatRotRate = 0; + } + + // target flag in matlab, importance, does look like it only gives feedback if pointing control is + // under 150 arcsec ?? +} + +void Guidance::targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4], + double *refSatRate) { + //------------------------------------------------------------------------------------- + // Calculation of target rotation rate + //------------------------------------------------------------------------------------- double timeElapsed = now.tv_sec + now.tv_usec * pow(10, -6) - (timeSavedQuaternion.tv_sec + timeSavedQuaternion.tv_usec * pow((double)timeSavedQuaternion.tv_usec, -6)); @@ -221,395 +509,6 @@ void Guidance::refRotationRate(int8_t timeElapsedMax, timeval now, double quatIn savedQuaternion[3] = quatInertialTarget[3]; } -void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues, - acsctrl::GpsDataProcessed *gpsDataProcessed, - acsctrl::MekfData *mekfData, timeval now, - double targetQuat[4], double refSatRate[3]) { - //------------------------------------------------------------------------------------- - // Calculation of target quaternion for target pointing - //------------------------------------------------------------------------------------- - // Transform longitude, latitude and altitude to cartesian coordiantes (earth - // fixed/centered frame) - double targetCart[3] = {0, 0, 0}; - - MathOperations::cartesianFromLatLongAlt( - acsParameters.targetModeControllerParameters.latitudeTgt, - acsParameters.targetModeControllerParameters.longitudeTgt, - acsParameters.targetModeControllerParameters.altitudeTgt, targetCart); - // Position of the satellite in the earth/fixed frame via GPS - double posSatE[3] = {0, 0, 0}; - std::memcpy(posSatE, gpsDataProcessed->gpsPosition.value, 3 * sizeof(double)); - double targetDirE[3] = {0, 0, 0}; - VectorOperations::subtract(targetCart, posSatE, targetDirE, 3); - - // Transformation between ECEF and IJK frame - double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot); - MathOperations::inverseMatrixDimThree(*dcmEJ, *dcmJE); - - double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot); - - // Target Direction and position vector in the inertial frame - double targetDirJ[3] = {0, 0, 0}, posSatJ[3] = {0, 0, 0}; - MatrixOperations::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1); - MatrixOperations::multiply(*dcmJE, posSatE, posSatJ, 3, 3, 1); - - // negative x-Axis aligned with target (Camera/E-band transmitter position) - double xAxis[3] = {0, 0, 0}; - VectorOperations::normalize(targetDirJ, xAxis, 3); - VectorOperations::mulScalar(xAxis, -1, xAxis, 3); - - // Transform velocity into inertial frame - double velocityE[3]; - std::memcpy(velocityE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double)); - double velocityJ[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0}; - MatrixOperations::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1); - MatrixOperations::multiply(*dcmJEDot, posSatE, velPart2, 3, 3, 1); - VectorOperations::add(velPart1, velPart2, velocityJ, 3); - - // orbital normal vector - double orbitalNormalJ[3] = {0, 0, 0}; - VectorOperations::cross(posSatJ, velocityJ, orbitalNormalJ); - VectorOperations::normalize(orbitalNormalJ, orbitalNormalJ, 3); - - // y-Axis of satellite in orbit plane so that z-axis parallel to long side of picture resolution - double yAxis[3] = {0, 0, 0}; - VectorOperations::cross(orbitalNormalJ, xAxis, yAxis); - VectorOperations::normalize(yAxis, yAxis, 3); - - // z-Axis completes RHS - double zAxis[3] = {0, 0, 0}; - VectorOperations::cross(xAxis, yAxis, zAxis); - - // Complete transformation matrix - double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]}, - {xAxis[1], yAxis[1], zAxis[1]}, - {xAxis[2], yAxis[2], zAxis[2]}}; - double quatInertialTarget[4] = {0, 0, 0, 0}; - QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget); - - int8_t timeElapsedMax = acsParameters.targetModeControllerParameters.timeElapsedMax; - refRotationRate(timeElapsedMax, now, quatInertialTarget, refSatRate); - - // Transform in system relative to satellite frame - double quatBJ[4] = {0, 0, 0, 0}; - std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double)); - QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat); -} - -void Guidance::targetQuatPtgGs(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, - acsctrl::SusDataProcessed *susDataProcessed, - acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now, - double targetQuat[4], double refSatRate[3]) { - //------------------------------------------------------------------------------------- - // Calculation of target quaternion for ground station pointing - //------------------------------------------------------------------------------------- - // Transform longitude, latitude and altitude to cartesian coordiantes (earth - // fixed/centered frame) - double groundStationCart[3] = {0, 0, 0}; - - MathOperations::cartesianFromLatLongAlt( - acsParameters.targetModeControllerParameters.latitudeTgt, - acsParameters.targetModeControllerParameters.longitudeTgt, - acsParameters.targetModeControllerParameters.altitudeTgt, groundStationCart); - // Position of the satellite in the earth/fixed frame via GPS - double posSatE[3] = {0, 0, 0}; - double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180; - double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180; - MathOperations::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad, - sensorValues->gpsSet.altitude.value, posSatE); - double targetDirE[3] = {0, 0, 0}; - VectorOperations::subtract(groundStationCart, posSatE, targetDirE, 3); - - // Transformation between ECEF and IJK frame - double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot); - MathOperations::inverseMatrixDimThree(*dcmEJ, *dcmJE); - - double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot); - - // Target Direction and position vector in the inertial frame - double targetDirJ[3] = {0, 0, 0}, posSatJ[3] = {0, 0, 0}; - MatrixOperations::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1); - MatrixOperations::multiply(*dcmJE, posSatE, posSatJ, 3, 3, 1); - - // negative x-Axis aligned with target (Camera/E-band transmitter position) - double xAxis[3] = {0, 0, 0}; - VectorOperations::normalize(targetDirJ, xAxis, 3); - VectorOperations::mulScalar(xAxis, -1, xAxis, 3); - - // get Sun Vector Model in ECI - double sunJ[3]; - std::memcpy(sunJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double)); - VectorOperations::normalize(sunJ, sunJ, 3); - - // calculate z-axis as projection of sun vector into plane defined by x-axis as normal vector - // z = sPerpenticular = s - sParallel = s - (x*s)/norm(x)^2 * x - double xDotS = VectorOperations::dot(xAxis, sunJ); - xDotS /= pow(VectorOperations::norm(xAxis, 3), 2); - double sunParallel[3], zAxis[3]; - VectorOperations::mulScalar(xAxis, xDotS, sunParallel, 3); - VectorOperations::subtract(sunJ, sunParallel, zAxis, 3); - VectorOperations::normalize(zAxis, zAxis, 3); - - // calculate y-axis - double yAxis[3]; - VectorOperations::cross(zAxis, xAxis, yAxis); - VectorOperations::normalize(yAxis, yAxis, 3); - - // Complete transformation matrix - double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]}, - {xAxis[1], yAxis[1], zAxis[1]}, - {xAxis[2], yAxis[2], zAxis[2]}}; - double quatInertialTarget[4] = {0, 0, 0, 0}; - QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget); - - int8_t timeElapsedMax = acsParameters.targetModeControllerParameters.timeElapsedMax; - refRotationRate(timeElapsedMax, now, quatInertialTarget, refSatRate); - - // Transform in system relative to satellite frame - double quatBJ[4] = {0, 0, 0, 0}; - std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double)); - QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat); -} - -void Guidance::sunQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, - acsctrl::SusDataProcessed *susDataProcessed, - acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now, - double targetQuat[4], double refSatRate[3]) { - //------------------------------------------------------------------------------------- - // Calculation of target quaternion to sun - //------------------------------------------------------------------------------------- - double quatBJ[4] = {0, 0, 0, 0}; - double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double)); - QuaternionOperations::toDcm(quatBJ, dcmBJ); - - double sunDirJ[3] = {0, 0, 0}, sunDirB[3] = {0, 0, 0}; - if (susDataProcessed->sunIjkModel.isValid()) { - std::memcpy(sunDirJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double)); - MatrixOperations::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1); - } else if (susDataProcessed->susVecTot.isValid()) { - std::memcpy(sunDirB, susDataProcessed->susVecTot.value, 3 * sizeof(double)); - } else { - return; - } - - // Transformation between ECEF and IJK frame - double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot); - MathOperations::inverseMatrixDimThree(*dcmEJ, *dcmJE); - double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot); - - // positive z-Axis of EIVE in direction of sun - double zAxis[3] = {0, 0, 0}; - VectorOperations::normalize(sunDirB, zAxis, 3); - - // Assign helper vector (north pole inertial) - double helperVec[3] = {0, 0, 1}; - - // - double yAxis[3] = {0, 0, 0}; - VectorOperations::cross(zAxis, helperVec, yAxis); - VectorOperations::normalize(yAxis, yAxis, 3); - - // - double xAxis[3] = {0, 0, 0}; - VectorOperations::cross(yAxis, zAxis, xAxis); - VectorOperations::normalize(xAxis, xAxis, 3); - - // Transformation matrix to Sun, no further transforamtions, reference is already - // the EIVE body frame - double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]}, - {xAxis[1], yAxis[1], zAxis[1]}, - {xAxis[2], yAxis[2], zAxis[2]}}; - double quatSun[4] = {0, 0, 0, 0}; - QuaternionOperations::fromDcm(dcmTgt, quatSun); - - targetQuat[0] = quatSun[0]; - targetQuat[1] = quatSun[1]; - targetQuat[2] = quatSun[2]; - targetQuat[3] = quatSun[3]; - - //---------------------------------------------------------------------------- - // Calculation of reference rotation rate - //---------------------------------------------------------------------------- - refSatRate[0] = 0; - refSatRate[1] = 0; - refSatRate[2] = 0; -} - -void Guidance::quatNadirPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, - timeval now, double targetQuat[4], - double refSatRate[3]) { // old version of Nadir Pointing - //------------------------------------------------------------------------------------- - // Calculation of target quaternion for Nadir pointing - //------------------------------------------------------------------------------------- - // Position of the satellite in the earth/fixed frame via GPS - double posSatE[3] = {0, 0, 0}; - double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180; - double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180; - MathOperations::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad, - sensorValues->gpsSet.altitude.value, posSatE); - double targetDirE[3] = {0, 0, 0}; - VectorOperations::mulScalar(posSatE, -1, targetDirE, 3); - - // Transformation between ECEF and IJK frame - double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot); - MathOperations::inverseMatrixDimThree(*dcmEJ, *dcmJE); - - double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot); - - // Transformation between ECEF and Body frame - double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double quatBJ[4] = {0, 0, 0, 0}; - std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double)); - QuaternionOperations::toDcm(quatBJ, dcmBJ); - MatrixOperations::multiply(*dcmBJ, *dcmJE, *dcmBE, 3, 3, 3); - - // Target Direction in the body frame - double targetDirB[3] = {0, 0, 0}; - MatrixOperations::multiply(*dcmBE, targetDirE, targetDirB, 3, 3, 1); - - // 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]; - double noramlizedTargetDirB[3] = {0, 0, 0}; - VectorOperations::normalize(targetDirB, noramlizedTargetDirB, 3); - VectorOperations::normalize(refDir, refDir, 3); - double normTargetDirB = VectorOperations::norm(noramlizedTargetDirB, 3); - double normRefDir = VectorOperations::norm(refDir, 3); - double crossDir[3] = {0, 0, 0}; - double dotDirections = VectorOperations::dot(noramlizedTargetDirB, refDir); - VectorOperations::cross(noramlizedTargetDirB, refDir, crossDir); - targetQuat[0] = crossDir[0]; - targetQuat[1] = crossDir[1]; - targetQuat[2] = crossDir[2]; - targetQuat[3] = sqrt(pow(normTargetDirB, 2) * pow(normRefDir, 2) + dotDirections); - VectorOperations::normalize(targetQuat, targetQuat, 4); - - //------------------------------------------------------------------------------------- - // Calculation of reference rotation rate - //------------------------------------------------------------------------------------- - refSatRate[0] = 0; - refSatRate[1] = 0; - refSatRate[2] = 0; -} - -void Guidance::quatNadirPtgThreeAxes(ACS::SensorValues *sensorValues, - acsctrl::GpsDataProcessed *gpsDataProcessed, - acsctrl::MekfData *mekfData, timeval now, double targetQuat[4], - double refSatRate[3]) { - //------------------------------------------------------------------------------------- - // Calculation of target quaternion for Nadir pointing - //------------------------------------------------------------------------------------- - // Position of the satellite in the earth/fixed frame via GPS - double posSatE[3] = {0, 0, 0}; - double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180; - double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180; - MathOperations::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad, - sensorValues->gpsSet.altitude.value, posSatE); - double targetDirE[3] = {0, 0, 0}; - VectorOperations::mulScalar(posSatE, -1, targetDirE, 3); - - // Transformation between ECEF and IJK frame - double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot); - MathOperations::inverseMatrixDimThree(*dcmEJ, *dcmJE); - - double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot); - - // Target Direction in the body frame - double targetDirJ[3] = {0, 0, 0}; - MatrixOperations::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1); - - // negative x-Axis aligned with target (Camera position) - double xAxis[3] = {0, 0, 0}; - VectorOperations::normalize(targetDirJ, xAxis, 3); - VectorOperations::mulScalar(xAxis, -1, xAxis, 3); - - // z-Axis parallel to long side of picture resolution - double zAxis[3] = {0, 0, 0}, velocityE[3]; - std::memcpy(velocityE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double)); - double velocityJ[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0}; - MatrixOperations::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1); - MatrixOperations::multiply(*dcmJEDot, posSatE, velPart2, 3, 3, 1); - VectorOperations::add(velPart1, velPart2, velocityJ, 3); - VectorOperations::cross(xAxis, velocityJ, zAxis); - VectorOperations::normalize(zAxis, zAxis, 3); - - // y-Axis completes RHS - double yAxis[3] = {0, 0, 0}; - VectorOperations::cross(zAxis, xAxis, yAxis); - - // Complete transformation matrix - double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]}, - {xAxis[1], yAxis[1], zAxis[1]}, - {xAxis[2], yAxis[2], zAxis[2]}}; - double quatInertialTarget[4] = {0, 0, 0, 0}; - QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget); - - int8_t timeElapsedMax = acsParameters.nadirModeControllerParameters.timeElapsedMax; - refRotationRate(timeElapsedMax, now, quatInertialTarget, refSatRate); - - // Transform in system relative to satellite frame - double quatBJ[4] = {0, 0, 0, 0}; - std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double)); - QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat); -} - -void Guidance::inertialQuatPtg(double targetQuat[4], double refSatRate[3]) { - std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat, - 4 * sizeof(double)); - std::memcpy(refSatRate, acsParameters.inertialModeControllerParameters.refRotRate, - 3 * sizeof(double)); -} - -void Guidance::comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double quatRef[4], - double refSatRate[3], double quatErrorComplete[4], double quatError[3], - double deltaRate[3]) { - double satRate[3] = {0, 0, 0}; - std::memcpy(satRate, mekfData->satRotRateMekf.value, 3 * sizeof(double)); - VectorOperations::subtract(satRate, refSatRate, deltaRate, 3); - // valid checks ? - double quatErrorMtx[4][4] = {{quatRef[3], quatRef[2], -quatRef[1], -quatRef[0]}, - {-quatRef[2], quatRef[3], quatRef[0], -quatRef[1]}, - {quatRef[1], -quatRef[0], quatRef[3], -quatRef[2]}, - {quatRef[0], -quatRef[1], quatRef[2], quatRef[3]}}; - - MatrixOperations::multiply(*quatErrorMtx, targetQuat, quatErrorComplete, 4, 4, 1); - - if (quatErrorComplete[3] < 0) { - quatErrorComplete[3] *= -1; - } - - quatError[0] = quatErrorComplete[0]; - quatError[1] = quatErrorComplete[1]; - quatError[2] = quatErrorComplete[2]; - - // target flag in matlab, importance, does look like it only gives feedback if pointing control is - // under 150 arcsec ?? -} - ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv) { bool rw1valid = (sensorValues->rw1Set.state.value && sensorValues->rw1Set.state.isValid()); @@ -641,6 +540,19 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, } } +void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3]) { + if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE) or + not std::filesystem::exists(SD_1_SKEWED_PTG_FILE)) { // ToDo: if file does not exist anymore + std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir, + 3 * sizeof(double)); + } else { + std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDirLeop, + 3 * sizeof(double)); + } + std::memcpy(satRateSafe, acsParameters.safeModeControllerParameters.satRateRef, + 3 * sizeof(double)); +} + ReturnValue_t Guidance::solarArrayDeploymentComplete() { if (std::filesystem::exists(SD_0_SKEWED_PTG_FILE)) { std::remove(SD_0_SKEWED_PTG_FILE); diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index e991d272..da9d429b 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -1,10 +1,3 @@ -/* - * Guidance.h - * - * Created on: 6 Jun 2022 - * Author: Robin Marquardt - */ - #ifndef GUIDANCE_H_ #define GUIDANCE_H_ @@ -24,49 +17,40 @@ class Guidance { // Function to get the target quaternion and refence rotation rate from gps position and // position of the ground station - void targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues, - acsctrl::GpsDataProcessed *gpsDataProcessed, - acsctrl::MekfData *mekfData, timeval now, double targetQuat[4], - double refSatRate[3]); - void targetQuatPtgGs(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, - acsctrl::SusDataProcessed *susDataProcessed, - acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now, - double targetQuat[4], double refSatRate[3]); - void targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, - acsctrl::SusDataProcessed *susDataProcessed, - acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now, - double targetQuat[4], double refSatRate[3]); + void targetQuatPtgSingleAxis(timeval now, double posSatE[3], double velSatE[3], double sunDirI[3], + double refDirB[3], double quatBI[4], double targetQuat[4], + double targetSatRotRate[3]); + void targetQuatPtgThreeAxes(timeval now, double posSatE[3], double velSatE[3], double quatIX[4], + double targetSatRotRate[3]); + void targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3], double quatIX[4], + double targetSatRotRate[3]); // Function to get the target quaternion and refence rotation rate for sun pointing after ground // station - void sunQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, - acsctrl::SusDataProcessed *susDataProcessed, - acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now, double targetQuat[4], - double refSatRate[3]); + void targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double refSatRate[3]); // Function to get the target quaternion and refence rotation rate from gps position for Nadir // pointing - void quatNadirPtgThreeAxes(ACS::SensorValues *sensorValues, - acsctrl::GpsDataProcessed *gpsDataProcessed, - acsctrl::MekfData *mekfData, timeval now, double targetQuat[4], - double refSatRate[3]); - void quatNadirPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, - timeval now, double targetQuat[4], double refSatRate[3]); + void targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], double quatBI[4], + double targetQuat[4], double refDirB[3], double refSatRate[3]); + void targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], double velSatE[3], + double targetQuat[4], double refSatRate[3]); - // Function to get the target quaternion and refence rotation rate from parameters for inertial - // pointing - void inertialQuatPtg(double targetQuat[4], double refSatRate[3]); + // @note: Calculates the error quaternion between the current orientation and the target + // quaternion, considering a reference quaternion. Additionally the difference between the actual + // and a desired satellite rotational rate is calculated, again considering a reference rotational + // rate. Lastly gives back the error angle of the error quaternion. + void comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4], + double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3], + double errorQuat[4], double errorSatRotRate[3], double errorAngle); + void comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4], + double targetSatRotRate[3], double errorQuat[4], double errorSatRotRate[3], + double errorAngle); - // @note: compares target Quaternion and reference quaternion, also actual satellite rate and - // desired - void comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double quatRef[4], - double refSatRate[3], double quatErrorComplete[4], double quatError[3], - double deltaRate[3]); + void targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4], + double *targetSatRotRate); - void refRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4], - double *refSatRate); - - // @note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid + // @note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid // reation wheel maybe can be done in "commanding.h" ReturnValue_t getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv); diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp index f54893a2..d90f3f51 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp +++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp @@ -14,7 +14,7 @@ /*Initialisation of values for parameters in constructor*/ MultiplicativeKalmanFilter::MultiplicativeKalmanFilter(AcsParameters *acsParameters_) - : initialQuaternion{0.5, 0.5, 0.5, 0.5}, + : 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_); @@ -27,12 +27,10 @@ void MultiplicativeKalmanFilter::loadAcsParameters(AcsParameters *acsParameters_ kalmanFilterParameters = &(acsParameters_->kalmanFilterParameters); } -void MultiplicativeKalmanFilter::reset() {} - -void MultiplicativeKalmanFilter::init( +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) { // valids for "model measurements"? + const bool validMagModel, acsctrl::MekfData *mekfData) { // valids for "model measurements"? // check for valid mag/sun if (validMagField_ && validSS && validSSModel && validMagModel) { validInit = true; @@ -190,9 +188,13 @@ void MultiplicativeKalmanFilter::init( initialCovarianceMatrix[5][3] = initGyroCov[2][0]; initialCovarianceMatrix[5][4] = initGyroCov[2][1]; initialCovarianceMatrix[5][5] = initGyroCov[2][2]; + updateDataSetWithoutData(mekfData, MekfStatus::INITIALIZED); + return KALMAN_INITIALIZED; } else { // no initialisation possible, no valid measurements validInit = false; + updateDataSetWithoutData(mekfData, MekfStatus::UNINITIALIZED); + return KALMAN_UNINITIALIZED; } } @@ -208,33 +210,13 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c // Check for GYR Measurements int MDF = 0; // Matrix Dimension Factor if (!validGYRs_) { - { - PoolReadGuard pg(mekfData); - if (pg.getReadResult() == returnvalue::OK) { - double unitQuat[4] = {0.0, 0.0, 0.0, 1.0}; - double zeroVec[3] = {0.0, 0.0, 0.0}; - std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double)); - std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double)); - mekfData->setValidity(false, true); - } - } - validMekf = false; - return KALMAN_NO_GYR_MEAS; + updateDataSetWithoutData(mekfData, MekfStatus::NO_GYR_DATA); + return KALMAN_NO_GYR_DATA; } // Check for Model Calculations else if (!validSSModel || !validMagModel) { - { - PoolReadGuard pg(mekfData); - if (pg.getReadResult() == returnvalue::OK) { - double unitQuat[4] = {0.0, 0.0, 0.0, 1.0}; - double zeroVec[3] = {0.0, 0.0, 0.0}; - std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double)); - std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double)); - mekfData->setValidity(false, true); - } - } - validMekf = false; - return KALMAN_NO_MODEL; + updateDataSetWithoutData(mekfData, MekfStatus::NO_MODEL_VECTORS); + return KALMAN_NO_MODEL_VECTORS; } // Check Measurements available from SS, MAG, STR if (validSS && validMagField_ && validSTR_) { @@ -260,17 +242,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c MDF = 3; } else { sensorsAvail = 8; // no measurements - validMekf = false; - { - PoolReadGuard pg(mekfData); - if (pg.getReadResult() == returnvalue::OK) { - double unitQuat[4] = {0.0, 0.0, 0.0, 1.0}; - double zeroVec[3] = {0.0, 0.0, 0.0}; - std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double)); - std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double)); - mekfData->setValidity(false, true); - } - } + updateDataSetWithoutData(mekfData, MekfStatus::NO_SUS_MGM_STR_DATA); return returnvalue::FAILED; } @@ -881,18 +853,8 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c double invResidualCov[MDF][MDF] = {{0}}; int inversionFailed = MathOperations::inverseMatrix(*residualCov, *invResidualCov, MDF); if (inversionFailed) { - { - PoolReadGuard pg(mekfData); - if (pg.getReadResult() == returnvalue::OK) { - double unitQuat[4] = {0.0, 0.0, 0.0, 1.0}; - double zeroVec[3] = {0.0, 0.0, 0.0}; - std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double)); - std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double)); - mekfData->setValidity(false, true); - } - } - validMekf = false; - return KALMAN_INVERSION_FAILED; // RETURN VALUE ? -- Like: Kalman Inversion Failed + updateDataSetWithoutData(mekfData, MekfStatus::COVARIANCE_INVERSION_FAILED); + return KALMAN_COVARIANCE_INVERSION_FAILED; // RETURN VALUE ? -- Like: Kalman Inversion Failed } // [K = P * H' / (H * P * H' + R)] @@ -1121,20 +1083,47 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c MatrixOperations::multiply(*discTimeMatrix, *cov1, *cov1, 6, 6, 6); MatrixOperations::add(*cov0, *cov1, *initialCovarianceMatrix, 6, 6); - validMekf = true; - // Discrete Time Step + updateDataSet(mekfData, MekfStatus::RUNNING, quatBJ, rotRateEst); + return KALMAN_RUNNING; +} - // Check for new data in measurement -> SensorProcessing ? +void MultiplicativeKalmanFilter::reset(acsctrl::MekfData *mekfData) { + double resetQuaternion[4] = {0, 0, 0, 1}; + double resetCovarianceMatrix[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}}; + std::memcpy(initialQuaternion, resetQuaternion, 4 * sizeof(double)); + std::memcpy(initialCovarianceMatrix, resetCovarianceMatrix, 6 * 6 * sizeof(double)); + updateDataSetWithoutData(mekfData, MekfStatus::UNINITIALIZED); +} +void MultiplicativeKalmanFilter::updateDataSetWithoutData(acsctrl::MekfData *mekfData, + MekfStatus mekfStatus) { { PoolReadGuard pg(mekfData); if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(mekfData->quatMekf.value, quatBJ, 4 * sizeof(double)); - std::memcpy(mekfData->satRotRateMekf.value, rotRateEst, 3 * sizeof(double)); + double unitQuat[4] = {0.0, 0.0, 0.0, 1.0}; + double zeroVec[3] = {0.0, 0.0, 0.0}; + std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double)); + mekfData->quatMekf.setValid(false); + std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double)); + mekfData->satRotRateMekf.setValid(false); + mekfData->mekfStatus = mekfStatus; + mekfData->setValidity(true, false); + } + } +} + +void MultiplicativeKalmanFilter::updateDataSet(acsctrl::MekfData *mekfData, + MekfStatus mekfStatus, double quat[4], + double satRotRate[3]) { + { + PoolReadGuard pg(mekfData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(mekfData->quatMekf.value, quat, 4 * sizeof(double)); + std::memcpy(mekfData->satRotRateMekf.value, satRotRate, 3 * sizeof(double)); + mekfData->mekfStatus = mekfStatus; mekfData->setValidity(true, true); } } - - return returnvalue::OK; } diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.h b/mission/controller/acs/MultiplicativeKalmanFilter.h index a2f81272..dd02cf9f 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.h +++ b/mission/controller/acs/MultiplicativeKalmanFilter.h @@ -15,8 +15,7 @@ #ifndef MULTIPLICATIVEKALMANFILTER_H_ #define MULTIPLICATIVEKALMANFILTER_H_ -#include //uint8_t -#include /*purpose, timeval ?*/ +#include #include "../controllerdefinitions/AcsCtrlDefinitions.h" #include "AcsParameters.h" @@ -30,18 +29,19 @@ class MultiplicativeKalmanFilter { MultiplicativeKalmanFilter(AcsParameters *acsParameters_); virtual ~MultiplicativeKalmanFilter(); - void reset(); // NOT YET DEFINED - should only reset all mekf variables + void reset(acsctrl::MekfData *mekfData); /* @brief: init() - This function initializes the Kalman Filter and will provide the first * quaternion through the QUEST algorithm * @param: magneticField_ magnetic field vector in the body frame - * sunDir_ sun direction vector in the body frame - * sunDirJ sun direction vector in the ECI frame - * magFieldJ magnetic field vector in the ECI frame + * sunDir_ sun direction vector in the body frame + * sunDirJ sun direction vector in the ECI frame + * magFieldJ magnetic field vector in the ECI frame */ - void 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); + 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); /* @brief: mekfEst() - This function calculates the quaternion and gyro bias of the Kalman Filter * for the current step after the initalization @@ -63,11 +63,26 @@ class MultiplicativeKalmanFilter { const double *sunDirJ, const bool validSSModel, const double *magFieldJ, const bool validMagModel, double sampleTime, acsctrl::MekfData *mekfData); + enum MekfStatus : uint8_t { + UNINITIALIZED = 0, + NO_GYR_DATA = 1, + NO_MODEL_VECTORS = 2, + NO_SUS_MGM_STR_DATA = 3, + COVARIANCE_INVERSION_FAILED = 4, + INITIALIZED = 10, + RUNNING = 11, + }; + // resetting Mekf static constexpr uint8_t IF_KAL_ID = CLASS_ID::ACS_KALMAN; - static constexpr ReturnValue_t KALMAN_NO_GYR_MEAS = returnvalue::makeCode(IF_KAL_ID, 1); - static constexpr ReturnValue_t KALMAN_NO_MODEL = returnvalue::makeCode(IF_KAL_ID, 2); - static constexpr ReturnValue_t KALMAN_INVERSION_FAILED = returnvalue::makeCode(IF_KAL_ID, 3); + static constexpr ReturnValue_t KALMAN_UNINITIALIZED = returnvalue::makeCode(IF_KAL_ID, 2); + static constexpr ReturnValue_t KALMAN_NO_GYR_DATA = returnvalue::makeCode(IF_KAL_ID, 3); + static constexpr ReturnValue_t KALMAN_NO_MODEL_VECTORS = returnvalue::makeCode(IF_KAL_ID, 4); + static constexpr ReturnValue_t KALMAN_NO_SUS_MGM_STR_DATA = returnvalue::makeCode(IF_KAL_ID, 5); + static constexpr ReturnValue_t KALMAN_COVARIANCE_INVERSION_FAILED = + returnvalue::makeCode(IF_KAL_ID, 6); + static constexpr ReturnValue_t KALMAN_INITIALIZED = returnvalue::makeCode(IF_KAL_ID, 7); + static constexpr ReturnValue_t KALMAN_RUNNING = returnvalue::makeCode(IF_KAL_ID, 8); private: /*Parameters*/ @@ -80,16 +95,17 @@ class MultiplicativeKalmanFilter { double initialQuaternion[4]; /*after reset?QUEST*/ double initialCovarianceMatrix[6][6]; /*after reset?QUEST*/ double propagatedQuaternion[4]; /*Filter Quaternion for next step*/ - bool validMekf; uint8_t sensorsAvail; /*Outputs*/ double quatBJ[4]; /* Output Quaternion */ double biasGYR[3]; /*Between measured and estimated sat Rate*/ /*Parameter INIT*/ - // double alpha, gamma, beta; /*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]); }; #endif /* ACS_MULTIPLICATIVEKALMANFILTER_H_ */ diff --git a/mission/controller/acs/Navigation.cpp b/mission/controller/acs/Navigation.cpp index a9de5817..c6310302 100644 --- a/mission/controller/acs/Navigation.cpp +++ b/mission/controller/acs/Navigation.cpp @@ -1,10 +1,3 @@ -/* - * Navigation.cpp - * - * Created on: 23 May 2022 - * Author: Robin Marquardt - */ - #include "Navigation.h" #include @@ -21,37 +14,37 @@ Navigation::Navigation(AcsParameters *acsParameters_) : multiplicativeKalmanFilt Navigation::~Navigation() {} -void Navigation::useMekf(ACS::SensorValues *sensorValues, - acsctrl::GyrDataProcessed *gyrDataProcessed, - acsctrl::MgmDataProcessed *mgmDataProcessed, - acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MekfData *mekfData, - ReturnValue_t *mekfValid) { - double quatJB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value, +ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues, + acsctrl::GyrDataProcessed *gyrDataProcessed, + acsctrl::MgmDataProcessed *mgmDataProcessed, + acsctrl::SusDataProcessed *susDataProcessed, + acsctrl::MekfData *mekfData) { + double quatIB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value, sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value}; - bool quatJBValid = sensorValues->strSet.caliQx.isValid() && + bool quatIBValid = sensorValues->strSet.caliQx.isValid() && sensorValues->strSet.caliQy.isValid() && sensorValues->strSet.caliQz.isValid() && sensorValues->strSet.caliQw.isValid(); if (kalmanInit) { - *mekfValid = multiplicativeKalmanFilter.mekfEst( - quatJB, quatJBValid, gyrDataProcessed->gyrVecTot.value, + return multiplicativeKalmanFilter.mekfEst( + quatIB, quatIBValid, gyrDataProcessed->gyrVecTot.value, gyrDataProcessed->gyrVecTot.isValid(), mgmDataProcessed->mgmVecTot.value, 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); // VALIDS FOR QUAT AND RATE ?? + mgmDataProcessed->magIgrfModel.isValid(), acsParameters.onBoardParams.sampleTime, mekfData); } else { - multiplicativeKalmanFilter.init( + ReturnValue_t result; + result = multiplicativeKalmanFilter.init( mgmDataProcessed->mgmVecTot.value, mgmDataProcessed->mgmVecTot.isValid(), susDataProcessed->susVecTot.value, susDataProcessed->susVecTot.isValid(), susDataProcessed->sunIjkModel.value, susDataProcessed->sunIjkModel.isValid(), - mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid()); + mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid(), mekfData); kalmanInit = true; - *mekfValid = returnvalue::OK; - - // Maybe we need feedback from kalmanfilter to identify if there was a problem with the - // init of kalman filter where does this class know from that kalman filter was not - // initialized ? + return result; } } + +void Navigation::resetMekf(acsctrl::MekfData *mekfData) { + multiplicativeKalmanFilter.reset(mekfData); +} diff --git a/mission/controller/acs/Navigation.h b/mission/controller/acs/Navigation.h index 2474cb67..e054bfbf 100644 --- a/mission/controller/acs/Navigation.h +++ b/mission/controller/acs/Navigation.h @@ -1,10 +1,3 @@ -/* - * Navigation.h - * - * Created on: 19 Apr 2022 - * Author: Robin Marquardt - */ - #ifndef NAVIGATION_H_ #define NAVIGATION_H_ @@ -16,14 +9,14 @@ class Navigation { public: - Navigation(AcsParameters *acsParameters_); // Input mode ? + Navigation(AcsParameters *acsParameters_); virtual ~Navigation(); - void useMekf(ACS::SensorValues *sensorValues, acsctrl::GyrDataProcessed *gyrDataProcessed, - acsctrl::MgmDataProcessed *mgmDataProcessed, - acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MekfData *mekfData, - ReturnValue_t *mekfValid); - void processSensorData(); + ReturnValue_t useMekf(ACS::SensorValues *sensorValues, + acsctrl::GyrDataProcessed *gyrDataProcessed, + acsctrl::MgmDataProcessed *mgmDataProcessed, + acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MekfData *mekfData); + void resetMekf(acsctrl::MekfData *mekfData); protected: private: diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 0845768a..e268d786 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -1,10 +1,3 @@ -/* - * SensorProcessing.cpp - * - * Created on: 7 Mar 2022 - * Author: Robin Marquardt - */ - #include "SensorProcessing.h" #include diff --git a/mission/controller/acs/SusConverter.cpp b/mission/controller/acs/SusConverter.cpp index 7020742a..f56ad32e 100644 --- a/mission/controller/acs/SusConverter.cpp +++ b/mission/controller/acs/SusConverter.cpp @@ -63,8 +63,7 @@ void SusConverter::calcAngle(const uint16_t susChannel[6]) { } void SusConverter::calibration(const float coeffAlpha[9][10], const float coeffBeta[9][10]) { - uint8_t index; - float k, l; + uint8_t index, k, l; // while loop iterates above all calibration cells to use the different calibration functions in // each cell @@ -75,10 +74,10 @@ void SusConverter::calibration(const float coeffAlpha[9][10], const float coeffB while (l < 3) { l++; // if-condition to check in which cell the data point has to be - if ((alphaBetaRaw[0] > ((completeCellWidth * ((k - 1) / 3)) - halfCellWidth) && - alphaBetaRaw[0] < ((completeCellWidth * (k / 3)) - halfCellWidth)) && - (alphaBetaRaw[1] > ((completeCellWidth * ((l - 1) / 3)) - halfCellWidth) && - alphaBetaRaw[1] < ((completeCellWidth * (l / 3)) - halfCellWidth))) { + if ((alphaBetaRaw[0] > ((completeCellWidth * ((k - 1) / 3.)) - halfCellWidth) && + alphaBetaRaw[0] < ((completeCellWidth * (k / 3.)) - halfCellWidth)) && + (alphaBetaRaw[1] > ((completeCellWidth * ((l - 1) / 3.)) - halfCellWidth) && + alphaBetaRaw[1] < ((completeCellWidth * (l / 3.)) - halfCellWidth))) { index = (3 * (k - 1) + l) - 1; // calculate the index of the datapoint for the right cell alphaBetaCalibrated[0] = coeffAlpha[index][0] + coeffAlpha[index][1] * alphaBetaRaw[0] + diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 60394421..74a32a4a 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -27,7 +27,7 @@ void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_) { } void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, - const double *qError, const double *deltaRate, const double *rwPseudoInv, + const double *errorQuat, const double *deltaRate, const double *rwPseudoInv, double *torqueRws) { //------------------------------------------------------------------------------------------------ // Compute gain matrix K and P matrix @@ -37,6 +37,8 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters double qErrorMin = pointingLawParameters->qiMin; double omMax = pointingLawParameters->omMax; + double qError[3] = {errorQuat[0], errorQuat[1], errorQuat[2]}; + double cInt = 2 * om * zeta; double kInt = 2 * pow(om, 2); diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h index e3908bd0..0675262c 100644 --- a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h +++ b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h @@ -91,10 +91,12 @@ enum PoolIds : lp_id_t { // MEKF SAT_ROT_RATE_MEKF, QUAT_MEKF, + MEKF_STATUS, // Ctrl Values TGT_QUAT, ERROR_QUAT, ERROR_ANG, + TGT_ROT_RATE, // Actuator Cmd RW_TARGET_TORQUE, RW_TARGET_SPEED, @@ -109,7 +111,7 @@ 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 MEKF_SET_ENTRIES = 2; -static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 3; +static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 4; static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3; /** @@ -238,6 +240,7 @@ class MekfData : public StaticLocalDataSet { lp_vec_t quatMekf = lp_vec_t(sid.objectId, QUAT_MEKF, this); lp_vec_t satRotRateMekf = lp_vec_t(sid.objectId, SAT_ROT_RATE_MEKF, this); + lp_var_t mekfStatus = lp_var_t(sid.objectId, MEKF_STATUS, this); private: }; @@ -249,6 +252,7 @@ class CtrlValData : public StaticLocalDataSet { lp_vec_t tgtQuat = lp_vec_t(sid.objectId, TGT_QUAT, this); lp_vec_t errQuat = lp_vec_t(sid.objectId, ERROR_QUAT, this); lp_var_t errAng = lp_var_t(sid.objectId, ERROR_ANG, this); + lp_vec_t tgtRotRate = lp_vec_t(sid.objectId, TGT_ROT_RATE, this); private: }; diff --git a/mission/system/objects/AcsSubsystem.cpp b/mission/system/objects/AcsSubsystem.cpp index edb1c6d3..c812394c 100644 --- a/mission/system/objects/AcsSubsystem.cpp +++ b/mission/system/objects/AcsSubsystem.cpp @@ -47,6 +47,11 @@ ReturnValue_t AcsSubsystem::initialize() { if (result != returnvalue::OK) { sif::error << "AcsSubsystem: Subscribing for acs::MULTIPLE_RW_INVALID failed" << std::endl; } + result = manager->subscribeToEvent(eventQueue->getId(), + event::getEventId(acs::MEKF_INVALID_MODE_VIOLATION)); + if (result != returnvalue::OK) { + sif::error << "AcsSubsystem: Subscribing for acs::MULTIPLE_RW_INVALID failed" << std::endl; + } return Subsystem::initialize(); } @@ -71,7 +76,8 @@ void AcsSubsystem::handleEventMessages() { } } if (event.getEvent() == acs::SAFE_RATE_RECOVERY || - event.getEvent() == acs::MULTIPLE_RW_INVALID) { + event.getEvent() == acs::MULTIPLE_RW_INVALID || + event.getEvent() == acs::MEKF_INVALID_MODE_VIOLATION) { CommandMessage msg; ModeMessage::setCmdModeMessage(msg, acs::AcsMode::SAFE, 0); status = commandQueue->sendMessage(commandQueue->getId(), &msg);