diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index f9238755..506f6dfc 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -36,9 +36,9 @@ void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3 3 * sizeof(double)); } -void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, +void Guidance::targetQuatPtgSingleAxis(acsctrl::MekfData *mekfData, acsctrl::SusDataProcessed *susDataProcessed, - acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now, + timeval now, double targetQuat[4], double refSatRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion to groundstation or given latitude, longitude and altitude @@ -52,13 +52,6 @@ void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl: acsParameters.targetModeControllerParameters.longitudeTgt, acsParameters.targetModeControllerParameters.altitudeTgt, targetCart); - // Position of the satellite in the earth/fixed frame via GPS - double posSatE[3] = {0, 0, 0}; - double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180; - double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180; - MathOperations::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad, - sensorValues->gpsSet.altitude.value, posSatE); - // Target direction in the ECEF frame double targetDirE[3] = {0, 0, 0}; VectorOperations::subtract(targetCart, posSatE, targetDirE, 3); @@ -108,8 +101,6 @@ void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl: //------------------------------------------------------------------------------------- // Calculation of reference rotation rate //------------------------------------------------------------------------------------- - double velSatE[3] = {0, 0, 0}; - 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); @@ -221,9 +212,7 @@ void Guidance::refRotationRate(int8_t timeElapsedMax, timeval now, double quatIn savedQuaternion[3] = quatInertialTarget[3]; } -void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues, - acsctrl::GpsDataProcessed *gpsDataProcessed, - acsctrl::MekfData *mekfData, timeval now, +void Guidance::targetQuatPtgThreeAxes(acsctrl::MekfData *mekfData, timeval now, double targetQuat[4], double refSatRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion for target pointing @@ -231,14 +220,10 @@ void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues, // Transform longitude, latitude and altitude to cartesian coordiantes (earth // fixed/centered frame) double targetCart[3] = {0, 0, 0}; - MathOperations::cartesianFromLatLongAlt( acsParameters.targetModeControllerParameters.latitudeTgt, acsParameters.targetModeControllerParameters.longitudeTgt, acsParameters.targetModeControllerParameters.altitudeTgt, targetCart); - // Position of the satellite in the earth/fixed frame via GPS - double posSatE[3] = {0, 0, 0}; - std::memcpy(posSatE, gpsDataProcessed->gpsPosition.value, 3 * sizeof(double)); double targetDirE[3] = {0, 0, 0}; VectorOperations::subtract(targetCart, posSatE, targetDirE, 3); @@ -300,10 +285,7 @@ void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues, QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat); } -void Guidance::targetQuatPtgGs(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, - acsctrl::SusDataProcessed *susDataProcessed, - acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now, - double targetQuat[4], double refSatRate[3]) { +void Guidance::targetQuatPtgGs(timeval now, double targetQuat[4], double refSatRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion for ground station pointing //------------------------------------------------------------------------------------- @@ -315,12 +297,6 @@ void Guidance::targetQuatPtgGs(ACS::SensorValues *sensorValues, acsctrl::MekfDat acsParameters.targetModeControllerParameters.latitudeTgt, acsParameters.targetModeControllerParameters.longitudeTgt, acsParameters.targetModeControllerParameters.altitudeTgt, groundStationCart); - // Position of the satellite in the earth/fixed frame via GPS - double posSatE[3] = {0, 0, 0}; - double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180; - double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180; - MathOperations::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad, - sensorValues->gpsSet.altitude.value, posSatE); double targetDirE[3] = {0, 0, 0}; VectorOperations::subtract(groundStationCart, posSatE, targetDirE, 3); @@ -379,10 +355,7 @@ void Guidance::targetQuatPtgGs(ACS::SensorValues *sensorValues, acsctrl::MekfDat QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat); } -void Guidance::sunQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, - acsctrl::SusDataProcessed *susDataProcessed, - acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now, - double targetQuat[4], double refSatRate[3]) { +void Guidance::sunQuatPtg(timeval now, double targetQuat[4], double refSatRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion to sun //------------------------------------------------------------------------------------- @@ -448,18 +421,11 @@ void Guidance::sunQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *me refSatRate[2] = 0; } -void Guidance::quatNadirPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, - timeval now, double targetQuat[4], +void Guidance::quatNadirPtgSingleAxis(timeval now, double targetQuat[4], double refSatRate[3]) { // old version of Nadir Pointing //------------------------------------------------------------------------------------- // Calculation of target quaternion for Nadir pointing //------------------------------------------------------------------------------------- - // Position of the satellite in the earth/fixed frame via GPS - double posSatE[3] = {0, 0, 0}; - double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180; - double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180; - MathOperations::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad, - sensorValues->gpsSet.altitude.value, posSatE); double targetDirE[3] = {0, 0, 0}; VectorOperations::mulScalar(posSatE, -1, targetDirE, 3); @@ -512,19 +478,11 @@ void Guidance::quatNadirPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl:: refSatRate[2] = 0; } -void Guidance::quatNadirPtgThreeAxes(ACS::SensorValues *sensorValues, - acsctrl::GpsDataProcessed *gpsDataProcessed, - acsctrl::MekfData *mekfData, timeval now, double targetQuat[4], - double refSatRate[3]) { +void Guidance::quatNadirPtgThreeAxes(double posSateE[3], double velSateE[3], timeval now, + double targetQuat[4], double refSatRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion for Nadir pointing //------------------------------------------------------------------------------------- - // Position of the satellite in the earth/fixed frame via GPS - double posSatE[3] = {0, 0, 0}; - double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180; - double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180; - MathOperations::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad, - sensorValues->gpsSet.altitude.value, posSatE); double targetDirE[3] = {0, 0, 0}; VectorOperations::mulScalar(posSatE, -1, targetDirE, 3); @@ -548,7 +506,7 @@ void Guidance::quatNadirPtgThreeAxes(ACS::SensorValues *sensorValues, VectorOperations::mulScalar(xAxis, -1, xAxis, 3); // z-Axis parallel to long side of picture resolution - double zAxis[3] = {0, 0, 0}, velocityE[3]; + double zAxis[3] = {0, 0, 0}; 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); @@ -584,9 +542,8 @@ void Guidance::inertialQuatPtg(double targetQuat[4], double refSatRate[3]) { 3 * sizeof(double)); } -void Guidance::comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double quatRef[4], - double refSatRate[3], double quatErrorComplete[4], double quatError[3], - double deltaRate[3]) { +void Guidance::comparePtg(double targetQuat[4], double quatRef[4], double refSatRate[3], + double quatErrorComplete[4], double quatError[3], double deltaRate[3]) { double satRate[3] = {0, 0, 0}; std::memcpy(satRate, mekfData->satRotRateMekf.value, 3 * sizeof(double)); VectorOperations::subtract(satRate, refSatRate, deltaRate, 3);