diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 14a2d83c..89853b9f 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -146,11 +146,14 @@ void AcsController::performControlOperation() { } void AcsController::performAttitudeControl() { - timeval timeAbsolute; Clock::getClock_timeval(&timeAbsolute); - timeval timeRelative; Clock::getClockMonotonic(&timeRelative); + 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); @@ -159,7 +162,7 @@ void AcsController::performAttitudeControl() { tleTooOldFlag = false; } - sensorProcessing.process(timeAbsolute, timeRelative, &sensorValues, &mgmDataProcessed, + sensorProcessing.process(timeAbsolute, timeDelta, &sensorValues, &mgmDataProcessed, &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters); fusedRotationEstimation.estimateFusedRotationRateSafe(&susDataProcessed, &mgmDataProcessed, &gyrDataProcessed, &fusedRotRateData); @@ -168,13 +171,13 @@ void AcsController::performAttitudeControl() { switch (mode) { case acs::SAFE: - if (result != MultiplicativeKalmanFilter::MEKF_RUNNING && + 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 && !mekfLost) { + if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE and not mekfLost) { triggerEvent(acs::MEKF_AUTOMATIC_RESET); navigation.resetMekf(&mekfData); mekfLost = true; @@ -197,14 +200,14 @@ void AcsController::performAttitudeControl() { case acs::PTG_TARGET_GS: case acs::PTG_NADIR: case acs::PTG_INERTIAL: - if (result != MultiplicativeKalmanFilter::MEKF_RUNNING && + 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 && !mekfLost) { + if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE and not mekfLost) { triggerEvent(acs::MEKF_AUTOMATIC_RESET); navigation.resetMekf(&mekfData); mekfLost = true; @@ -418,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); @@ -439,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, @@ -463,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); @@ -484,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 89919ccc..1b184b80 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -50,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/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 1d82d317..b483372d 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -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,7 +300,7 @@ 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], @@ -362,7 +364,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 +374,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 +410,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 +451,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) { 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 +489,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..371ae147 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 double timeDelta, 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 double timeDelta, 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 c0d2a128..56217a8f 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -588,7 +588,7 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong } } -void SensorProcessing::process(timeval timeAbsolute, timeval timeRelative, +void SensorProcessing::process(timeval timeAbsolute, double timeDelta, ACS::SensorValues *sensorValues, acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::SusDataProcessed *susDataProcessed, @@ -596,11 +596,6 @@ void SensorProcessing::process(timeval timeAbsolute, timeval timeRelative, acsctrl::GpsDataProcessed *gpsDataProcessed, const AcsParameters *acsParameters) { sensorValues->update(); - double timeDelta = 0; - if (timeRelative.tv_sec != 0 and savedTimeRelative.tv_sec != 0) { - timeDelta = timevalOperations::toDouble(timeRelative - savedTimeRelative); - } - savedTimeRelative = timeRelative; processGps( sensorValues->gpsSet.latitude.value, sensorValues->gpsSet.longitude.value, diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index 70b78845..fea0fd01 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -24,7 +24,7 @@ class SensorProcessing { SensorProcessing(); virtual ~SensorProcessing(); - void process(timeval timeAbsolute, timeval timeRelative, ACS::SensorValues *sensorValues, + void process(timeval timeAbsolute, double timeDelta, ACS::SensorValues *sensorValues, acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::SusDataProcessed *susDataProcessed, acsctrl::GyrDataProcessed *gyrDataProcessed, @@ -74,7 +74,6 @@ class SensorProcessing { void lowPassFilter(double *newValue, double *oldValue, const double weight); - timeval savedTimeRelative; double savedMgmVecTot[3] = {0.0, 0.0, 0.0}; double savedSusVecTot[3] = {0.0, 0.0, 0.0};