diff --git a/CHANGELOG.md b/CHANGELOG.md index 9ccce344..46c8c554 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -20,6 +20,23 @@ change warranting a new major release: ## Changed - Readded calibration matrices for MGM calibration. +- Added calculation of satellite velocity vector from GPS position data +- Added detumble mode using GYR values +- Added inertial pointing mode +- Added nadir pointing mode +- Added ground station target mode +- Added antistiction for RWs +- Added `sunTargetSafe` differentiation for LEOP +- Added check for existance of `SD_0_SKEWED_PTG_FILE` and `SD_1_SKEWED_PTG_FILE` to determine + which `sunTargetSafe` to use +- Added `gpsVelocity` and `gpsPosition` to `gpsProcessed` +- Removed deprecated `OutputValues` +- Added `HasParametersIF` to `AcsParameters` +- Added `ReceivesParameterMessagesIF` and `ParameterHelper` to `AcsController` +- Updated `AcsParameters` with actual values and changed structure +- Sun vector model and magnetic field vector model calculations are always executed now +- `domainId` is now used as identifier for parameter structs +- Changed onboard GYR value handling from deg/s to rad/s ## Fixed @@ -32,6 +49,15 @@ change warranting a new major release: - Bugfixes in 'SensorProcessing' where previously MGM values would be calibrated before being transformed in body RF. However, the calibration values are in the body RF. Also fixed the validity flag of 'mgmVecTotDerivative'. +- Fixed calculation of model sun vector +- Fixed calculation of model magnetic field vector +- Fixed MEKF algorithm +- Fixed several variable initializations +- Fixed several variable types +- Fixed use of `sunMagAngleMin` for safe mode +- Fixed MEKF not using correct `sampleTime` +- Fixed assignment of `SUS0` and `SUS6` calibration matrices due to wiring being mixed up +- Various smaller bugfixes # [v1.25.0] 2023-02-06 diff --git a/linux/devices/GpsHyperionLinuxController.cpp b/linux/devices/GpsHyperionLinuxController.cpp index ec9cb61a..cb0523d9 100644 --- a/linux/devices/GpsHyperionLinuxController.cpp +++ b/linux/devices/GpsHyperionLinuxController.cpp @@ -157,7 +157,7 @@ bool GpsHyperionLinuxController::readGpsDataFromGpsd() { } }; // GPS is off, no point in reading data from GPSD. - if(mode == MODE_OFF) { + if (mode == MODE_OFF) { return false; } if (readMode == ReadModes::SOCKET) { diff --git a/mission/acsDefs.h b/mission/acsDefs.h index 170305bb..ac093f6f 100644 --- a/mission/acsDefs.h +++ b/mission/acsDefs.h @@ -11,11 +11,11 @@ enum AcsMode { OFF = HasModesIF::MODE_OFF, SAFE = 10, DETUMBLE = 11, - IDLE = 12, - PTG_TARGET_NADIR = 13, + PTG_IDLE = 12, + PTG_NADIR = 13, PTG_TARGET = 14, PTG_TARGET_GS = 15, - PTG_TARGET_INERTIAL = 16, + PTG_INERTIAL = 16, }; static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM; diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 14069b0a..b590786b 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -15,6 +15,7 @@ AcsController::AcsController(object_id_t objectId) detumble(&acsParameters), ptgCtrl(&acsParameters), detumbleCounter{0}, + parameterHelper(this), mgmDataRaw(this), mgmDataProcessed(this), susDataRaw(this), @@ -27,7 +28,25 @@ AcsController::AcsController(object_id_t objectId) actuatorCmdData(this) {} ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) { - return returnvalue::OK; + ReturnValue_t result = actionHelper.handleActionMessage(message); + if (result == returnvalue::OK) { + return result; + } + result = parameterHelper.handleParameterMessage(message); + if (result == returnvalue::OK) { + return result; + } + return result; +} + +MessageQueueId_t AcsController::getCommandQueue() const { return commandQueue->getId(); } + +ReturnValue_t AcsController::getParameter(uint8_t domainId, uint8_t parameterId, + ParameterWrapper *parameterWrapper, + const ParameterWrapper *newValues, + uint16_t startAtIndex) { + return acsParameters.getParameter(domainId, parameterId, parameterWrapper, newValues, + startAtIndex); } void AcsController::performControlOperation() { @@ -52,9 +71,11 @@ void AcsController::performControlOperation() { case acs::DETUMBLE: performDetumble(); break; + case acs::PTG_IDLE: case acs::PTG_TARGET: - case acs::PTG_TARGET_NADIR: - case acs::PTG_TARGET_INERTIAL: + case acs::PTG_TARGET_GS: + case acs::PTG_NADIR: + case acs::PTG_INERTIAL: performPointingCtrl(); break; } @@ -86,10 +107,6 @@ void AcsController::performControlOperation() { } void AcsController::performSafe() { - // Concept: SAFE MODE WITH MEKF - // -do the sensor processing, maybe is does make more sense do call this class function in - // another place since we have to do it for every mode regardless of safe or not - ACS::SensorValues sensorValues; timeval now; @@ -128,10 +145,10 @@ void AcsController::performSafe() { { PoolReadGuard pg(&ctrlValData); if (pg.getReadResult() == returnvalue::OK) { - double zeroQuat[4] = {0, 0, 0, 0}; - std::memcpy(ctrlValData.tgtQuat.value, zeroQuat, 4 * sizeof(double)); + 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, zeroQuat, 4 * sizeof(double)); + std::memcpy(ctrlValData.errQuat.value, unitQuat, 4 * sizeof(double)); ctrlValData.errQuat.setValid(false); ctrlValData.errAng.value = errAng; ctrlValData.errAng.setValid(true); @@ -174,7 +191,8 @@ void AcsController::performSafe() { // PoolReadGuard pg(&dipoleSet); // MutexGuard mg(torquer::lazyLock()); // torquer::NEW_ACTUATION_FLAG = true; - // dipoleSet.setDipoles(cmdDipolUnits[0], cmdDipolUnits[1], cmdDipolUnits[2], torqueDuration); + // dipoleSet.setDipoles(cmdDipolUnits[0], cmdDipolUnits[1], cmdDipolUnits[2], + // torqueDuration); // } } @@ -252,29 +270,146 @@ void AcsController::performPointingCtrl() { &mekfData, &validMekf); double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0}; - guidance.targetQuatPtg(&sensorValues, &mekfData, &susDataProcessed, now, targetQuat, refSatRate); + double quatRef[4] = {0, 0, 0, 0}; + uint8_t enableAntiStiction = true; + double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0}, deltaRate[3] = {0, 0, 0}; // ToDo: check if pointer needed - guidance.comparePtg(targetQuat, &mekfData, refSatRate, quatErrorComplete, quatError, deltaRate); double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); - double torquePtgRws[4] = {0, 0, 0, 0}, mode = 0; - ptgCtrl.ptgGroundstation(mode, quatError, deltaRate, *rwPseudoInv, torquePtgRws); - double rwTrqNs[4] = {0, 0, 0, 0}; - ptgCtrl.ptgNullspace( - &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), - &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); - double cmdSpeedRws[4] = {0, 0, 0, 0}; // Should be given to the actuator reaction wheel as input - actuatorCmd.cmdSpeedToRws( - &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), - &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), torquePtgRws, - rwTrqNs, cmdSpeedRws); + double torquePtgRws[4] = {0, 0, 0, 0}, rwTrqNs[4] = {0, 0, 0, 0}; + double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0}; double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0}; // Desaturation Dipol - ptgCtrl.ptgDesaturation(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); + + 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); + ptgCtrl.ptgNullspace( + &acsParameters.targetModeControllerParameters, &(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, + mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, + &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), + &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); + + 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, + *rwPseudoInv, torquePtgRws); + ptgCtrl.ptgNullspace( + &acsParameters.targetModeControllerParameters, &(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, + mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, + &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), + &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); + 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, + *rwPseudoInv, torquePtgRws); + ptgCtrl.ptgNullspace( + &acsParameters.targetModeControllerParameters, &(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, + mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, + &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), + &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); + 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, + *rwPseudoInv, torquePtgRws); + ptgCtrl.ptgNullspace( + &acsParameters.nadirModeControllerParameters, &(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.nadirModeControllerParameters, 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); + break; + + case acs::PTG_INERTIAL: + guidance.inertialQuatPtg(targetQuat, refSatRate); + std::memcpy(quatRef, acsParameters.inertialModeControllerParameters.quatRef, + 4 * sizeof(double)); + enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction; + guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, + deltaRate); + ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, quatError, deltaRate, + *rwPseudoInv, torquePtgRws); + ptgCtrl.ptgNullspace( + &acsParameters.inertialModeControllerParameters, &(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.inertialModeControllerParameters, 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); + break; + } + + if (enableAntiStiction) { + bool rwAvailable[4] = {true, true, true, true}; // WHICH INPUT SENSOR SET? + int32_t rwSpeed[4] = { + (sensorValues.rw1Set.currSpeed.value), (sensorValues.rw2Set.currSpeed.value), + (sensorValues.rw3Set.currSpeed.value), (sensorValues.rw4Set.currSpeed.value)}; + ptgCtrl.rwAntistiction(rwAvailable, rwSpeed, torqueRwsScaled); + } + + double cmdSpeedRws[4] = {0, 0, 0, 0}; // Should be given to the actuator reaction wheel as input + actuatorCmd.cmdSpeedToRws(&(sensorValues.rw1Set.currSpeed.value), + &(sensorValues.rw2Set.currSpeed.value), + &(sensorValues.rw3Set.currSpeed.value), + &(sensorValues.rw4Set.currSpeed.value), torqueRwsScaled, cmdSpeedRws); actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits); int16_t cmdDipolUnitsInt[3] = {0, 0, 0}; @@ -371,6 +506,8 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD // GPS Processed localDataPoolMap.emplace(acsctrl::PoolIds::GC_LATITUDE, &gcLatitude); localDataPoolMap.emplace(acsctrl::PoolIds::GD_LONGITUDE, &gdLongitude); + localDataPoolMap.emplace(acsctrl::PoolIds::GPS_POSITION, &gpsPosition); + localDataPoolMap.emplace(acsctrl::PoolIds::GPS_VELOCITY, &gpsVelocity); poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), false, 5.0}); // MEKF localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf); @@ -426,7 +563,7 @@ ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode, return INVALID_SUBMODE; } } else if ((mode == MODE_ON) || (mode == MODE_NORMAL)) { - if ((submode < acs::AcsMode::SAFE) or (submode > acs::AcsMode::PTG_TARGET_INERTIAL)) { + if ((submode < acs::AcsMode::SAFE) or (submode > acs::AcsMode::PTG_INERTIAL)) { return INVALID_SUBMODE; } else { return returnvalue::OK; @@ -488,7 +625,6 @@ void AcsController::copyMgmData() { void AcsController::copySusData() { { PoolReadGuard pg(&sensorValues.susSets[0]); - if (pg.getReadResult() == returnvalue::OK) { std::memcpy(susDataRaw.sus0.value, sensorValues.susSets[0].channels.value, 6 * sizeof(uint16_t)); diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 308700e4..6a18e988 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -3,6 +3,8 @@ #include #include +#include +#include #include "acs/ActuatorCmd.h" #include "acs/Guidance.h" @@ -18,12 +20,17 @@ #include "mission/devices/devicedefinitions/SusDefinitions.h" #include "mission/devices/devicedefinitions/imtqHandlerDefinitions.h" -class AcsController : public ExtendedControllerBase { +class AcsController : public ExtendedControllerBase, public ReceivesParameterMessagesIF { public: static constexpr dur_millis_t INIT_DELAY = 500; AcsController(object_id_t objectId); + MessageQueueId_t getCommandQueue() const; + ReturnValue_t getParameter(uint8_t domainId, uint8_t parameterId, + ParameterWrapper* parameterWrapper, const ParameterWrapper* newValues, + uint16_t startAtIndex) override; + protected: void performSafe(); void performDetumble(); @@ -42,6 +49,8 @@ class AcsController : public ExtendedControllerBase { uint8_t detumbleCounter; + ParameterHelper parameterHelper; + enum class InternalState { STARTUP, INITIAL_DELAY, READY }; InternalState internalState = InternalState::STARTUP; @@ -134,6 +143,8 @@ class AcsController : public ExtendedControllerBase { acsctrl::GpsDataProcessed gpsDataProcessed; PoolEntry gcLatitude = PoolEntry(); PoolEntry gdLongitude = PoolEntry(); + PoolEntry gpsPosition = PoolEntry(3); + PoolEntry gpsVelocity = PoolEntry(3); // MEKF acsctrl::MekfData mekfData; diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index cc9959ab..13642fe9 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -3,542 +3,611 @@ #include #include -AcsParameters::AcsParameters(){}; //(uint8_t parameterModuleId) : - // parameterModuleId(parameterModuleId) {} +AcsParameters::AcsParameters() {} AcsParameters::~AcsParameters() {} -/*ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint16_t parameterId, +ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, ParameterWrapper* parameterWrapper, const ParameterWrapper* newValues, uint16_t startAtIndex) { - if (domainId == parameterModuleId) { - switch (parameterId >> 8) { - case 0x0: // direct members - switch (parameterId & 0xFF) { - default: - return INVALID_IDENTIFIER_ID; - } - break; - case 0x1: // OnBoardParams - switch (parameterId & 0xFF) { - case 0x0: - parameterWrapper->set(onBoardParams.sampleTime); - break; - default: - return INVALID_IDENTIFIER_ID; - } - break; - case 0x2: // InertiaEIVE - switch (parameterId & 0xFF) { - case 0x0: - parameterWrapper->set(inertiaEIVE.inertiaMatrix); - break; - case 0x1: - parameterWrapper->set(inertiaEIVE.inertiaMatrixInverse); - break; - default: - return INVALID_IDENTIFIER_ID; - } - break; - case 0x3: // MgmHandlingParameters - switch (parameterId & 0xFF) { - case 0x0: - parameterWrapper->set(mgmHandlingParameters.mgm0orientationMatrix); - break; - case 0x1: - parameterWrapper->set(mgmHandlingParameters.mgm1orientationMatrix); - break; - case 0x2: - parameterWrapper->set(mgmHandlingParameters.mgm2orientationMatrix); - break; - case 0x3: - parameterWrapper->set(mgmHandlingParameters.mgm3orientationMatrix); - break; - case 0x4: - parameterWrapper->set(mgmHandlingParameters.mgm4orientationMatrix); - break; - case 0x5: - parameterWrapper->set(mgmHandlingParameters.mgm0hardIronOffset); - break; - case 0x6: - parameterWrapper->set(mgmHandlingParameters.mgm1hardIronOffset); - break; - case 0x7: - parameterWrapper->set(mgmHandlingParameters.mgm2hardIronOffset); - break; - case 0x8: - parameterWrapper->set(mgmHandlingParameters.mgm3hardIronOffset); - break; - case 0x9: - parameterWrapper->set(mgmHandlingParameters.mgm4hardIronOffset); - break; - case 0xA: - parameterWrapper->set(mgmHandlingParameters.mgm0softIronInverse); - break; - case 0xB: - parameterWrapper->set(mgmHandlingParameters.mgm1softIronInverse); - break; - case 0xC: - parameterWrapper->set(mgmHandlingParameters.mgm2softIronInverse); - break; - case 0xD: - parameterWrapper->set(mgmHandlingParameters.mgm3softIronInverse); - break; - case 0xE: - parameterWrapper->set(mgmHandlingParameters.mgm4softIronInverse); - break; - default: - return INVALID_IDENTIFIER_ID; - } - break; - case 0x4: // SusHandlingParameters - switch (parameterId & 0xFF) { - case 0x0: - parameterWrapper->set(susHandlingParameters.sus0orientationMatrix); - break; - case 0x1: - parameterWrapper->set(susHandlingParameters.sus1orientationMatrix); - break; - case 0x2: - parameterWrapper->set(susHandlingParameters.sus2orientationMatrix); - break; - case 0x3: - parameterWrapper->set(susHandlingParameters.sus3orientationMatrix); - break; - case 0x4: - parameterWrapper->set(susHandlingParameters.sus4orientationMatrix); - break; - case 0x5: - parameterWrapper->set(susHandlingParameters.sus5orientationMatrix); - break; - case 0x6: - parameterWrapper->set(susHandlingParameters.sus6orientationMatrix); - break; - case 0x7: - parameterWrapper->set(susHandlingParameters.sus7orientationMatrix); - break; - case 0x8: - parameterWrapper->set(susHandlingParameters.sus8orientationMatrix); - break; - case 0x9: - parameterWrapper->set(susHandlingParameters.sus9orientationMatrix); - break; - case 0xA: - parameterWrapper->set(susHandlingParameters.sus10orientationMatrix); - break; - case 0xB: - parameterWrapper->set(susHandlingParameters.sus11orientationMatrix); - break; - case 0xC: - parameterWrapper->set(susHandlingParameters.sus0coeffAlpha); - break; - case 0xD: - parameterWrapper->set(susHandlingParameters.sus0coeffBeta); - break; - case 0xE: - parameterWrapper->set(susHandlingParameters.sus1coeffAlpha); - break; - case 0xF: - parameterWrapper->set(susHandlingParameters.sus1coeffBeta); - break; - case 0x10: - parameterWrapper->set(susHandlingParameters.sus2coeffAlpha); - break; - case 0x11: - parameterWrapper->set(susHandlingParameters.sus2coeffBeta); - break; - case 0x12: - parameterWrapper->set(susHandlingParameters.sus3coeffAlpha); - break; - case 0x13: - parameterWrapper->set(susHandlingParameters.sus3coeffBeta); - break; - case 0x14: - parameterWrapper->set(susHandlingParameters.sus4coeffAlpha); - break; - case 0x15: - parameterWrapper->set(susHandlingParameters.sus4coeffBeta); - break; - case 0x16: - parameterWrapper->set(susHandlingParameters.sus5coeffAlpha); - break; - case 0x17: - parameterWrapper->set(susHandlingParameters.sus5coeffBeta); - break; - case 0x18: - parameterWrapper->set(susHandlingParameters.sus6coeffAlpha); - break; - case 0x19: - parameterWrapper->set(susHandlingParameters.sus6coeffBeta); - break; - case 0x1A: - parameterWrapper->set(susHandlingParameters.sus7coeffAlpha); - break; - case 0x1B: - parameterWrapper->set(susHandlingParameters.sus7coeffBeta); - break; - case 0x1C: - parameterWrapper->set(susHandlingParameters.sus8coeffAlpha); - break; - case 0x1D: - parameterWrapper->set(susHandlingParameters.sus8coeffBeta); - break; - case 0x1E: - parameterWrapper->set(susHandlingParameters.sus9coeffAlpha); - break; - case 0x1F: - parameterWrapper->set(susHandlingParameters.sus9coeffBeta); - break; - case 0x20: - parameterWrapper->set(susHandlingParameters.sus10coeffAlpha); - break; - case 0x21: - parameterWrapper->set(susHandlingParameters.sus10coeffBeta); - break; - case 0x22: - parameterWrapper->set(susHandlingParameters.sus11coeffAlpha); - break; - case 0x23: - parameterWrapper->set(susHandlingParameters.sus11coeffBeta); - break; - case 0x24: - parameterWrapper->set(susHandlingParameters.filterAlpha); - break; - case 0x25: - parameterWrapper->set(susHandlingParameters.sunThresh); - break; - default: - return INVALID_IDENTIFIER_ID; - } - break; - case (0x5): // GyrHandlingParameters - switch (parameterId & 0xFF) { - case 0x0: - parameterWrapper->set(gyrHandlingParameters.gyr0orientationMatrix); - break; - case 0x1: - parameterWrapper->set(gyrHandlingParameters.gyr1orientationMatrix); - break; - case 0x2: - parameterWrapper->set(gyrHandlingParameters.gyr2orientationMatrix); - break; - case 0x3: - parameterWrapper->set(gyrHandlingParameters.gyr3orientationMatrix); - break; - case 0x4: - parameterWrapper->set(gyrHandlingParameters.gyrFusionWeight); - break; - default: - return INVALID_IDENTIFIER_ID; - } - break; - case (0x6): // RwHandlingParameters - switch (parameterId & 0xFF) { - case 0x0: - parameterWrapper->set(rwHandlingParameters.rw0orientationMatrix); - break; - case 0x1: - parameterWrapper->set(rwHandlingParameters.rw1orientationMatrix); - break; - case 0x2: - parameterWrapper->set(rwHandlingParameters.rw2orientationMatrix); - break; - case 0x3: - parameterWrapper->set(rwHandlingParameters.rw3orientationMatrix); - break; - case 0x4: - parameterWrapper->set(rwHandlingParameters.inertiaWheel); - break; - case 0x5: - parameterWrapper->set(rwHandlingParameters.maxTrq); - break; - default: - return INVALID_IDENTIFIER_ID; - } - break; - case (0x7): // RwMatrices - switch (parameterId & 0xFF) { - case 0x0: - parameterWrapper->set(rwMatrices.alignmentMatrix); - break; - case 0x1: - parameterWrapper->set(rwMatrices.pseudoInverse); - break; - case 0x2: - parameterWrapper->set(rwMatrices.without0); - break; - case 0x3: - parameterWrapper->set(rwMatrices.without1); - break; - case 0x4: - parameterWrapper->set(rwMatrices.without2); - break; - case 0x5: - parameterWrapper->set(rwMatrices.without3); - break; - default: - return INVALID_IDENTIFIER_ID; - } - break; - case (0x8): // SafeModeControllerParameters - switch (parameterId & 0xFF) { - case 0x0: - parameterWrapper->set(safeModeControllerParameters.k_rate_mekf); - break; - case 0x1: - parameterWrapper->set(safeModeControllerParameters.k_align_mekf); - break; - case 0x2: - parameterWrapper->set(safeModeControllerParameters.k_rate_no_mekf); - break; - case 0x3: - parameterWrapper->set(safeModeControllerParameters.k_align_no_mekf); - break; - case 0x4: - parameterWrapper->set(safeModeControllerParameters.sunMagAngleMin); - break; - case 0x5: - parameterWrapper->set(safeModeControllerParameters.sunTargetDir); - break; - case 0x6: - parameterWrapper->set(safeModeControllerParameters.satRateRef); - break; - default: - return INVALID_IDENTIFIER_ID; - } - break; - case (0x9): // DetumbleCtrlParameters - switch (parameterId & 0xFF) { - case 0x0: - parameterWrapper->set(detumbleCtrlParameters.gainD); - break; - default: - return INVALID_IDENTIFIER_ID; - } - break; - case (0xA): // PointingModeControllerParameters - switch (parameterId & 0xFF) { - case 0x0: - parameterWrapper->set(targetModeControllerParameters.updtFlag); - break; - case 0x1: - parameterWrapper->set(targetModeControllerParameters.A_rw); - break; - case 0x2: - parameterWrapper->set(targetModeControllerParameters.refDirection); - break; - case 0x3: - parameterWrapper->set(targetModeControllerParameters.refRotRate); - break; - case 0x4: - parameterWrapper->set(targetModeControllerParameters.quatRef); - break; - case 0x5: - parameterWrapper->set(targetModeControllerParameters.avoidBlindStr); - break; - case 0x6: - parameterWrapper->set(targetModeControllerParameters.blindAvoidStart); - break; - case 0x7: - parameterWrapper->set(targetModeControllerParameters.blindAvoidStop); - break; - case 0x8: - parameterWrapper->set(targetModeControllerParameters.blindRotRate); - break; - case 0x9: - parameterWrapper->set(targetModeControllerParameters.zeta); - break; - case 0xA: - parameterWrapper->set(targetModeControllerParameters.zetaLow); - break; - case 0xB: - parameterWrapper->set(targetModeControllerParameters.om); - break; - case 0xC: - parameterWrapper->set(targetModeControllerParameters.omLow); - break; - case 0xD: - parameterWrapper->set(targetModeControllerParameters.omMax); - break; - case 0xE: - parameterWrapper->set(targetModeControllerParameters.qiMin); - break; - case 0xF: - parameterWrapper->set(targetModeControllerParameters.gainNullspace); - break; - case 0x10: - parameterWrapper->set(targetModeControllerParameters.desatMomentumRef); - break; - case 0x11: - parameterWrapper->set(targetModeControllerParameters.deSatGainFactor); - break; - case 0x12: - parameterWrapper->set(targetModeControllerParameters.desatOn); - break; - case 0x13: - parameterWrapper->set(targetModeControllerParameters.omegaEarth); - break; - default: - return INVALID_IDENTIFIER_ID; - } - break; - case (0xB): // StrParameters - switch (parameterId & 0xFF) { - case 0x0: - parameterWrapper->set(strParameters.exclusionAngle); - break; - case 0x1: - parameterWrapper->set(strParameters.boresightAxis); - break; - default: - return INVALID_IDENTIFIER_ID; - } - break; - case (0xC): // GpsParameters - switch (parameterId & 0xFF) { - default: - return INVALID_IDENTIFIER_ID; - } - break; - case (0xD): // GroundStationParameters - switch (parameterId & 0xFF) { - case 0x0: - parameterWrapper->set(groundStationParameters.latitudeGs); - break; - case 0x1: - parameterWrapper->set(groundStationParameters.longitudeGs); - break; - case 0x2: - parameterWrapper->set(groundStationParameters.altitudeGs); - break; - case 0x3: - parameterWrapper->set(groundStationParameters.earthRadiusEquat); - break; - case 0x4: - parameterWrapper->set(groundStationParameters.earthRadiusPolar); - break; - default: - return INVALID_IDENTIFIER_ID; - } - break; - case (0xE): // SunModelParameters - switch (parameterId & 0xFF) { - case 0x0: - parameterWrapper->set(sunModelParameters.useSunModel); - break; - case 0x1: - parameterWrapper->set(sunModelParameters.domega); - break; - case 0x2: - parameterWrapper->set(sunModelParameters.omega_0); - break; - case 0x3: - parameterWrapper->set(sunModelParameters.m_0); - break; - case 0x4: - parameterWrapper->set(sunModelParameters.dm); - break; - case 0x5: - parameterWrapper->set(sunModelParameters.e); - break; - case 0x6: - parameterWrapper->set(sunModelParameters.e1); - break; - case 0x7: - parameterWrapper->set(sunModelParameters.p1); - break; - case 0x8: - parameterWrapper->set(sunModelParameters.p2); - break; - default: - return INVALID_IDENTIFIER_ID; - } - break; - case (0xF): // KalmanFilterParameters - switch (parameterId & 0xFF) { - case 0x0: - parameterWrapper->set(kalmanFilterParameters.activateKalmanFilter); - break; - case 0x1: - parameterWrapper->set(kalmanFilterParameters.requestResetFlag); - break; - case 0x2: - parameterWrapper->set( - kalmanFilterParameters.maxToleratedTimeBetweenKalmanFilterExecutionSteps); - break; - case 0x3: - parameterWrapper->set(kalmanFilterParameters.processNoiseOmega); - break; - case 0x4: - parameterWrapper->set(kalmanFilterParameters.processNoiseQuaternion); - break; - case 0x5: - parameterWrapper->set(kalmanFilterParameters.sensorNoiseSTR); - break; - case 0x6: - parameterWrapper->set(kalmanFilterParameters.sensorNoiseSS); - break; - case 0x7: - parameterWrapper->set(kalmanFilterParameters.sensorNoiseMAG); - break; - case 0x8: - parameterWrapper->set(kalmanFilterParameters.sensorNoiseGYR); - break; - case 0x9: - parameterWrapper->set(kalmanFilterParameters.sensorNoiseArwGYR); - break; - case 0xA: - parameterWrapper->set(kalmanFilterParameters.sensorNoiseBsGYR); - break; - default: - return INVALID_IDENTIFIER_ID; - } - break; - case (0x10): // MagnetorquesParameter - switch (parameterId & 0xFF) { - case 0x0: - parameterWrapper->set(magnetorquesParameter.mtq0orientationMatrix); - break; - case 0x1: - parameterWrapper->set(magnetorquesParameter.mtq1orientationMatrix); - break; - case 0x2: - parameterWrapper->set(magnetorquesParameter.mtq2orientationMatrix); - break; - case 0x3: - parameterWrapper->set(magnetorquesParameter.alignmentMatrixMtq); - break; - case 0x4: - parameterWrapper->set(magnetorquesParameter.inverseAlignment); - break; - case 0x5: - parameterWrapper->set(magnetorquesParameter.DipolMax); - break; - default: - return INVALID_IDENTIFIER_ID; - } - break; - case (0x11): // DetumbleParameter - switch (parameterId & 0xFF) { - case 0x0: - parameterWrapper->set(detumbleParameter.detumblecounter); - break; - case 0x1: - parameterWrapper->set(detumbleParameter.omegaDetumbleStart); - break; - case 0x2: - parameterWrapper->set(detumbleParameter.omegaDetumbleEnd); - break; - default: - return INVALID_IDENTIFIER_ID; - } - break; - default: - return INVALID_IDENTIFIER_ID; - } - return returnvalue::OK; - } else { - return INVALID_DOMAIN_ID; + switch (domainId) { + case 0x0: // direct members + switch (parameterId) { + default: + return INVALID_IDENTIFIER_ID; + } + break; + case 0x1: // OnBoardParams + switch (parameterId) { + case 0x0: + parameterWrapper->set(onBoardParams.sampleTime); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case 0x2: // InertiaEIVE + switch (parameterId) { + case 0x0: + parameterWrapper->set(inertiaEIVE.inertiaMatrix); + break; + case 0x1: + parameterWrapper->set(inertiaEIVE.inertiaMatrixDeployed); + break; + case 0x2: + parameterWrapper->set(inertiaEIVE.inertiaMatrixUndeployed); + break; + case 0x3: + parameterWrapper->set(inertiaEIVE.inertiaMatrixPanel1); + break; + case 0x4: + parameterWrapper->set(inertiaEIVE.inertiaMatrixPanel3); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case 0x3: // MgmHandlingParameters + switch (parameterId) { + case 0x0: + parameterWrapper->set(mgmHandlingParameters.mgm0orientationMatrix); + break; + case 0x1: + parameterWrapper->set(mgmHandlingParameters.mgm1orientationMatrix); + break; + case 0x2: + parameterWrapper->set(mgmHandlingParameters.mgm2orientationMatrix); + break; + case 0x3: + parameterWrapper->set(mgmHandlingParameters.mgm3orientationMatrix); + break; + case 0x4: + parameterWrapper->set(mgmHandlingParameters.mgm4orientationMatrix); + break; + case 0x5: + parameterWrapper->set(mgmHandlingParameters.mgm0hardIronOffset); + break; + case 0x6: + parameterWrapper->set(mgmHandlingParameters.mgm1hardIronOffset); + break; + case 0x7: + parameterWrapper->set(mgmHandlingParameters.mgm2hardIronOffset); + break; + case 0x8: + parameterWrapper->set(mgmHandlingParameters.mgm3hardIronOffset); + break; + case 0x9: + parameterWrapper->set(mgmHandlingParameters.mgm4hardIronOffset); + break; + case 0xA: + parameterWrapper->set(mgmHandlingParameters.mgm0softIronInverse); + break; + case 0xB: + parameterWrapper->set(mgmHandlingParameters.mgm1softIronInverse); + break; + case 0xC: + parameterWrapper->set(mgmHandlingParameters.mgm2softIronInverse); + break; + case 0xD: + parameterWrapper->set(mgmHandlingParameters.mgm3softIronInverse); + break; + case 0xE: + parameterWrapper->set(mgmHandlingParameters.mgm4softIronInverse); + break; + case 0xF: + parameterWrapper->set(mgmHandlingParameters.mgm02variance); + break; + case 0x10: + parameterWrapper->set(mgmHandlingParameters.mgm13variance); + break; + case 0x11: + parameterWrapper->set(mgmHandlingParameters.mgm4variance); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case 0x4: // SusHandlingParameters + switch (parameterId) { + case 0x0: + parameterWrapper->set(susHandlingParameters.sus0orientationMatrix); + break; + case 0x1: + parameterWrapper->set(susHandlingParameters.sus1orientationMatrix); + break; + case 0x2: + parameterWrapper->set(susHandlingParameters.sus2orientationMatrix); + break; + case 0x3: + parameterWrapper->set(susHandlingParameters.sus3orientationMatrix); + break; + case 0x4: + parameterWrapper->set(susHandlingParameters.sus4orientationMatrix); + break; + case 0x5: + parameterWrapper->set(susHandlingParameters.sus5orientationMatrix); + break; + case 0x6: + parameterWrapper->set(susHandlingParameters.sus6orientationMatrix); + break; + case 0x7: + parameterWrapper->set(susHandlingParameters.sus7orientationMatrix); + break; + case 0x8: + parameterWrapper->set(susHandlingParameters.sus8orientationMatrix); + break; + case 0x9: + parameterWrapper->set(susHandlingParameters.sus9orientationMatrix); + break; + case 0xA: + parameterWrapper->set(susHandlingParameters.sus10orientationMatrix); + break; + case 0xB: + parameterWrapper->set(susHandlingParameters.sus11orientationMatrix); + break; + case 0xC: + parameterWrapper->set(susHandlingParameters.sus0coeffAlpha); + break; + case 0xD: + parameterWrapper->set(susHandlingParameters.sus0coeffBeta); + break; + case 0xE: + parameterWrapper->set(susHandlingParameters.sus1coeffAlpha); + break; + case 0xF: + parameterWrapper->set(susHandlingParameters.sus1coeffBeta); + break; + case 0x10: + parameterWrapper->set(susHandlingParameters.sus2coeffAlpha); + break; + case 0x11: + parameterWrapper->set(susHandlingParameters.sus2coeffBeta); + break; + case 0x12: + parameterWrapper->set(susHandlingParameters.sus3coeffAlpha); + break; + case 0x13: + parameterWrapper->set(susHandlingParameters.sus3coeffBeta); + break; + case 0x14: + parameterWrapper->set(susHandlingParameters.sus4coeffAlpha); + break; + case 0x15: + parameterWrapper->set(susHandlingParameters.sus4coeffBeta); + break; + case 0x16: + parameterWrapper->set(susHandlingParameters.sus5coeffAlpha); + break; + case 0x17: + parameterWrapper->set(susHandlingParameters.sus5coeffBeta); + break; + case 0x18: + parameterWrapper->set(susHandlingParameters.sus6coeffAlpha); + break; + case 0x19: + parameterWrapper->set(susHandlingParameters.sus6coeffBeta); + break; + case 0x1A: + parameterWrapper->set(susHandlingParameters.sus7coeffAlpha); + break; + case 0x1B: + parameterWrapper->set(susHandlingParameters.sus7coeffBeta); + break; + case 0x1C: + parameterWrapper->set(susHandlingParameters.sus8coeffAlpha); + break; + case 0x1D: + parameterWrapper->set(susHandlingParameters.sus8coeffBeta); + break; + case 0x1E: + parameterWrapper->set(susHandlingParameters.sus9coeffAlpha); + break; + case 0x1F: + parameterWrapper->set(susHandlingParameters.sus9coeffBeta); + break; + case 0x20: + parameterWrapper->set(susHandlingParameters.sus10coeffAlpha); + break; + case 0x21: + parameterWrapper->set(susHandlingParameters.sus10coeffBeta); + break; + case 0x22: + parameterWrapper->set(susHandlingParameters.sus11coeffAlpha); + break; + case 0x23: + parameterWrapper->set(susHandlingParameters.sus11coeffBeta); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0x5): // GyrHandlingParameters + switch (parameterId) { + case 0x0: + parameterWrapper->set(gyrHandlingParameters.gyr0orientationMatrix); + break; + case 0x1: + parameterWrapper->set(gyrHandlingParameters.gyr1orientationMatrix); + break; + case 0x2: + parameterWrapper->set(gyrHandlingParameters.gyr2orientationMatrix); + break; + case 0x3: + parameterWrapper->set(gyrHandlingParameters.gyr3orientationMatrix); + break; + case 0x4: + parameterWrapper->set(gyrHandlingParameters.gyr0bias); + break; + case 0x5: + parameterWrapper->set(gyrHandlingParameters.gyr1bias); + break; + case 0x6: + parameterWrapper->set(gyrHandlingParameters.gyr2bias); + break; + case 0x7: + parameterWrapper->set(gyrHandlingParameters.gyr3bias); + break; + case 0x8: + parameterWrapper->set(gyrHandlingParameters.gyr02variance); + break; + case 0x9: + parameterWrapper->set(gyrHandlingParameters.gyr13variance); + break; + case 0xA: + parameterWrapper->set(gyrHandlingParameters.preferAdis); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0x6): // RwHandlingParameters + switch (parameterId) { + case 0x0: + parameterWrapper->set(rwHandlingParameters.inertiaWheel); + break; + case 0x1: + parameterWrapper->set(rwHandlingParameters.maxTrq); + break; + case 0x2: + parameterWrapper->set(rwHandlingParameters.stictionSpeed); + break; + case 0x3: + parameterWrapper->set(rwHandlingParameters.stictionReleaseSpeed); + break; + case 0x4: + parameterWrapper->set(rwHandlingParameters.stictionTorque); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0x7): // RwMatrices + switch (parameterId) { + case 0x0: + parameterWrapper->set(rwMatrices.alignmentMatrix); + break; + case 0x1: + parameterWrapper->set(rwMatrices.pseudoInverse); + break; + case 0x2: + parameterWrapper->set(rwMatrices.without0); + break; + case 0x3: + parameterWrapper->set(rwMatrices.without1); + break; + case 0x4: + parameterWrapper->set(rwMatrices.without2); + break; + case 0x5: + parameterWrapper->set(rwMatrices.without3); + break; + case 0x6: + parameterWrapper->set(rwMatrices.nullspace); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0x8): // SafeModeControllerParameters + switch (parameterId) { + case 0x0: + parameterWrapper->set(safeModeControllerParameters.k_rate_mekf); + break; + case 0x1: + parameterWrapper->set(safeModeControllerParameters.k_align_mekf); + break; + case 0x2: + parameterWrapper->set(safeModeControllerParameters.k_rate_no_mekf); + break; + case 0x3: + parameterWrapper->set(safeModeControllerParameters.k_align_no_mekf); + break; + case 0x4: + parameterWrapper->set(safeModeControllerParameters.sunMagAngleMin); + break; + case 0x5: + parameterWrapper->set(safeModeControllerParameters.sunTargetDirLeop); + break; + case 0x6: + parameterWrapper->set(safeModeControllerParameters.sunTargetDir); + break; + case 0x7: + parameterWrapper->set(safeModeControllerParameters.satRateRef); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0x9): // TargetModeControllerParameters + 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; + case 0x10: + parameterWrapper->set(targetModeControllerParameters.avoidBlindStr); + break; + case 0x11: + parameterWrapper->set(targetModeControllerParameters.blindAvoidStart); + break; + case 0x12: + parameterWrapper->set(targetModeControllerParameters.blindAvoidStop); + break; + case 0x13: + parameterWrapper->set(targetModeControllerParameters.blindRotRate); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0xA): // NadirModeControllerParameters + switch (parameterId) { + case 0x0: + parameterWrapper->set(nadirModeControllerParameters.zeta); + break; + case 0x1: + parameterWrapper->set(nadirModeControllerParameters.om); + break; + case 0x2: + parameterWrapper->set(nadirModeControllerParameters.omMax); + break; + case 0x3: + parameterWrapper->set(nadirModeControllerParameters.qiMin); + break; + case 0x4: + parameterWrapper->set(nadirModeControllerParameters.gainNullspace); + break; + case 0x5: + parameterWrapper->set(nadirModeControllerParameters.desatMomentumRef); + break; + case 0x6: + parameterWrapper->set(nadirModeControllerParameters.deSatGainFactor); + break; + case 0x7: + parameterWrapper->set(nadirModeControllerParameters.desatOn); + break; + case 0x8: + parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction); + break; + case 0x9: + parameterWrapper->set(nadirModeControllerParameters.refDirection); + break; + case 0xA: + parameterWrapper->set(nadirModeControllerParameters.quatRef); + break; + case 0xC: + parameterWrapper->set(nadirModeControllerParameters.timeElapsedMax); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0xB): // InertialModeControllerParameters + switch (parameterId) { + case 0x0: + parameterWrapper->set(inertialModeControllerParameters.zeta); + break; + case 0x1: + parameterWrapper->set(inertialModeControllerParameters.om); + break; + case 0x2: + parameterWrapper->set(inertialModeControllerParameters.omMax); + break; + case 0x3: + parameterWrapper->set(inertialModeControllerParameters.qiMin); + break; + case 0x4: + parameterWrapper->set(inertialModeControllerParameters.gainNullspace); + break; + case 0x5: + parameterWrapper->set(inertialModeControllerParameters.desatMomentumRef); + break; + case 0x6: + parameterWrapper->set(inertialModeControllerParameters.deSatGainFactor); + break; + case 0x7: + parameterWrapper->set(inertialModeControllerParameters.desatOn); + break; + case 0x8: + parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction); + break; + case 0x9: + parameterWrapper->set(inertialModeControllerParameters.tgtQuat); + break; + case 0xA: + parameterWrapper->set(inertialModeControllerParameters.refRotRate); + break; + case 0xC: + parameterWrapper->set(inertialModeControllerParameters.quatRef); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0xC): // StrParameters + switch (parameterId) { + case 0x0: + parameterWrapper->set(strParameters.exclusionAngle); + break; + case 0x1: + parameterWrapper->set(strParameters.boresightAxis); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0xD): // GpsParameters + switch (parameterId) { + case 0x0: + parameterWrapper->set(gpsParameters.timeDiffVelocityMax); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0xE): // SunModelParameters + switch (parameterId) { + case 0x0: + parameterWrapper->set(sunModelParameters.domega); + break; + case 0x1: + parameterWrapper->set(sunModelParameters.omega_0); + break; + case 0x2: + parameterWrapper->set(sunModelParameters.m_0); + break; + case 0x3: + parameterWrapper->set(sunModelParameters.dm); + break; + case 0x4: + parameterWrapper->set(sunModelParameters.e); + break; + case 0x5: + parameterWrapper->set(sunModelParameters.e1); + break; + case 0x6: + parameterWrapper->set(sunModelParameters.p1); + break; + case 0x7: + parameterWrapper->set(sunModelParameters.p2); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0xF): // KalmanFilterParameters + switch (parameterId) { + case 0x0: + parameterWrapper->set(kalmanFilterParameters.sensorNoiseSTR); + break; + case 0x1: + parameterWrapper->set(kalmanFilterParameters.sensorNoiseSS); + break; + case 0x2: + parameterWrapper->set(kalmanFilterParameters.sensorNoiseMAG); + break; + case 0x3: + parameterWrapper->set(kalmanFilterParameters.sensorNoiseGYR); + break; + case 0x4: + parameterWrapper->set(kalmanFilterParameters.sensorNoiseArwGYR); + break; + case 0x5: + parameterWrapper->set(kalmanFilterParameters.sensorNoiseBsGYR); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0x10): // MagnetorquesParameter + switch (parameterId) { + case 0x0: + parameterWrapper->set(magnetorquesParameter.mtq0orientationMatrix); + break; + case 0x1: + parameterWrapper->set(magnetorquesParameter.mtq1orientationMatrix); + break; + case 0x2: + parameterWrapper->set(magnetorquesParameter.mtq2orientationMatrix); + break; + case 0x3: + parameterWrapper->set(magnetorquesParameter.alignmentMatrixMtq); + break; + case 0x4: + parameterWrapper->set(magnetorquesParameter.inverseAlignment); + break; + case 0x5: + parameterWrapper->set(magnetorquesParameter.DipolMax); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0x11): // DetumbleParameter + switch (parameterId) { + case 0x0: + parameterWrapper->set(detumbleParameter.detumblecounter); + break; + case 0x1: + parameterWrapper->set(detumbleParameter.omegaDetumbleStart); + break; + case 0x2: + parameterWrapper->set(detumbleParameter.omegaDetumbleEnd); + break; + case 0x3: + parameterWrapper->set(detumbleParameter.gainD); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + default: + return INVALID_DOMAIN_ID; } -}*/ + return returnvalue::OK; +} diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index c4653590..f1c0fb63 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -12,22 +12,36 @@ typedef unsigned char uint8_t; -class AcsParameters /*: public HasParametersIF*/ { +class AcsParameters : public HasParametersIF { public: AcsParameters(); virtual ~AcsParameters(); - /* - virtual ReturnValue_t getParameter(uint8_t domainId, uint16_t parameterId, - ParameterWrapper *parameterWrapper, - const ParameterWrapper *newValues, uint16_t startAtIndex); - */ + + ReturnValue_t getParameter(uint8_t domainId, uint8_t parameterId, + ParameterWrapper *parameterWrapper, const ParameterWrapper *newValues, + uint16_t startAtIndex) override; + struct OnBoardParams { - double sampleTime = 0.1; // [s] + double sampleTime = 0.4; // [s] } onBoardParams; struct InertiaEIVE { - double inertiaMatrix[3][3] = {{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.5, 1.0}}; - double inertiaMatrixInverse[3][3]; + double inertiaMatrix[3][3] = {{0.1539829, -0.0001821456, -0.0050135}, + {-0.0001821456, 0.1701302, 0.0004748963}, + {-0.0050135, 0.0004748963, 0.08374296}}; // 19.11.2021 + // Possible inertia matrices + double inertiaMatrixDeployed[3][3] = {{0.1539829, -0.0001821456, -0.0050135}, + {-0.0001821456, 0.1701302, 0.0004748963}, + {-0.0050135, 0.0004748963, 0.08374296}}; // 19.11.2021 + double inertiaMatrixUndeployed[3][3] = {{0.122485, -0.0001798426, -0.005008}, + {-0.0001798426, 0.162240, 0.000475596}, + {-0.005008, 0.000475596, 0.060136}}; // 19.11.2021 + double inertiaMatrixPanel1[3][3] = {{0.13823347, -0.0001836122, -0.00501207}, + {-0.0001836122, 0.16619787, 0.0083537}, + {-0.00501207, 0.0083537, 0.07192588}}; // 19.11.2021 + double inertiaMatrixPanel3[3][3] = {{0.13823487, -0.000178376, -0.005009767}, + {-0.000178376, 0.166172, -0.007403}, + {-0.005009767, -0.007403, 0.07195314}}; } inertiaEIVE; struct MgmHandlingParameters { @@ -62,17 +76,16 @@ class AcsParameters /*: public HasParametersIF*/ { float mgm02variance[3] = {1, 1, 1}; float mgm13variance[3] = {1, 1, 1}; float mgm4variance[3] = {1, 1, 1}; - } mgmHandlingParameters; struct SusHandlingParameters { - float sus0orientationMatrix[3][3] = {{0, 0, 1}, {1, 0, 0}, {0, 1, 0}}; // FM07 + float sus0orientationMatrix[3][3] = {{0, 0, 1}, {1, 0, 0}, {0, 1, 0}}; // FM10 float sus1orientationMatrix[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}}; // FM06 float sus2orientationMatrix[3][3] = {{-1, 0, 0}, {0, 0, -1}, {0, -1, 0}}; // FM13 float sus3orientationMatrix[3][3] = {{1, 0, 0}, {0, 0, 1}, {0, -1, 0}}; // FM14 float sus4orientationMatrix[3][3] = {{0, -1, 0}, {1, 0, 0}, {0, 0, 1}}; // FM05 float sus5orientationMatrix[3][3] = {{1, 0, 0}, {0, -1, 0}, {0, 0, -1}}; // FM02 - float sus6orientationMatrix[3][3] = {{0, 0, 1}, {1, 0, 0}, {0, 1, 0}}; // FM10 + float sus6orientationMatrix[3][3] = {{0, 0, 1}, {1, 0, 0}, {0, 1, 0}}; // FM07 float sus7orientationMatrix[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}}; // FM01 float sus8orientationMatrix[3][3] = {{-1, 0, 0}, {0, 0, -1}, {0, -1, 0}}; // FM03 float sus9orientationMatrix[3][3] = {{1, 0, 0}, {0, 0, 1}, {0, -1, 0}}; // FM11 @@ -80,61 +93,61 @@ class AcsParameters /*: public HasParametersIF*/ { float sus11orientationMatrix[3][3] = {{1, 0, 0}, {0, -1, 0}, {0, 0, -1}}; // FM08 float sus0coeffAlpha[9][10] = { - {10.4400948050067, 1.38202655603079, 0.975299591736672, 0.0172133914423707, - -0.0163482459492803, 0.035730152619911, 0.00021725657060767, -0.000181685375645396, - -0.000124096561459262, 0.00040790566176981}, - {6.38281281805793, 1.81388255990089, 0.28679524291736, 0.0218036823758417, - 0.010516766426651, 0.000446101708841615, 0.00020187044149361, 0.000114957457831415, - 1.63114413539632e-05, -2.0187452317724e-05}, - {-29.3049094555, -0.506844002611835, 1.64911970541112, -0.0336282997119334, - 0.053185806861685, -0.028164943139695, -0.00021098074590512, 0.000643681643489995, - -0.000249094601806692, 0.000231466668650876}, - {-4.76233790255328, 1.1780710601961, -0.194257188545164, 0.00471817228628384, - -0.00183773644319332, -0.00570261621182479, -7.99203367291902e-05, 7.75752247926601e-05, - -9.78534772816957e-06, -4.72083745991256e-05}, - {0.692159025649028, 1.11895461388667, 0.341706834956496, 0.000237989648019541, - -0.000188322779563912, 0.000227310789253953, 0.000133001646828401, -0.000305810826248463, - 0.00010150571088124, -0.000367705461590854}, - {3.38094203317731, 1.24778838596815, 0.067807236112956, -0.00379395536123526, - -0.00339180589343601, -0.00188754615986649, -7.52406312245606e-05, 4.58398750278147e-05, - 6.97244631313601e-05, 2.50519145070895e-05}, - {-7.10546287716029, 0.459472977452686, -1.12251049944014, 0.0175406972371191, - -0.0310525406867782, -0.0531315970690727, -0.000121107664597462, 0.000544665437051928, - -1.78466217018177e-05, -0.00058976234038192}, - {1.60633684055984, 1.1975095485662, 0.180159204664965, -0.00259157601062089, - -0.0038106317634397, 0.000956686555225968, 4.28416721502134e-06, 5.84532336259517e-06, - -2.73407888222758e-05, 5.45131881032866e-06}, - {43.3732235586222, 0.528096786861784, -3.41255850703983, -0.0161629934278675, - 0.0790998053536612, 0.0743822668655928, 0.000237176965460634, -0.000426691336904078, - -0.000889196131314391, -0.000509766491897672}}; + {13.0465222152293, 0.0639132159808454, 2.98083557560227, -0.0773202212713293, + 0.0949075412003712, 0.0503055998355815, -0.00104133434256204, 0.000633099036136146, + 0.00091428505258307, 0.000259857066722932}, + {1.66740227859888, 1.55804368674744, 0.209274741749388, 0.0123798418560859, + 0.00724950517167516, -0.000577445375457582, 8.94374551545955e-05, 6.94513586221567e-05, + -1.06065583714065e-05, -1.43899892666699e-05}, + {8.71610925597519, 1.42112818752419, -0.549859300501301, 0.0374581774684577, + 0.0617635595955198, 0.0447491072679598, 0.00069998577106559, 0.00101018723225412, + -4.88501228194031e-06, -0.000434861113274231}, + {-2.3555601314395, 1.29430213886389, 0.179499593411187, 0.00440896450927253, + 0.00352052300927628, 0.00434187143967281, -9.66615195654703e-05, 3.64923075694275e-05, + 6.09619017310129e-05, 4.23908862836885e-05}, + {-0.858019663974047, 1.10138705956076, 0.278789852526915, -0.000199798507752607, + 0.00112092406838628, -0.00177346866231588, 0.000217816070307086, -0.000240713988238257, + 0.000150795563555828, -0.000279246491927943}, + {7.93661480471297, 1.33902098855997, -0.64010306493848, -0.00307944184518557, + -0.00511421127083497, 0.0204008636376403, -9.50042323904954e-05, 6.01530207062221e-05, + 9.13233708460098e-05, -0.000206717750924323}, + {16.2658124154565, 0.191301571705827, 1.02390350838635, 0.0258487436355216, + -0.0219752092833362, 0.0236916776412211, -0.000350496453661261, -0.000123849795280597, + -0.000532190902882765, 9.36018171121253e-05}, + {-1.53023612303052, 1.29132951637076, 0.181159073530008, -0.0023490608317645, + -0.00370741703297037, -0.000229071300377431, -1.6634455407558e-05, 1.11387154630828e-05, + 1.02609175615251e-05, -9.64717658954667e-06}, + {-32.9918791079688, 0.093536793089853, 4.76858627395571, 0.0595845684553358, + -0.054845749101257, -0.133247382500001, -0.000688999201915199, 7.67286265747961e-05, + 0.000868163357631254, 0.00120099606910313}}; float sus0coeffBeta[9][10] = { - {1.03872648284911, -0.213507239271552, 1.43193059498181, -0.000972717820830235, - -0.00661046096415371, 0.00974284211491888, 2.96098456891215e-05, -8.2933115634257e-05, - -5.52178824394723e-06, 5.73935295303589e-05}, - {3.42242235823356, 0.0848392511283237, 1.24574390342586, 0.00356248195980133, - 0.00100415659893053, -0.00460120247716139, 3.84891005422427e-05, 2.70236417852327e-05, - -7.58501977656551e-05, -8.79809730730992e-05}, - {14.0092526123741, 1.03126714946215, 1.0611008563785, 0.04076462444523, 0.0114106419194518, - 0.00746959159048058, 0.000388033225774727, -0.000124645014888926, -0.000296639947532341, - -0.00020861690864945}, - {1.3562422681189, -0.241585615891602, 1.49170424068611, 0.000179184170448335, - -0.00712399257616284, 0.0121433526723498, 3.29770580642447e-05, 8.78960210966787e-06, - -6.00508568552101e-05, 0.000101583822589461}, - {-0.718855428908583, -0.344067476078684, 1.12397093701762, 0.000236505431484729, - -0.000406441415248947, 0.00032834991502413, 0.000359422093285086, 8.18895560425272e-05, - 0.000316835483508523, 0.000151442890664899}, - {-0.268764016434841, -0.275272048639511, 1.26239753050527, -0.000511224336925231, - 0.0095628568270856, -0.00397960092451418, 1.39587366293607e-05, 1.31409051361129e-05, - -9.83662017231755e-05, 1.87078667116619e-05}, - {27.168106989145, -2.43346872338192, 1.91135512970771, 0.0553180826818016, - -0.0481878292619383, 0.0052773235604729, -0.000428011927975304, 0.000528018208222772, - -0.000285438191474895, -5.71327627917386e-05}, - {-0.169494136517622, -0.350851545482921, 1.19922076033643, 0.0101120903675328, - -0.00151674465424115, 0.00548694086125656, -0.000108240000970513, 1.57202185024105e-05, - -9.77555098179959e-05, 2.09624089449761e-05}, - {-32.3807957489507, 1.8271436443167, 2.51530814328123, -0.0532334586403461, - -0.0355980127727253, -0.0213373892796204, 0.00045506092539885, 0.000545065581027688, - 0.000141998709314758, 0.000101051304611037}}; + {12.7380220453847, -0.6087309901836, 2.60957722462363, -0.0415319939920917, + 0.0444944768824276, 0.0223231464060241, -0.000421503508733887, -9.39560038638717e-05, + 0.000821479971871302, -4.5330528329465e-05}, + {1.96846333975847, -0.33921438143463, 1.23957110477613, -0.00948832495296823, + 0.00107211134687287, -0.00410820045700199, -9.33679611473279e-05, 3.72984782145427e-05, + -4.04514487800062e-05, -7.6296149087237e-05}, + {5.7454444934481, -1.58476383793609, -0.418479494289251, -0.0985177320630941, + -0.0862179276808015, 0.0126762052037897, -0.00118207758271301, -0.000190361442918412, + 0.0011723869613426, 0.000122882034141316}, + {2.11042287406433, -0.225942746245056, 1.18084080712528, -0.00103013931607172, + -0.00675606790663387, -0.00106646109062746, 1.7708839355979e-05, -3.13642668374253e-05, + -5.87601932564404e-05, -3.92033314627704e-05}, + {2.96049248725882, -0.286261455028255, 1.09122556181319, -0.000672369023155898, + 0.000574446975796023, 0.000120303729680796, 0.000292285799270644, 0.000170497873487264, + 0.000259925974231328, 0.000222437797823852}, + {1.65218061201483, -0.19535446105784, 1.39609640918411, 0.000961524354787167, + 0.00592400381724333, -0.0078500192096718, -7.02791628080906e-07, -2.07197580883822e-05, + -4.33518182614169e-05, 4.66993119419691e-05}, + {-19.56673237415, 1.06558565338761, 0.151160448373445, -0.0252628659378108, + 0.0281230551050938, -0.0217328869907185, 0.000241309440918385, -0.000116449585258429, + 0.000401546410974577, -0.000147563886502726}, + {1.56167171538684, -0.155299366654736, 1.20084049723279, 0.00457348893890231, + 0.00118888040006052, 0.0029920178735941, -5.583448120596e-05, -2.34496315691865e-05, + -5.3309466243918e-05, 6.20289310356821e-06}, + {1.95050549495182, -2.74909818412705, 3.80268788018641, 0.0629242254381785, + 0.0581479035315726, -0.111361283351269, -0.00047845777495158, -0.00075354297736741, + -0.000186887396585446, 0.00119710704771344}}; float sus1coeffAlpha[9][10] = { {-27.6783250420482, -0.964805032861791, -0.503974297997131, -0.0446471081874084, -0.048219538329297, 0.000958491361905381, -0.000290972187162876, -0.000657145721554176, @@ -416,61 +429,61 @@ class AcsParameters /*: public HasParametersIF*/ { -0.0542697403292778, 0.0285360568428252, 0.000845084580479371, 0.00114184315411245, -0.000169538153750085, -0.000336529204350355}}; float sus6coeffAlpha[9][10] = { - {13.0465222152293, 0.0639132159808454, 2.98083557560227, -0.0773202212713293, - 0.0949075412003712, 0.0503055998355815, -0.00104133434256204, 0.000633099036136146, - 0.00091428505258307, 0.000259857066722932}, - {1.66740227859888, 1.55804368674744, 0.209274741749388, 0.0123798418560859, - 0.00724950517167516, -0.000577445375457582, 8.94374551545955e-05, 6.94513586221567e-05, - -1.06065583714065e-05, -1.43899892666699e-05}, - {8.71610925597519, 1.42112818752419, -0.549859300501301, 0.0374581774684577, - 0.0617635595955198, 0.0447491072679598, 0.00069998577106559, 0.00101018723225412, - -4.88501228194031e-06, -0.000434861113274231}, - {-2.3555601314395, 1.29430213886389, 0.179499593411187, 0.00440896450927253, - 0.00352052300927628, 0.00434187143967281, -9.66615195654703e-05, 3.64923075694275e-05, - 6.09619017310129e-05, 4.23908862836885e-05}, - {-0.858019663974047, 1.10138705956076, 0.278789852526915, -0.000199798507752607, - 0.00112092406838628, -0.00177346866231588, 0.000217816070307086, -0.000240713988238257, - 0.000150795563555828, -0.000279246491927943}, - {7.93661480471297, 1.33902098855997, -0.64010306493848, -0.00307944184518557, - -0.00511421127083497, 0.0204008636376403, -9.50042323904954e-05, 6.01530207062221e-05, - 9.13233708460098e-05, -0.000206717750924323}, - {16.2658124154565, 0.191301571705827, 1.02390350838635, 0.0258487436355216, - -0.0219752092833362, 0.0236916776412211, -0.000350496453661261, -0.000123849795280597, - -0.000532190902882765, 9.36018171121253e-05}, - {-1.53023612303052, 1.29132951637076, 0.181159073530008, -0.0023490608317645, - -0.00370741703297037, -0.000229071300377431, -1.6634455407558e-05, 1.11387154630828e-05, - 1.02609175615251e-05, -9.64717658954667e-06}, - {-32.9918791079688, 0.093536793089853, 4.76858627395571, 0.0595845684553358, - -0.054845749101257, -0.133247382500001, -0.000688999201915199, 7.67286265747961e-05, - 0.000868163357631254, 0.00120099606910313}}; + {10.4400948050067, 1.38202655603079, 0.975299591736672, 0.0172133914423707, + -0.0163482459492803, 0.035730152619911, 0.00021725657060767, -0.000181685375645396, + -0.000124096561459262, 0.00040790566176981}, + {6.38281281805793, 1.81388255990089, 0.28679524291736, 0.0218036823758417, + 0.010516766426651, 0.000446101708841615, 0.00020187044149361, 0.000114957457831415, + 1.63114413539632e-05, -2.0187452317724e-05}, + {-29.3049094555, -0.506844002611835, 1.64911970541112, -0.0336282997119334, + 0.053185806861685, -0.028164943139695, -0.00021098074590512, 0.000643681643489995, + -0.000249094601806692, 0.000231466668650876}, + {-4.76233790255328, 1.1780710601961, -0.194257188545164, 0.00471817228628384, + -0.00183773644319332, -0.00570261621182479, -7.99203367291902e-05, 7.75752247926601e-05, + -9.78534772816957e-06, -4.72083745991256e-05}, + {0.692159025649028, 1.11895461388667, 0.341706834956496, 0.000237989648019541, + -0.000188322779563912, 0.000227310789253953, 0.000133001646828401, -0.000305810826248463, + 0.00010150571088124, -0.000367705461590854}, + {3.38094203317731, 1.24778838596815, 0.067807236112956, -0.00379395536123526, + -0.00339180589343601, -0.00188754615986649, -7.52406312245606e-05, 4.58398750278147e-05, + 6.97244631313601e-05, 2.50519145070895e-05}, + {-7.10546287716029, 0.459472977452686, -1.12251049944014, 0.0175406972371191, + -0.0310525406867782, -0.0531315970690727, -0.000121107664597462, 0.000544665437051928, + -1.78466217018177e-05, -0.00058976234038192}, + {1.60633684055984, 1.1975095485662, 0.180159204664965, -0.00259157601062089, + -0.0038106317634397, 0.000956686555225968, 4.28416721502134e-06, 5.84532336259517e-06, + -2.73407888222758e-05, 5.45131881032866e-06}, + {43.3732235586222, 0.528096786861784, -3.41255850703983, -0.0161629934278675, + 0.0790998053536612, 0.0743822668655928, 0.000237176965460634, -0.000426691336904078, + -0.000889196131314391, -0.000509766491897672}}; float sus6coeffBeta[9][10] = { - {12.7380220453847, -0.6087309901836, 2.60957722462363, -0.0415319939920917, - 0.0444944768824276, 0.0223231464060241, -0.000421503508733887, -9.39560038638717e-05, - 0.000821479971871302, -4.5330528329465e-05}, - {1.96846333975847, -0.33921438143463, 1.23957110477613, -0.00948832495296823, - 0.00107211134687287, -0.00410820045700199, -9.33679611473279e-05, 3.72984782145427e-05, - -4.04514487800062e-05, -7.6296149087237e-05}, - {5.7454444934481, -1.58476383793609, -0.418479494289251, -0.0985177320630941, - -0.0862179276808015, 0.0126762052037897, -0.00118207758271301, -0.000190361442918412, - 0.0011723869613426, 0.000122882034141316}, - {2.11042287406433, -0.225942746245056, 1.18084080712528, -0.00103013931607172, - -0.00675606790663387, -0.00106646109062746, 1.7708839355979e-05, -3.13642668374253e-05, - -5.87601932564404e-05, -3.92033314627704e-05}, - {2.96049248725882, -0.286261455028255, 1.09122556181319, -0.000672369023155898, - 0.000574446975796023, 0.000120303729680796, 0.000292285799270644, 0.000170497873487264, - 0.000259925974231328, 0.000222437797823852}, - {1.65218061201483, -0.19535446105784, 1.39609640918411, 0.000961524354787167, - 0.00592400381724333, -0.0078500192096718, -7.02791628080906e-07, -2.07197580883822e-05, - -4.33518182614169e-05, 4.66993119419691e-05}, - {-19.56673237415, 1.06558565338761, 0.151160448373445, -0.0252628659378108, - 0.0281230551050938, -0.0217328869907185, 0.000241309440918385, -0.000116449585258429, - 0.000401546410974577, -0.000147563886502726}, - {1.56167171538684, -0.155299366654736, 1.20084049723279, 0.00457348893890231, - 0.00118888040006052, 0.0029920178735941, -5.583448120596e-05, -2.34496315691865e-05, - -5.3309466243918e-05, 6.20289310356821e-06}, - {1.95050549495182, -2.74909818412705, 3.80268788018641, 0.0629242254381785, - 0.0581479035315726, -0.111361283351269, -0.00047845777495158, -0.00075354297736741, - -0.000186887396585446, 0.00119710704771344}}; + {1.03872648284911, -0.213507239271552, 1.43193059498181, -0.000972717820830235, + -0.00661046096415371, 0.00974284211491888, 2.96098456891215e-05, -8.2933115634257e-05, + -5.52178824394723e-06, 5.73935295303589e-05}, + {3.42242235823356, 0.0848392511283237, 1.24574390342586, 0.00356248195980133, + 0.00100415659893053, -0.00460120247716139, 3.84891005422427e-05, 2.70236417852327e-05, + -7.58501977656551e-05, -8.79809730730992e-05}, + {14.0092526123741, 1.03126714946215, 1.0611008563785, 0.04076462444523, 0.0114106419194518, + 0.00746959159048058, 0.000388033225774727, -0.000124645014888926, -0.000296639947532341, + -0.00020861690864945}, + {1.3562422681189, -0.241585615891602, 1.49170424068611, 0.000179184170448335, + -0.00712399257616284, 0.0121433526723498, 3.29770580642447e-05, 8.78960210966787e-06, + -6.00508568552101e-05, 0.000101583822589461}, + {-0.718855428908583, -0.344067476078684, 1.12397093701762, 0.000236505431484729, + -0.000406441415248947, 0.00032834991502413, 0.000359422093285086, 8.18895560425272e-05, + 0.000316835483508523, 0.000151442890664899}, + {-0.268764016434841, -0.275272048639511, 1.26239753050527, -0.000511224336925231, + 0.0095628568270856, -0.00397960092451418, 1.39587366293607e-05, 1.31409051361129e-05, + -9.83662017231755e-05, 1.87078667116619e-05}, + {27.168106989145, -2.43346872338192, 1.91135512970771, 0.0553180826818016, + -0.0481878292619383, 0.0052773235604729, -0.000428011927975304, 0.000528018208222772, + -0.000285438191474895, -5.71327627917386e-05}, + {-0.169494136517622, -0.350851545482921, 1.19922076033643, 0.0101120903675328, + -0.00151674465424115, 0.00548694086125656, -0.000108240000970513, 1.57202185024105e-05, + -9.77555098179959e-05, 2.09624089449761e-05}, + {-32.3807957489507, 1.8271436443167, 2.51530814328123, -0.0532334586403461, + -0.0355980127727253, -0.0213373892796204, 0.00045506092539885, 0.000545065581027688, + 0.000141998709314758, 0.000101051304611037}}; float sus7coeffAlpha[9][10] = { {-92.1126183408754, -3.77261746189525, -4.50604668349213, -0.0909560776043523, -0.15646903318971, -0.0766293642415356, -0.00059452135473577, -0.00144790037129283, @@ -751,9 +764,6 @@ class AcsParameters /*: public HasParametersIF*/ { {116.975421945286, -5.53022680362263, -5.61081660666997, 0.109754904982136, 0.167666815691513, 0.163137400730063, -0.000609874123906977, -0.00205336098697513, -0.000889232196185857, -0.00168429567131815}}; - - float filterAlpha; - float sunThresh; } susHandlingParameters; struct GyrHandlingParameters { @@ -761,110 +771,119 @@ class AcsParameters /*: public HasParametersIF*/ { double gyr1orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}}; double gyr2orientationMatrix[3][3] = {{0, 0, -1}, {0, -1, 0}, {-1, 0, 0}}; double gyr3orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}}; - // var = sqrt(sigma), sigma = RND*sqrt(freq), following values are RND^2 and not var as freq is - // assumed to be equal for the same class of sensors + + double gyr0bias[3] = {0.06318149743589743, 0.4283235025641024, -0.16383500000000004}; + double gyr1bias[3] = {-0.12855128205128205, 1.6737307692307695, 1.031724358974359}; + double gyr2bias[3] = {0.15039212820512823, 0.7094475589743591, -0.22298363589743594}; + double gyr3bias[3] = {0.0021730769230769217, -0.6655897435897435, 0.034096153846153845}; + + /* var = sqrt(sigma), sigma = RND*sqrt(freq), following values are RND^2 and not var as freq is + * assumed to be equal for the same class of sensors */ float gyr02variance[3] = {pow(3.0e-3 * sqrt(2), 2), // RND_x = 3.0e-3 deg/s/sqrt(Hz) rms pow(3.0e-3 * sqrt(2), 2), // RND_y = 3.0e-3 deg/s/sqrt(Hz) rms pow(4.3e-3 * sqrt(2), 2)}; // RND_z = 4.3e-3 deg/s/sqrt(Hz) rms float gyr13variance[3] = {pow(11e-3, 2), pow(11e-3, 2), pow(11e-3, 2)}; - enum PreferAdis { NO = 0, YES = 1 }; - uint8_t preferAdis = PreferAdis::YES; + uint8_t preferAdis = true; } gyrHandlingParameters; struct RwHandlingParameters { - double rw0orientationMatrix[3][3]; - double rw1orientationMatrix[3][3]; - double rw2orientationMatrix[3][3]; - double rw3orientationMatrix[3][3]; double inertiaWheel = 0.000028198; - double maxTrq = 0.0032; // 3.2 [mNm] + double maxTrq = 0.0032; // 3.2 [mNm] + double stictionSpeed = 100; // 80; // RPM + double stictionReleaseSpeed = 120; // RPM + double stictionTorque = 0.0006; } rwHandlingParameters; struct RwMatrices { double alignmentMatrix[3][4] = {{0.9205, 0.0000, -0.9205, 0.0000}, {0.0000, -0.9205, 0.0000, 0.9205}, {0.3907, 0.3907, 0.3907, 0.3907}}; - double pseudoInverse[4][3] = {{0.4434, -0.2845, 0.3597}, - {0.2136, -0.3317, 1.0123}, - {-0.8672, -0.1406, 0.1778}, - {0.6426, 0.4794, 1.3603}}; - double without0[4][3]; - double without1[4][3]; - double without2[4][3]; - double without3[4][3]; - double nullspace[4] = {-0.7358, 0.5469, -0.3637, -0.1649}; + double pseudoInverse[4][3] = { + {0.5432, 0, 0.6398}, {0, -0.5432, 0.6398}, {-0.5432, 0, 0.6398}, {0, 0.5432, 0.6398}}; + double without0[4][3] = { + {0, 0, 0}, {0.5432, -0.5432, 1.2797}, {-1.0864, 0, 0}, {0.5432, 0.5432, 1.2797}}; + double without1[4][3] = { + {0.5432, -0.5432, 1.2797}, {0, 0, 0}, {-0.5432, -0.5432, 1.2797}, {0, 1.0864, 0}}; + double without2[4][3] = { + {1.0864, 0, 0}, {-0.5432, -0.5432, 1.2797}, {0, 0, 0}, {-0.5432, 0.5432, 1.2797}}; + double without3[4][3] = { + {0.5432, 0.5432, 1.2797}, {0, -1.0864, 0}, {-0.5432, 0.5432, 1.2797}, {0, 0, 0}}; + double nullspace[4] = {-0.5000, 0.5000, -0.5000, 0.5000}; } rwMatrices; struct SafeModeControllerParameters { double k_rate_mekf = 0.00059437; double k_align_mekf = 0.000056875; - double k_rate_no_mekf; - double k_align_no_mekf; - double sunMagAngleMin; + double k_rate_no_mekf = 0.00059437; + double k_align_no_mekf = 0.000056875; - double sunTargetDir[3] = {1, 0, 0}; // Body frame - double satRateRef[3]; // Body frame + double sunMagAngleMin = 5 * M_PI / 180; + double sunTargetDirLeop[3] = {0, .5, .5}; + double sunTargetDir[3] = {0, 0, 1}; + + double satRateRef[3] = {0, 0, 0}; } safeModeControllerParameters; - struct DetumbleCtrlParameters { - double gainD = pow(10.0, -3.3); - - } detumbleCtrlParameters; - - // ToDo: mutiple structs for different pointing mode controllers? - struct PointingModeControllerParameters { - double updtFlag; - double A_rw[3][12]; - - double refDirection[3] = {1, 0, 0}; - double refRotRate[3] = {0, 0, 0}; - double quatRef[4] = {0, 0, 0, 1}; - bool avoidBlindStr = true; - double blindAvoidStart = 1.5; - double blindAvoidStop = 2.5; - double blindRotRate = 1 * M_PI / 180; - + struct PointingLawParameters { double zeta = 0.3; - double zetaLow; double om = 0.3; - double omLow; double omMax = 1 * M_PI / 180; double qiMin = 0.1; double gainNullspace = 0.01; double desatMomentumRef[3] = {0, 0, 0}; double deSatGainFactor = 1000; - bool desatOn = true; + uint8_t desatOn = true; + uint8_t enableAntiStiction = true; + } pointingLawParameters; - double omegaEarth = 0.000072921158553; + 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 quatRef[4] = {0, 0, 0, 1}; + int8_t timeElapsedMax = 10; // rot rate calculations - } inertialModeControllerParameters, nadirModeControllerParameters, targetModeControllerParameters; + // 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] + + // For one-axis control: + uint8_t avoidBlindStr = true; + double blindAvoidStart = 1.5; + double blindAvoidStop = 2.5; + double blindRotRate = 1 * M_PI / 180; + } targetModeControllerParameters; + + struct NadirModeControllerParameters : PointingLawParameters { + double refDirection[3] = {-1, 0, 0}; // Antenna + double quatRef[4] = {0, 0, 0, 1}; + int8_t timeElapsedMax = 10; // rot rate calculations + } nadirModeControllerParameters; + + struct InertialModeControllerParameters : PointingLawParameters { + double tgtQuat[4] = {0, 0, 0, 1}; + double refRotRate[3] = {0, 0, 0}; + double quatRef[4] = {0, 0, 0, 1}; + } inertialModeControllerParameters; struct StrParameters { double exclusionAngle = 20 * M_PI / 180; - // double strOrientationMatrix[3][3]; - double boresightAxis[3] = {0.7593, 0.0000, -0.6508}; // in body/geometry frame + double boresightAxis[3] = {0.7593, 0.0000, -0.6508}; // geometry frame } strParameters; struct GpsParameters { + double timeDiffVelocityMax = 30; //[s] } gpsParameters; - struct GroundStationParameters { - double latitudeGs = 48.7495 * M_PI / 180.; // [rad] Latitude - double longitudeGs = 9.10384 * M_PI / 180.; // [rad] Longitude - double altitudeGs = 500; // [m] Altitude - double earthRadiusEquat = 6378137; // [m] - double earthRadiusPolar = 6356752.314; // [m] - } groundStationParameters; // Stuttgart - struct SunModelParameters { - enum UseSunModel { NO = 0, YES = 3 }; - uint8_t useSunModel; float domega = 36000.771; - float omega_0 = 282.94 * M_PI / 180.; // RAAN plus argument of perigee - float m_0 = 357.5256; // coefficients for mean anomaly + float omega_0 = 280.46 * M_PI / 180.; // RAAN plus argument of + // perigee + float m_0 = 357.5277; // coefficients for mean anomaly float dm = 35999.049; // coefficients for mean anomaly float e = 23.4392911 * M_PI / 180.; // angle of earth's rotation axis float e1 = 0.74508 * M_PI / 180.; @@ -874,19 +893,13 @@ class AcsParameters /*: public HasParametersIF*/ { } sunModelParameters; struct KalmanFilterParameters { - uint8_t activateKalmanFilter; - uint8_t requestResetFlag; - double maxToleratedTimeBetweenKalmanFilterExecutionSteps; - double processNoiseOmega[3]; - double processNoiseQuaternion[4]; - double sensorNoiseSTR = 0.1 * M_PI / 180; double sensorNoiseSS = 8 * M_PI / 180; double sensorNoiseMAG = 4 * M_PI / 180; - double sensorNoiseRMU[3]; + double sensorNoiseGYR = 0.1 * M_PI / 180; - double sensorNoiseArwRmu; // Angular Random Walk - double sensorNoiseBsRMU; // Bias Stability + double sensorNoiseArwGYR = 3 * 0.0043 * M_PI / sqrt(10) / 180; // Angular Random Walk + double sensorNoiseBsGYR = 3 * M_PI / 180 / 3600; // Bias Stability } kalmanFilterParameters; struct MagnetorquesParameter { @@ -901,8 +914,8 @@ class AcsParameters /*: public HasParametersIF*/ { struct DetumbleParameter { uint8_t detumblecounter = 75; // 30 s - double omegaDetumbleStart = 2; - double omegaDetumbleEnd = 0.4; + double omegaDetumbleStart = 2 * M_PI / 180; + double omegaDetumbleEnd = 0.4 * M_PI / 180; double gainD = pow(10.0, -3.3); } detumbleParameter; }; diff --git a/mission/controller/acs/ActuatorCmd.cpp b/mission/controller/acs/ActuatorCmd.cpp index 771f9d6f..4e0052e0 100644 --- a/mission/controller/acs/ActuatorCmd.cpp +++ b/mission/controller/acs/ActuatorCmd.cpp @@ -21,26 +21,27 @@ ActuatorCmd::ActuatorCmd(AcsParameters *acsParameters_) { acsParameters = *acsPa ActuatorCmd::~ActuatorCmd() {} -void ActuatorCmd::cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1, - const int32_t *speedRw2, const int32_t *speedRw3, - const double *rwTrqIn, const double *rwTrqNS, double *rwCmdSpeed) { - using namespace Math; +void ActuatorCmd::scalingTorqueRws(const double *rwTrq, double *rwTrqScaled) { // Scaling the commanded torque to a maximum value - double torque[4] = {0, 0, 0, 0}; double maxTrq = acsParameters.rwHandlingParameters.maxTrq; - VectorOperations::add(rwTrqIn, rwTrqNS, torque, 4); double maxValue = 0; for (int i = 0; i < 4; i++) { // size of torque, always 4 ? - if (abs(torque[i]) > maxValue) { - maxValue = abs(torque[i]); + if (abs(rwTrq[i]) > maxValue) { + maxValue = abs(rwTrq[i]); } } if (maxValue > maxTrq) { double scalingFactor = maxTrq / maxValue; - VectorOperations::mulScalar(torque, scalingFactor, torque, 4); + VectorOperations::mulScalar(rwTrq, scalingFactor, rwTrqScaled, 4); } +} + +void ActuatorCmd::cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1, + const int32_t *speedRw2, const int32_t *speedRw3, + const double *rwTorque, double *rwCmdSpeed) { + using namespace Math; // Calculating the commanded speed in RPM for every reaction wheel double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3}; @@ -50,7 +51,7 @@ void ActuatorCmd::cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1 double radToRpm = 60 / (2 * PI); // factor for conversion to RPM // W_RW = Torque_RW / I_RW * delta t [rad/s] double factor = commandTime / inertiaWheel * radToRpm; - VectorOperations::mulScalar(torque, factor, deltaSpeed, 4); + VectorOperations::mulScalar(rwTorque, factor, deltaSpeed, 4); VectorOperations::add(speedRws, deltaSpeed, rwCmdSpeed, 4); } diff --git a/mission/controller/acs/ActuatorCmd.h b/mission/controller/acs/ActuatorCmd.h index 181bf175..04e2b61f 100644 --- a/mission/controller/acs/ActuatorCmd.h +++ b/mission/controller/acs/ActuatorCmd.h @@ -1,10 +1,3 @@ -/* - * ActuatorCmd.h - * - * Created on: 4 Aug 2022 - * Author: Robin Marquardt - */ - #ifndef ACTUATORCMD_H_ #define ACTUATORCMD_H_ @@ -18,6 +11,14 @@ class ActuatorCmd { ActuatorCmd(AcsParameters *acsParameters_); // Input mode ? virtual ~ActuatorCmd(); + /* + * @brief: scalingTorqueRws() scales the torque via maximum part in case this part is + * higher then the maximum torque + * @param: rwTrq given torque for reaction wheels + * rwTrqScaled possible scaled torque + */ + void scalingTorqueRws(const double *rwTrq, double *rwTrqScaled); + /* * @brief: cmdSpeedToRws() will set the maximum possible torque for the reaction * wheels, also will calculate the needed revolutions per minute for the RWs, which will be given @@ -28,8 +29,7 @@ class ActuatorCmd { * reaction wheel */ void cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2, - const int32_t *speedRw3, const double *rwTrqIn, const double *rwTrqNS, - double *rwCmdSpeed); + const int32_t *speedRw3, const double *rwTorque, double *rwCmdSpeed); /* * @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 35a7295a..383a265f 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -13,6 +13,8 @@ #include #include +#include + #include "string.h" #include "util/CholeskyDecomposition.h" #include "util/MathOperations.h" @@ -22,57 +24,53 @@ Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = *acsParamete Guidance::~Guidance() {} void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3]) { - for (int i = 0; i < 3; i++) { - sunTargetSafe[i] = acsParameters.safeModeControllerParameters.sunTargetDir[i]; - satRateSafe[i] = acsParameters.safeModeControllerParameters.satRateRef[i]; + 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)); } - - // memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir, 24); + std::memcpy(satRateSafe, acsParameters.safeModeControllerParameters.satRateRef, + 3 * sizeof(double)); } -void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, - acsctrl::SusDataProcessed *susDataProcessed, timeval now, - double targetQuat[4], double refSatRate[3]) { +void Guidance::targetQuatPtgSingleAxis(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 groundstation + // Calculation of target quaternion to groundstation or given latitude, longitude and altitude //------------------------------------------------------------------------------------- - // Transform longitude, latitude and altitude of groundstation to cartesian coordiantes (earth + // Transform longitude, latitude and altitude to cartesian coordiantes (earth // fixed/centered frame) - double groundStationCart[3] = {0, 0, 0}; + double targetCart[3] = {0, 0, 0}; - MathOperations::cartesianFromLatLongAlt(acsParameters.groundStationParameters.latitudeGs, - acsParameters.groundStationParameters.longitudeGs, - acsParameters.groundStationParameters.altitudeGs, - groundStationCart); + 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}; - MathOperations::cartesianFromLatLongAlt(sensorValues->gpsSet.latitude.value, - sensorValues->gpsSet.longitude.value, + 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 double targetDirE[3] = {0, 0, 0}; - VectorOperations::subtract(groundStationCart, posSatE, targetDirE, 3); + 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}}; - MathOperations::dcmEJ(now, *dcmEJ); - MathOperations::inverseMatrixDimThree(*dcmEJ, *dcmJE); - // Derivative of dmcEJ WITHOUT PRECISSION AND NUTATION 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}}; - double dcmDot[3][3] = {{0, 1, 0}, {-1, 0, 0}, {0, 0, 0}}; - double omegaEarth = acsParameters.targetModeControllerParameters.omegaEarth; - - // TEST SECTION ! - // double dcmTEST[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - // MatrixOperations::multiply(&acsParameters.magnetorquesParameter.mtq0orientationMatrix, - // dcmTEST, dcmTEST, 3, 3, 3); - - MatrixOperations::multiply(*dcmDot, *dcmEJ, *dcmEJDot, 3, 3, 3); - MatrixOperations::multiplyScalar(*dcmEJDot, omegaEarth, *dcmEJDot, 3, 3); MathOperations::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot); // Transformation between ECEF and Body frame @@ -111,9 +109,7 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData // Calculation of reference rotation rate //------------------------------------------------------------------------------------- double velSatE[3] = {0, 0, 0}; - velSatE[0] = 0.0; // sensorValues->gps0Velocity[0]; - velSatE[1] = 0.0; // sensorValues->gps0Velocity[1]; - velSatE[2] = 0.0; // sensorValues->gps0Velocity[2]; + 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 MatrixOperations::multiply(*dcmBE, velSatE, velSatBPart1, 3, 3, 1); @@ -134,10 +130,10 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData // Calculation of reference rotation rate in case of star tracker blinding //------------------------------------------------------------------------------------- if (acsParameters.targetModeControllerParameters.avoidBlindStr) { - double sunDirJ[3] = {0, 0, 0}; 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 { @@ -183,14 +179,414 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData } } -void Guidance::comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double refSatRate[3], - double quatErrorComplete[4], double quatError[3], double deltaRate[3]) { - double quatRef[4] = {0, 0, 0, 0}; - quatRef[0] = acsParameters.targetModeControllerParameters.quatRef[0]; - quatRef[1] = acsParameters.targetModeControllerParameters.quatRef[1]; - quatRef[2] = acsParameters.targetModeControllerParameters.quatRef[2]; - quatRef[3] = acsParameters.targetModeControllerParameters.quatRef[3]; +void Guidance::refRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4], + double *refSatRate) { + //------------------------------------------------------------------------------------- + // Calculation of reference 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)); + if (timeElapsed < timeElapsedMax) { + double qDiff[4] = {0, 0, 0, 0}; + VectorOperations::subtract(quatInertialTarget, savedQuaternion, qDiff, 4); + VectorOperations::mulScalar(qDiff, 1 / timeElapsed, qDiff, 4); + double tgtQuatVec[3] = {quatInertialTarget[0], quatInertialTarget[1], quatInertialTarget[2]}, + qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]}; + double sum1[3] = {0, 0, 0}, sum2[3] = {0, 0, 0}, sum3[3] = {0, 0, 0}, sum[3] = {0, 0, 0}; + VectorOperations::cross(quatInertialTarget, qDiff, sum1); + VectorOperations::mulScalar(tgtQuatVec, qDiff[3], sum2, 3); + VectorOperations::mulScalar(qDiffVec, quatInertialTarget[3], sum3, 3); + VectorOperations::add(sum1, sum2, sum, 3); + VectorOperations::subtract(sum, sum3, sum, 3); + double omegaRefNew[3] = {0, 0, 0}; + VectorOperations::mulScalar(sum, -2, omegaRefNew, 3); + + VectorOperations::mulScalar(omegaRefNew, 2, refSatRate, 3); + VectorOperations::subtract(refSatRate, omegaRefSaved, refSatRate, 3); + omegaRefSaved[0] = omegaRefNew[0]; + omegaRefSaved[1] = omegaRefNew[1]; + omegaRefSaved[2] = omegaRefNew[2]; + } else { + refSatRate[0] = 0; + refSatRate[1] = 0; + refSatRate[2] = 0; + } + + timeSavedQuaternion = now; + savedQuaternion[0] = quatInertialTarget[0]; + savedQuaternion[1] = quatInertialTarget[1]; + savedQuaternion[2] = quatInertialTarget[2]; + 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); @@ -210,8 +606,8 @@ void Guidance::comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, dou 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 ?? + // target flag in matlab, importance, does look like it only gives feedback if pointing control is + // under 150 arcsec ?? } void Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv) { @@ -300,7 +696,7 @@ void Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, double * // @note: This one takes the normal pseudoInverse of all four raction wheels valid. // Does not make sense, but is implemented that way in MATLAB ?! // Thought: It does not really play a role, because in case there are more then one - // reaction wheel the pointing control is destined to fail. + // reaction wheel invalid the pointing control is destined to fail. rwPseudoInv[0] = acsParameters.rwMatrices.pseudoInverse[0][0]; rwPseudoInv[1] = acsParameters.rwMatrices.pseudoInverse[0][1]; rwPseudoInv[2] = acsParameters.rwMatrices.pseudoInverse[0][2]; diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index 4c699561..0401d07c 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -21,16 +21,49 @@ class Guidance { void getTargetParamsSafe(double sunTargetSafe[3], double satRateRef[3]); - // Function to get the target quaternion and refence rotation rate from gps position and position - // of the ground station - void targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, - acsctrl::SusDataProcessed *susDataProcessed, timeval now, double targetQuat[4], - double refSatRate[3]); + // 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]); + + // 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]); + + // 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]); + + // Function to get the target quaternion and refence rotation rate from parameters for inertial + // pointing + void inertialQuatPtg(double targetQuat[4], double refSatRate[3]); // @note: compares target Quaternion and reference quaternion, also actual satellite rate and // desired - void comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double refSatRate[3], - double quatErrorComplete[4], double quatError[3], double deltaRate[3]); + void comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double quatRef[4], + double refSatRate[3], double quatErrorComplete[4], double quatError[3], + double deltaRate[3]); + + 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 // reation wheel maybe can be done in "commanding.h" @@ -39,6 +72,12 @@ class Guidance { private: AcsParameters acsParameters; bool strBlindAvoidFlag = false; + timeval timeSavedQuaternion; + double savedQuaternion[4] = {0, 0, 0, 0}; + double omegaRefSaved[3] = {0, 0, 0}; + + static constexpr char SD_0_SKEWED_PTG_FILE[] = "/mnt/sd0/conf/deployment"; + static constexpr char SD_1_SKEWED_PTG_FILE[] = "/mnt/sd1/conf/deployment"; }; #endif /* ACS_GUIDANCE_H_ */ diff --git a/mission/controller/acs/Igrf13Model.cpp b/mission/controller/acs/Igrf13Model.cpp index fcd95b68..63992010 100644 --- a/mission/controller/acs/Igrf13Model.cpp +++ b/mission/controller/acs/Igrf13Model.cpp @@ -1,22 +1,19 @@ -/* - * Igrf13Model.cpp - * - * Created on: 10 Mar 2022 - * Author: Robin Marquardt - */ - #include "Igrf13Model.h" -#include -#include -#include -#include -#include +#include +#include +#include +#include #include #include +#include + +#include #include "util/MathOperations.h" +using namespace Math; + Igrf13Model::Igrf13Model() {} Igrf13Model::~Igrf13Model() {} @@ -25,7 +22,7 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude, double* magFieldModelInertial) { double phi = longitude, theta = gcLatitude; // geocentric /* Here is the co-latitude needed*/ - theta -= 90 * Math::PI / 180; + theta -= 90 * PI / 180; theta *= (-1); double rE = 6371200.0; // radius earth [m] @@ -43,7 +40,7 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude, /* Calculation of Legendre Polynoms (normalised) */ if (n == m) { P2 = sin(theta) * P11; - dP2 = sin(theta) * dP11 - cos(theta) * P11; + dP2 = sin(theta) * dP11 + cos(theta) * P11; P11 = P2; P10 = P11; P20 = 0; @@ -70,11 +67,11 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude, magFieldModel[0] += pow(rE / (altitude + rE), (n + 2)) * (n + 1) * ((updatedG[m][n - 1] * cos(m * phi) + updatedH[m][n - 1] * sin(m * phi)) * P2); - /* gradient of scalar potential towards phi */ + /* gradient of scalar potential towards theta */ magFieldModel[1] += pow(rE / (altitude + rE), (n + 2)) * ((updatedG[m][n - 1] * cos(m * phi) + updatedH[m][n - 1] * sin(m * phi)) * dP2); - /* gradient of scalar potential towards theta */ + /* gradient of scalar potential towards phi */ magFieldModel[2] += pow(rE / (altitude + rE), (n + 2)) * ((-updatedG[m][n - 1] * sin(m * phi) + updatedH[m][n - 1] * cos(m * phi)) * P2 * m); @@ -85,31 +82,30 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude, magFieldModel[1] *= -1; magFieldModel[2] *= (-1 / sin(theta)); - /* Next step: transform into inertial KOS (IJK)*/ - // Julean Centuries - double JD2000Floor = 0; double JD2000 = MathOperations::convertUnixToJD2000(timeOfMagMeasurement); - JD2000Floor = floor(JD2000); - double JC2000 = JD2000Floor / 36525; - - double gst = 100.4606184 + 36000.77005361 * JC2000 + 0.00038793 * pow(JC2000, 2) - - 0.000000026 * pow(JC2000, 3); // greenwich sidereal time - gst *= PI / 180; // convert to radians - double sec = - (JD2000 - JD2000Floor) * 86400; // Seconds on this day (Universal time) // FROM GPS ? - double omega0 = 0.00007292115; // mean angular velocity earth [rad/s] - gst += omega0 * sec; + double UT1 = JD2000 / 36525.; + double gst = + 280.46061837 + 360.98564736629 * JD2000 + 0.0003875 * pow(UT1, 2) - 2.6e-8 * pow(UT1, 3); + gst = std::fmod(gst, 360.); + gst *= PI / 180.; double lst = gst + longitude; // local sidereal time [rad] - magFieldModelInertial[0] = magFieldModel[0] * cos(theta) + - magFieldModel[1] * sin(theta) * cos(lst) - magFieldModel[1] * sin(lst); - magFieldModelInertial[1] = magFieldModel[0] * cos(theta) + - magFieldModel[1] * sin(theta) * sin(lst) + magFieldModel[1] * cos(lst); - magFieldModelInertial[2] = magFieldModel[0] * sin(theta) + magFieldModel[1] * cos(lst); + magFieldModelInertial[0] = + (magFieldModel[0] * cos(gcLatitude) + magFieldModel[1] * sin(gcLatitude)) * cos(lst) - + magFieldModel[2] * sin(lst); + magFieldModelInertial[1] = + (magFieldModel[0] * cos(gcLatitude) + magFieldModel[1] * sin(gcLatitude)) * sin(lst) + + magFieldModel[2] * cos(lst); + magFieldModelInertial[2] = + magFieldModel[0] * sin(gcLatitude) - magFieldModel[1] * cos(gcLatitude); double normVecMagFieldInert[3] = {0, 0, 0}; VectorOperations::normalize(magFieldModelInertial, normVecMagFieldInert, 3); + + magFieldModel[0] = 0; + magFieldModel[1] = 0; + magFieldModel[2] = 0; } void Igrf13Model::updateCoeffGH(timeval timeOfMagMeasurement) { @@ -123,3 +119,34 @@ void Igrf13Model::updateCoeffGH(timeval timeOfMagMeasurement) { } } } + +void Igrf13Model::schmidtNormalization() { + double kronDelta = 0; + schmidtFactors[0][0] = 1; + for (int n = 1; n <= igrfOrder; n++) { + if (n == 1) { + schmidtFactors[0][n - 1] = 1; + } else { + schmidtFactors[0][n - 1] = schmidtFactors[0][n - 2] * (2 * n - 1) / n; + } + for (int m = 1; m <= igrfOrder; m++) { + if (m == 1) { + kronDelta = 1; + } else { + kronDelta = 0; + } + schmidtFactors[m][n - 1] = + schmidtFactors[m - 1][n - 1] * sqrt((n - m + 1) * (kronDelta + 1) / (n + m)); + } + } + + for (int i = 0; i <= igrfOrder; i++) { + for (int j = 0; j <= (igrfOrder - 1); j++) { + coeffG[i][j] = schmidtFactors[i][j] * coeffG[i][j]; + coeffH[i][j] = schmidtFactors[i][j] * coeffH[i][j]; + + svG[i][j] = schmidtFactors[i][j] * svG[i][j]; + svH[i][j] = schmidtFactors[i][j] * svH[i][j]; + } + } +} diff --git a/mission/controller/acs/Igrf13Model.h b/mission/controller/acs/Igrf13Model.h index aa7e8b73..ea2c1044 100644 --- a/mission/controller/acs/Igrf13Model.h +++ b/mission/controller/acs/Igrf13Model.h @@ -43,14 +43,15 @@ class Igrf13Model /*:public HasParametersIF*/ { * - timeOfMagMeasurement: time of actual measurement [s] * * Outputs: - * - magFieldModelInertial: Magnetic Field Vector in IJK KOS [nT]*/ + * - magFieldModelInertial: Magnetic Field Vector in IJK RF [nT]*/ // Coefficient wary over year, could be updated sometimes. void updateCoeffGH(timeval timeOfMagMeasurement); // Secular variation (SV) double magFieldModel[3]; + void schmidtNormalization(); private: - const double coeffG[14][13] = { + double coeffG[14][13] = { {-29404.8, -2499.6, 1363.2, 903.0, -234.3, 66.0, 80.6, 23.7, 5.0, -1.9, 3.0, -2.0, 0.1}, {-1450.9, 2982.0, -2381.2, 809.5, 363.2, 65.5, -76.7, 9.7, 8.4, -6.2, -1.4, -0.1, -0.9}, {0.0, 1677.0, 1236.2, 86.3, 187.8, 72.9, -8.2, -17.6, 2.9, -0.1, -2.5, 0.5, 0.5}, @@ -66,7 +67,7 @@ class Igrf13Model /*:public HasParametersIF*/ { {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.3, -0.5}, {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.4}}; // [m][n] in nT - const double coeffH[14][13] = { + double coeffH[14][13] = { {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}, {4652.5, -2991.6, -82.1, 281.9, 47.7, -19.1, -51.5, 8.4, -23.4, 3.4, 0.0, -1.2, -0.9}, {0.0, -734.6, 241.9, -158.4, 208.3, 25.1, -16.9, -15.3, 11.0, -0.2, 2.5, 0.5, 0.6}, @@ -82,7 +83,7 @@ class Igrf13Model /*:public HasParametersIF*/ { {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, -0.4}, {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.6}}; // [m][n] in nT - const double svG[14][13] = { + double svG[14][13] = { {5.7, -11.0, 2.2, -1.2, -0.3, -0.5, -0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, {7.4, -7.0, -5.9, -1.6, 0.5, -0.3, -0.2, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0}, {0.0, -2.1, 3.1, -5.9, -0.6, 0.4, 0.0, -0.1, 0.0, 0.0, 0.0, 0.0, 0.0}, @@ -98,7 +99,7 @@ class Igrf13Model /*:public HasParametersIF*/ { {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, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; // [m][n] in nT - const double svH[14][13] = { + double svH[14][13] = { {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}, {-25.9, -30.2, 6.0, -0.1, 0.0, 0.0, 0.6, -0.2, 0.0, 0.0, 0.0, 0.0, 0.0}, {0.0, -22.4, -1.1, 6.5, 2.5, -1.6, 0.6, 0.6, 0.0, 0.0, 0.0, 0.0, 0.0}, @@ -114,6 +115,16 @@ class Igrf13Model /*:public HasParametersIF*/ { {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, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; // [m][n] in nT + double schmidtFactors[14][13] = { + {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, 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, 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, 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, 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, 0, 0}}; + ; + double updatedG[14][13]; double updatedH[14][13]; static const int igrfOrder = 13; // degree of truncation diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp index 199440d5..f54893a2 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp +++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp @@ -1,10 +1,3 @@ -/* - * MultiplicativeKalmanFilter.cpp - * - * Created on: 4 Feb 2022 - * Author: rooob - */ - #include "MultiplicativeKalmanFilter.h" #include @@ -14,6 +7,8 @@ #include #include +#include + #include "util/CholeskyDecomposition.h" #include "util/MathOperations.h" @@ -29,7 +24,7 @@ MultiplicativeKalmanFilter::~MultiplicativeKalmanFilter() {} void MultiplicativeKalmanFilter::loadAcsParameters(AcsParameters *acsParameters_) { inertiaEIVE = &(acsParameters_->inertiaEIVE); - kalmanFilterParameters = &(acsParameters_->kalmanFilterParameters); /*Sensor noises also here*/ + kalmanFilterParameters = &(acsParameters_->kalmanFilterParameters); } void MultiplicativeKalmanFilter::reset() {} @@ -41,14 +36,11 @@ void MultiplicativeKalmanFilter::init( // check for valid mag/sun if (validMagField_ && validSS && validSSModel && validMagModel) { validInit = true; - // AcsParameters mekfEstParams; - // loadAcsParameters(&mekfEstParams); // QUEST ALGO ----------------------------------------------------------------------- double sigmaSun = 0, sigmaMag = 0, sigmaGyro = 0; sigmaSun = kalmanFilterParameters->sensorNoiseSS; sigmaMag = kalmanFilterParameters->sensorNoiseMAG; - - sigmaGyro = 0.1 * 3.141 / 180; // acs parameters + sigmaGyro = kalmanFilterParameters->sensorNoiseGYR; double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0}, normSunJ[3] = {0, 0, 0}; @@ -136,7 +128,6 @@ void MultiplicativeKalmanFilter::init( matrixMag[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, matrixSunMag[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, matrixMagSun[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - /* vector*transpose(vector)*/ MatrixOperations::multiply(sunEstB, sunEstB, *matrixSun, 3, 1, 3); MatrixOperations::multiply(magEstB, magEstB, *matrixMag, 3, 1, 3); MatrixOperations::multiply(sunEstB, magEstB, *matrixSunMag, 3, 1, 3); @@ -199,8 +190,6 @@ void MultiplicativeKalmanFilter::init( initialCovarianceMatrix[5][3] = initGyroCov[2][0]; initialCovarianceMatrix[5][4] = initGyroCov[2][1]; initialCovarianceMatrix[5][5] = initGyroCov[2][2]; - // 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}}; } else { // no initialisation possible, no valid measurements validInit = false; @@ -208,14 +197,15 @@ void MultiplicativeKalmanFilter::init( } // --------------- MEKF DISCRETE TIME STEP ------------------------------- -ReturnValue_t MultiplicativeKalmanFilter::mekfEst( - const double *quaternionSTR, const bool validSTR_, const double *rateGYRs_, - const bool validGYRs_, const double *magneticField_, const bool validMagField_, - const double *sunDir_, const bool validSS, const double *sunDirJ, const bool validSSModel, - const double *magFieldJ, const bool validMagModel, acsctrl::MekfData *mekfData) { +ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, const bool validSTR_, + const double *rateGYRs_, const bool validGYRs_, + const double *magneticField_, + const bool validMagField_, const double *sunDir_, + const bool validSS, const double *sunDirJ, + const bool validSSModel, const double *magFieldJ, + const bool validMagModel, double sampleTime, + acsctrl::MekfData *mekfData) { // Check for GYR Measurements - // AcsParameters mekfEstParams; - // loadAcsParameters(&mekfEstParams); int MDF = 0; // Matrix Dimension Factor if (!validGYRs_) { { @@ -960,10 +950,8 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst( biasGYR[2] = updatedGyroBias[2]; /* ----------- PROPAGATION ----------*/ - // double sigmaU = kalmanFilterParameters->sensorNoiseBsGYR; - // double sigmaV = kalmanFilterParameters->sensorNoiseArwGYR; - double sigmaU = 3 * 3.141 / 180 / 3600; - double sigmaV = 3 * 0.0043 * 3.141 / sqrt(10) / 180; + double sigmaU = kalmanFilterParameters->sensorNoiseBsGYR; + double sigmaV = kalmanFilterParameters->sensorNoiseArwGYR; double discTimeMatrix[6][6] = {{-1, 0, 0, 0, 0, 0}, {0, -1, 0, 0, 0, 0}, {0, 0, -1, 0, 0, 0}, {0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}}; @@ -977,170 +965,135 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst( // Discrete Process Noise Covariance Q double discProcessNoiseCov[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}}; - double covQ1[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, - covQ2[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, - covQ3[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, - transCovQ2[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - if (normRotEst * sampleTime < 3.141 / 10) { - double fact1 = sampleTime * pow(sigmaV, 2) + pow(sampleTime, 3) * pow(sigmaU, 2 / 3); - MatrixOperations::multiplyScalar(*identityMatrix3, fact1, *covQ1, 3, 3); - double fact2 = -(0.5 * pow(sampleTime, 2) * pow(sigmaU, 2)); - MatrixOperations::multiplyScalar(*identityMatrix3, fact2, *covQ2, 3, 3); - MatrixOperations::transpose(*covQ2, *transCovQ2, 3); - double fact3 = sampleTime * pow(sigmaU, 2); - MatrixOperations::multiplyScalar(*identityMatrix3, fact3, *covQ3, 3, 3); + double covQ11[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, + covQ12[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, + covQ22[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, + covQ12trans[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + if (normRotEst * sampleTime < M_PI / 10) { + double fact11 = pow(sigmaV, 2) * sampleTime + 1. / 3. * pow(sigmaU, 2) * pow(sampleTime, 3); + MatrixOperations::multiplyScalar(*identityMatrix3, fact11, *covQ11, 3, 3); - discProcessNoiseCov[0][0] = covQ1[0][0]; - discProcessNoiseCov[0][1] = covQ1[0][1]; - discProcessNoiseCov[0][2] = covQ1[0][2]; - discProcessNoiseCov[0][3] = covQ2[0][0]; - discProcessNoiseCov[0][4] = covQ2[0][1]; - discProcessNoiseCov[0][5] = covQ2[0][2]; - discProcessNoiseCov[1][0] = covQ1[1][0]; - discProcessNoiseCov[1][1] = covQ1[1][1]; - discProcessNoiseCov[1][2] = covQ1[1][2]; - discProcessNoiseCov[1][3] = covQ2[1][0]; - discProcessNoiseCov[1][4] = covQ2[1][1]; - discProcessNoiseCov[1][5] = covQ2[1][2]; - discProcessNoiseCov[2][0] = covQ1[2][0]; - discProcessNoiseCov[2][1] = covQ1[2][1]; - discProcessNoiseCov[2][2] = covQ1[2][2]; - discProcessNoiseCov[2][3] = covQ2[2][0]; - discProcessNoiseCov[2][4] = covQ2[2][1]; - discProcessNoiseCov[2][5] = covQ2[2][2]; - discProcessNoiseCov[3][0] = transCovQ2[0][0]; - discProcessNoiseCov[3][1] = transCovQ2[0][1]; - discProcessNoiseCov[3][2] = transCovQ2[0][2]; - discProcessNoiseCov[3][3] = covQ3[0][0]; - discProcessNoiseCov[3][4] = covQ3[0][1]; - discProcessNoiseCov[3][5] = covQ3[0][2]; - discProcessNoiseCov[4][0] = transCovQ2[1][0]; - discProcessNoiseCov[4][1] = transCovQ2[1][1]; - discProcessNoiseCov[4][2] = transCovQ2[1][2]; - discProcessNoiseCov[4][3] = covQ3[1][0]; - discProcessNoiseCov[4][4] = covQ3[1][1]; - discProcessNoiseCov[4][5] = covQ3[1][2]; - discProcessNoiseCov[5][0] = transCovQ2[2][0]; - discProcessNoiseCov[5][1] = transCovQ2[2][1]; - discProcessNoiseCov[5][2] = transCovQ2[2][2]; - discProcessNoiseCov[5][3] = covQ3[2][0]; - discProcessNoiseCov[5][4] = covQ3[2][1]; - discProcessNoiseCov[5][5] = covQ3[2][2]; + double fact12 = -(1. / 2. * pow(sigmaU, 2) * pow(sampleTime, 2)); + MatrixOperations::multiplyScalar(*identityMatrix3, fact12, *covQ12, 3, 3); + std::memcpy(*covQ12trans, *covQ12, 3 * 3 * sizeof(double)); + + double fact22 = pow(sigmaU, 2) * sampleTime; + MatrixOperations::multiplyScalar(*identityMatrix3, fact22, *covQ22, 3, 3); } else { - // double fact1 = sampleTime*pow(sigmaV,2); - double covQ11[3][3], covQ12[3][3], covQ13[3][3]; - // MatrixOperations::multiplyScalar(*identityMatrix3, fact1, *covQ1, 3, 3); - double fact1 = (2 * normRotEst + sampleTime - 2 * sin(normRotEst * sampleTime) - - pow(normRotEst, 3) / 3 * pow(sampleTime, 3)) / - pow(normRotEst, 5); - MatrixOperations::multiply(*crossRotEst, *crossRotEst, *covQ11, 3, 3, 3); - MatrixOperations::multiplyScalar(*covQ11, fact1, *covQ11, 3, 3); - double fact2 = pow(sampleTime, 3); - MatrixOperations::multiplyScalar(*identityMatrix3, fact2, *covQ12, 3, 3); - MatrixOperations::subtract(*covQ12, *covQ11, *covQ11, 3, 3); - double fact3 = sampleTime * pow(sigmaV, 2); - MatrixOperations::multiplyScalar(*identityMatrix3, fact3, *covQ13, 3, 3); - MatrixOperations::add(*covQ13, *covQ11, *covQ1, 3, 3); + double fact22 = pow(sigmaU, 2) * sampleTime; + MatrixOperations::multiplyScalar(*identityMatrix3, fact22, *covQ22, 3, 3); - double covQ21[3][3], covQ22[3][3], covQ23[3][3]; - double fact4 = - (0.5 * pow(normRotEst, 2) * pow(sampleTime, 2) + cos(normRotEst * sampleTime) - 1) / + double covQ12_0[3][3], covQ12_1[3][3], covQ12_2[3][3], covQ12_01[3][3]; + double fact12_0 = (normRotEst * sampleTime - sin(normRotEst * sampleTime) / pow(normRotEst, 3)); + MatrixOperations::multiplyScalar(*crossRotEst, fact12_0, *covQ12_0, 3, 3); + double fact12_1 = 1. / 2. * pow(sampleTime, 2); + MatrixOperations::multiplyScalar(*identityMatrix3, fact12_1, *covQ12_1, 3, 3); + double fact12_2 = + (1. / 2. * pow(normRotEst, 2) * pow(sampleTime, 2) + cos(normRotEst * sampleTime) - 1) / pow(normRotEst, 4); - MatrixOperations::multiply(*crossRotEst, *crossRotEst, *covQ21, 3, 3, 3); - MatrixOperations::multiplyScalar(*covQ21, fact4, *covQ21, 3, 3); - double fact5 = 0.5 * pow(sampleTime, 2); - MatrixOperations::multiplyScalar(*identityMatrix3, fact5, *covQ22, 3, 3); - MatrixOperations::add(*covQ22, *covQ21, *covQ21, 3, 3); - double fact6 = normRotEst * sampleTime - sin(normRotEst * sampleTime) / pow(normRotEst, 3); - MatrixOperations::multiplyScalar(*crossRotEst, fact6, *covQ23, 3, 3); - MatrixOperations::subtract(*covQ23, *covQ21, *covQ21, 3, 3); - double fact7 = pow(sigmaU, 2); - MatrixOperations::multiplyScalar(*covQ21, fact7, *covQ2, 3, 3); + MatrixOperations::multiply(*crossRotEst, *crossRotEst, *covQ12_2, 3, 3, 3); + MatrixOperations::multiplyScalar(*covQ12_2, fact12_2, *covQ12_2, 3, 3); + MatrixOperations::subtract(*covQ12_0, *covQ12_1, *covQ12_01, 3, 3); + MatrixOperations::subtract(*covQ12_01, *covQ12_2, *covQ12, 3, 3); + MatrixOperations::multiplyScalar(*covQ12, pow(sigmaU, 2), *covQ12, 3, 3); + MatrixOperations::transpose(*covQ12, *covQ12trans, 3); - MatrixOperations::multiplyScalar(*identityMatrix3, fact7, *covQ3, 3, 3); - - discProcessNoiseCov[0][0] = covQ1[0][0]; - discProcessNoiseCov[0][1] = covQ1[0][1]; - discProcessNoiseCov[0][2] = covQ1[0][2]; - discProcessNoiseCov[0][3] = covQ2[0][0]; - discProcessNoiseCov[0][4] = covQ2[0][1]; - discProcessNoiseCov[0][5] = covQ2[0][2]; - discProcessNoiseCov[1][0] = covQ1[1][0]; - discProcessNoiseCov[1][1] = covQ1[1][1]; - discProcessNoiseCov[1][2] = covQ1[1][2]; - discProcessNoiseCov[1][3] = covQ2[1][0]; - discProcessNoiseCov[1][4] = covQ2[1][1]; - discProcessNoiseCov[1][5] = covQ2[1][2]; - discProcessNoiseCov[2][0] = covQ1[2][0]; - discProcessNoiseCov[2][1] = covQ1[2][1]; - discProcessNoiseCov[2][2] = covQ1[2][2]; - discProcessNoiseCov[2][3] = covQ2[2][0]; - discProcessNoiseCov[2][4] = covQ2[2][1]; - discProcessNoiseCov[2][5] = covQ2[2][2]; - discProcessNoiseCov[3][0] = covQ2[0][0]; - discProcessNoiseCov[3][1] = covQ2[0][1]; - discProcessNoiseCov[3][2] = covQ2[0][2]; - discProcessNoiseCov[3][3] = covQ3[0][0]; - discProcessNoiseCov[3][4] = covQ3[0][1]; - discProcessNoiseCov[3][5] = covQ3[0][2]; - discProcessNoiseCov[4][0] = covQ2[1][0]; - discProcessNoiseCov[4][1] = covQ2[1][1]; - discProcessNoiseCov[4][2] = covQ2[1][2]; - discProcessNoiseCov[4][3] = covQ3[1][0]; - discProcessNoiseCov[4][4] = covQ3[1][1]; - discProcessNoiseCov[4][5] = covQ3[1][2]; - discProcessNoiseCov[5][0] = covQ2[2][0]; - discProcessNoiseCov[5][1] = covQ2[2][1]; - discProcessNoiseCov[5][2] = covQ2[2][2]; - discProcessNoiseCov[5][3] = covQ3[2][0]; - discProcessNoiseCov[5][4] = covQ3[2][1]; - discProcessNoiseCov[5][5] = covQ3[2][2]; + double covQ11_0[3][3], covQ11_1[3][3], covQ11_2[3][3], covQ11_12[3][3]; + double fact11_0 = pow(sigmaV, 2) * sampleTime; + MatrixOperations::multiplyScalar(*identityMatrix3, fact11_0, *covQ11_0, 3, 3); + double fact11_1 = 1. / 3. * pow(sampleTime, 3); + MatrixOperations::multiplyScalar(*identityMatrix3, fact11_1, *covQ11_1, 3, 3); + double fact11_2 = (2 * normRotEst * sampleTime - 2 * sin(normRotEst * sampleTime) - + 1. / 3. * pow(normRotEst, 3) * pow(sampleTime, 3)) / + pow(normRotEst, 5); + MatrixOperations::multiply(*crossRotEst, *crossRotEst, *covQ11_2, 3, 3, 3); + MatrixOperations::multiplyScalar(*covQ11_2, fact11_2, *covQ11_2, 3, 3); + MatrixOperations::subtract(*covQ11_1, *covQ11_2, *covQ11_12, 3, 3); + MatrixOperations::multiplyScalar(*covQ11_12, pow(sigmaU, 2), *covQ11_12, 3, 3); + MatrixOperations::add(*covQ11_0, *covQ11_12, *covQ11, 3, 3); } + discProcessNoiseCov[0][0] = covQ11[0][0]; + discProcessNoiseCov[0][1] = covQ11[0][1]; + discProcessNoiseCov[0][2] = covQ11[0][2]; + discProcessNoiseCov[0][3] = covQ12[0][0]; + discProcessNoiseCov[0][4] = covQ12[0][1]; + discProcessNoiseCov[0][5] = covQ12[0][2]; + discProcessNoiseCov[1][0] = covQ11[1][0]; + discProcessNoiseCov[1][1] = covQ11[1][1]; + discProcessNoiseCov[1][2] = covQ11[1][2]; + discProcessNoiseCov[1][3] = covQ12[1][0]; + discProcessNoiseCov[1][4] = covQ12[1][1]; + discProcessNoiseCov[1][5] = covQ12[1][2]; + discProcessNoiseCov[2][0] = covQ11[2][0]; + discProcessNoiseCov[2][1] = covQ11[2][1]; + discProcessNoiseCov[2][2] = covQ11[2][2]; + discProcessNoiseCov[2][3] = covQ12[2][0]; + discProcessNoiseCov[2][4] = covQ12[2][1]; + discProcessNoiseCov[2][5] = covQ12[2][2]; + discProcessNoiseCov[3][0] = covQ12trans[0][0]; + discProcessNoiseCov[3][1] = covQ12trans[0][1]; + discProcessNoiseCov[3][2] = covQ12trans[0][2]; + discProcessNoiseCov[3][3] = covQ22[0][0]; + discProcessNoiseCov[3][4] = covQ22[0][1]; + discProcessNoiseCov[3][5] = covQ22[0][2]; + discProcessNoiseCov[4][0] = covQ12trans[1][0]; + discProcessNoiseCov[4][1] = covQ12trans[1][1]; + discProcessNoiseCov[4][2] = covQ12trans[1][2]; + discProcessNoiseCov[4][3] = covQ22[1][0]; + discProcessNoiseCov[4][4] = covQ22[1][1]; + discProcessNoiseCov[4][5] = covQ22[1][2]; + discProcessNoiseCov[5][0] = covQ12trans[2][0]; + discProcessNoiseCov[5][1] = covQ12trans[2][1]; + discProcessNoiseCov[5][2] = covQ12trans[2][2]; + discProcessNoiseCov[5][3] = covQ22[2][0]; + discProcessNoiseCov[5][4] = covQ22[2][1]; + discProcessNoiseCov[5][5] = covQ22[2][2]; // State Transition Matrix phi - double phi1[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, - phi2[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, + double phi11[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, + phi12[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, phi[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}}; - double phi11[3][3], phi12[3][3]; - double fact1 = sin(normRotEst * sampleTime); - MatrixOperations::multiplyScalar(*crossRotEst, fact1, *phi11, 3, 3); - double fact2 = (1 - cos(normRotEst * sampleTime)) / pow(normRotEst, 2); - MatrixOperations::multiply(*crossRotEst, *crossRotEst, *phi12, 3, 3, 3); - MatrixOperations::multiplyScalar(*phi12, fact2, *phi12, 3, 3); - MatrixOperations::subtract(*identityMatrix3, *phi11, *phi11, 3, 3); - MatrixOperations::add(*phi11, *phi12, *phi1, 3, 3); + double phi11_1[3][3], phi11_2[3][3], phi11_01[3][3]; + double fact11_1 = sin(normRotEst * sampleTime) / normRotEst; + MatrixOperations::multiplyScalar(*crossRotEst, fact11_1, *phi11_1, 3, 3); + double fact11_2 = (1 - cos(normRotEst * sampleTime)) / pow(normRotEst, 2); + MatrixOperations::multiply(*crossRotEst, *crossRotEst, *phi11_2, 3, 3, 3); + MatrixOperations::multiplyScalar(*phi11_2, fact11_2, *phi11_2, 3, 3); + MatrixOperations::subtract(*identityMatrix3, *phi11_1, *phi11_01, 3, 3); + MatrixOperations::add(*phi11_01, *phi11_2, *phi11, 3, 3); - double phi21[3][3], phi22[3][3]; - MatrixOperations::multiplyScalar(*crossRotEst, fact2, *phi21, 3, 3); - MatrixOperations::multiplyScalar(*identityMatrix3, sampleTime, *phi22, 3, 3); - MatrixOperations::subtract(*phi21, *phi22, *phi21, 3, 3); - double fact3 = (normRotEst * sampleTime - sin(normRotEst * sampleTime) / pow(normRotEst, 3)); - MatrixOperations::multiply(*crossRotEst, *crossRotEst, *phi22, 3, 3, 3); - MatrixOperations::multiplyScalar(*phi22, fact3, *phi22, 3, 3); - MatrixOperations::subtract(*phi21, *phi22, *phi2, 3, 3); + double phi12_0[3][3], phi12_1[3][3], phi12_2[3][3], phi12_01[3][3]; + double fact12_0 = fact11_2; + MatrixOperations::multiplyScalar(*crossRotEst, fact12_0, *phi12_0, 3, 3); + MatrixOperations::multiplyScalar(*identityMatrix3, sampleTime, *phi12_1, 3, 3); + double fact12_2 = (normRotEst * sampleTime - sin(normRotEst * sampleTime) / pow(normRotEst, 3)); + MatrixOperations::multiply(*crossRotEst, *crossRotEst, *phi12_2, 3, 3, 3); + MatrixOperations::multiplyScalar(*phi12_2, fact12_2, *phi12_2, 3, 3); + MatrixOperations::subtract(*phi12_0, *phi12_1, *phi12_01, 3, 3); + MatrixOperations::subtract(*phi12_01, *phi12_2, *phi12, 3, 3); - phi[0][0] = phi1[0][0]; - phi[0][1] = phi1[0][1]; - phi[0][2] = phi1[0][2]; - phi[0][3] = phi2[0][0]; - phi[0][4] = phi2[0][1]; - phi[0][5] = phi2[0][2]; - phi[1][0] = phi1[1][0]; - phi[1][1] = phi1[1][1]; - phi[1][2] = phi1[1][2]; - phi[1][3] = phi2[1][0]; - phi[1][4] = phi2[1][1]; - phi[1][5] = phi2[1][2]; - phi[2][0] = phi1[2][0]; - phi[2][1] = phi1[2][1]; - phi[2][2] = phi1[2][2]; - phi[2][3] = phi2[2][0]; - phi[2][4] = phi2[2][1]; - phi[2][5] = phi2[2][2]; + phi[0][0] = phi11[0][0]; + phi[0][1] = phi11[0][1]; + phi[0][2] = phi11[0][2]; + phi[0][3] = phi12[0][0]; + phi[0][4] = phi12[0][1]; + phi[0][5] = phi12[0][2]; + phi[1][0] = phi11[1][0]; + phi[1][1] = phi11[1][1]; + phi[1][2] = phi11[1][2]; + phi[1][3] = phi12[1][0]; + phi[1][4] = phi12[1][1]; + phi[1][5] = phi12[1][2]; + phi[2][0] = phi11[2][0]; + phi[2][1] = phi11[2][1]; + phi[2][2] = phi11[2][2]; + phi[2][3] = phi12[2][0]; + phi[2][4] = phi12[2][1]; + phi[2][5] = phi12[2][2]; // Propagated Quaternion - double rotSin[3] = {0, 0, 0}, omega1[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double rotSin[3] = {0, 0, 0}, rotCosMat[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double rotCos = cos(0.5 * normRotEst * sampleTime); double sinFac = sin(0.5 * normRotEst * sampleTime) / normRotEst; VectorOperations::mulScalar(rotRateEst, sinFac, rotSin, 3); @@ -1148,25 +1101,26 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst( double skewSin[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; MathOperations::skewMatrix(rotSin, *skewSin); - MatrixOperations::multiplyScalar(*identityMatrix3, rotCos, *omega1, 3, 3); - MatrixOperations::subtract(*omega1, *skewSin, *omega1, 3, 3); - double omega[4][4] = {{omega1[0][0], omega1[0][1], omega1[0][2], rotSin[0]}, - {omega1[1][0], omega1[1][1], omega1[1][2], rotSin[1]}, - {omega1[2][0], omega1[2][1], omega1[2][2], rotSin[2]}, + MatrixOperations::multiplyScalar(*identityMatrix3, rotCos, *rotCosMat, 3, 3); + double subMatUL[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MatrixOperations::subtract(*rotCosMat, *skewSin, *subMatUL, 3, 3); + double omega[4][4] = {{subMatUL[0][0], subMatUL[0][1], subMatUL[0][2], rotSin[0]}, + {subMatUL[1][0], subMatUL[1][1], subMatUL[1][2], rotSin[1]}, + {subMatUL[2][0], subMatUL[2][1], subMatUL[2][2], rotSin[2]}, {-rotSin[0], -rotSin[1], -rotSin[2], rotCos}}; MatrixOperations::multiply(*omega, quatBJ, propagatedQuaternion, 4, 4, 1); // Update Covariance Matrix - double cov1[6][6], cov2[6][6], transDiscTimeMatrix[6][6], transPhi[6][6]; + double cov0[6][6], cov1[6][6], transPhi[6][6], transDiscTimeMatrix[6][6]; + MatrixOperations::transpose(*phi, *transPhi, 6); + MatrixOperations::multiply(*covMatPlus, *transPhi, *cov0, 6, 6, 6); + MatrixOperations::multiply(*phi, *cov0, *cov0, 6, 6, 6); + MatrixOperations::transpose(*discTimeMatrix, *transDiscTimeMatrix, 6); MatrixOperations::multiply(*discProcessNoiseCov, *transDiscTimeMatrix, *cov1, 6, 6, 6); MatrixOperations::multiply(*discTimeMatrix, *cov1, *cov1, 6, 6, 6); - MatrixOperations::transpose(*phi, *transPhi, 6); - MatrixOperations::multiply(*covMatPlus, *transPhi, *cov2, 6, 6, 6); - MatrixOperations::multiply(*phi, *cov2, *cov2, 6, 6, 6); - - MatrixOperations::add(*cov2, *cov1, *initialCovarianceMatrix, 6, 6); + MatrixOperations::add(*cov0, *cov1, *initialCovarianceMatrix, 6, 6); validMekf = true; // Discrete Time Step diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.h b/mission/controller/acs/MultiplicativeKalmanFilter.h index 5eb47418..79d39510 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.h +++ b/mission/controller/acs/MultiplicativeKalmanFilter.h @@ -61,7 +61,7 @@ class MultiplicativeKalmanFilter { const bool validGYRs_, const double *magneticField_, const bool validMagField_, const double *sunDir_, const bool validSS, const double *sunDirJ, const bool validSSModel, const double *magFieldJ, - const bool validMagModel, acsctrl::MekfData *mekfData); + const bool validMagModel, double sampleTime, acsctrl::MekfData *mekfData); // Declaration of Events (like init) and memberships // static const uint8_t INTERFACE_ID = CLASS_ID::MEKF; //CLASS IDS ND @@ -79,7 +79,6 @@ class MultiplicativeKalmanFilter { AcsParameters::KalmanFilterParameters *kalmanFilterParameters; double quaternion_STR_SB[4]; bool validInit; - double sampleTime = 0.1; /*States*/ double initialQuaternion[4]; /*after reset?QUEST*/ diff --git a/mission/controller/acs/Navigation.cpp b/mission/controller/acs/Navigation.cpp index 921ae604..a9de5817 100644 --- a/mission/controller/acs/Navigation.cpp +++ b/mission/controller/acs/Navigation.cpp @@ -39,7 +39,7 @@ void Navigation::useMekf(ACS::SensorValues *sensorValues, mgmDataProcessed->mgmVecTot.isValid(), susDataProcessed->susVecTot.value, susDataProcessed->susVecTot.isValid(), susDataProcessed->sunIjkModel.value, susDataProcessed->sunIjkModel.isValid(), mgmDataProcessed->magIgrfModel.value, - mgmDataProcessed->magIgrfModel.isValid(), + mgmDataProcessed->magIgrfModel.isValid(), acsParameters.onBoardParams.sampleTime, mekfData); // VALIDS FOR QUAT AND RATE ?? } else { multiplicativeKalmanFilter.init( diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 5b4ddc84..24547247 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -1,3 +1,10 @@ +/* + * SensorProcessing.cpp + * + * Created on: 7 Mar 2022 + * Author: Robin Marquardt + */ + #include "SensorProcessing.h" #include @@ -14,8 +21,7 @@ using namespace Math; -SensorProcessing::SensorProcessing(AcsParameters *acsParameters_) - : savedMgmVecTot{0, 0, 0}, validMagField(false), validGcLatitude(false) {} +SensorProcessing::SensorProcessing(AcsParameters *acsParameters_) {} SensorProcessing::~SensorProcessing() {} @@ -27,19 +33,35 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const acsctrl::GpsDataProcessed *gpsDataProcessed, const double gpsAltitude, bool gpsValid, acsctrl::MgmDataProcessed *mgmDataProcessed) { + // ---------------- IGRF- 13 Implementation here ------------------------------------------------ + double magIgrfModel[3] = {0.0, 0.0, 0.0}; + if (gpsValid) { + // Should be existing class object which will be called and modified here. + Igrf13Model igrf13; + // So the line above should not be done here. Update: Can be done here as long updated coffs + // stored in acsParameters ? + igrf13.schmidtNormalization(); + igrf13.updateCoeffGH(timeOfMgmMeasurement); + // maybe put a condition here, to only update after a full day, this + // class function has around 700 steps to perform + igrf13.magFieldComp(gpsDataProcessed->gdLongitude.value, gpsDataProcessed->gcLatitude.value, + gpsAltitude, timeOfMgmMeasurement, magIgrfModel); + } if (!mgm0valid && !mgm1valid && !mgm2valid && !mgm3valid && !mgm4valid) { { PoolReadGuard pg(mgmDataProcessed); if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(mgmDataProcessed->mgm0vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(mgmDataProcessed->mgm1vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(mgmDataProcessed->mgm2vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(mgmDataProcessed->mgm3vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(mgmDataProcessed->mgm4vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(mgmDataProcessed->mgmVecTot.value, zeroVector, 3 * sizeof(float)); - std::memcpy(mgmDataProcessed->mgmVecTotDerivative.value, zeroVector, 3 * sizeof(float)); - std::memcpy(mgmDataProcessed->magIgrfModel.value, zeroVector, 3 * sizeof(double)); + float zeroVec[3] = {0.0, 0.0, 0.0}; + std::memcpy(mgmDataProcessed->mgm0vec.value, zeroVec, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgm1vec.value, zeroVec, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgm2vec.value, zeroVec, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgm3vec.value, zeroVec, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgm4vec.value, zeroVec, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgmVecTot.value, zeroVec, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgmVecTotDerivative.value, zeroVec, 3 * sizeof(float)); mgmDataProcessed->setValidity(false, true); + std::memcpy(mgmDataProcessed->magIgrfModel.value, magIgrfModel, 3 * sizeof(double)); + mgmDataProcessed->magIgrfModel.setValid(gpsValid); } } return; @@ -134,19 +156,6 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const } timeOfSavedMagFieldEst = timeOfMgmMeasurement; - // ---------------- IGRF- 13 Implementation here ------------------------------------------------ - double magIgrfModel[3] = {0.0, 0.0, 0.0}; - if (gpsValid) { - // Should be existing class object which will be called and modified here. - Igrf13Model igrf13; - // So the line above should not be done here. Update: Can be done here as long updated coffs - // stored in acsParameters ? - igrf13.updateCoeffGH(timeOfMgmMeasurement); - // maybe put a condition here, to only update after a full day, this - // class function has around 700 steps to perform - igrf13.magFieldComp(gpsDataProcessed->gdLongitude.value, gpsDataProcessed->gcLatitude.value, - gpsAltitude, timeOfMgmMeasurement, magIgrfModel); - } { PoolReadGuard pg(mgmDataProcessed); if (pg.getReadResult() == returnvalue::OK) { @@ -182,6 +191,25 @@ void SensorProcessing::processSus( timeval timeOfSusMeasurement, const AcsParameters::SusHandlingParameters *susParameters, const AcsParameters::SunModelParameters *sunModelParameters, acsctrl::SusDataProcessed *susDataProcessed) { + /* -------- Sun Model Direction (IJK frame) ------- */ + double JD2000 = MathOperations::convertUnixToJD2000(timeOfSusMeasurement); + + // Julean Centuries + double sunIjkModel[3] = {0.0, 0.0, 0.0}; + double JC2000 = JD2000 / 36525.; + + double meanLongitude = + sunModelParameters->omega_0 + (sunModelParameters->domega * JC2000) * PI / 180.; + double meanAnomaly = (sunModelParameters->m_0 + sunModelParameters->dm * JC2000) * PI / 180.; + + double eclipticLongitude = meanLongitude + sunModelParameters->p1 * sin(meanAnomaly) + + sunModelParameters->p2 * sin(2 * meanAnomaly); + + double epsilon = sunModelParameters->e - (sunModelParameters->e1) * JC2000; + + sunIjkModel[0] = cos(eclipticLongitude); + sunIjkModel[1] = sin(eclipticLongitude) * cos(epsilon); + sunIjkModel[2] = sin(eclipticLongitude) * sin(epsilon); if (sus0valid) { sus0valid = susConverter.checkSunSensorData(sus0Value); } @@ -224,22 +252,24 @@ void SensorProcessing::processSus( { PoolReadGuard pg(susDataProcessed); if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susDataProcessed->sus0vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus1vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus2vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus3vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus4vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus5vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus6vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus7vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus8vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus9vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus10vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus11vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(susDataProcessed->susVecTot.value, zeroVector, 3 * sizeof(float)); - std::memcpy(susDataProcessed->susVecTotDerivative.value, zeroVector, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sunIjkModel.value, zeroVector, 3 * sizeof(double)); + float zeroVec[3] = {0.0, 0.0, 0.0}; + std::memcpy(susDataProcessed->sus0vec.value, zeroVec, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus1vec.value, zeroVec, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus2vec.value, zeroVec, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus3vec.value, zeroVec, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus4vec.value, zeroVec, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus5vec.value, zeroVec, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus6vec.value, zeroVec, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus7vec.value, zeroVec, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus8vec.value, zeroVec, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus9vec.value, zeroVec, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus10vec.value, zeroVec, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus11vec.value, zeroVec, 3 * sizeof(float)); + std::memcpy(susDataProcessed->susVecTot.value, zeroVec, 3 * sizeof(float)); + std::memcpy(susDataProcessed->susVecTotDerivative.value, zeroVec, 3 * sizeof(float)); susDataProcessed->setValidity(false, true); + std::memcpy(susDataProcessed->sunIjkModel.value, sunIjkModel, 3 * sizeof(double)); + susDataProcessed->sunIjkModel.setValid(true); } } return; @@ -258,16 +288,6 @@ void SensorProcessing::processSus( susParameters->sus0coeffBeta), sus0VecBody, 3, 3, 1); } - { - PoolReadGuard pg(susDataProcessed); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susDataProcessed->sus0vec.value, sus0VecBody, 3 * sizeof(float)); - susDataProcessed->sus0vec.setValid(sus0valid); - if (!sus0valid) { - std::memcpy(susDataProcessed->sus0vec.value, zeroVector, 3 * sizeof(float)); - } - } - } if (sus1valid) { MatrixOperations::multiply( susParameters->sus1orientationMatrix[0], @@ -275,16 +295,6 @@ void SensorProcessing::processSus( susParameters->sus1coeffBeta), sus1VecBody, 3, 3, 1); } - { - PoolReadGuard pg(susDataProcessed); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susDataProcessed->sus1vec.value, sus1VecBody, 3 * sizeof(float)); - susDataProcessed->sus1vec.setValid(sus1valid); - if (!sus1valid) { - std::memcpy(susDataProcessed->sus1vec.value, zeroVector, 3 * sizeof(float)); - } - } - } if (sus2valid) { MatrixOperations::multiply( susParameters->sus2orientationMatrix[0], @@ -389,30 +399,10 @@ void SensorProcessing::processSus( for (uint8_t i = 0; i < 3; i++) { susVecTotDerivative[i] = (susVecTot[i] - savedSusVecTot[i]) / timeDiff; savedSusVecTot[i] = susVecTot[i]; + susVecTotDerivativeValid = true; } } timeOfSavedSusDirEst = timeOfSusMeasurement; - - /* -------- Sun Model Direction (IJK frame) ------- */ - // if (useSunModel) eventuell - double JD2000 = MathOperations::convertUnixToJD2000(timeOfSusMeasurement); - - // Julean Centuries - double sunIjkModel[3] = {0.0, 0.0, 0.0}; - double JC2000 = JD2000 / 36525; - - double meanLongitude = - (sunModelParameters->omega_0 + (sunModelParameters->domega) * JC2000) * PI / 180; - double meanAnomaly = (sunModelParameters->m_0 + sunModelParameters->dm * JC2000) * PI / 180.; - - double eclipticLongitude = meanLongitude + sunModelParameters->p1 * sin(meanAnomaly) + - sunModelParameters->p2 * sin(2 * meanAnomaly); - - double epsilon = sunModelParameters->e - (sunModelParameters->e1) * JC2000; - - sunIjkModel[0] = cos(eclipticLongitude); - sunIjkModel[1] = sin(eclipticLongitude) * cos(epsilon); - sunIjkModel[2] = sin(eclipticLongitude) * sin(epsilon); { PoolReadGuard pg(susDataProcessed); if (pg.getReadResult() == returnvalue::OK) { @@ -469,6 +459,7 @@ void SensorProcessing::processGyr( { PoolReadGuard pg(gyrDataProcessed); if (pg.getReadResult() == returnvalue::OK) { + double zeroVector[3] = {0.0, 0.0, 0.0}; std::memcpy(gyrDataProcessed->gyr0vec.value, zeroVector, 3 * sizeof(double)); std::memcpy(gyrDataProcessed->gyr1vec.value, zeroVector, 3 * sizeof(double)); std::memcpy(gyrDataProcessed->gyr2vec.value, zeroVector, 3 * sizeof(double)); @@ -488,6 +479,8 @@ void SensorProcessing::processGyr( const double gyr0Value[3] = {gyr0axXvalue, gyr0axYvalue, gyr0axZvalue}; MatrixOperations::multiply(gyrParameters->gyr0orientationMatrix[0], gyr0Value, gyr0ValueBody, 3, 3, 1); + VectorOperations::subtract(gyr0ValueBody, gyrParameters->gyr0bias, gyr0ValueBody, 3); + VectorOperations::mulScalar(gyr0ValueBody, M_PI / 180, gyr0ValueBody, 3); for (uint8_t i = 0; i < 3; i++) { sensorFusionNumerator[i] += gyr0ValueBody[i] / gyrParameters->gyr02variance[i]; sensorFusionDenominator[i] += 1 / gyrParameters->gyr02variance[i]; @@ -497,6 +490,8 @@ void SensorProcessing::processGyr( const double gyr1Value[3] = {gyr1axXvalue, gyr1axYvalue, gyr1axZvalue}; MatrixOperations::multiply(gyrParameters->gyr1orientationMatrix[0], gyr1Value, gyr1ValueBody, 3, 3, 1); + VectorOperations::subtract(gyr1ValueBody, gyrParameters->gyr1bias, gyr1ValueBody, 3); + VectorOperations::mulScalar(gyr1ValueBody, M_PI / 180, gyr1ValueBody, 3); for (uint8_t i = 0; i < 3; i++) { sensorFusionNumerator[i] += gyr1ValueBody[i] / gyrParameters->gyr13variance[i]; sensorFusionDenominator[i] += 1 / gyrParameters->gyr13variance[i]; @@ -506,6 +501,8 @@ void SensorProcessing::processGyr( const double gyr2Value[3] = {gyr2axXvalue, gyr2axYvalue, gyr2axZvalue}; MatrixOperations::multiply(gyrParameters->gyr2orientationMatrix[0], gyr2Value, gyr2ValueBody, 3, 3, 1); + VectorOperations::subtract(gyr2ValueBody, gyrParameters->gyr2bias, gyr2ValueBody, 3); + VectorOperations::mulScalar(gyr2ValueBody, M_PI / 180, gyr2ValueBody, 3); for (uint8_t i = 0; i < 3; i++) { sensorFusionNumerator[i] += gyr2ValueBody[i] / gyrParameters->gyr02variance[i]; sensorFusionDenominator[i] += 1 / gyrParameters->gyr02variance[i]; @@ -515,6 +512,8 @@ void SensorProcessing::processGyr( const double gyr3Value[3] = {gyr3axXvalue, gyr3axYvalue, gyr3axZvalue}; MatrixOperations::multiply(gyrParameters->gyr3orientationMatrix[0], gyr3Value, gyr3ValueBody, 3, 3, 1); + VectorOperations::subtract(gyr3ValueBody, gyrParameters->gyr3bias, gyr3ValueBody, 3); + VectorOperations::mulScalar(gyr3ValueBody, M_PI / 180, gyr3ValueBody, 3); for (uint8_t i = 0; i < 3; i++) { sensorFusionNumerator[i] += gyr3ValueBody[i] / gyrParameters->gyr13variance[i]; sensorFusionDenominator[i] += 1 / gyrParameters->gyr13variance[i]; @@ -525,7 +524,7 @@ void SensorProcessing::processGyr( // take ADIS measurements, if both avail // if just one ADIS measurement avail, perform sensor fusion double gyrVecTot[3] = {0.0, 0.0, 0.0}; - if ((gyr0valid && gyr2valid) && gyrParameters->preferAdis == gyrParameters->PreferAdis::YES) { + if ((gyr0valid && gyr2valid) && gyrParameters->preferAdis == true) { double gyr02ValuesSum[3]; VectorOperations::add(gyr0ValueBody, gyr2ValueBody, gyr02ValuesSum, 3); VectorOperations::mulScalar(gyr02ValuesSum, .5, gyrVecTot, 3); @@ -552,30 +551,45 @@ void SensorProcessing::processGyr( } } -void SensorProcessing::processGps(const double gps0latitude, const double gps0longitude, +void SensorProcessing::processGps(const double gpsLatitude, const double gpsLongitude, + const double gpsAltitude, const double gpsUnixSeconds, const bool validGps, + const AcsParameters::GpsParameters *gpsParameters, acsctrl::GpsDataProcessed *gpsDataProcessed) { // name to convert not process - double gdLongitude, gcLatitude; + double gdLongitude = 0, gcLatitude = 0, posSatE[3] = {0, 0, 0}, gpsVelocityE[3] = {0, 0, 0}; if (validGps) { // Transforming from Degree to Radians and calculation geocentric lattitude from geodetic - gdLongitude = gps0longitude * PI / 180; - double latitudeRad = gps0latitude * PI / 180; + gdLongitude = gpsLongitude * PI / 180.; + double latitudeRad = gpsLatitude * PI / 180.; double eccentricityWgs84 = 0.0818195; double factor = 1 - pow(eccentricityWgs84, 2); gcLatitude = atan(factor * tan(latitudeRad)); - // validGcLatitude = true; + + // Calculation of the satellite velocity in earth fixed frame + double posSatE[3] = {0, 0, 0}, deltaDistance[3] = {0, 0, 0}, gpsVelocityE[3] = {0, 0, 0}; + MathOperations::cartesianFromLatLongAlt(latitudeRad, gdLongitude, gpsAltitude, posSatE); + if (validSavedPosSatE && + (gpsUnixSeconds - timeOfSavedPosSatE) < (gpsParameters->timeDiffVelocityMax)) { + VectorOperations::subtract(posSatE, savedPosSatE, deltaDistance, 3); + double timeDiffGpsMeas = gpsUnixSeconds - timeOfSavedPosSatE; + VectorOperations::mulScalar(deltaDistance, 1. / timeDiffGpsMeas, gpsVelocityE, 3); + } + savedPosSatE[0] = posSatE[0]; + savedPosSatE[1] = posSatE[1]; + savedPosSatE[2] = posSatE[2]; + + timeOfSavedPosSatE = gpsUnixSeconds; + validSavedPosSatE = true; } { PoolReadGuard pg(gpsDataProcessed); if (pg.getReadResult() == returnvalue::OK) { gpsDataProcessed->gdLongitude.value = gdLongitude; gpsDataProcessed->gcLatitude.value = gcLatitude; - gpsDataProcessed->setValidity(validGps, validGps); - if (!validGps) { - gpsDataProcessed->gdLongitude.value = 0.0; - gpsDataProcessed->gcLatitude.value = 0.0; - } + std::memcpy(gpsDataProcessed->gpsPosition.value, posSatE, 3 * sizeof(double)); + std::memcpy(gpsDataProcessed->gpsVelocity.value, gpsVelocityE, 3 * sizeof(double)); + gpsDataProcessed->setValidity(validGps, true); } } } @@ -587,10 +601,12 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues, acsctrl::GpsDataProcessed *gpsDataProcessed, const AcsParameters *acsParameters) { sensorValues->update(); - processGps(sensorValues->gpsSet.latitude.value, sensorValues->gpsSet.longitude.value, - (sensorValues->gpsSet.latitude.isValid() && sensorValues->gpsSet.longitude.isValid() && - sensorValues->gpsSet.altitude.isValid()), - gpsDataProcessed); + processGps( + sensorValues->gpsSet.latitude.value, sensorValues->gpsSet.longitude.value, + sensorValues->gpsSet.altitude.value, sensorValues->gpsSet.unixSeconds.value, + (sensorValues->gpsSet.latitude.isValid() && sensorValues->gpsSet.longitude.isValid() && + sensorValues->gpsSet.altitude.isValid() && sensorValues->gpsSet.unixSeconds.isValid()), + &acsParameters->gpsParameters, gpsDataProcessed); processMgm(sensorValues->mgm0Lis3Set.fieldStrengths.value, sensorValues->mgm0Lis3Set.fieldStrengths.isValid(), diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index 61bd7a96..49c659b0 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -65,17 +65,20 @@ class SensorProcessing { void processStr(); - void processGps(const double gps0latitude, const double gps0longitude, const bool validGps, + void processGps(const double gpsLatitude, const double gpsLongitude, const double gpsAltitude, + const double gpsUnixSeconds, const bool validGps, + const AcsParameters::GpsParameters *gpsParameters, acsctrl::GpsDataProcessed *gpsDataProcessed); - double savedMgmVecTot[3]; + double savedMgmVecTot[3] = {0.0, 0.0, 0.0}; timeval timeOfSavedMagFieldEst; - double savedSusVecTot[3]; + double savedSusVecTot[3] = {0.0, 0.0, 0.0}; timeval timeOfSavedSusDirEst; - bool validMagField; - bool validGcLatitude; + bool validMagField = false; - const float zeroVector[3] = {0.0, 0.0, 0.0}; + double savedPosSatE[3] = {0.0, 0.0, 0.0}; + double timeOfSavedPosSatE = 0.0; + bool validSavedPosSatE = false; SusConverter susConverter; AcsParameters acsParameters; diff --git a/mission/controller/acs/SensorValues.h b/mission/controller/acs/SensorValues.h index ef897207..ec1795fc 100644 --- a/mission/controller/acs/SensorValues.h +++ b/mission/controller/acs/SensorValues.h @@ -1,12 +1,12 @@ #ifndef SENSORVALUES_H_ #define SENSORVALUES_H_ +#include "fsfw_hal/devicehandlers/GyroL3GD20Handler.h" #include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h" #include "fsfw_hal/devicehandlers/MgmRM3100Handler.h" #include "linux/devices/devicedefinitions/StarTrackerDefinitions.h" #include "mission/devices/devicedefinitions/GPSDefinitions.h" #include "mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h" -#include "mission/devices/devicedefinitions/GyroL3GD20Definitions.h" #include "mission/devices/devicedefinitions/RwDefinitions.h" #include "mission/devices/devicedefinitions/SusDefinitions.h" #include "mission/devices/devicedefinitions/imtqHandlerDefinitions.h" diff --git a/mission/controller/acs/control/Detumble.cpp b/mission/controller/acs/control/Detumble.cpp index 4c8217dc..705bf599 100644 --- a/mission/controller/acs/control/Detumble.cpp +++ b/mission/controller/acs/control/Detumble.cpp @@ -22,7 +22,7 @@ Detumble::Detumble(AcsParameters *acsParameters_) { loadAcsParameters(acsParamet Detumble::~Detumble() {} void Detumble::loadAcsParameters(AcsParameters *acsParameters_) { - detumbleCtrlParameters = &(acsParameters_->detumbleCtrlParameters); + detumbleParameter = &(acsParameters_->detumbleParameter); magnetorquesParameter = &(acsParameters_->magnetorquesParameter); } @@ -31,7 +31,7 @@ ReturnValue_t Detumble::bDotLaw(const double *magRate, const bool magRateValid, if (!magRateValid || !magFieldValid) { return DETUMBLE_NO_SENSORDATA; } - double gain = detumbleCtrlParameters->gainD; + double gain = detumbleParameter->gainD; double factor = -gain / pow(VectorOperations::norm(magField, 3), 2); VectorOperations::mulScalar(magRate, factor, magMom, 3); return returnvalue::OK; @@ -50,3 +50,15 @@ ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool magRateVal return returnvalue::OK; } + +ReturnValue_t Detumble::bDotLawGyro(const double *satRate, const bool *satRateValid, + const double *magField, const bool *magFieldValid, + double *magMom) { + if (!satRateValid || !magFieldValid) { + return DETUMBLE_NO_SENSORDATA; + } + double gain = detumbleParameter->gainD; + double factor = -gain / pow(VectorOperations::norm(magField, 3), 2); + VectorOperations::mulScalar(satRate, factor, magMom, 3); + return returnvalue::OK; +} diff --git a/mission/controller/acs/control/Detumble.h b/mission/controller/acs/control/Detumble.h index bea9b1bf..6a3e6eaa 100644 --- a/mission/controller/acs/control/Detumble.h +++ b/mission/controller/acs/control/Detumble.h @@ -25,7 +25,7 @@ class Detumble { static const uint8_t INTERFACE_ID = CLASS_ID::DETUMBLE; static const ReturnValue_t DETUMBLE_NO_SENSORDATA = MAKE_RETURN_CODE(0x01); - /* @brief: Load AcsParameters für this class + /* @brief: Load AcsParameters for this class * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters */ void loadAcsParameters(AcsParameters *acsParameters_); @@ -35,8 +35,13 @@ class Detumble { ReturnValue_t bangbangLaw(const double *magRate, const bool magRateValid, double *magMom); + ReturnValue_t bangbangLaw(const double *magRate, const bool *magRateValid, double *magMom); + + ReturnValue_t bDotLawGyro(const double *satRate, const bool *satRateValid, const double *magField, + const bool *magFieldValid, double *magMom); + private: - AcsParameters::DetumbleCtrlParameters *detumbleCtrlParameters; + AcsParameters::DetumbleParameter *detumbleParameter; AcsParameters::MagnetorquesParameter *magnetorquesParameter; }; diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 07b595ec..352c0319 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -21,21 +21,21 @@ PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { loadAcsParameters(acsParameter PtgCtrl::~PtgCtrl() {} void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_) { - pointingModeControllerParameters = &(acsParameters_->targetModeControllerParameters); inertiaEIVE = &(acsParameters_->inertiaEIVE); rwHandlingParameters = &(acsParameters_->rwHandlingParameters); rwMatrices = &(acsParameters_->rwMatrices); } -void PtgCtrl::ptgGroundstation(const double mode, const double *qError, const double *deltaRate, - const double *rwPseudoInv, double *torqueRws) { +void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, + const double *qError, const double *deltaRate, const double *rwPseudoInv, + double *torqueRws) { //------------------------------------------------------------------------------------------------ // Compute gain matrix K and P matrix //------------------------------------------------------------------------------------------------ - double om = pointingModeControllerParameters->om; - double zeta = pointingModeControllerParameters->zeta; - double qErrorMin = pointingModeControllerParameters->qiMin; - double omMax = pointingModeControllerParameters->omMax; + double om = pointingLawParameters->om; + double zeta = pointingLawParameters->zeta; + double qErrorMin = pointingLawParameters->qiMin; + double omMax = pointingLawParameters->omMax; double cInt = 2 * om * zeta; double kInt = 2 * pow(om, 2); @@ -104,12 +104,14 @@ void PtgCtrl::ptgGroundstation(const double mode, const double *qError, const do double torque[3] = {0, 0, 0}; VectorOperations::add(torqueRate, torqueQuat, torque, 3); MatrixOperations::multiply(rwPseudoInv, torque, torqueRws, 4, 3, 1); + VectorOperations::mulScalar(torqueRws, -1, torqueRws, 4); } -void PtgCtrl::ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double *satRate, +void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters, + double *magFieldEst, bool magFieldEstValid, double *satRate, int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2, int32_t *speedRw3, double *mgtDpDes) { - if (!(magFieldEstValid) || !(pointingModeControllerParameters->desatOn)) { + if (!(magFieldEstValid) || !(pointingLawParameters->desatOn)) { mgtDpDes[0] = 0; mgtDpDes[1] = 0; mgtDpDes[2] = 0; @@ -127,17 +129,18 @@ void PtgCtrl::ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double VectorOperations::add(momentumSat, momentumRw, momentumTotal, 3); // calculating momentum error double deltaMomentum[3] = {0, 0, 0}; - VectorOperations::subtract( - momentumTotal, pointingModeControllerParameters->desatMomentumRef, deltaMomentum, 3); + VectorOperations::subtract(momentumTotal, pointingLawParameters->desatMomentumRef, + deltaMomentum, 3); // resulting magnetic dipole command double crossMomentumMagField[3] = {0, 0, 0}; VectorOperations::cross(deltaMomentum, magFieldEst, crossMomentumMagField); double normMag = VectorOperations::norm(magFieldEst, 3), factor = 0; - factor = (pointingModeControllerParameters->deSatGainFactor) / normMag; + factor = (pointingLawParameters->deSatGainFactor) / normMag; VectorOperations::mulScalar(crossMomentumMagField, factor, mgtDpDes, 3); } -void PtgCtrl::ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1, +void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters, + const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs) { double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3}; double wheelMomentum[4] = {0, 0, 0, 0}; @@ -149,7 +152,7 @@ void PtgCtrl::ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1, VectorOperations::subtract(speedRws, rpmOffset, diffRwSpeed, 4); VectorOperations::mulScalar(diffRwSpeed, rwHandlingParameters->inertiaWheel, wheelMomentum, 4); - double gainNs = pointingModeControllerParameters->gainNullspace; + double gainNs = pointingLawParameters->gainNullspace; double nullSpaceMatrix[4][4] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; MathOperations::vecTransposeVecMatrix(rwMatrices->nullspace, rwMatrices->nullspace, *nullSpaceMatrix, 4); @@ -157,3 +160,32 @@ void PtgCtrl::ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1, VectorOperations::mulScalar(rwTrqNs, gainNs, rwTrqNs, 4); VectorOperations::mulScalar(rwTrqNs, -1, rwTrqNs, 4); } + +void PtgCtrl::rwAntistiction(const bool *rwAvailable, const int32_t *omegaRw, + double *torqueCommand) { + for (uint8_t i = 0; i < 4; i++) { + if (rwAvailable[i]) { + if (torqueMemory[i] != 0) { + if ((omegaRw[i] * torqueMemory[i]) > rwHandlingParameters->stictionReleaseSpeed) { + torqueMemory[i] = 0; + } else { + torqueCommand[i] = torqueMemory[i] * rwHandlingParameters->stictionTorque; + } + } else { + if ((omegaRw[i] < rwHandlingParameters->stictionSpeed) && + (omegaRw[i] > -rwHandlingParameters->stictionSpeed)) { + if (omegaRw[i] < omegaMemory[i]) { + torqueMemory[i] = -1; + } else { + torqueMemory[i] = 1; + } + + torqueCommand[i] = torqueMemory[i] * rwHandlingParameters->stictionTorque; + } + } + } else { + torqueMemory[i] = 0; + } + omegaMemory[i] = omegaRw[i]; + } +} diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index 64b4110c..559216f7 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -33,7 +33,7 @@ class PtgCtrl { static const uint8_t INTERFACE_ID = CLASS_ID::PTG; static const ReturnValue_t PTGCTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01); - /* @brief: Load AcsParameters für this class + /* @brief: Load AcsParameters for this class * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters */ void loadAcsParameters(AcsParameters *acsParameters_); @@ -41,21 +41,32 @@ class PtgCtrl { /* @brief: Calculates the needed torque for the pointing control mechanism * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters */ - void ptgGroundstation(const double mode, const double *qError, const double *deltaRate, - const double *rwPseudoInv, double *torqueRws); + void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError, + const double *deltaRate, const double *rwPseudoInv, double *torqueRws); - void ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double *satRate, + void ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters, + double *magFieldEst, bool magFieldEstValid, double *satRate, int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2, int32_t *speedRw3, double *mgtDpDes); - void ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2, + void ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters, + const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs); + /* @brief: Commands the stiction torque in case wheel speed is to low + * @param: rwAvailable Boolean Flag for all reaction wheels + * omegaRw current wheel speed of reaction wheels + * torqueCommand modified torque after antistiction + */ + void rwAntistiction(const bool *rwAvailable, const int32_t *omegaRw, double *torqueCommand); + private: - AcsParameters::PointingModeControllerParameters *pointingModeControllerParameters; AcsParameters::RwHandlingParameters *rwHandlingParameters; AcsParameters::InertiaEIVE *inertiaEIVE; AcsParameters::RwMatrices *rwMatrices; + + double torqueMemory[4] = {0, 0, 0, 0}; + double omegaMemory[4] = {0, 0, 0, 0}; }; #endif /* ACS_CONTROL_PTGCTRL_H_ */ diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp index 54ae27ee..aa04cbb6 100644 --- a/mission/controller/acs/control/SafeCtrl.cpp +++ b/mission/controller/acs/control/SafeCtrl.cpp @@ -87,7 +87,7 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid, return returnvalue::OK; } -// Will be the version in worst case scenario in event of no working MEKF (nor RMUs) +// Will be the version in worst case scenario in event of no working MEKF (nor GYRs) void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, double *sunRateB, bool sunRateBValid, double *magFieldB, bool magFieldBValid, double *magRateB, bool magRateBValid, double *sunDirRef, @@ -127,20 +127,17 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, doubl VectorOperations::mulScalar(estSatRate, 0.5, estSatRate, 3); /* Only valid if angle between sun direction and magnetic field direction - is sufficiently large */ - - double sinAngle = 0; - sinAngle = sin(acos(cos(cosAngleSunMag))); - - if (!(sinAngle > sin(safeModeControllerParameters->sunMagAngleMin * M_PI / 180))) { + * is sufficiently large */ + double angleSunMag = acos(cosAngleSunMag); + if (angleSunMag < safeModeControllerParameters->sunMagAngleMin) { return; } - // Rate for Torque Calculation + // Rate for Torque Calculation double diffRate[3] = {0, 0, 0}; /* ADD TO MONITORING */ VectorOperations::subtract(estSatRate, satRateRef, diffRate, 3); - // Torque Align calculation + // Torque Align calculation double kRateNoMekf = 0, kAlignNoMekf = 0; kRateNoMekf = safeModeControllerParameters->k_rate_no_mekf; kAlignNoMekf = safeModeControllerParameters->k_align_no_mekf; diff --git a/mission/controller/acs/util/MathOperations.h b/mission/controller/acs/util/MathOperations.h index 6484a72d..b8d1fa4d 100644 --- a/mission/controller/acs/util/MathOperations.h +++ b/mission/controller/acs/util/MathOperations.h @@ -96,8 +96,16 @@ class MathOperations { static void cartesianFromLatLongAlt(const T1 lat, const T1 longi, const T1 alt, T2 *cartesianOutput) { - double radiusPolar = 6378137; - double radiusEqua = 6356752.314; + /* @brief: cartesianFromLatLongAlt() - calculates cartesian coordinates in ECEF from latitude, + * longitude and altitude + * @param: lat geodetic latitude [rad] + * longi longitude [rad] + * alt altitude [m] + * cartesianOutput Cartesian Coordinates in ECEF (3x1) + * @source: Fundamentals of Spacecraft Attitude Determination and Control, P.34ff + * Landis Markley and John L. Crassidis*/ + double radiusPolar = 6356752.314; + double radiusEqua = 6378137; double eccentricity = sqrt(1 - pow(radiusPolar, 2) / pow(radiusEqua, 2)); double auxRadius = radiusEqua / sqrt(1 - pow(eccentricity, 2) * pow(sin(lat), 2)); @@ -106,13 +114,13 @@ class MathOperations { cartesianOutput[1] = (auxRadius + alt) * cos(lat) * sin(longi); cartesianOutput[2] = ((1 - pow(eccentricity, 2)) * auxRadius + alt) * sin(lat); } - - /* @brief: dcmEJ() - calculates the transformation matrix between ECEF and ECI frame - * @param: time Current time - * outputDcmEJ Transformation matrix from ECI (J) to ECEF (E) [3][3] - * @source: Fundamentals of Spacecraft Attitude Determination and Control, P.32ff - * Landis Markley and John L. Crassidis*/ - static void dcmEJ(timeval time, T1 *outputDcmEJ) { + static void dcmEJ(timeval time, T1 *outputDcmEJ, T1 *outputDotDcmEJ) { + /* @brief: dcmEJ() - calculates the transformation matrix between ECEF and ECI frame + * @param: time Current time + * outputDcmEJ Transformation matrix from ECI (J) to ECEF (E) [3][3] + * outputDotDcmEJ Derivative of transformation matrix [3][3] + * @source: Fundamentals of Spacecraft Attitude Determination and Control, P.32ff + * Landis Markley and John L. Crassidis*/ double JD2000Floor = 0; double JD2000 = convertUnixToJD2000(time); // Getting Julian Century from Day start : JD (Y,M,D,0,0,0) @@ -143,6 +151,16 @@ class MathOperations { outputDcmEJ[6] = 0; outputDcmEJ[7] = 0; outputDcmEJ[8] = 1; + + // Derivative of dmcEJ WITHOUT PRECISSION AND NUTATION + double dcmEJCalc[3][3] = {{outputDcmEJ[0], outputDcmEJ[1], outputDcmEJ[2]}, + {outputDcmEJ[3], outputDcmEJ[4], outputDcmEJ[5]}, + {outputDcmEJ[6], outputDcmEJ[7], outputDcmEJ[8]}}; + double dcmDot[3][3] = {{0, 1, 0}, {-1, 0, 0}, {0, 0, 0}}; + double omegaEarth = 0.000072921158553; + double dotDcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MatrixOperations::multiply(*dcmDot, *dcmEJCalc, *dotDcmEJ, 3, 3, 3); + MatrixOperations::multiplyScalar(*dotDcmEJ, omegaEarth, outputDotDcmEJ, 3, 3); } /* @brief: ecfToEciWithNutPre() - calculates the transformation matrix between ECEF and ECI frame @@ -215,7 +233,7 @@ class MathOperations { precession[2][2] = cos(theta2); //------------------------------------------------------------------------------------- - // Calculation of Transformation from earth Nutation size + // Calculation of Transformation from earth Nutation N //------------------------------------------------------------------------------------- double nutation[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; // lunar asc node @@ -230,7 +248,6 @@ class MathOperations { // % true obliquity of the ecliptic eps p.71 (simplified) double e = 23.43929111 * PI / 180 - 46.8150 / 3600 * JC2000TT * PI / 180; - ; nutation[0][0] = cos(dp); nutation[1][0] = cos(e + de) * sin(dp); @@ -260,10 +277,9 @@ class MathOperations { MatrixOperations::multiply(*nutationPrecession, *thetaDot, outputDotDcmEJ, 3, 3, 3); } - static void inverseMatrixDimThree(const T1 *matrix, T1 *output) { int i, j; - double determinant; + double determinant = 0; double mat[3][3] = {{matrix[0], matrix[1], matrix[2]}, {matrix[3], matrix[4], matrix[5]}, {matrix[6], matrix[7], matrix[8]}}; @@ -272,8 +288,8 @@ class MathOperations { determinant = determinant + (mat[0][i] * (mat[1][(i + 1) % 3] * mat[2][(i + 2) % 3] - mat[1][(i + 2) % 3] * mat[2][(i + 1) % 3])); } - // cout<<"\size\ndeterminant: "< { lp_var_t gcLatitude = lp_var_t(sid.objectId, GC_LATITUDE, this); lp_var_t gdLongitude = lp_var_t(sid.objectId, GD_LONGITUDE, this); + lp_vec_t gpsPosition = lp_vec_t(sid.objectId, GPS_POSITION, this); + lp_vec_t gpsVelocity = lp_vec_t(sid.objectId, GPS_VELOCITY, this); private: }; diff --git a/mission/system/tree/acsModeTree.cpp b/mission/system/tree/acsModeTree.cpp index fe87dff5..0474d0a1 100644 --- a/mission/system/tree/acsModeTree.cpp +++ b/mission/system/tree/acsModeTree.cpp @@ -55,13 +55,13 @@ auto ACS_TABLE_SAFE_TRANS_0 = auto ACS_TABLE_SAFE_TRANS_1 = std::make_pair((acs::AcsMode::SAFE << 24) | 3, FixedArrayList()); -auto ACS_SEQUENCE_IDLE = std::make_pair(acs::AcsMode::IDLE, FixedArrayList()); +auto ACS_SEQUENCE_IDLE = std::make_pair(acs::AcsMode::PTG_IDLE, FixedArrayList()); auto ACS_TABLE_IDLE_TGT = - std::make_pair((acs::AcsMode::IDLE << 24) | 1, FixedArrayList()); + std::make_pair((acs::AcsMode::PTG_IDLE << 24) | 1, FixedArrayList()); auto ACS_TABLE_IDLE_TRANS_0 = - std::make_pair((acs::AcsMode::IDLE << 24) | 2, FixedArrayList()); + std::make_pair((acs::AcsMode::PTG_IDLE << 24) | 2, FixedArrayList()); auto ACS_TABLE_IDLE_TRANS_1 = - std::make_pair((acs::AcsMode::IDLE << 24) | 3, FixedArrayList()); + std::make_pair((acs::AcsMode::PTG_IDLE << 24) | 3, FixedArrayList()); auto ACS_TABLE_PTG_TRANS_0 = std::make_pair((acs::AcsMode::PTG_TARGET << 24) | 2, FixedArrayList()); @@ -81,18 +81,18 @@ auto ACS_TABLE_PTG_TARGET_GS_TRANS_1 = std::make_pair((acs::AcsMode::PTG_TARGET_GS << 24) | 3, FixedArrayList()); auto ACS_SEQUENCE_PTG_TARGET_NADIR = - std::make_pair(acs::AcsMode::PTG_TARGET_NADIR, FixedArrayList()); -auto ACS_TABLE_PTG_TARGET_NADIR_TGT = std::make_pair((acs::AcsMode::PTG_TARGET_NADIR << 24) | 1, - FixedArrayList()); -auto ACS_TABLE_PTG_TARGET_NADIR_TRANS_1 = std::make_pair( - (acs::AcsMode::PTG_TARGET_NADIR << 24) | 3, FixedArrayList()); + std::make_pair(acs::AcsMode::PTG_NADIR, FixedArrayList()); +auto ACS_TABLE_PTG_TARGET_NADIR_TGT = + std::make_pair((acs::AcsMode::PTG_NADIR << 24) | 1, FixedArrayList()); +auto ACS_TABLE_PTG_TARGET_NADIR_TRANS_1 = + std::make_pair((acs::AcsMode::PTG_NADIR << 24) | 3, FixedArrayList()); auto ACS_SEQUENCE_PTG_TARGET_INERTIAL = - std::make_pair(acs::AcsMode::PTG_TARGET_INERTIAL, FixedArrayList()); -auto ACS_TABLE_PTG_TARGET_INERTIAL_TGT = std::make_pair( - (acs::AcsMode::PTG_TARGET_INERTIAL << 24) | 1, FixedArrayList()); -auto ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1 = std::make_pair( - (acs::AcsMode::PTG_TARGET_INERTIAL << 24) | 3, FixedArrayList()); + std::make_pair(acs::AcsMode::PTG_INERTIAL, FixedArrayList()); +auto ACS_TABLE_PTG_TARGET_INERTIAL_TGT = + std::make_pair((acs::AcsMode::PTG_INERTIAL << 24) | 1, FixedArrayList()); +auto ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1 = + std::make_pair((acs::AcsMode::PTG_INERTIAL << 24) | 3, FixedArrayList()); void satsystem::acs::init() { ModeListEntry entry; @@ -291,7 +291,7 @@ void buildIdleSequence(Subsystem& ss, ModeListEntry& eh) { check(sequence.insert(eh), ctxc); }; // Build IDLE target - iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::IDLE, ACS_TABLE_IDLE_TGT.second); + iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_IDLE, ACS_TABLE_IDLE_TGT.second); iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_IDLE_TGT.second); iht(objects::RW_ASS, NML, 0, ACS_TABLE_IDLE_TGT.second); iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_TGT.second); @@ -307,7 +307,7 @@ void buildIdleSequence(Subsystem& ss, ModeListEntry& eh) { ss.addTable(&ACS_TABLE_IDLE_TRANS_0.second, ACS_TABLE_IDLE_TRANS_0.first, false, true); // Build IDLE transition 1 - iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::IDLE, ACS_TABLE_IDLE_TRANS_1.second); + iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_IDLE, ACS_TABLE_IDLE_TRANS_1.second); ss.addTable(&ACS_TABLE_IDLE_TRANS_1.second, ACS_TABLE_IDLE_TRANS_1.first, false, true); // Build IDLE sequence @@ -350,8 +350,7 @@ void buildTargetPtSequence(Subsystem& ss, ModeListEntry& eh) { // Transition 0 already built // Build TARGET PT transition 1 - iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_TARGET, - ACS_TABLE_PTG_TARGET_TRANS_1.second); + iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_TARGET, ACS_TABLE_PTG_TARGET_TRANS_1.second); check(ss.addTable(&ACS_TABLE_PTG_TARGET_TRANS_1.second, ACS_TABLE_PTG_TARGET_TRANS_1.first, false, true), ctxc); @@ -399,7 +398,7 @@ void buildTargetPtNadirSequence(Subsystem& ss, ModeListEntry& eh) { // Transition 0 already built // Build TARGET PT transition 1 - iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_TARGET_NADIR, + iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_NADIR, ACS_TABLE_PTG_TARGET_NADIR_TRANS_1.second); check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_NADIR_TRANS_1.first, &ACS_TABLE_PTG_TARGET_NADIR_TRANS_1.second)), @@ -485,7 +484,7 @@ void buildTargetPtInertialSequence(Subsystem& ss, ModeListEntry& eh) { }; // Build TARGET PT table - iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_TARGET_INERTIAL, + iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_INERTIAL, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second); iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second); iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second); @@ -498,7 +497,7 @@ void buildTargetPtInertialSequence(Subsystem& ss, ModeListEntry& eh) { // Transition 0 already built // Build TARGET PT transition 1 - iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_TARGET_INERTIAL, + iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_INERTIAL, ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1.second); check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1.first, &ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1.second)),