diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index f5260234..0add58aa 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -16,13 +16,13 @@ Guidance::Guidance(AcsParameters *acsParameters_) : acsParameters(*acsParameters Guidance::~Guidance() {} -void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double refDirB[3], - double quatIB[4], double targetQuat[4], - double targetSatRotRate[3]) { +void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double velSatE[3], + double sunDirI[3], double refDirB[3], double quatBI[4], + double targetQuat[4], double targetSatRotRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion to groundstation or given latitude, longitude and altitude //------------------------------------------------------------------------------------- - // transform longitude, latitude and altitude to cartesian coordiantes (ECEF) + // transform longitude, latitude and altitude to ECEF double targetE[3] = {0, 0, 0}; MathOperations::cartesianFromLatLongAlt( @@ -30,7 +30,11 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double re acsParameters.targetModeControllerParameters.longitudeTgt, acsParameters.targetModeControllerParameters.altitudeTgt, targetE); - // transformation between ECEF and ECI frame + // target direction in the ECEF frame + 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}}; @@ -40,36 +44,44 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double re double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; MathOperations::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot); - // 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); - VectorOperations::subtract(targetI, posSatI, targetDirI, 3); + // 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}}; - // reference direction in ECI frame - double refDirI[3] = {0, 0, 0}; - QuaternionOperations::multiplyVector(quatIB, refDirB, refDirI); + QuaternionOperations::toDcm(quatBI, dcmBI); + MatrixOperations::multiply(*dcmBI, *dcmIE, *dcmBE, 3, 3, 3); - // rotation quaternion from two vectors - double crossDirI[3] = {0, 0, 0}; - double dotDirections = VectorOperations::dot(targetDirI, refDirI); - VectorOperations::cross(targetDirI, refDirI, crossDirI); - targetQuat[0] = crossDirI[0]; - targetQuat[1] = crossDirI[1]; - targetQuat[2] = crossDirI[2]; - targetQuat[3] = sqrt(pow(VectorOperations::norm(targetDirI, 3), 2) * - pow(VectorOperations::norm(refDirI, 3), 2) + - dotDirections); + // 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 + // 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 + // 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(*dcmBJ, *dcmJEDot, *dcmBEDot, 3, 3, 3); + MatrixOperations::multiply(*dcmBI, *dcmIEDot, *dcmBEDot, 3, 3, 3); MatrixOperations::multiply(*dcmBEDot, posSatE, velSatBPart2, 3, 3, 1); VectorOperations::add(velSatBPart1, velSatBPart2, velSatB, 3); @@ -79,21 +91,14 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double re double satRateDir[3] = {0, 0, 0}; VectorOperations::cross(velSatB, targetDirB, satRateDir); VectorOperations::normalize(satRateDir, satRateDir, 3); - VectorOperations::mulScalar(satRateDir, normRefSatRate, refSatRate, 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}; - - if (susDataProcessed->sunIjkModel.isValid()) { - double sunDirJ[3] = {0, 0, 0}; - std::memcpy(sunDirJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double)); - MatrixOperations::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1); - } else { - std::memcpy(sunDirB, susDataProcessed->susVecTot.value, 3 * sizeof(double)); - } + MatrixOperations::multiply(*dcmBI, sunDirI, sunDirB, 3, 3, 1); double exclAngle = acsParameters.strParameters.exclusionAngle, blindStart = acsParameters.targetModeControllerParameters.blindAvoidStart, @@ -103,18 +108,14 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double re 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; @@ -124,9 +125,7 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double re blindRefRate[1] = 0; blindRefRate[2] = 0; } - - VectorOperations::add(blindRefRate, refSatRate, refSatRate, 3); - + VectorOperations::add(blindRefRate, targetSatRotRate, targetSatRotRate, 3); } else { strBlindAvoidFlag = false; } diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index e5560379..b494c7a9 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -16,7 +16,9 @@ class Guidance { // Function to get the target quaternion and refence rotation rate from gps position and // position of the ground station - void targetQuatPtgSingleAxis(timeval now, double targetQuat[4], double targetSatRotRate[3]); + 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],