broke everything to fix everything

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<double>::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad,
-                                                  sensorValues->gpsSet.altitude.value, posSatE);
-
   //	Target direction in the ECEF frame
   double targetDirE[3] = {0, 0, 0};
   VectorOperations<double>::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<double>::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,
+void Guidance::targetQuatPtgThreeAxes(acsctrl::MekfData *mekfData, timeval now,
-                                      acsctrl::GpsDataProcessed *gpsDataProcessed,
-                                      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<double>::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<double>::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,
+void Guidance::targetQuatPtgGs(timeval now, double targetQuat[4], double refSatRate[3]) {
-                               acsctrl::SusDataProcessed *susDataProcessed,
-                               acsctrl::GpsDataProcessed *gpsDataProcessed, 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<double>::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad,
-                                                  sensorValues->gpsSet.altitude.value, posSatE);
   double targetDirE[3] = {0, 0, 0};
   VectorOperations<double>::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,
+void Guidance::sunQuatPtg(timeval now, double targetQuat[4], double refSatRate[3]) {
-                          acsctrl::SusDataProcessed *susDataProcessed,
-                          acsctrl::GpsDataProcessed *gpsDataProcessed, 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,
+void Guidance::quatNadirPtgSingleAxis(timeval now, double targetQuat[4],
-                                      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<double>::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad,
-                                                  sensorValues->gpsSet.altitude.value, posSatE);
   double targetDirE[3] = {0, 0, 0};
   VectorOperations<double>::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,
+void Guidance::quatNadirPtgThreeAxes(double posSateE[3], double velSateE[3], timeval now,
-                                     acsctrl::GpsDataProcessed *gpsDataProcessed,
+                                     double targetQuat[4], double refSatRate[3]) {
-                                     acsctrl::MekfData *mekfData, 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<double>::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad,
-                                                  sensorValues->gpsSet.altitude.value, posSatE);
   double targetDirE[3] = {0, 0, 0};
   VectorOperations<double>::mulScalar(posSatE, -1, targetDirE, 3);
 
@@ -548,7 +506,7 @@ void Guidance::quatNadirPtgThreeAxes(ACS::SensorValues *sensorValues,
   VectorOperations<double>::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<double>::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],
+void Guidance::comparePtg(double targetQuat[4], double quatRef[4], double refSatRate[3],
-                          double refSatRate[3], double quatErrorComplete[4], double quatError[3],
+                          double quatErrorComplete[4], double quatError[3], double deltaRate[3]) {
-                          double deltaRate[3]) {
   double satRate[3] = {0, 0, 0};
   std::memcpy(satRate, mekfData->satRotRateMekf.value, 3 * sizeof(double));
   VectorOperations<double>::subtract(satRate, refSatRate, deltaRate, 3);