From 04e1cb52acd98fe95a8113741e78af6b06ed215d Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 20 Feb 2023 15:59:01 +0100 Subject: [PATCH] when did i push this last --- mission/controller/AcsController.cpp | 38 ++-- mission/controller/acs/AcsParameters.h | 16 +- mission/controller/acs/Guidance.cpp | 184 +++++++++----------- mission/controller/acs/Guidance.h | 8 +- mission/controller/acs/SensorProcessing.cpp | 7 - 5 files changed, 120 insertions(+), 133 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index ccdeb077..172ea4ff 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -249,26 +249,21 @@ void AcsController::performPointingCtrl() { satRotRate[3] = {0, 0, 0}, errorSatRotRate[3] = {0, 0, 0}; switch (submode) { case acs::PTG_IDLE: - guidance.targetQuatPtgSun(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now, - targetQuat, refSatRate); - std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef, - 4 * sizeof(double)); - enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; - guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, - deltaRate); - ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate, - *rwPseudoInv, torquePtgRws); + guidance.targetQuatPtgSun(susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate); + guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, + targetSatRotRate, errorQuat, errorSatRotRate, errorAngle); ptgCtrl.ptgNullspace( - &acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), + &acsParameters.idleModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); ptgCtrl.ptgDesaturation( - &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, + &acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); + enableAntiStiction = acsParameters.idleModeControllerParameters.enableAntiStiction; break; case acs::PTG_TARGET: @@ -296,17 +291,15 @@ void AcsController::performPointingCtrl() { mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); + enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; break; case acs::PTG_TARGET_GS: - guidance.targetQuatPtgGs(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now, - targetQuat, refSatRate); - std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef, - 4 * sizeof(double)); - enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; - guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, - deltaRate); - ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate, + guidance.targetQuatPtgGs(now, gpsDataProcessed.gpsPosition.value, + susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate); + guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, + targetSatRotRate, errorQuat, errorSatRotRate, errorAngle); + ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); ptgCtrl.ptgNullspace( &acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), @@ -319,16 +312,16 @@ void AcsController::performPointingCtrl() { mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); + enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; break; case acs::PTG_NADIR: guidance.targetQuatPtgNadirThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat, refSatRate); std::memcpy(quatRef, acsParameters.nadirModeControllerParameters.quatRef, 4 * sizeof(double)); - enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction; guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, deltaRate); - ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, quatError, deltaRate, + ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); ptgCtrl.ptgNullspace( &acsParameters.nadirModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), @@ -341,13 +334,13 @@ void AcsController::performPointingCtrl() { mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); + enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction; break; case acs::PTG_INERTIAL: guidance.targetQuatPtgInertial(targetQuat, refSatRate); std::memcpy(quatRef, acsParameters.inertialModeControllerParameters.quatRef, 4 * sizeof(double)); - enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction; guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, deltaRate); ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, quatError, deltaRate, @@ -363,6 +356,7 @@ void AcsController::performPointingCtrl() { mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); + enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction; break; } diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 365a04bd..f0448b67 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -841,10 +841,12 @@ class AcsParameters : public HasParametersIF { uint8_t enableAntiStiction = true; } pointingLawParameters; + struct IdleModeControllerParameters : PointingLawParameters { + } idleModeControllerParameters; + struct TargetModeControllerParameters : PointingLawParameters { double refDirection[3] = {-1, 0, 0}; // Antenna - double refRotRate[3] = {0, 0, 0}; // Not used atm, do we want an option to - // give this as an input- currently en calculation is done + double refRotRate[3] = {0, 0, 0}; double quatRef[4] = {0, 0, 0, 1}; int8_t timeElapsedMax = 10; // rot rate calculations @@ -860,6 +862,16 @@ class AcsParameters : public HasParametersIF { double blindRotRate = 1 * M_PI / 180; } targetModeControllerParameters; + struct GsTargetModeControllerParameters : PointingLawParameters { + double refDirection[3] = {-1, 0, 0}; // Antenna + int8_t timeElapsedMax = 10; // rot rate calculations + + // Default is Stuttgart GS + double latitudeTgt = 48.7495 * M_PI / 180.; // [rad] Latitude + double longitudeTgt = 9.10384 * M_PI / 180.; // [rad] Longitude + double altitudeTgt = 500; // [m] + } gsTargetModeControllerParameters; + struct NadirModeControllerParameters : PointingLawParameters { double refDirection[3] = {-1, 0, 0}; // Antenna double quatRef[4] = {0, 0, 0, 1}; diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 4d8dfa88..f2d3cceb 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -17,12 +17,12 @@ Guidance::Guidance(AcsParameters *acsParameters_) : acsParameters(*acsParameters Guidance::~Guidance() {} void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double targetQuat[4], - double refSatRate[3]) { + double targetSatRotRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion to groundstation or given latitude, longitude and altitude //------------------------------------------------------------------------------------- - // Transform longitude, latitude and altitude to cartesian coordiantes (earth - // fixed/centered frame) + // transform longitude, latitude and altitude to cartesian coordiantes (earth + // fixed/centered frame) double targetCart[3] = {0, 0, 0}; MathOperations::cartesianFromLatLongAlt( @@ -30,19 +30,19 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ta acsParameters.targetModeControllerParameters.longitudeTgt, acsParameters.targetModeControllerParameters.altitudeTgt, targetCart); - // Target direction in the ECEF frame + // target direction in the ECEF frame double targetDirE[3] = {0, 0, 0}; VectorOperations::subtract(targetCart, posSatE, targetDirE, 3); - // Transformation between ECEF and ECI frame - double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot); - MathOperations::inverseMatrixDimThree(*dcmEJ, *dcmJE); + // transformation between ECEF and ECI frame + double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot); + MathOperations::inverseMatrixDimThree(*dcmEI, *dcmIE); - double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot); + double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot); // Transformation between ECEF and Body frame // double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; @@ -52,20 +52,16 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ta // QuaternionOperations::toDcm(quatBJ, dcmBJ); // MatrixOperations::multiply(*dcmBJ, *dcmJE, *dcmBE, 3, 3, 3); - // Target Direction in the body frame + // target Direction in the body frame double targetDirB[3] = {0, 0, 0}; MatrixOperations::multiply(*dcmBE, targetDirE, targetDirB, 3, 3, 1); // rotation quaternion from two vectors - 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 refDirB[3] = {0, 0, 0}; + std::memcpy(refDirB, acsParameters.targetModeControllerParameters.refDirection, + 3 * sizeof(double)); double normTargetDirB = VectorOperations::norm(noramlizedTargetDirB, 3); - double normRefDir = VectorOperations::norm(refDir, 3); + double normRefDirB = VectorOperations::norm(refDir, 3); double crossDir[3] = {0, 0, 0}; double dotDirections = VectorOperations::dot(noramlizedTargetDirB, refDir); VectorOperations::cross(noramlizedTargetDirB, refDir, crossDir); @@ -152,7 +148,7 @@ void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double vel //------------------------------------------------------------------------------------- // Calculation of target quaternion for target pointing //------------------------------------------------------------------------------------- - // transform longitude, latitude and altitude to cartesian coordiantes (ECEF) + // transform longitude, latitude and altitude to cartesian coordiantes (ECEF) double targetE[3] = {0, 0, 0}; MathOperations::cartesianFromLatLongAlt( acsParameters.targetModeControllerParameters.latitudeTgt, @@ -161,7 +157,7 @@ void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double vel double targetDirE[3] = {0, 0, 0}; VectorOperations::subtract(targetE, posSatE, targetDirE, 3); - // transformation between ECEF and ECI frame + // 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}}; @@ -171,7 +167,7 @@ void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double vel double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; MathOperations::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot); - // target direction and position vector in the inertial frame + // target direction and position vector in the inertial frame double targetDirI[3] = {0, 0, 0}, posSatI[3] = {0, 0, 0}; MatrixOperations::multiply(*dcmIE, targetDirE, targetDirI, 3, 3, 1); MatrixOperations::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1); @@ -213,133 +209,96 @@ void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double vel targetRotationRate(timeElapsedMax, now, quatIX, targetSatRotRate); } -void Guidance::targetQuatPtgGs(timeval now, double targetQuat[4], double refSatRate[3]) { +void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3], double quatIX[4], + double targetSatRotRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion for ground station pointing //------------------------------------------------------------------------------------- - // Transform longitude, latitude and altitude to cartesian coordiantes (earth - // fixed/centered frame) - double groundStationCart[3] = {0, 0, 0}; + // transform longitude, latitude and altitude to cartesian coordiantes (ECEF) + double groundStationE[3] = {0, 0, 0}; MathOperations::cartesianFromLatLongAlt( - acsParameters.targetModeControllerParameters.latitudeTgt, - acsParameters.targetModeControllerParameters.longitudeTgt, - acsParameters.targetModeControllerParameters.altitudeTgt, groundStationCart); + acsParameters.gsTargetModeControllerParameters.latitudeTgt, + acsParameters.gsTargetModeControllerParameters.longitudeTgt, + acsParameters.gsTargetModeControllerParameters.altitudeTgt, groundStationE); double targetDirE[3] = {0, 0, 0}; - VectorOperations::subtract(groundStationCart, posSatE, targetDirE, 3); + VectorOperations::subtract(groundStationE, posSatE, targetDirE, 3); - // Transformation between ECEF and ECI frame - double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot); - MathOperations::inverseMatrixDimThree(*dcmEJ, *dcmJE); + // transformation between ECEF and ECI frame + double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot); + MathOperations::inverseMatrixDimThree(*dcmEI, *dcmIE); - double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot); + double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot); - // Target Direction and position vector in the inertial frame - double targetDirJ[3] = {0, 0, 0}, posSatJ[3] = {0, 0, 0}; - MatrixOperations::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1); - MatrixOperations::multiply(*dcmJE, posSatE, posSatJ, 3, 3, 1); + // target direction and position vector in the inertial frame + double targetDirI[3] = {0, 0, 0}, posSatI[3] = {0, 0, 0}; + MatrixOperations::multiply(*dcmIE, targetDirE, targetDirI, 3, 3, 1); + MatrixOperations::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1); - // negative x-Axis aligned with target (Camera/E-band transmitter position) + // negative x-axis aligned with target + // this aligns with the camera, E- and S-band antennas double xAxis[3] = {0, 0, 0}; - VectorOperations::normalize(targetDirJ, xAxis, 3); + VectorOperations::normalize(targetDirI, xAxis, 3); VectorOperations::mulScalar(xAxis, -1, xAxis, 3); - // get Sun Vector Model in ECI - double sunJ[3]; - std::memcpy(sunJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double)); - VectorOperations::normalize(sunJ, sunJ, 3); + // 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, sunJ); + 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(sunJ, sunParallel, zAxis, 3); + VectorOperations::subtract(sunDirI, sunParallel, zAxis, 3); VectorOperations::normalize(zAxis, zAxis, 3); - // calculate y-axis + // y-axis completes RHS double yAxis[3]; VectorOperations::cross(zAxis, xAxis, yAxis); VectorOperations::normalize(yAxis, yAxis, 3); - // Complete transformation matrix + // 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]}}; double quatInertialTarget[4] = {0, 0, 0, 0}; QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget); - int8_t timeElapsedMax = acsParameters.targetModeControllerParameters.timeElapsedMax; - refRotationRate(timeElapsedMax, now, quatInertialTarget, refSatRate); - - // Transform in system relative to satellite frame - double quatBJ[4] = {0, 0, 0, 0}; - std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double)); - QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat); + int8_t timeElapsedMax = acsParameters.gsTargetModeControllerParameters.timeElapsedMax; + targetRotationRate(timeElapsedMax, now, quatInertialTarget, targetSatRotRate); } -void Guidance::targetQuatPtgSun(timeval now, double targetQuat[4], double refSatRate[3]) { +void Guidance::targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double refSatRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion to sun //------------------------------------------------------------------------------------- - double quatBJ[4] = {0, 0, 0, 0}; - double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double)); - QuaternionOperations::toDcm(quatBJ, dcmBJ); - - double sunDirJ[3] = {0, 0, 0}, sunDirB[3] = {0, 0, 0}; - if (susDataProcessed->sunIjkModel.isValid()) { - std::memcpy(sunDirJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double)); - MatrixOperations::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1); - } else if (susDataProcessed->susVecTot.isValid()) { - std::memcpy(sunDirB, susDataProcessed->susVecTot.value, 3 * sizeof(double)); - } else { - return; - } - - // Transformation between ECEF and ECI frame - double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot); - MathOperations::inverseMatrixDimThree(*dcmEJ, *dcmJE); - double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot); - - // positive z-Axis of EIVE in direction of sun + // positive z-Axis of EIVE in direction of sun double zAxis[3] = {0, 0, 0}; - VectorOperations::normalize(sunDirB, zAxis, 3); + VectorOperations::normalize(sunDirI, zAxis, 3); - // Assign helper vector (north pole inertial) + // assign helper vector (north pole inertial) double helperVec[3] = {0, 0, 1}; - // Construct y-axis from helper vector and z-axis + // 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); - // Construct x-axis from y- and z-axes + // construct x-axis from y- and z-axes double xAxis[3] = {0, 0, 0}; VectorOperations::cross(yAxis, zAxis, xAxis); VectorOperations::normalize(xAxis, xAxis, 3); - // Transformation matrix to Sun, no further transforamtions, reference is already - // the EIVE body frame + // 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]}}; - double quatSun[4] = {0, 0, 0, 0}; - QuaternionOperations::fromDcm(dcmTgt, quatSun); - - targetQuat[0] = quatSun[0]; - targetQuat[1] = quatSun[1]; - targetQuat[2] = quatSun[2]; - targetQuat[3] = quatSun[3]; + QuaternionOperations::fromDcm(dcmTgt, targetQuat); //---------------------------------------------------------------------------- // Calculation of reference rotation rate @@ -501,6 +460,31 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do // under 150 arcsec ?? } +void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4], + double targetSatRotRate[3], double errorQuat[4], + double errorSatRotRate[3], double errorAngle) { + // First calculate error quaternion between current and target orientation + QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat); + // Keep scalar part of quaternion positive + if (errorQuat[3] < 0) { + VectorOperations::mulScalar(errorQuat, -1, errorQuat, 4); + } + // Calculate error angle + errorAngle = QuaternionOperations::getAngle(errorQuat, true); + + // Only give back error satellite rotational rate if orientation has already been aquired + if (errorAngle < 2. / 180. * M_PI) { + // Then substract the combined required satellite rotational rates from the actual rate + VectorOperations::subtract(currentSatRotRate, targetSatRotRate, errorSatRotRate, 3); + } else { + // If orientation has not been aquired yet set satellite rotational rate to zero + errorSatRotRate = 0; + } + + // target flag in matlab, importance, does look like it only gives feedback if pointing control is + // under 150 arcsec ?? +} + void Guidance::targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4], double *refSatRate) { //------------------------------------------------------------------------------------- diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index d67431d7..4dc8ec86 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -19,11 +19,12 @@ class Guidance { void targetQuatPtgSingleAxis(timeval now, 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 targetQuat[4], double targetSatRotRate[3]); + void targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3], double quatIX[4], + double targetSatRotRate[3]); // Function to get the target quaternion and refence rotation rate for sun pointing after ground // station - void targetQuatPtgSun(timeval now, double targetQuat[4], double targetSatRotRate[3]); + void targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double refSatRate[3]); // Function to get the target quaternion and refence rotation rate from gps position for Nadir // pointing @@ -41,6 +42,9 @@ class Guidance { void comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4], double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3], double errorQuat[4], double errorSatRotRate[3], double errorAngle); + void comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4], + double targetSatRotRate[3], double errorQuat[4], double errorSatRotRate[3], + double errorAngle); void targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4], double *targetSatRotRate); diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 0845768a..e268d786 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -1,10 +1,3 @@ -/* - * SensorProcessing.cpp - * - * Created on: 7 Mar 2022 - * Author: Robin Marquardt - */ - #include "SensorProcessing.h" #include