diff --git a/CHANGELOG.md b/CHANGELOG.md index d3a8ed70..fc611ceb 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -69,6 +69,16 @@ will consitute of a breaking change warranting a new major release: during which the SUS was not working as well as the maximum amount of invalid messages. - Updated battery internal resistance to new value +## Changed + +- `Power Controller` now uses monotonic clock for calculating time difference +- `ACS Controller` now uses monotonic clock for calculating time difference and the normal clock + for model calculations. The `timeDelta` is now calculated in the controller instead of + everywhere where it is needed. +- `ACS Controller` now has the function `performAttitudeControl` which is called prior to passing + on to the relevant mode functions. It handles all telemetry relevant functions, which were + always called, regardless of the mode. + # [v7.1.0] 2023-10-11 - Bumped `eive-tmtc` to v5.8.0. diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 1350bcf0..89853b9f 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -136,25 +136,7 @@ void AcsController::performControlOperation() { } case InternalState::READY: { if (mode != MODE_OFF) { - switch (mode) { - case acs::SAFE: - switch (submode) { - case SUBMODE_NONE: - performSafe(); - break; - case acs::DETUMBLE: - performDetumble(); - break; - } - break; - case acs::PTG_IDLE: - case acs::PTG_TARGET: - case acs::PTG_TARGET_GS: - case acs::PTG_NADIR: - case acs::PTG_INERTIAL: - performPointingCtrl(); - break; - } + performAttitudeControl(); } break; } @@ -163,39 +145,96 @@ void AcsController::performControlOperation() { } } -void AcsController::performSafe() { - timeval now; - Clock::getClock_timeval(&now); +void AcsController::performAttitudeControl() { + Clock::getClock_timeval(&timeAbsolute); + Clock::getClockMonotonic(&timeRelative); - ReturnValue_t result = navigation.useSpg4(now, &gpsDataProcessed); + if (timeRelative.tv_sec != 0 and oldTimeRelative.tv_sec != 0) { + timeDelta = timevalOperations::toDouble(timeRelative - oldTimeRelative); + } + oldTimeRelative = timeRelative; + + ReturnValue_t result = navigation.useSpg4(timeAbsolute, &gpsDataProcessed); if (result == Sgp4Propagator::TLE_TOO_OLD and not tleTooOldFlag) { triggerEvent(acs::TLE_TOO_OLD); tleTooOldFlag = true; } else if (result != Sgp4Propagator::TLE_TOO_OLD) { tleTooOldFlag = false; } - sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, - &gyrDataProcessed, &gpsDataProcessed, &acsParameters); + + sensorProcessing.process(timeAbsolute, timeDelta, &sensorValues, &mgmDataProcessed, + &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters); fusedRotationEstimation.estimateFusedRotationRateSafe(&susDataProcessed, &mgmDataProcessed, &gyrDataProcessed, &fusedRotRateData); result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, &mekfData, &acsParameters); - if (result != MultiplicativeKalmanFilter::MEKF_RUNNING && - result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) { - if (not mekfInvalidFlag) { - triggerEvent(acs::MEKF_INVALID_INFO, (uint32_t)mekfData.mekfStatus.value); - mekfInvalidFlag = true; - } - if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) { - triggerEvent(acs::MEKF_AUTOMATIC_RESET); - navigation.resetMekf(&mekfData); - mekfLost = true; - } - } else if (mekfInvalidFlag) { - triggerEvent(acs::MEKF_RECOVERY); - mekfInvalidFlag = false; - } + switch (mode) { + case acs::SAFE: + if (result != MultiplicativeKalmanFilter::MEKF_RUNNING and + result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) { + if (not mekfInvalidFlag) { + triggerEvent(acs::MEKF_INVALID_INFO, (uint32_t)mekfData.mekfStatus.value); + mekfInvalidFlag = true; + } + if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE and not mekfLost) { + triggerEvent(acs::MEKF_AUTOMATIC_RESET); + navigation.resetMekf(&mekfData); + mekfLost = true; + } + } else if (mekfInvalidFlag) { + triggerEvent(acs::MEKF_RECOVERY); + mekfInvalidFlag = false; + } + switch (submode) { + case SUBMODE_NONE: + performSafe(); + break; + case acs::DETUMBLE: + performDetumble(); + break; + } + break; + case acs::PTG_IDLE: + case acs::PTG_TARGET: + case acs::PTG_TARGET_GS: + case acs::PTG_NADIR: + case acs::PTG_INERTIAL: + if (result != MultiplicativeKalmanFilter::MEKF_RUNNING and + result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) { + mekfInvalidCounter++; + if (not mekfInvalidFlag) { + triggerEvent(acs::MEKF_INVALID_INFO, (uint32_t)mekfData.mekfStatus.value); + mekfInvalidFlag = true; + } + if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE and not mekfLost) { + triggerEvent(acs::MEKF_AUTOMATIC_RESET); + navigation.resetMekf(&mekfData); + mekfLost = true; + } + if (mekfInvalidCounter > acsParameters.onBoardParams.mekfViolationTimer) { + // Trigger this so STR FDIR can set the device faulty. + EventManagerIF::triggerEvent(objects::STAR_TRACKER, acs::MEKF_INVALID_MODE_VIOLATION, 0, + 0); + mekfInvalidCounter = 0; + } + commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration, + cmdSpeedRws[0], cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], + acsParameters.rwHandlingParameters.rampTime); + return; + } else { + if (mekfInvalidFlag) { + triggerEvent(acs::MEKF_RECOVERY); + mekfInvalidFlag = false; + } + mekfInvalidCounter = 0; + } + performPointingCtrl(); + break; + } +} + +void AcsController::performSafe() { // get desired satellite rate, sun direction to align to and inertia double sunTargetDir[3] = {0, 0, 0}; guidance.getTargetParamsSafe(sunTargetDir); @@ -296,37 +335,6 @@ void AcsController::performSafe() { } void AcsController::performDetumble() { - timeval now; - Clock::getClock_timeval(&now); - - ReturnValue_t result = navigation.useSpg4(now, &gpsDataProcessed); - if (result == Sgp4Propagator::TLE_TOO_OLD and not tleTooOldFlag) { - triggerEvent(acs::TLE_TOO_OLD); - tleTooOldFlag = true; - } else { - tleTooOldFlag = false; - } - sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, - &gyrDataProcessed, &gpsDataProcessed, &acsParameters); - fusedRotationEstimation.estimateFusedRotationRateSafe(&susDataProcessed, &mgmDataProcessed, - &gyrDataProcessed, &fusedRotRateData); - result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, - &susDataProcessed, &mekfData, &acsParameters); - if (result != MultiplicativeKalmanFilter::MEKF_RUNNING && - result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) { - if (not mekfInvalidFlag) { - triggerEvent(acs::MEKF_INVALID_INFO, (uint32_t)mekfData.mekfStatus.value); - mekfInvalidFlag = true; - } - if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) { - triggerEvent(acs::MEKF_AUTOMATIC_RESET); - navigation.resetMekf(&mekfData); - mekfLost = true; - } - } else if (mekfInvalidFlag) { - triggerEvent(acs::MEKF_RECOVERY); - mekfInvalidFlag = false; - } acs::SafeModeStrategy safeCtrlStrat = detumble.detumbleStrategy( mgmDataProcessed.mgmVecTot.isValid(), gyrDataProcessed.gyrVecTot.isValid(), mgmDataProcessed.mgmVecTotDerivative.isValid(), @@ -389,51 +397,9 @@ void AcsController::performDetumble() { } void AcsController::performPointingCtrl() { - timeval now; - Clock::getClock_timeval(&now); - - ReturnValue_t result = navigation.useSpg4(now, &gpsDataProcessed); - if (result == Sgp4Propagator::TLE_TOO_OLD and not tleTooOldFlag) { - triggerEvent(acs::TLE_TOO_OLD); - tleTooOldFlag = true; - } else { - tleTooOldFlag = false; - } - sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, - &gyrDataProcessed, &gpsDataProcessed, &acsParameters); - result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, - &susDataProcessed, &mekfData, &acsParameters); - if (result != MultiplicativeKalmanFilter::MEKF_RUNNING && - result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) { - mekfInvalidCounter++; - if (not mekfInvalidFlag) { - triggerEvent(acs::MEKF_INVALID_INFO, (uint32_t)mekfData.mekfStatus.value); - mekfInvalidFlag = true; - } - if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) { - triggerEvent(acs::MEKF_AUTOMATIC_RESET); - navigation.resetMekf(&mekfData); - mekfLost = true; - } - if (mekfInvalidCounter > acsParameters.onBoardParams.mekfViolationTimer) { - // Trigger this so STR FDIR can set the device faulty. - EventManagerIF::triggerEvent(objects::STAR_TRACKER, acs::MEKF_INVALID_MODE_VIOLATION, 0, 0); - mekfInvalidCounter = 0; - } - commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0], - cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], - acsParameters.rwHandlingParameters.rampTime); - return; - } else { - if (mekfInvalidFlag) { - triggerEvent(acs::MEKF_RECOVERY); - mekfInvalidFlag = false; - } - mekfInvalidCounter = 0; - } uint8_t enableAntiStiction = true; double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); + ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); if (result == returnvalue::FAILED) { if (multipleRwUnavailableCounter >= acsParameters.rwHandlingParameters.multipleRwInvalidTimeout) { @@ -455,7 +421,7 @@ void AcsController::performPointingCtrl() { switch (mode) { case acs::PTG_IDLE: - guidance.targetQuatPtgSun(now, susDataProcessed.sunIjkModel.value, targetQuat, + guidance.targetQuatPtgSun(timeDelta, susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate); guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, targetSatRotRate, errorQuat, errorSatRotRate, errorAngle); @@ -476,7 +442,7 @@ void AcsController::performPointingCtrl() { break; case acs::PTG_TARGET: - guidance.targetQuatPtgThreeAxes(now, gpsDataProcessed.gpsPosition.value, + guidance.targetQuatPtgThreeAxes(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value, gpsDataProcessed.gpsVelocity.value, targetQuat, targetSatRotRate); guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, @@ -500,7 +466,7 @@ void AcsController::performPointingCtrl() { break; case acs::PTG_TARGET_GS: - guidance.targetQuatPtgGs(now, gpsDataProcessed.gpsPosition.value, + guidance.targetQuatPtgGs(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value, susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate); guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, targetSatRotRate, errorQuat, errorSatRotRate, errorAngle); @@ -521,9 +487,9 @@ void AcsController::performPointingCtrl() { break; case acs::PTG_NADIR: - guidance.targetQuatPtgNadirThreeAxes(now, gpsDataProcessed.gpsPosition.value, - gpsDataProcessed.gpsVelocity.value, targetQuat, - targetSatRotRate); + guidance.targetQuatPtgNadirThreeAxes( + timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value, + gpsDataProcessed.gpsVelocity.value, targetQuat, targetSatRotRate); guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, targetSatRotRate, acsParameters.nadirModeControllerParameters.quatRef, acsParameters.nadirModeControllerParameters.refRotRate, errorQuat, diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index e03f2a55..1b184b80 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -37,6 +37,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes uint16_t startAtIndex) override; protected: + void performAttitudeControl(); void performSafe(); void performDetumble(); void performPointingCtrl(); @@ -49,6 +50,11 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes bool enableHkSets = false; + timeval timeAbsolute; + timeval timeRelative; + double timeDelta = 0.0; + timeval oldTimeRelative; + AcsParameters acsParameters; SensorProcessing sensorProcessing; FusedRotationEstimation fusedRotationEstimation; diff --git a/mission/controller/PowerController.cpp b/mission/controller/PowerController.cpp index 2a735782..f8a02c09 100644 --- a/mission/controller/PowerController.cpp +++ b/mission/controller/PowerController.cpp @@ -157,7 +157,7 @@ ReturnValue_t PowerController::checkModeCommand(Mode_t mode, Submode_t submode, void PowerController::calculateStateOfCharge() { // get time - Clock::getClock_timeval(&now); + Clock::getClockMonotonic(&now); // update EPS HK values ReturnValue_t result = updateEpsData(); @@ -286,7 +286,12 @@ ReturnValue_t PowerController::calculateOpenCircuitVoltageCharge() { } ReturnValue_t PowerController::calculateCoulombCounterCharge() { - double timeDiff = timevalOperations::toDouble(now - oldTime); + double timeDiff = 0.0; + if (oldTime.tv_sec != 0) { + timeDiff = timevalOperations::toDouble(now - oldTime); + } else { + return returnvalue::FAILED; + } if (timeDiff > maxAllowedTimeDiff) { // should not be a permanent state so no spam protection required triggerEvent(power::TIMEDELTA_OUT_OF_BOUNDS, static_cast(timeDiff * 10)); diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 1d82d317..bf834d12 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -4,21 +4,21 @@ #include #include #include -#include +#include +#include #include - -#include "string.h" -#include "util/CholeskyDecomposition.h" -#include "util/MathOperations.h" +#include Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = acsParameters_; } Guidance::~Guidance() {} -void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double velSatE[3], - double sunDirI[3], double refDirB[3], double quatBI[4], - double targetQuat[4], double targetSatRotRate[3]) { +[[deprecated]] void Guidance::targetQuatPtgSingleAxis(const timeval timeAbsolute, double posSatE[3], + double velSatE[3], double sunDirI[3], + double refDirB[3], double quatBI[4], + double targetQuat[4], + double targetSatRotRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion to groundstation or given latitude, longitude and altitude //------------------------------------------------------------------------------------- @@ -38,7 +38,7 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ve double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot); + MathOperations::ecfToEciWithNutPre(timeAbsolute, *dcmEI, *dcmEIDot); MathOperations::inverseMatrixDimThree(*dcmEI, *dcmIE); double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; @@ -136,8 +136,9 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ve QuaternionOperations::multiply(quatIB, targetQuat, targetQuat); } -void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double velSatE[3], - double targetQuat[4], double targetSatRotRate[3]) { +void Guidance::targetQuatPtgThreeAxes(const timeval timeAbsolute, const double timeDelta, + double posSatE[3], double velSatE[3], double targetQuat[4], + double targetSatRotRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion for target pointing //------------------------------------------------------------------------------------- @@ -154,7 +155,7 @@ void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double vel double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot); + MathOperations::ecfToEciWithNutPre(timeAbsolute, *dcmEI, *dcmEIDot); MathOperations::inverseMatrixDimThree(*dcmEI, *dcmIE); double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; @@ -199,11 +200,12 @@ void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double vel QuaternionOperations::fromDcm(dcmIX, targetQuat); int8_t timeElapsedMax = acsParameters->targetModeControllerParameters.timeElapsedMax; - targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate); + targetRotationRate(timeElapsedMax, timeDelta, targetQuat, targetSatRotRate); } -void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3], - double targetQuat[4], double targetSatRotRate[3]) { +void Guidance::targetQuatPtgGs(const timeval timeAbsolute, const double timeDelta, + double posSatE[3], double sunDirI[3], double targetQuat[4], + double targetSatRotRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion for ground station pointing //------------------------------------------------------------------------------------- @@ -221,7 +223,7 @@ void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3] double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot); + MathOperations::ecfToEciWithNutPre(timeAbsolute, *dcmEI, *dcmEIDot); MathOperations::inverseMatrixDimThree(*dcmEI, *dcmIE); double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; @@ -263,10 +265,10 @@ void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3] QuaternionOperations::fromDcm(dcmTgt, targetQuat); int8_t timeElapsedMax = acsParameters->gsTargetModeControllerParameters.timeElapsedMax; - targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate); + targetRotationRate(timeElapsedMax, timeDelta, targetQuat, targetSatRotRate); } -void Guidance::targetQuatPtgSun(timeval now, double sunDirI[3], double targetQuat[4], +void Guidance::targetQuatPtgSun(double timeDelta, double sunDirI[3], double targetQuat[4], double targetSatRotRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion to sun @@ -298,12 +300,13 @@ void Guidance::targetQuatPtgSun(timeval now, double sunDirI[3], double targetQua // Calculation of reference rotation rate //---------------------------------------------------------------------------- int8_t timeElapsedMax = acsParameters->gsTargetModeControllerParameters.timeElapsedMax; - targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate); + targetRotationRate(timeElapsedMax, timeDelta, targetQuat, targetSatRotRate); } -void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], double quatBI[4], - double targetQuat[4], double refDirB[3], - double refSatRate[3]) { +[[deprecated]] void Guidance::targetQuatPtgNadirSingleAxis(const timeval timeAbsolute, + double posSatE[3], double quatBI[4], + double targetQuat[4], double refDirB[3], + double refSatRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion for Nadir pointing //------------------------------------------------------------------------------------- @@ -314,7 +317,7 @@ void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], doub double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot); + MathOperations::ecfToEciWithNutPre(timeAbsolute, *dcmEI, *dcmEIDot); MathOperations::inverseMatrixDimThree(*dcmEI, *dcmIE); double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; @@ -362,7 +365,8 @@ void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], doub QuaternionOperations::multiply(quatIB, targetQuat, targetQuat); } -void Guidance::targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], double velSatE[3], +void Guidance::targetQuatPtgNadirThreeAxes(const timeval timeAbsolute, const double timeDelta, + double posSatE[3], double velSatE[3], double targetQuat[4], double refSatRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion for Nadir pointing @@ -371,7 +375,7 @@ void Guidance::targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], doubl double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot); + MathOperations::ecfToEciWithNutPre(timeAbsolute, *dcmEI, *dcmEIDot); MathOperations::inverseMatrixDimThree(*dcmEI, *dcmIE); double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; @@ -407,7 +411,7 @@ void Guidance::targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], doubl QuaternionOperations::fromDcm(dcmTgt, targetQuat); int8_t timeElapsedMax = acsParameters->nadirModeControllerParameters.timeElapsedMax; - targetRotationRate(timeElapsedMax, now, targetQuat, refSatRate); + targetRotationRate(timeElapsedMax, timeDelta, targetQuat, refSatRate); } void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4], @@ -448,23 +452,21 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do VectorOperations::subtract(currentSatRotRate, targetSatRotRate, errorSatRotRate, 3); } -void Guidance::targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4], - double *refSatRate) { +void Guidance::targetRotationRate(const int8_t timeElapsedMax, const double timeDelta, + double quatInertialTarget[4], double *refSatRate) { //------------------------------------------------------------------------------------- // Calculation of target rotation rate //------------------------------------------------------------------------------------- - double timeElapsed = now.tv_sec + now.tv_usec * 1e-6 - - (timeSavedQuaternion.tv_sec + timeSavedQuaternion.tv_usec * 1e-6); if (VectorOperations::norm(savedQuaternion, 4) == 0) { std::memcpy(savedQuaternion, quatInertialTarget, sizeof(savedQuaternion)); } - if (timeElapsed < timeElapsedMax) { + if (timeDelta < timeElapsedMax and timeDelta != 0.0) { double q[4] = {0, 0, 0, 0}, qS[4] = {0, 0, 0, 0}; QuaternionOperations::inverse(quatInertialTarget, q); QuaternionOperations::inverse(savedQuaternion, qS); double qDiff[4] = {0, 0, 0, 0}; VectorOperations::subtract(q, qS, qDiff, 4); - VectorOperations::mulScalar(qDiff, 1 / timeElapsed, qDiff, 4); + VectorOperations::mulScalar(qDiff, 1. / timeDelta, qDiff, 4); double tgtQuatVec[3] = {q[0], q[1], q[2]}; double qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]}; @@ -488,11 +490,7 @@ void Guidance::targetRotationRate(int8_t timeElapsedMax, timeval now, double qua refSatRate[2] = 0; } - timeSavedQuaternion = now; - savedQuaternion[0] = quatInertialTarget[0]; - savedQuaternion[1] = quatInertialTarget[1]; - savedQuaternion[2] = quatInertialTarget[2]; - savedQuaternion[3] = quatInertialTarget[3]; + std::memcpy(savedQuaternion, quatInertialTarget, sizeof(savedQuaternion)); } ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index 7b81e411..1cf5b8a9 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -17,25 +17,26 @@ class Guidance { // Function to get the target quaternion and reference rotation rate from gps position and // position of the ground station - void targetQuatPtgSingleAxis(timeval now, double posSatE[3], double velSatE[3], double sunDirI[3], - double refDirB[3], double quatBI[4], double targetQuat[4], - double targetSatRotRate[3]); - void targetQuatPtgThreeAxes(timeval now, double posSatE[3], double velSatE[3], double quatIX[4], - double targetSatRotRate[3]); - void targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3], double quatIX[4], - double targetSatRotRate[3]); + void targetQuatPtgSingleAxis(const timeval timeAbsolute, double posSatE[3], double velSatE[3], + double sunDirI[3], double refDirB[3], double quatBI[4], + double targetQuat[4], double targetSatRotRate[3]); + void targetQuatPtgThreeAxes(const timeval timeAbsolute, const double timeDelta, double posSatE[3], + double velSatE[3], double quatIX[4], double targetSatRotRate[3]); + void targetQuatPtgGs(const timeval timeAbsolute, const double timeDelta, double posSatE[3], + double sunDirI[3], double quatIX[4], double targetSatRotRate[3]); // Function to get the target quaternion and reference rotation rate for sun pointing after ground // station - void targetQuatPtgSun(timeval now, double sunDirI[3], double targetQuat[4], + void targetQuatPtgSun(const double timeDelta, double sunDirI[3], double targetQuat[4], double targetSatRotRate[3]); // Function to get the target quaternion and refence rotation rate from gps position for Nadir // pointing - void targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], double quatBI[4], + void targetQuatPtgNadirSingleAxis(const timeval timeAbsolute, double posSatE[3], double quatBI[4], double targetQuat[4], double refDirB[3], double refSatRate[3]); - void targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], double velSatE[3], - double targetQuat[4], double refSatRate[3]); + void targetQuatPtgNadirThreeAxes(const timeval timeAbsolute, const double timeDelta, + double posSatE[3], double velSatE[3], double targetQuat[4], + double refSatRate[3]); // @note: Calculates the error quaternion between the current orientation and the target // quaternion, considering a reference quaternion. Additionally the difference between the actual @@ -48,8 +49,8 @@ class Guidance { double targetSatRotRate[3], double errorQuat[4], double errorSatRotRate[3], double &errorAngle); - void targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4], - double *targetSatRotRate); + void targetRotationRate(const int8_t timeElapsedMax, const double timeDelta, + double quatInertialTarget[4], double *targetSatRotRate); // @note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid // reation wheel maybe can be done in "commanding.h" @@ -59,7 +60,6 @@ class Guidance { const AcsParameters *acsParameters; bool strBlindAvoidFlag = false; - timeval timeSavedQuaternion; double savedQuaternion[4] = {0, 0, 0, 0}; double omegaRefSaved[3] = {0, 0, 0}; diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 4405790c..56217a8f 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -7,7 +7,7 @@ SensorProcessing::~SensorProcessing() {} void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value, bool mgm1valid, const float *mgm2Value, bool mgm2valid, const float *mgm3Value, bool mgm3valid, const float *mgm4Value, - bool mgm4valid, timeval timeOfMgmMeasurement, + bool mgm4valid, timeval timeAbsolute, double timeDelta, const AcsParameters::MgmHandlingParameters *mgmParameters, acsctrl::GpsDataProcessed *gpsDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed) { @@ -18,11 +18,12 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const if (gpsDataProcessed->source.value != acs::GpsSource::NONE) { Igrf13Model igrf13; igrf13.schmidtNormalization(); - igrf13.updateCoeffGH(timeOfMgmMeasurement); + igrf13.updateCoeffGH(timeAbsolute); + // ToDo // 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, - gpsDataProcessed->altitude.value, timeOfMgmMeasurement, magIgrfModel); + gpsDataProcessed->altitude.value, timeAbsolute, magIgrfModel); gpsValid = true; } if (not mgm0valid and not mgm1valid and not mgm2valid and not mgm3valid and @@ -129,14 +130,12 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const //-----------------------Mgm Rate Computation --------------------------------------------------- double mgmVecTotDerivative[3] = {0.0, 0.0, 0.0}; bool mgmVecTotDerivativeValid = false; - double timeDiff = timevalOperations::toDouble(timeOfMgmMeasurement - timeOfSavedMagFieldEst); - if (timeOfSavedMagFieldEst.tv_sec != 0 and timeDiff > 0 and - VectorOperations::norm(savedMgmVecTot, 3) != 0) { + if (timeDelta > 0 and VectorOperations::norm(savedMgmVecTot, 3) != 0) { VectorOperations::subtract(mgmVecTot, savedMgmVecTot, mgmVecTotDerivative, 3); - VectorOperations::mulScalar(mgmVecTotDerivative, 1. / timeDiff, mgmVecTotDerivative, 3); + VectorOperations::mulScalar(mgmVecTotDerivative, 1. / timeDelta, mgmVecTotDerivative, + 3); mgmVecTotDerivativeValid = true; } - timeOfSavedMagFieldEst = timeOfMgmMeasurement; std::memcpy(savedMgmVecTot, mgmVecTot, sizeof(savedMgmVecTot)); if (VectorOperations::norm(mgmVecTotDerivative, 3) != 0 and @@ -177,11 +176,12 @@ void SensorProcessing::processSus( const uint16_t *sus6Value, bool sus6valid, const uint16_t *sus7Value, bool sus7valid, const uint16_t *sus8Value, bool sus8valid, const uint16_t *sus9Value, bool sus9valid, const uint16_t *sus10Value, bool sus10valid, const uint16_t *sus11Value, bool sus11valid, - timeval timeOfSusMeasurement, const AcsParameters::SusHandlingParameters *susParameters, + timeval timeAbsolute, double timeDelta, + const AcsParameters::SusHandlingParameters *susParameters, const AcsParameters::SunModelParameters *sunModelParameters, acsctrl::SusDataProcessed *susDataProcessed) { /* -------- Sun Model Direction (IJK frame) ------- */ - double JD2000 = MathOperations::convertUnixToJD2000(timeOfSusMeasurement); + double JD2000 = MathOperations::convertUnixToJD2000(timeAbsolute); // Julean Centuries double sunIjkModel[3] = {0.0, 0.0, 0.0}; @@ -354,11 +354,10 @@ void SensorProcessing::processSus( double susVecTotDerivative[3] = {0.0, 0.0, 0.0}; bool susVecTotDerivativeValid = false; - double timeDiff = timevalOperations::toDouble(timeOfSusMeasurement - timeOfSavedSusDirEst); - if (timeOfSavedSusDirEst.tv_sec != 0 and timeDiff > 0 and - VectorOperations::norm(savedSusVecTot, 3) != 0) { + if (timeDelta > 0 and VectorOperations::norm(savedSusVecTot, 3) != 0) { VectorOperations::subtract(susVecTot, savedSusVecTot, susVecTotDerivative, 3); - VectorOperations::mulScalar(susVecTotDerivative, 1. / timeDiff, susVecTotDerivative, 3); + VectorOperations::mulScalar(susVecTotDerivative, 1. / timeDelta, susVecTotDerivative, + 3); susVecTotDerivativeValid = true; } std::memcpy(savedSusVecTot, susVecTot, sizeof(savedSusVecTot)); @@ -367,7 +366,6 @@ void SensorProcessing::processSus( lowPassFilter(susVecTotDerivative, susDataProcessed->susVecTotDerivative.value, susParameters->susRateFilterWeight); } - timeOfSavedSusDirEst = timeOfSusMeasurement; { PoolReadGuard pg(susDataProcessed); if (pg.getReadResult() == returnvalue::OK) { @@ -414,7 +412,7 @@ void SensorProcessing::processGyr( const double gyr2axXvalue, bool gyr2axXvalid, const double gyr2axYvalue, bool gyr2axYvalid, const double gyr2axZvalue, bool gyr2axZvalid, const double gyr3axXvalue, bool gyr3axXvalid, const double gyr3axYvalue, bool gyr3axYvalid, const double gyr3axZvalue, bool gyr3axZvalid, - timeval timeOfGyrMeasurement, const AcsParameters::GyrHandlingParameters *gyrParameters, + const AcsParameters::GyrHandlingParameters *gyrParameters, acsctrl::GyrDataProcessed *gyrDataProcessed) { bool gyr0valid = (gyr0axXvalid && gyr0axYvalid && gyr0axZvalid); bool gyr1valid = (gyr1axXvalid && gyr1axYvalid && gyr1axZvalid); @@ -521,7 +519,7 @@ void SensorProcessing::processGyr( } void SensorProcessing::processGps(const double gpsLatitude, const double gpsLongitude, - const double gpsAltitude, const double gpsUnixSeconds, + const double gpsAltitude, const double timeDelta, const bool validGps, const AcsParameters::GpsParameters *gpsParameters, acsctrl::GpsDataProcessed *gpsDataProcessed) { @@ -563,18 +561,14 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong // Calculation of the satellite velocity in earth fixed frame double deltaDistance[3] = {0, 0, 0}; MathOperations::cartesianFromLatLongAlt(latitudeRad, gdLongitude, altitude, posSatE); - if (validSavedPosSatE and - (gpsUnixSeconds - timeOfSavedPosSatE) < (gpsParameters->timeDiffVelocityMax) and - (gpsUnixSeconds - timeOfSavedPosSatE) > 0) { + if (validSavedPosSatE and timeDelta < (gpsParameters->timeDiffVelocityMax) and timeDelta > 0) { VectorOperations::subtract(posSatE, savedPosSatE, deltaDistance, 3); - double timeDiffGpsMeas = gpsUnixSeconds - timeOfSavedPosSatE; - VectorOperations::mulScalar(deltaDistance, 1. / timeDiffGpsMeas, gpsVelocityE, 3); + VectorOperations::mulScalar(deltaDistance, 1. / timeDelta, gpsVelocityE, 3); } savedPosSatE[0] = posSatE[0]; savedPosSatE[1] = posSatE[1]; savedPosSatE[2] = posSatE[2]; - timeOfSavedPosSatE = gpsUnixSeconds; validSavedPosSatE = true; gpsSource = acs::GpsSource::GPS; @@ -594,13 +588,15 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong } } -void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues, +void SensorProcessing::process(timeval timeAbsolute, double timeDelta, + ACS::SensorValues *sensorValues, acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::SusDataProcessed *susDataProcessed, acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::GpsDataProcessed *gpsDataProcessed, const AcsParameters *acsParameters) { sensorValues->update(); + processGps( sensorValues->gpsSet.latitude.value, sensorValues->gpsSet.longitude.value, sensorValues->gpsSet.altitude.value, sensorValues->gpsSet.unixSeconds.value, @@ -617,7 +613,8 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues, sensorValues->mgm3Rm3100Set.fieldStrengths.value, sensorValues->mgm3Rm3100Set.fieldStrengths.isValid(), sensorValues->imtqMgmSet.mtmRawNt.value, sensorValues->imtqMgmSet.mtmRawNt.isValid(), - now, &acsParameters->mgmHandlingParameters, gpsDataProcessed, mgmDataProcessed); + timeAbsolute, timeDelta, &acsParameters->mgmHandlingParameters, gpsDataProcessed, + mgmDataProcessed); processSus(sensorValues->susSets[0].channels.value, sensorValues->susSets[0].channels.isValid(), sensorValues->susSets[1].channels.value, sensorValues->susSets[1].channels.isValid(), @@ -631,8 +628,8 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues, sensorValues->susSets[9].channels.value, sensorValues->susSets[9].channels.isValid(), sensorValues->susSets[10].channels.value, sensorValues->susSets[10].channels.isValid(), sensorValues->susSets[11].channels.value, sensorValues->susSets[11].channels.isValid(), - now, &acsParameters->susHandlingParameters, &acsParameters->sunModelParameters, - susDataProcessed); + timeAbsolute, timeDelta, &acsParameters->susHandlingParameters, + &acsParameters->sunModelParameters, susDataProcessed); processGyr( sensorValues->gyr0AdisSet.angVelocX.value, sensorValues->gyr0AdisSet.angVelocX.isValid(), @@ -646,7 +643,7 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues, sensorValues->gyr2AdisSet.angVelocZ.value, sensorValues->gyr2AdisSet.angVelocZ.isValid(), sensorValues->gyr3L3gSet.angVelocX.value, sensorValues->gyr3L3gSet.angVelocX.isValid(), sensorValues->gyr3L3gSet.angVelocY.value, sensorValues->gyr3L3gSet.angVelocY.isValid(), - sensorValues->gyr3L3gSet.angVelocZ.value, sensorValues->gyr3L3gSet.angVelocZ.isValid(), now, + sensorValues->gyr3L3gSet.angVelocZ.value, sensorValues->gyr3L3gSet.angVelocZ.isValid(), &acsParameters->gyrHandlingParameters, gyrDataProcessed); } diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index 77ff2869..fea0fd01 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -24,22 +24,21 @@ class SensorProcessing { SensorProcessing(); virtual ~SensorProcessing(); - void process(timeval now, ACS::SensorValues *sensorValues, + void process(timeval timeAbsolute, double timeDelta, ACS::SensorValues *sensorValues, acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::SusDataProcessed *susDataProcessed, acsctrl::GyrDataProcessed *gyrDataProcessed, - acsctrl::GpsDataProcessed *gpsDataProcessed, - const AcsParameters *acsParameters); // Will call protected functions + acsctrl::GpsDataProcessed *gpsDataProcessed, const AcsParameters *acsParameters); + private: static constexpr float ZERO_VEC_F[3] = {0, 0, 0}; static constexpr double ZERO_VEC_D[3] = {0, 0, 0}; static constexpr double ECCENTRICITY_WGS84 = 0.0818195; protected: - // short description needed for every function void processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value, bool mgm1valid, const float *mgm2Value, bool mgm2valid, const float *mgm3Value, bool mgm3valid, - const float *mgm4Value, bool mgm4valid, timeval timeOfMgmMeasurement, + const float *mgm4Value, bool mgm4valid, timeval timeAbsolute, double timeDelta, const AcsParameters::MgmHandlingParameters *mgmParameters, acsctrl::GpsDataProcessed *gpsDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed); @@ -52,7 +51,7 @@ class SensorProcessing { bool sus7valid, const uint16_t *sus8Value, bool sus8valid, const uint16_t *sus9Value, bool sus9valid, const uint16_t *sus10Value, bool sus10valid, const uint16_t *sus11Value, bool sus11valid, - timeval timeOfSusMeasurement, + timeval timeAbsolute, double timeDelta, const AcsParameters::SusHandlingParameters *susParameters, const AcsParameters::SunModelParameters *sunModelParameters, acsctrl::SusDataProcessed *susDataProcessed); @@ -65,7 +64,6 @@ class SensorProcessing { bool gyr2axYvalid, const double gyr2axZvalue, bool gyr2axZvalid, const double gyr3axXvalue, bool gyr3axXvalid, const double gyr3axYvalue, bool gyr3axYvalid, const double gyr3axZvalue, bool gyr3axZvalid, - timeval timeOfGyrMeasurement, const AcsParameters::GyrHandlingParameters *gyrParameters, acsctrl::GyrDataProcessed *gyrDataProcessed); @@ -77,13 +75,9 @@ class SensorProcessing { void lowPassFilter(double *newValue, double *oldValue, const double weight); double savedMgmVecTot[3] = {0.0, 0.0, 0.0}; - timeval timeOfSavedMagFieldEst; double savedSusVecTot[3] = {0.0, 0.0, 0.0}; - timeval timeOfSavedSusDirEst; - bool validMagField = false; double savedPosSatE[3] = {0.0, 0.0, 0.0}; - double timeOfSavedPosSatE = 0.0; bool validSavedPosSatE = false; SusConverter susConverter;