diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 7d1ebc60..9057f5e4 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -249,15 +249,18 @@ void AcsController::performPointingCtrl() { double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0}; switch (submode) { - case SUBMODE_PTG_TARGET: - guidance.targetQuatPtg(&sensorValues, &mekfData, &susDataProcessed, now, targetQuat, - refSatRate); - break; case SUBMODE_PTG_SUN: - guidance.sunQuatPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate); + guidance.sunQuatPtg(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now, + targetQuat, refSatRate); break; + case SUBMODE_PTG_TARGET: + guidance.targetQuatPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat, + refSatRate); + break; + case SUBMODE_PTG_NADIR: - guidance.quatNadirPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate); + guidance.quatNadirPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat, + refSatRate); break; case SUBMODE_PTG_INERTIAL: guidance.inertialQuatPtg(targetQuat, refSatRate); diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 199fce9c..afd4d41e 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -30,9 +30,10 @@ void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3 // memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir, 24); } -void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, - acsctrl::SusDataProcessed *susDataProcessed, timeval now, - double targetQuat[4], double refSatRate[3]) { +void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, + acsctrl::SusDataProcessed *susDataProcessed, + acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now, + double targetQuat[4], double refSatRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion to groundstation or given latitude, longitude and altitude //------------------------------------------------------------------------------------- @@ -102,9 +103,7 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData // Calculation of reference rotation rate //------------------------------------------------------------------------------------- double velSatE[3] = {0, 0, 0}; - velSatE[0] = outputValues->gpsVelocity[0]; // sensorValues->gps0Velocity[0]; - velSatE[1] = outputValues->gpsVelocity[1]; // sensorValues->gps0Velocity[1]; - velSatE[2] = outputValues->gpsVelocity[2]; // sensorValues->gps0Velocity[2]; + std::memcpy(velSatE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double)); 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); @@ -127,7 +126,7 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData if (acsParameters.targetModeControllerParameters.avoidBlindStr) { double sunDirB[3] = {0, 0, 0}; - if (outputValues->sunDirModelValid) { + 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); @@ -174,8 +173,10 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData } } -void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, - timeval now, double targetQuat[4], double refSatRate[3]) { +void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues, + acsctrl::GpsDataProcessed *gpsDataProcessed, + acsctrl::MekfData *mekfData, timeval now, + double targetQuat[4], double refSatRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion for target pointing //------------------------------------------------------------------------------------- @@ -217,8 +218,8 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues VectorOperations::mulScalar(xAxis, -1, xAxis, 3); // Transform velocity into inertial frame - double velocityE[3] = {outputValues->gpsVelocity[0], outputValues->gpsVelocity[1], - outputValues->gpsVelocity[2]}; + double velocityE[3]; + std::memcpy(velocityE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double)); double velocityJ[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0}; MatrixOperations::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1); MatrixOperations::multiply(*dcmJEDot, posSatE, velPart2, 3, 3, 1); @@ -251,7 +252,7 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues double timeElapsed = now.tv_sec + now.tv_usec * pow(10, -6) - (timeSavedQuaternionNadir.tv_sec + - timeSavedQuaternionNadir.tv_usec * pow(timeSavedQuaternionNadir.tv_usec, -6)); + timeSavedQuaternionNadir.tv_usec * pow((double)timeSavedQuaternionNadir.tv_usec, -6)); if (timeElapsed < acsParameters.pointingModeControllerParameters.nadirTimeElapsedMax) { double qDiff[4] = {0, 0, 0, 0}; VectorOperations::subtract(quatInertialTarget, savedQuaternionNadir, qDiff, 4); @@ -287,38 +288,28 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues // Transform in system relative to satellite frame double quatBJ[4] = {0, 0, 0, 0}; - quatBJ[0] = outputValues->quatMekfBJ[0]; - quatBJ[1] = outputValues->quatMekfBJ[1]; - quatBJ[2] = outputValues->quatMekfBJ[2]; - quatBJ[3] = outputValues->quatMekfBJ[3]; + std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double)); QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat); } -void Guidance::sunQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, - timeval now, double targetQuat[4], double refSatRate[3]) { +void Guidance::sunQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, + acsctrl::SusDataProcessed *susDataProcessed, + acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now, + 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}}; - quatBJ[0] = outputValues->quatMekfBJ[0]; - quatBJ[1] = outputValues->quatMekfBJ[1]; - quatBJ[2] = outputValues->quatMekfBJ[2]; - quatBJ[3] = outputValues->quatMekfBJ[3]; + 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 (outputValues->sunDirModelValid) { - sunDirJ[0] = outputValues->sunDirModel[0]; - sunDirJ[1] = outputValues->sunDirModel[1]; - sunDirJ[2] = outputValues->sunDirModel[2]; + if (susDataProcessed->sunIjkModel.isValid()) { + std::memcpy(sunDirJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double)); MatrixOperations::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1); - } - - else { - sunDirB[0] = outputValues->sunDirEst[0]; - sunDirB[1] = outputValues->sunDirEst[1]; - sunDirB[2] = outputValues->sunDirEst[2]; + } else { + std::memcpy(sunDirB, susDataProcessed->susVecTot.value, 3 * sizeof(double)); } /* @@ -367,8 +358,8 @@ void Guidance::sunQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues *ou double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180; MathOperations::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad, sensorValues->gpsSet.altitude.value, posSatE); - double velocityE[3] = {outputValues->gpsVelocity[0], outputValues->gpsVelocity[1], - outputValues->gpsVelocity[2]}; + double velocityE[3]; + std::memcpy(velocityE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double)); double velocityJ[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0}; MatrixOperations::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1); MatrixOperations::multiply(*dcmJEDot, posSatE, velPart2, 3, 3, 1); @@ -410,9 +401,8 @@ void Guidance::sunQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues *ou refSatRate[2] = 0; } -void Guidance::quatNadirPtgOldVersion(ACS::SensorValues *sensorValues, - ACS::OutputValues *outputValues, timeval now, - double targetQuat[4], +void Guidance::quatNadirPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, + timeval now, double targetQuat[4], double refSatRate[3]) { // old version of Nadir Pointing //------------------------------------------------------------------------------------- // Calculation of target quaternion for Nadir pointing @@ -440,10 +430,7 @@ void Guidance::quatNadirPtgOldVersion(ACS::SensorValues *sensorValues, double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double quatBJ[4] = {0, 0, 0, 0}; - quatBJ[0] = outputValues->quatMekfBJ[0]; - quatBJ[1] = outputValues->quatMekfBJ[1]; - quatBJ[2] = outputValues->quatMekfBJ[2]; - quatBJ[3] = outputValues->quatMekfBJ[3]; + std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double)); QuaternionOperations::toDcm(quatBJ, dcmBJ); MatrixOperations::multiply(*dcmBJ, *dcmJE, *dcmBE, 3, 3, 3); @@ -478,8 +465,10 @@ void Guidance::quatNadirPtgOldVersion(ACS::SensorValues *sensorValues, refSatRate[2] = 0; } -void Guidance::quatNadirPtg(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, - timeval now, double targetQuat[4], double refSatRate[3]) { +void Guidance::quatNadirPtgThreeAxes(ACS::SensorValues *sensorValues, + acsctrl::GpsDataProcessed *gpsDataProcessed, + acsctrl::MekfData *mekfData, timeval now, double targetQuat[4], + double refSatRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion for Nadir pointing //------------------------------------------------------------------------------------- @@ -512,9 +501,8 @@ void Guidance::quatNadirPtg(ACS::SensorValues *sensorValues, ACS::OutputValues * VectorOperations::mulScalar(xAxis, -1, xAxis, 3); // z-Axis parallel to long side of picture resolution - double zAxis[3] = {0, 0, 0}; - double velocityE[3] = {outputValues->gpsVelocity[0], outputValues->gpsVelocity[1], - outputValues->gpsVelocity[2]}; + double zAxis[3] = {0, 0, 0}, velocityE[3]; + std::memcpy(velocityE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double)); double velocityJ[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0}; MatrixOperations::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1); MatrixOperations::multiply(*dcmJEDot, posSatE, velPart2, 3, 3, 1); @@ -539,7 +527,7 @@ void Guidance::quatNadirPtg(ACS::SensorValues *sensorValues, ACS::OutputValues * double timeElapsed = now.tv_sec + now.tv_usec * pow(10, -6) - (timeSavedQuaternionNadir.tv_sec + - timeSavedQuaternionNadir.tv_usec * pow(timeSavedQuaternionNadir.tv_usec, -6)); + timeSavedQuaternionNadir.tv_usec * pow((double)timeSavedQuaternionNadir.tv_usec, -6)); if (timeElapsed < acsParameters.pointingModeControllerParameters.nadirTimeElapsedMax) { double qDiff[4] = {0, 0, 0, 0}; VectorOperations::subtract(quatInertialTarget, savedQuaternionNadir, qDiff, 4); @@ -575,10 +563,7 @@ void Guidance::quatNadirPtg(ACS::SensorValues *sensorValues, ACS::OutputValues * // Transform in system relative to satellite frame double quatBJ[4] = {0, 0, 0, 0}; - quatBJ[0] = outputValues->quatMekfBJ[0]; - quatBJ[1] = outputValues->quatMekfBJ[1]; - quatBJ[2] = outputValues->quatMekfBJ[2]; - quatBJ[3] = outputValues->quatMekfBJ[3]; + std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double)); QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat); } @@ -608,7 +593,6 @@ void Guidance::comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, dou {quatRef[1], -quatRef[0], quatRef[3], -quatRef[2]}, {quatRef[0], -quatRef[1], quatRef[2], quatRef[3]}}; - double quatErrorComplete[4] = {0, 0, 0, 0}; MatrixOperations::multiply(*quatErrorMtx, targetQuat, quatErrorComplete, 4, 4, 1); if (quatErrorComplete[3] < 0) { @@ -619,8 +603,8 @@ void Guidance::comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, dou quatError[1] = quatErrorComplete[1]; quatError[2] = quatErrorComplete[2]; - // target flag in matlab, importance, does look like it only gives - // feedback if pointing control is under 150 arcsec ?? + // target flag in matlab, importance, does look like it only gives feedback if pointing control is + // under 150 arcsec ?? } void Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv) { diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index 1b6bfcb6..176d51a2 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -23,26 +23,31 @@ class Guidance { // Function to get the target quaternion and refence rotation rate from gps position and position // of the ground station - void targetQuatPtgOldVersion(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, - acsctrl::SusDataProcessed *susDataProcessed, timeval now, + void targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues, + acsctrl::GpsDataProcessed *gpsDataProcessed, + acsctrl::MekfData *mekfData, timeval now, double targetQuat[4], + double refSatRate[3]); + void targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, + acsctrl::SusDataProcessed *susDataProcessed, + acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now, double targetQuat[4], double refSatRate[3]); - void targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, - acsctrl::SusDataProcessed *susDataProcessed, timeval now, double targetQuat[4], - double refSatRate[3]); // Function to get the target quaternion and refence rotation rate for sun pointing after ground // station - void sunQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, timeval now, - double targetQuat[4], double refSatRate[3]); + void sunQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, + acsctrl::SusDataProcessed *susDataProcessed, + acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now, double targetQuat[4], + double refSatRate[3]); // Function to get the target quaternion and refence rotation rate from gps position for Nadir // pointing - void quatNadirPtgOldVersion(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, + void quatNadirPtgThreeAxes(ACS::SensorValues *sensorValues, + acsctrl::GpsDataProcessed *gpsDataProcessed, + acsctrl::MekfData *mekfData, timeval now, double targetQuat[4], + double refSatRate[3]); + void quatNadirPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, timeval now, double targetQuat[4], double refSatRate[3]); - void quatNadirPtg(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, timeval now, - double targetQuat[4], double refSatRate[3]); - // Function to get the target quaternion and refence rotation rate from parameters for inertial // pointing void inertialQuatPtg(double targetQuat[4], double refSatRate[3]);