From 4d22f7f8898ae85b35346fad33df4d69e7ed265c Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 31 Jan 2024 15:41:21 +0100 Subject: [PATCH 01/12] make detumble state machine robust agains mode changes --- mission/acs/defs.h | 3 +++ mission/controller/AcsController.cpp | 26 ++++++++++++++++++++++---- mission/controller/AcsController.h | 4 ++++ 3 files changed, 29 insertions(+), 4 deletions(-) diff --git a/mission/acs/defs.h b/mission/acs/defs.h index 67fcf024..47f7f170 100644 --- a/mission/acs/defs.h +++ b/mission/acs/defs.h @@ -70,6 +70,9 @@ static constexpr Event SAFE_RATE_VIOLATION = MAKE_EVENT(0, severity::MEDIUM); static constexpr Event PTG_RATE_VIOLATION = MAKE_EVENT(10, severity::MEDIUM); //! [EXPORT] : [COMMENT] The system has recovered from a rate rotation violation. static constexpr Event RATE_RECOVERY = MAKE_EVENT(1, severity::MEDIUM); +//! [EXPORT] : [COMMENT] The detumble transition has failed. +//! //! P1: Last detumble state before failure. +static constexpr Event DETUMBLE_TRANSITION_FAILED = MAKE_EVENT(11, severity::HIGH); //! [EXPORT] : [COMMENT] Multiple RWs are invalid, uncommandable and therefore higher ACS modes //! cannot be maintained. static constexpr Event MULTIPLE_RW_INVALID = MAKE_EVENT(2, severity::HIGH); diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 264e9f24..2036e6ab 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -568,9 +568,16 @@ void AcsController::handleDetumbling() { break; case DetumbleState::DETUMBLE_FROM_PTG: triggerEvent(acs::PTG_RATE_VIOLATION); + detumbleTransitionCountdow.resetTimer(); detumbleState = DetumbleState::PTG_TO_SAFE_TRANSITION; break; case DetumbleState::PTG_TO_SAFE_TRANSITION: + if (detumbleTransitionCountdow.hasTimedOut()) { + triggerEvent(acs::DETUMBLE_TRANSITION_FAILED, 2); + detumbleCounter = 0; + detumbleState = DetumbleState::NO_DETUMBLE; + break; + } if (mode == acs::AcsMode::SAFE) { detumbleState = DetumbleState::DETUMBLE_FROM_SAFE; } @@ -578,9 +585,14 @@ void AcsController::handleDetumbling() { case DetumbleState::DETUMBLE_FROM_SAFE: detumbleCounter = 0; // Triggers detumble mode transition in subsystem - triggerEvent(acs::SAFE_RATE_VIOLATION); - startTransition(mode, acs::SafeSubmode::DETUMBLE); - detumbleState = DetumbleState::IN_DETUMBLE; + if (mode == acs::AcsMode::SAFE) { + triggerEvent(acs::SAFE_RATE_VIOLATION); + startTransition(mode, acs::SafeSubmode::DETUMBLE); + detumbleState = DetumbleState::IN_DETUMBLE; + break; + } + triggerEvent(acs::DETUMBLE_TRANSITION_FAILED, 3); + detumbleState = DetumbleState::NO_DETUMBLE; break; case DetumbleState::IN_DETUMBLE: if (fusedRotRateData.rotRateTotal.isValid() and @@ -885,6 +897,7 @@ ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode, } void AcsController::modeChanged(Mode_t mode, Submode_t submode) { + guidance.resetValues(); if (mode == acs::AcsMode::SAFE) { { PoolReadGuard pg(&rw1SpeedSet); @@ -903,7 +916,12 @@ void AcsController::modeChanged(Mode_t mode, Submode_t submode) { rw4SpeedSet.setRwSpeed(0, 10); } } - guidance.resetValues(); + if (submode == acs::SafeSubmode::DETUMBLE) { + detumbleState = DetumbleState::IN_DETUMBLE; + } + if (detumbleState == DetumbleState::IN_DETUMBLE and submode != acs::SafeSubmode::DETUMBLE) { + detumbleState = DetumbleState::NO_DETUMBLE; + } return ExtendedControllerBase::modeChanged(mode, submode); } diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index ed893479..e8102f35 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -283,6 +283,10 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes // Initial delay to make sure all pool variables have been initialized their owners Countdown initialCountdown = Countdown(INIT_DELAY); + + // Countdown after which the detumbling mode change should have been finished + static constexpr dur_millis_t MAX_DURATION = 60 * 1e3; + Countdown detumbleTransitionCountdow = Countdown(MAX_DURATION); }; #endif /* MISSION_CONTROLLER_ACSCONTROLLER_H_ */ From 8b9c0d3abf487fda3433fa7f4b23a3804a161090 Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 31 Jan 2024 15:41:56 +0100 Subject: [PATCH 02/12] changelog --- CHANGELOG.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 21792d0e..b3ca4e51 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,10 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +## Fixed + +- Detumbling State Machine is now robust to commanded mode changes. + # [v7.6.0] 2024-01-30 - Bumped `eive-tmtc` to v5.13.0 From 8be94cf2dcbca978d6fad8bc793ba12df6d6a211 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 2 Feb 2024 12:38:16 +0100 Subject: [PATCH 03/12] overloading is fun (but should be done right) --- mission/controller/acs/Guidance.cpp | 18 ++++-------------- 1 file changed, 4 insertions(+), 14 deletions(-) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 12957ab2..3cadb861 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -408,9 +408,7 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3], double errorQuat[4], double errorSatRotRate[3], double &errorAngle) { // First calculate error quaternion between current and target orientation - double invTargetQuat[4] = {0, 0, 0, 0}; - QuaternionOperations::inverse(targetQuat, invTargetQuat); - QuaternionOperations::multiply(currentQuat, invTargetQuat, errorQuat); + QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat); // Last calculate add rotation from reference quaternion QuaternionOperations::multiply(refQuat, errorQuat, errorQuat); // Keep scalar part of quaternion positive @@ -435,17 +433,9 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4], double targetSatRotRate[3], double errorQuat[4], double errorSatRotRate[3], double &errorAngle) { - // First calculate error quaternion between current and target orientation - QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat); - // Keep scalar part of quaternion positive - if (errorQuat[3] < 0) { - VectorOperations::mulScalar(errorQuat, -1, errorQuat, 4); - } - // Calculate error angle - errorAngle = QuaternionOperations::getAngle(errorQuat, true); - - // Calculate error satellite rotational rate - VectorOperations::subtract(currentSatRotRate, targetSatRotRate, errorSatRotRate, 3); + double refQuat[4] = {0, 0, 0, 1}, refSatRotRate[3] = {0, 0, 0}; + comparePtg(currentQuat, currentSatRotRate, targetQuat, targetSatRotRate, refQuat, refSatRotRate, + errorQuat, errorSatRotRate, errorAngle); } void Guidance::targetRotationRate(const int8_t timeElapsedMax, const double timeDelta, From 84ba6262a88fb4929310f07882ba137104eaa798 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 2 Feb 2024 14:32:12 +0100 Subject: [PATCH 04/12] cleanup --- mission/controller/AcsController.cpp | 16 +- mission/controller/acs/Guidance.cpp | 431 +++++++-------------------- mission/controller/acs/Guidance.h | 39 +-- 3 files changed, 120 insertions(+), 366 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 264e9f24..0c85560c 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -420,8 +420,8 @@ void AcsController::performPointingCtrl() { switch (mode) { case acs::PTG_IDLE: - guidance.targetQuatPtgSun(timeDelta, susDataProcessed.sunIjkModel.value, targetQuat, - targetSatRotRate); + guidance.targetQuatPtgIdle(timeDelta, susDataProcessed.sunIjkModel.value, targetQuat, + targetSatRotRate); guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate, errorQuat, errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate, @@ -440,9 +440,9 @@ void AcsController::performPointingCtrl() { break; case acs::PTG_TARGET: - guidance.targetQuatPtgThreeAxes(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value, - gpsDataProcessed.gpsVelocity.value, targetQuat, - targetSatRotRate); + guidance.targetQuatPtgTarget(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value, + gpsDataProcessed.gpsVelocity.value, targetQuat, + targetSatRotRate); guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate, acsParameters.targetModeControllerParameters.quatRef, acsParameters.targetModeControllerParameters.refRotRate, errorQuat, @@ -483,9 +483,9 @@ void AcsController::performPointingCtrl() { break; case acs::PTG_NADIR: - guidance.targetQuatPtgNadirThreeAxes( - timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value, - gpsDataProcessed.gpsVelocity.value, targetQuat, targetSatRotRate); + guidance.targetQuatPtgNadirTarget(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value, + gpsDataProcessed.gpsVelocity.value, targetQuat, + targetSatRotRate); guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate, acsParameters.nadirModeControllerParameters.quatRef, acsParameters.nadirModeControllerParameters.refRotRate, errorQuat, diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 3cadb861..7413265b 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -4,404 +4,190 @@ Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = acsParameter Guidance::~Guidance() {} -[[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]) { +void Guidance::targetQuatPtgIdle(double timeDelta, double sunDirI[3], double targetQuat[4], + double targetSatRotRate[3]) { //------------------------------------------------------------------------------------- - // Calculation of target quaternion to groundstation or given latitude, longitude and altitude + // Calculation of target quaternion to sun //------------------------------------------------------------------------------------- - // transform longitude, latitude and altitude to ECEF - double targetE[3] = {0, 0, 0}; + // positive z-Axis of EIVE in direction of sun + double zAxisXI[3] = {0, 0, 0}; + VectorOperations::normalize(sunDirI, zAxisXI, 3); - MathOperations::cartesianFromLatLongAlt( - acsParameters->targetModeControllerParameters.latitudeTgt, - acsParameters->targetModeControllerParameters.longitudeTgt, - acsParameters->targetModeControllerParameters.altitudeTgt, targetE); + // assign helper vector (north pole inertial) + double helperXI[3] = {0, 0, 1}; - // target direction in the ECEF frame - double targetDirE[3] = {0, 0, 0}; - VectorOperations::subtract(targetE, posSatE, targetDirE, 3); + // construct y-axis from helper vector and z-axis + double yAxisXI[3] = {0, 0, 0}; + VectorOperations::cross(zAxisXI, helperXI, yAxisXI); + VectorOperations::normalize(yAxisXI, yAxisXI, 3); - // transformation between ECEF and ECI frame - double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::ecfToEciWithNutPre(timeAbsolute, *dcmEI, *dcmEIDot); - MathOperations::inverseMatrixDimThree(*dcmEI, *dcmIE); + // x-axis completes RHS + double xAxisXI[3] = {0, 0, 0}; + VectorOperations::cross(yAxisXI, zAxisXI, xAxisXI); + VectorOperations::normalize(xAxisXI, xAxisXI, 3); - double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot); + // join transformation matrix + double dcmXI[3][3] = {{xAxisXI[0], yAxisXI[0], zAxisXI[0]}, + {xAxisXI[1], yAxisXI[1], zAxisXI[1]}, + {xAxisXI[2], yAxisXI[2], zAxisXI[2]}}; + QuaternionOperations::fromDcm(dcmXI, targetQuat); - // transformation between ECEF and Body frame - double dcmBI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - - QuaternionOperations::toDcm(quatBI, dcmBI); - MatrixOperations::multiply(*dcmBI, *dcmIE, *dcmBE, 3, 3, 3); - - // target Direction in the body frame - double targetDirB[3] = {0, 0, 0}; - MatrixOperations::multiply(*dcmBE, targetDirE, targetDirB, 3, 3, 1); - - // rotation quaternion from two vectors - double refDir[3] = {0, 0, 0}; - refDir[0] = acsParameters->targetModeControllerParameters.refDirection[0]; - refDir[1] = acsParameters->targetModeControllerParameters.refDirection[1]; - refDir[2] = acsParameters->targetModeControllerParameters.refDirection[2]; - double noramlizedTargetDirB[3] = {0, 0, 0}; - VectorOperations::normalize(targetDirB, noramlizedTargetDirB, 3); - VectorOperations::normalize(refDir, refDir, 3); - 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 - //------------------------------------------------------------------------------------- - 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); - double dcmBEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MatrixOperations::multiply(*dcmBI, *dcmIEDot, *dcmBEDot, 3, 3, 3); - MatrixOperations::multiply(*dcmBEDot, posSatE, velSatBPart2, 3, 3, 1); - VectorOperations::add(velSatBPart1, velSatBPart2, velSatB, 3); - - double normVelSatB = VectorOperations::norm(velSatB, 3); - double normRefSatRate = normVelSatB / normTargetDirB; - - double satRateDir[3] = {0, 0, 0}; - VectorOperations::cross(velSatB, targetDirB, satRateDir); - VectorOperations::normalize(satRateDir, satRateDir, 3); - VectorOperations::mulScalar(satRateDir, normRefSatRate, targetSatRotRate, 3); - - //------------------------------------------------------------------------------------- - // Calculation of reference rotation rate in case of star tracker blinding - //------------------------------------------------------------------------------------- - if (acsParameters->targetModeControllerParameters.avoidBlindStr) { - double sunDirB[3] = {0, 0, 0}; - MatrixOperations::multiply(*dcmBI, sunDirI, sunDirB, 3, 3, 1); - - double exclAngle = acsParameters->strParameters.exclusionAngle, - blindStart = acsParameters->targetModeControllerParameters.blindAvoidStart, - blindEnd = acsParameters->targetModeControllerParameters.blindAvoidStop; - double sightAngleSun = - VectorOperations::dot(acsParameters->strParameters.boresightAxis, sunDirB); - - if (!(strBlindAvoidFlag)) { - double critSightAngle = blindStart * exclAngle; - if (sightAngleSun < critSightAngle) { - strBlindAvoidFlag = true; - } - } else { - if (sightAngleSun < blindEnd * exclAngle) { - double normBlindRefRate = acsParameters->targetModeControllerParameters.blindRotRate; - double blindRefRate[3] = {0, 0, 0}; - if (sunDirB[1] < 0) { - blindRefRate[0] = normBlindRefRate; - blindRefRate[1] = 0; - blindRefRate[2] = 0; - } else { - blindRefRate[0] = -normBlindRefRate; - blindRefRate[1] = 0; - blindRefRate[2] = 0; - } - VectorOperations::add(blindRefRate, targetSatRotRate, targetSatRotRate, 3); - } else { - strBlindAvoidFlag = false; - } - } - } - // revert calculated quaternion from qBX to qIX - double quatIB[4] = {0, 0, 0, 1}; - QuaternionOperations::inverse(quatBI, quatIB); - QuaternionOperations::multiply(quatIB, targetQuat, targetQuat); + // calculate of reference rotation rate + targetRotationRate(timeDelta, targetQuat, targetSatRotRate); } -void Guidance::targetQuatPtgThreeAxes(const timeval timeAbsolute, const double timeDelta, - double posSatE[3], double velSatE[3], double targetQuat[4], - double targetSatRotRate[3]) { +void Guidance::targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta, double posSatF[3], + double velSatF[3], double targetQuat[4], + double targetSatRotRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion for target pointing //------------------------------------------------------------------------------------- // transform longitude, latitude and altitude to cartesian coordiantes (ECEF) - double targetE[3] = {0, 0, 0}; + double targetF[3] = {0, 0, 0}; MathOperations::cartesianFromLatLongAlt( acsParameters->targetModeControllerParameters.latitudeTgt, acsParameters->targetModeControllerParameters.longitudeTgt, - acsParameters->targetModeControllerParameters.altitudeTgt, targetE); - double targetDirE[3] = {0, 0, 0}; - VectorOperations::subtract(targetE, posSatE, targetDirE, 3); - - // transformation between ECEF and ECI frame - double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::ecfToEciWithNutPre(timeAbsolute, *dcmEI, *dcmEIDot); - MathOperations::inverseMatrixDimThree(*dcmEI, *dcmIE); - - double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot); + acsParameters->targetModeControllerParameters.altitudeTgt, targetF); + double targetDirF[3] = {0, 0, 0}; + VectorOperations::subtract(targetF, posSatF, targetDirF, 3); // target direction in the ECI frame double posSatI[3] = {0, 0, 0}, targetI[3] = {0, 0, 0}, targetDirI[3] = {0, 0, 0}; - MatrixOperations::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1); - MatrixOperations::multiply(*dcmIE, targetE, targetI, 3, 3, 1); + CoordinateTransformations::positionEcfToEci(posSatF, posSatI, &timeAbsolute); + CoordinateTransformations::positionEcfToEci(targetF, targetI, &timeAbsolute); VectorOperations::subtract(targetI, posSatI, targetDirI, 3); // x-axis aligned with target direction // this aligns with the camera, E- and S-band antennas - double xAxis[3] = {0, 0, 0}; - VectorOperations::normalize(targetDirI, xAxis, 3); + double xAxisXI[3] = {0, 0, 0}; + VectorOperations::normalize(targetDirI, xAxisXI, 3); // transform velocity into inertial frame - double velocityI[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0}; - MatrixOperations::multiply(*dcmIE, velSatE, velPart1, 3, 3, 1); - MatrixOperations::multiply(*dcmIEDot, posSatE, velPart2, 3, 3, 1); - VectorOperations::add(velPart1, velPart2, velocityI, 3); + double velSatI[3] = {0, 0, 0}; + CoordinateTransformations::velocityEcfToEci(velSatF, posSatF, velSatI, &timeAbsolute); // orbital normal vector of target and velocity vector double orbitalNormalI[3] = {0, 0, 0}; - VectorOperations::cross(posSatI, velocityI, orbitalNormalI); + VectorOperations::cross(posSatI, velSatI, orbitalNormalI); VectorOperations::normalize(orbitalNormalI, orbitalNormalI, 3); // y-axis of satellite in orbit plane so that z-axis is parallel to long side of picture // resolution - double yAxis[3] = {0, 0, 0}; - VectorOperations::cross(orbitalNormalI, xAxis, yAxis); - VectorOperations::normalize(yAxis, yAxis, 3); + double yAxisXI[3] = {0, 0, 0}; + VectorOperations::cross(orbitalNormalI, xAxisXI, yAxisXI); + VectorOperations::normalize(yAxisXI, yAxisXI, 3); // z-axis completes RHS - double zAxis[3] = {0, 0, 0}; - VectorOperations::cross(xAxis, yAxis, zAxis); + double zAxisXI[3] = {0, 0, 0}; + VectorOperations::cross(xAxisXI, yAxisXI, zAxisXI); // join transformation matrix - double dcmIX[3][3] = {{xAxis[0], yAxis[0], zAxis[0]}, - {xAxis[1], yAxis[1], zAxis[1]}, - {xAxis[2], yAxis[2], zAxis[2]}}; + double dcmIX[3][3] = {{xAxisXI[0], yAxisXI[0], zAxisXI[0]}, + {xAxisXI[1], yAxisXI[1], zAxisXI[1]}, + {xAxisXI[2], yAxisXI[2], zAxisXI[2]}}; QuaternionOperations::fromDcm(dcmIX, targetQuat); - int8_t timeElapsedMax = acsParameters->targetModeControllerParameters.timeElapsedMax; - targetRotationRate(timeElapsedMax, timeDelta, targetQuat, targetSatRotRate); + targetRotationRate(timeDelta, targetQuat, targetSatRotRate); } -void Guidance::targetQuatPtgGs(const timeval timeAbsolute, const double timeDelta, - double posSatE[3], double sunDirI[3], double targetQuat[4], +void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, double posSatF[3], + double sunDirI[3], double targetQuat[4], double targetSatRotRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion for ground station pointing //------------------------------------------------------------------------------------- // transform longitude, latitude and altitude to cartesian coordiantes (ECEF) - double groundStationE[3] = {0, 0, 0}; - + double posGroundStationF[3] = {0, 0, 0}; MathOperations::cartesianFromLatLongAlt( acsParameters->gsTargetModeControllerParameters.latitudeTgt, acsParameters->gsTargetModeControllerParameters.longitudeTgt, - acsParameters->gsTargetModeControllerParameters.altitudeTgt, groundStationE); - double targetDirE[3] = {0, 0, 0}; - VectorOperations::subtract(groundStationE, posSatE, targetDirE, 3); - - // transformation between ECEF and ECI frame - double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::ecfToEciWithNutPre(timeAbsolute, *dcmEI, *dcmEIDot); - MathOperations::inverseMatrixDimThree(*dcmEI, *dcmIE); - - double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot); + acsParameters->gsTargetModeControllerParameters.altitudeTgt, posGroundStationF); // target direction in the ECI frame - double posSatI[3] = {0, 0, 0}, groundStationI[3] = {0, 0, 0}, groundStationDirI[3] = {0, 0, 0}; - MatrixOperations::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1); - MatrixOperations::multiply(*dcmIE, groundStationE, groundStationI, 3, 3, 1); - VectorOperations::subtract(groundStationI, posSatI, groundStationDirI, 3); + double posSatI[3] = {0, 0, 0}, posGroundStationI[3] = {0, 0, 0}, groundStationDirI[3] = {0, 0, 0}; + CoordinateTransformations::positionEcfToEci(posSatF, posSatI, &timeAbsolute); + CoordinateTransformations::positionEcfToEci(posGroundStationI, posGroundStationI, &timeAbsolute); + VectorOperations::subtract(posGroundStationI, posSatI, groundStationDirI, 3); // negative x-axis aligned with target direction // this aligns with the camera, E- and S-band antennas - double xAxis[3] = {0, 0, 0}; - VectorOperations::normalize(groundStationDirI, xAxis, 3); - VectorOperations::mulScalar(xAxis, -1, xAxis, 3); + double xAxisXI[3] = {0, 0, 0}; + VectorOperations::normalize(groundStationDirI, xAxisXI, 3); + VectorOperations::mulScalar(xAxisXI, -1, xAxisXI, 3); // get sun vector model in ECI VectorOperations::normalize(sunDirI, sunDirI, 3); // calculate z-axis as projection of sun vector into plane defined by x-axis as normal vector // z = sPerpenticular = s - sParallel = s - (x*s)/norm(x)^2 * x - double xDotS = VectorOperations::dot(xAxis, sunDirI); - xDotS /= pow(VectorOperations::norm(xAxis, 3), 2); - double sunParallel[3], zAxis[3]; - VectorOperations::mulScalar(xAxis, xDotS, sunParallel, 3); - VectorOperations::subtract(sunDirI, sunParallel, zAxis, 3); - VectorOperations::normalize(zAxis, zAxis, 3); + double xDotS = VectorOperations::dot(xAxisXI, sunDirI); + xDotS /= pow(VectorOperations::norm(xAxisXI, 3), 2); + double sunParallel[3], zAxisXI[3]; + VectorOperations::mulScalar(xAxisXI, xDotS, sunParallel, 3); + VectorOperations::subtract(sunDirI, sunParallel, zAxisXI, 3); + VectorOperations::normalize(zAxisXI, zAxisXI, 3); // y-axis completes RHS - double yAxis[3]; - VectorOperations::cross(zAxis, xAxis, yAxis); - VectorOperations::normalize(yAxis, yAxis, 3); + double yAxisXI[3]; + VectorOperations::cross(zAxisXI, xAxisXI, yAxisXI); + VectorOperations::normalize(yAxisXI, yAxisXI, 3); // join transformation matrix - double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]}, - {xAxis[1], yAxis[1], zAxis[1]}, - {xAxis[2], yAxis[2], zAxis[2]}}; - QuaternionOperations::fromDcm(dcmTgt, targetQuat); + double dcmXI[3][3] = {{xAxisXI[0], yAxisXI[0], zAxisXI[0]}, + {xAxisXI[1], yAxisXI[1], zAxisXI[1]}, + {xAxisXI[2], yAxisXI[2], zAxisXI[2]}}; + QuaternionOperations::fromDcm(dcmXI, targetQuat); - int8_t timeElapsedMax = acsParameters->gsTargetModeControllerParameters.timeElapsedMax; - targetRotationRate(timeElapsedMax, timeDelta, targetQuat, targetSatRotRate); + targetRotationRate(timeDelta, targetQuat, targetSatRotRate); } -void Guidance::targetQuatPtgSun(double timeDelta, double sunDirI[3], double targetQuat[4], - double targetSatRotRate[3]) { - //------------------------------------------------------------------------------------- - // Calculation of target quaternion to sun - //------------------------------------------------------------------------------------- - // positive z-Axis of EIVE in direction of sun - double zAxis[3] = {0, 0, 0}; - VectorOperations::normalize(sunDirI, zAxis, 3); - - // assign helper vector (north pole inertial) - double helperVec[3] = {0, 0, 1}; - - // construct y-axis from helper vector and z-axis - double yAxis[3] = {0, 0, 0}; - VectorOperations::cross(zAxis, helperVec, yAxis); - VectorOperations::normalize(yAxis, yAxis, 3); - - // x-axis completes RHS - double xAxis[3] = {0, 0, 0}; - VectorOperations::cross(yAxis, zAxis, xAxis); - VectorOperations::normalize(xAxis, xAxis, 3); - - // join transformation matrix - double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]}, - {xAxis[1], yAxis[1], zAxis[1]}, - {xAxis[2], yAxis[2], zAxis[2]}}; - QuaternionOperations::fromDcm(dcmTgt, targetQuat); - - //---------------------------------------------------------------------------- - // Calculation of reference rotation rate - //---------------------------------------------------------------------------- - int8_t timeElapsedMax = acsParameters->gsTargetModeControllerParameters.timeElapsedMax; - targetRotationRate(timeElapsedMax, timeDelta, targetQuat, targetSatRotRate); -} - -[[deprecated]] void Guidance::targetQuatPtgNadirSingleAxis(const timeval timeAbsolute, - double posSatE[3], double quatBI[4], - double targetQuat[4], double refDirB[3], - double refSatRate[3]) { +void Guidance::targetQuatPtgNadirTarget(timeval timeAbsolute, const double timeDelta, + double posSatE[3], double velSatE[3], double targetQuat[4], + double refSatRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion for Nadir pointing //------------------------------------------------------------------------------------- - double targetDirE[3] = {0, 0, 0}; - VectorOperations::mulScalar(posSatE, -1, targetDirE, 3); - - // transformation between ECEF and ECI frame - double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::ecfToEciWithNutPre(timeAbsolute, *dcmEI, *dcmEIDot); - MathOperations::inverseMatrixDimThree(*dcmEI, *dcmIE); - - double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot); - - // transformation between ECEF and Body frame - double dcmBI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - QuaternionOperations::toDcm(quatBI, dcmBI); - MatrixOperations::multiply(*dcmBI, *dcmIE, *dcmBE, 3, 3, 3); - - // target Direction in the body frame - double targetDirB[3] = {0, 0, 0}; - MatrixOperations::multiply(*dcmBE, targetDirE, targetDirB, 3, 3, 1); - - // rotation quaternion from two vectors - double refDir[3] = {0, 0, 0}; - refDir[0] = acsParameters->nadirModeControllerParameters.refDirection[0]; - refDir[1] = acsParameters->nadirModeControllerParameters.refDirection[1]; - refDir[2] = acsParameters->nadirModeControllerParameters.refDirection[2]; - double noramlizedTargetDirB[3] = {0, 0, 0}; - VectorOperations::normalize(targetDirB, noramlizedTargetDirB, 3); - VectorOperations::normalize(refDir, refDir, 3); - double normTargetDirB = VectorOperations::norm(noramlizedTargetDirB, 3); - double normRefDir = VectorOperations::norm(refDir, 3); - double crossDir[3] = {0, 0, 0}; - double dotDirections = VectorOperations::dot(noramlizedTargetDirB, refDir); - VectorOperations::cross(noramlizedTargetDirB, refDir, crossDir); - targetQuat[0] = crossDir[0]; - targetQuat[1] = crossDir[1]; - targetQuat[2] = crossDir[2]; - targetQuat[3] = sqrt(pow(normTargetDirB, 2) * pow(normRefDir, 2) + dotDirections); - VectorOperations::normalize(targetQuat, targetQuat, 4); - - //------------------------------------------------------------------------------------- - // Calculation of reference rotation rate - //------------------------------------------------------------------------------------- - refSatRate[0] = 0; - refSatRate[1] = 0; - refSatRate[2] = 0; - - // revert calculated quaternion from qBX to qIX - double quatIB[4] = {0, 0, 0, 1}; - QuaternionOperations::inverse(quatBI, quatIB); - QuaternionOperations::multiply(quatIB, targetQuat, targetQuat); -} - -void Guidance::targetQuatPtgNadirThreeAxes(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 - //------------------------------------------------------------------------------------- - // transformation between ECEF and ECI frame - double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::ecfToEciWithNutPre(timeAbsolute, *dcmEI, *dcmEIDot); - MathOperations::inverseMatrixDimThree(*dcmEI, *dcmIE); - - double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot); // satellite position in inertial reference frame double posSatI[3] = {0, 0, 0}; - MatrixOperations::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1); + CoordinateTransformations::positionEcfToEci(posSatE, posSatI, &timeAbsolute); // negative x-axis aligned with position vector // this aligns with the camera, E- and S-band antennas - double xAxis[3] = {0, 0, 0}; - VectorOperations::normalize(posSatI, xAxis, 3); - VectorOperations::mulScalar(xAxis, -1, xAxis, 3); + double xAxisXI[3] = {0, 0, 0}; + VectorOperations::normalize(posSatI, xAxisXI, 3); + VectorOperations::mulScalar(xAxisXI, -1, xAxisXI, 3); // make z-Axis parallel to major part of camera resolution - double zAxis[3] = {0, 0, 0}; - double velocityI[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0}; - MatrixOperations::multiply(*dcmIE, velSatE, velPart1, 3, 3, 1); - MatrixOperations::multiply(*dcmIEDot, posSatE, velPart2, 3, 3, 1); - VectorOperations::add(velPart1, velPart2, velocityI, 3); - VectorOperations::cross(xAxis, velocityI, zAxis); - VectorOperations::normalize(zAxis, zAxis, 3); + double zAxisXI[3] = {0, 0, 0}; + double velSatI[3] = {0, 0, 0}; + CoordinateTransformations::velocityEcfToEci(velSatE, posSatE, velSatI, &timeAbsolute); + VectorOperations::cross(xAxisXI, velSatI, zAxisXI); + VectorOperations::normalize(zAxisXI, zAxisXI, 3); // y-Axis completes RHS - double yAxis[3] = {0, 0, 0}; - VectorOperations::cross(zAxis, xAxis, yAxis); + double yAxisXI[3] = {0, 0, 0}; + VectorOperations::cross(zAxisXI, xAxisXI, yAxisXI); // join transformation matrix - double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]}, - {xAxis[1], yAxis[1], zAxis[1]}, - {xAxis[2], yAxis[2], zAxis[2]}}; - QuaternionOperations::fromDcm(dcmTgt, targetQuat); + double dcmXI[3][3] = {{xAxisXI[0], yAxisXI[0], zAxisXI[0]}, + {xAxisXI[1], yAxisXI[1], zAxisXI[1]}, + {xAxisXI[2], yAxisXI[2], zAxisXI[2]}}; + QuaternionOperations::fromDcm(dcmXI, targetQuat); - int8_t timeElapsedMax = acsParameters->nadirModeControllerParameters.timeElapsedMax; - targetRotationRate(timeElapsedMax, timeDelta, targetQuat, refSatRate); + targetRotationRate(timeDelta, targetQuat, refSatRate); +} + +void Guidance::targetRotationRate(const double timeDelta, double quatIX[4], double *refSatRate) { + if (VectorOperations::norm(quatIXprev, 4) == 0) { + std::memcpy(quatIXprev, quatIX, sizeof(quatIXprev)); + } + if (timeDelta != 0.0) { + QuaternionOperations::rotationFromQuaternions(quatIX, quatIXprev, timeDelta, refSatRate); + } else { + std::memcpy(refSatRate, ZERO_VEC3, 3 * sizeof(double)); + } + std::memcpy(quatIXprev, quatIX, sizeof(quatIXprev)); } void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4], @@ -438,19 +224,6 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do errorQuat, errorSatRotRate, errorAngle); } -void Guidance::targetRotationRate(const int8_t timeElapsedMax, const double timeDelta, - double quatIX[4], double *refSatRate) { - if (VectorOperations::norm(quatIXprev, 4) == 0) { - std::memcpy(quatIXprev, quatIX, sizeof(quatIXprev)); - } - if (timeDelta != 0.0) { - QuaternionOperations::rotationFromQuaternions(quatIX, quatIXprev, timeDelta, refSatRate); - } else { - std::memcpy(refSatRate, ZERO_VEC3, 3 * sizeof(double)); - } - std::memcpy(quatIXprev, quatIX, sizeof(quatIXprev)); -} - ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv) { bool rw1valid = (sensorValues->rw1Set.state.value and sensorValues->rw1Set.state.isValid()); diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index bdf6d7fb..5afcf7b8 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -1,6 +1,7 @@ #ifndef GUIDANCE_H_ #define GUIDANCE_H_ +#include #include #include #include @@ -24,33 +25,18 @@ class Guidance { ReturnValue_t solarArrayDeploymentComplete(); void resetValues(); - // Function to get the target quaternion and reference rotation rate from gps position and - // position of the ground station - 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], + void targetQuatPtgIdle(const double timeDelta, double sunDirI[3], double targetQuat[4], + double targetSatRotRate[3]); + void targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta, double posSatF[3], + double velSatE[3], double quatIX[4], double targetSatRotRate[3]); + void targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, double posSatF[3], double sunDirI[3], double quatIX[4], double targetSatRotRate[3]); + void targetQuatPtgNadirTarget(timeval timeAbsolute, const double timeDelta, double posSatF[3], + double velSatF[3], double targetQuat[4], double refSatRate[3]); - // Function to get the target quaternion and reference rotation rate for sun pointing after ground - // station - void targetQuatPtgSun(const double timeDelta, double sunDirI[3], double targetQuat[4], - double targetSatRotRate[3]); + void targetRotationRate(const double timeDelta, double quatInertialTarget[4], + double *targetSatRotRate); - // Function to get the target quaternion and refence rotation rate from gps position for Nadir - // pointing - void targetQuatPtgNadirSingleAxis(const timeval timeAbsolute, double posSatE[3], double quatBI[4], - double targetQuat[4], double refDirB[3], 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 - // and a desired satellite rotational rate is calculated, again considering a reference rotational - // rate. Lastly gives back the error angle of the error quaternion. void comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4], double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3], double errorQuat[4], double errorSatRotRate[3], double &errorAngle); @@ -58,11 +44,6 @@ class Guidance { double targetSatRotRate[3], double errorQuat[4], double errorSatRotRate[3], double &errorAngle); - 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" ReturnValue_t getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv); private: From e7dc9cddfd17b033ac733319110dbc59cf00701b Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 2 Feb 2024 14:33:50 +0100 Subject: [PATCH 05/12] nadir target lul --- mission/controller/AcsController.cpp | 5 ++--- mission/controller/acs/Guidance.cpp | 5 ++--- mission/controller/acs/Guidance.h | 4 ++-- 3 files changed, 6 insertions(+), 8 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 0c85560c..99d28d8b 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -483,9 +483,8 @@ void AcsController::performPointingCtrl() { break; case acs::PTG_NADIR: - guidance.targetQuatPtgNadirTarget(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value, - gpsDataProcessed.gpsVelocity.value, targetQuat, - targetSatRotRate); + guidance.targetQuatPtgNadir(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value, + gpsDataProcessed.gpsVelocity.value, targetQuat, targetSatRotRate); guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate, acsParameters.nadirModeControllerParameters.quatRef, acsParameters.nadirModeControllerParameters.refRotRate, errorQuat, diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 7413265b..46d98599 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -141,9 +141,8 @@ void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, dou targetRotationRate(timeDelta, targetQuat, targetSatRotRate); } -void Guidance::targetQuatPtgNadirTarget(timeval timeAbsolute, const double timeDelta, - double posSatE[3], double velSatE[3], double targetQuat[4], - double refSatRate[3]) { +void Guidance::targetQuatPtgNadir(timeval timeAbsolute, const double timeDelta, double posSatE[3], + double velSatE[3], double targetQuat[4], double refSatRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion for Nadir pointing //------------------------------------------------------------------------------------- diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index 5afcf7b8..15e88cdc 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -31,8 +31,8 @@ class Guidance { double velSatE[3], double quatIX[4], double targetSatRotRate[3]); void targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, double posSatF[3], double sunDirI[3], double quatIX[4], double targetSatRotRate[3]); - void targetQuatPtgNadirTarget(timeval timeAbsolute, const double timeDelta, double posSatF[3], - double velSatF[3], double targetQuat[4], double refSatRate[3]); + void targetQuatPtgNadir(timeval timeAbsolute, const double timeDelta, double posSatF[3], + double velSatF[3], double targetQuat[4], double refSatRate[3]); void targetRotationRate(const double timeDelta, double quatInertialTarget[4], double *targetSatRotRate); From 15cddfdeb76c6ccff225e2e9452df4a9ded5bc5b Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 2 Feb 2024 15:04:26 +0100 Subject: [PATCH 06/12] try to point STR away from earth during idle --- mission/controller/acs/Guidance.cpp | 20 ++++++++++---------- mission/controller/acs/Guidance.h | 4 ++-- 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 46d98599..89f3d958 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -4,24 +4,24 @@ Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = acsParameter Guidance::~Guidance() {} -void Guidance::targetQuatPtgIdle(double timeDelta, double sunDirI[3], double targetQuat[4], - double targetSatRotRate[3]) { - //------------------------------------------------------------------------------------- - // Calculation of target quaternion to sun - //------------------------------------------------------------------------------------- - // positive z-Axis of EIVE in direction of sun +void Guidance::targetQuatPtgIdle(const double timeDelta, timeval timeAbsolute, + const double sunDirI[3], const double posSatF[4], + double targetQuat[4], double targetSatRotRate[3]) { + // positive z-Axis of EIVE in direction of sun double zAxisXI[3] = {0, 0, 0}; VectorOperations::normalize(sunDirI, zAxisXI, 3); - // assign helper vector (north pole inertial) - double helperXI[3] = {0, 0, 1}; + // determine helper vector to point x-Axis and therefore the STR away from Earth + double helperXI[3] = {0, 0, 0}, posSatI[3] = {0, 0, 0}; + CoordinateTransformations::positionEcfToEci(posSatF, posSatI, &timeAbsolute); + VectorOperations::normalize(posSatI, helperXI, 3); - // construct y-axis from helper vector and z-axis + // construct y-axis from helper vector and z-axis double yAxisXI[3] = {0, 0, 0}; VectorOperations::cross(zAxisXI, helperXI, yAxisXI); VectorOperations::normalize(yAxisXI, yAxisXI, 3); - // x-axis completes RHS + // x-axis completes RHS double xAxisXI[3] = {0, 0, 0}; VectorOperations::cross(yAxisXI, zAxisXI, xAxisXI); VectorOperations::normalize(xAxisXI, xAxisXI, 3); diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index 15e88cdc..973c0bd3 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -25,8 +25,8 @@ class Guidance { ReturnValue_t solarArrayDeploymentComplete(); void resetValues(); - void targetQuatPtgIdle(const double timeDelta, double sunDirI[3], double targetQuat[4], - double targetSatRotRate[3]); + void targetQuatPtgIdle(const double timeDelta, timeval timeAbsolute, const double sunDirI[3], + const double posSatF[4], double targetQuat[4], double targetSatRotRate[3]); void targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta, double posSatF[3], double velSatE[3], double quatIX[4], double targetSatRotRate[3]); void targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, double posSatF[3], From f0e551fa54f1c4e9a1a36897294aad92f8f7a9f6 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 2 Feb 2024 15:08:55 +0100 Subject: [PATCH 07/12] fix --- mission/controller/AcsController.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 99d28d8b..67596fc9 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -420,8 +420,8 @@ void AcsController::performPointingCtrl() { switch (mode) { case acs::PTG_IDLE: - guidance.targetQuatPtgIdle(timeDelta, susDataProcessed.sunIjkModel.value, targetQuat, - targetSatRotRate); + guidance.targetQuatPtgIdle(timeDelta, timeAbsolute, susDataProcessed.sunIjkModel.value, + gpsDataProcessed.gpsPosition.value, targetQuat, targetSatRotRate); guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate, errorQuat, errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate, From 6f3876d20420515598d4e64cd70fbf5f6efb923d Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 5 Feb 2024 09:24:41 +0100 Subject: [PATCH 08/12] fixed quaternion multiplication bug --- mission/controller/acs/Guidance.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 89f3d958..e4e9ccab 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -192,10 +192,12 @@ void Guidance::targetRotationRate(const double timeDelta, double quatIX[4], doub void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4], double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3], double errorQuat[4], double errorSatRotRate[3], double &errorAngle) { - // First calculate error quaternion between current and target orientation - QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat); - // Last calculate add rotation from reference quaternion - QuaternionOperations::multiply(refQuat, errorQuat, errorQuat); + // First calculate error quaternion between current and target orientation without reference + // quaternion + double errorQuatWoRef[4] = {0, 0, 0, 0}; + QuaternionOperations::multiply(currentQuat, targetQuat, errorQuatWoRef); + // Then add rotation from reference quaternion + QuaternionOperations::multiply(refQuat, errorQuatWoRef, errorQuat); // Keep scalar part of quaternion positive if (errorQuat[3] < 0) { VectorOperations::mulScalar(errorQuat, -1, errorQuat, 4); From 6ca1dda807f16c03d4887528f3fbfabd014c2a6b Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 5 Feb 2024 09:39:35 +0100 Subject: [PATCH 09/12] fix my ocd --- mission/controller/AcsController.cpp | 2 +- mission/controller/acs/Guidance.cpp | 2 +- mission/controller/acs/Guidance.h | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 67596fc9..f5f246c2 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -420,7 +420,7 @@ void AcsController::performPointingCtrl() { switch (mode) { case acs::PTG_IDLE: - guidance.targetQuatPtgIdle(timeDelta, timeAbsolute, susDataProcessed.sunIjkModel.value, + guidance.targetQuatPtgIdle(timeAbsolute, timeDelta, susDataProcessed.sunIjkModel.value, gpsDataProcessed.gpsPosition.value, targetQuat, targetSatRotRate); guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate, errorQuat, errorSatRotRate, errorAngle); diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index e4e9ccab..3641e41e 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -4,7 +4,7 @@ Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = acsParameter Guidance::~Guidance() {} -void Guidance::targetQuatPtgIdle(const double timeDelta, timeval timeAbsolute, +void Guidance::targetQuatPtgIdle(timeval timeAbsolute, const double timeDelta, const double sunDirI[3], const double posSatF[4], double targetQuat[4], double targetSatRotRate[3]) { // positive z-Axis of EIVE in direction of sun diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index 973c0bd3..9835b762 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -25,7 +25,7 @@ class Guidance { ReturnValue_t solarArrayDeploymentComplete(); void resetValues(); - void targetQuatPtgIdle(const double timeDelta, timeval timeAbsolute, const double sunDirI[3], + void targetQuatPtgIdle(timeval timeAbsolute, const double timeDelta, const double sunDirI[3], const double posSatF[4], double targetQuat[4], double targetSatRotRate[3]); void targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta, double posSatF[3], double velSatE[3], double quatIX[4], double targetSatRotRate[3]); From ecc147ca7f877a2edcd61e31895fb05133a445d1 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 5 Feb 2024 09:43:20 +0100 Subject: [PATCH 10/12] changelog --- CHANGELOG.md | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 21792d0e..c44238ee 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,15 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +## Changed + +- Guidance now uses the coordinate functions from the FSFW. +- Idle should now point the STR away from the earth + +## Fixed + +- Fixed bugs in `Guidance::comparePtg` and corrected overloading + # [v7.6.0] 2024-01-30 - Bumped `eive-tmtc` to v5.13.0 From 484dba7eb6ec4526b89697c75771e0539ce9e6b0 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 5 Feb 2024 10:10:41 +0100 Subject: [PATCH 11/12] changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 0ec0f64f..41b80a3d 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,8 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +# [v7.6.1] 2024-02-05 + ## Changed - Guidance now uses the coordinate functions from the FSFW. From 681aebe011fa2c8d56edc99b5840573e9601fdbe Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 5 Feb 2024 10:11:01 +0100 Subject: [PATCH 12/12] bump version --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8957c6bf..a25fb8bc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,7 +11,7 @@ cmake_minimum_required(VERSION 3.13) set(OBSW_VERSION_MAJOR 7) set(OBSW_VERSION_MINOR 6) -set(OBSW_VERSION_REVISION 0) +set(OBSW_VERSION_REVISION 1) # set(CMAKE_VERBOSE TRUE)