added Gps Velocity Calculation

mission/controller/acs/AcsParameters.h

@@ -833,6 +833,7 @@ class AcsParameters /*: public HasParametersIF*/ {
   } strParameters;
 
   struct GpsParameters {
+	  double timeDiffVelocityMax = 30; //[s]
   } gpsParameters;
 
   struct GroundStationParameters {

mission/controller/acs/Guidance.cpp

@@ -111,9 +111,9 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues
   //	Calculation of reference rotation rate
   //-------------------------------------------------------------------------------------
   double velSatE[3] = {0, 0, 0};
-  velSatE[0] = 0.0;  // sensorValues->gps0Velocity[0];
+  velSatE[0] = outputValues->gpsVelocity[0];  // sensorValues->gps0Velocity[0];
-  velSatE[1] = 0.0;  // sensorValues->gps0Velocity[1];
+  velSatE[1] = outputValues->gpsVelocity[1];  // sensorValues->gps0Velocity[1];
-  velSatE[2] = 0.0;  // sensorValues->gps0Velocity[2];
+  velSatE[2] = outputValues->gpsVelocity[2];  // sensorValues->gps0Velocity[2];
   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);

mission/controller/acs/OutputValues.h

@@ -44,6 +44,7 @@ public:
 
 	double gcLatitude; // geocentric latitude, radian
 	double gdLongitude; // Radian longitude
+	double gpsVelocity[3]; //earth fixed frame
 };
 }
 

mission/controller/acs/SensorProcessing.cpp

@@ -454,7 +454,9 @@ void SensorProcessing::processGyr(
 }
 
 void SensorProcessing::processGps(const double gps0latitude, const double gps0longitude,
-                                  const bool validGps, double *gcLatitude, double *gdLongitude) {
+								  const double gps0altitude, const uint32_t gps0UnixSeconds,
+                                  const bool validGps, const AcsParameters::GpsParameters *gpsParameters,
+								  double *gcLatitude, double *gdLongitude, double *gpsVelocityE) {
   // name to convert not process
   if (validGps) {
     // Transforming from Degree to Radians and calculation geocentric lattitude from geodetic
@@ -464,6 +466,25 @@ void SensorProcessing::processGps(const double gps0latitude, const double gps0lo
     double factor = 1 - pow(eccentricityWgs84, 2);
     *gcLatitude = atan(factor * tan(latitudeRad));
     validGcLatitude = true;
+
+    // Calculation of the satellite velocity in earth fixed frame
+    double posSatE[3] = {0, 0, 0}, deltaDistance[3] = {0, 0, 0};
+    // RADIANS OR DEGREE ? Function needs rad as input
+    MathOperations<double>::cartesianFromLatLongAlt(gps0latitude, gps0longitude, gps0altitude, posSatE);
+    if (validSavedPosSatE && (gps0UnixSeconds - timeOfSavedPosSatE) < (gpsParameters->timeDiffVelocityMax)) {
+    	VectorOperations<double>::subtract(posSatE, savedPosSatE, deltaDistance, 3);
+    	double timeDiffGpsMeas = gps0UnixSeconds - timeOfSavedPosSatE;
+    	VectorOperations<double>::mulScalar(deltaDistance, 1/timeDiffGpsMeas, gpsVelocityE, 3);
+    }
+    savedPosSatE[0] = posSatE[0];
+    savedPosSatE[1] = posSatE[1];
+    savedPosSatE[2] = posSatE[2];
+
+    timeOfSavedPosSatE = gps0UnixSeconds;
+    validSavedPosSatE = true;
+  }
+  else {
+	validGcLatitude = false;
   }
 }
 
@@ -472,7 +493,10 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues,
                                const AcsParameters *acsParameters) {
   sensorValues->update();
   processGps(sensorValues->gpsSet.latitude.value, sensorValues->gpsSet.longitude.value,
-             sensorValues->gpsSet.isValid(), &outputValues->gcLatitude, &outputValues->gdLongitude);
+		  sensorValues->gpsSet.altitude.value, sensorValues->gpsSet.unixSeconds.value,
+             sensorValues->gpsSet.isValid(), &acsParameters->gpsParameters,
+			 &outputValues->gcLatitude, &outputValues->gdLongitude,
+			 &outputValues->gpsVelocity);
 
   outputValues->mgmUpdated = processMgm(
       sensorValues->mgm0Lis3Set.fieldStrengths.value,

mission/controller/acs/SensorProcessing.h

@@ -65,8 +65,10 @@ class SensorProcessing {
 
   void processStr();
 
-  void processGps(const double gps0latitude, const double gps0longitude, const bool validGps,
+  void processGps(const double gps0latitude, const double gps0longitude,
-                  double *gcLatitude, double *gdLongitude);
+		          const double gps0altitude, const uint32_t gps0UnixSeconds,
+				  const bool validGps, const AcsParameters::GpsParameters *gpsParameters,
+				  double *gcLatitude, double *gdLongitude, double *gpsVelocityE);
 
   double savedMagFieldEst[3];
   timeval timeOfSavedMagFieldEst;
@@ -75,6 +77,10 @@ class SensorProcessing {
   bool validMagField;
   bool validGcLatitude;
 
+  double savedPosSatE[3];
+  uint32_t timeOfSavedPosSatE;
+  bool validSavedPosSatE;
+
   SusConverter susConverter;
   AcsParameters acsParameters;
 };