mission/controller/AcsController.cpp

@@ -171,7 +171,7 @@ void AcsController::performPointingCtrl() {
       guidance.sunQuatPtg(&sensorValues, &outputValues, targetQuat, refSatRate);
       break;
     case SUBMODE_PTG_NADIR:
-      guidance.quatNadirPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate);
+      guidance.quatNadirPtgFLPVersion(&sensorValues, &outputValues, now, targetQuat, refSatRate);
       break;
     case SUBMODE_PTG_INERTIAL:
       guidance.inertialQuatPtg(targetQuat, refSatRate);

mission/controller/acs/AcsParameters.h

@@ -852,6 +852,7 @@ class AcsParameters /*: public HasParametersIF*/ {
     double nadirRefDirection[3] = {-1, 0, 0}; //Camera
     double refQuatInertial[4] = {0, 0, 0, 1};
     double refRotRateInertial[3] = {0, 0, 0};
+    int8_t nadirTimeElapsedMax = 10;
   } pointingModeControllerParameters, inertialModeControllerParameters, nadirModeControllerParameters, targetModeControllerParameters;
 
   struct StrParameters {

mission/controller/acs/Guidance.cpp

@@ -125,10 +125,10 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues
   //	Calculation of reference rotation rate in case of star tracker blinding
   //-------------------------------------------------------------------------------------
   if (acsParameters.targetModeControllerParameters.avoidBlindStr) {
-    double sunDirJ[3] = {0, 0, 0};
     double sunDirB[3] = {0, 0, 0};
 
     if (outputValues->sunDirModelValid) {
+      double sunDirJ[3] = {0, 0, 0};
       sunDirJ[0] = outputValues->sunDirModel[0];
       sunDirJ[1] = outputValues->sunDirModel[1];
       sunDirJ[2] = outputValues->sunDirModel[2];
@@ -301,6 +301,98 @@ void Guidance::quatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *
 
 }
 
+void Guidance::quatNadirPtgFLPVersion(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, 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);
+
+	//	Transformation between ECEF and IJK frame
+	double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+	double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+	double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+	MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
+	MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
+
+	double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+	MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
+
+	//	Target Direction in the body frame
+	double targetDirJ[3] = {0, 0, 0};
+	MatrixOperations<double>::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1);
+
+	// negative x-Axis aligned with target (Camera position)
+	double xAxis[3] = {0, 0, 0};
+	VectorOperations<double>::normalize(targetDirJ, xAxis, 3);
+	VectorOperations<double>::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 velocityJ[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0};
+	MatrixOperations<double>::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1);
+	MatrixOperations<double>::multiply(*dcmJEDot, posSatE, velPart2, 3, 3, 1);
+	VectorOperations<double>::add(velPart1, velPart2, velocityJ, 3);
+	VectorOperations<double>::cross(xAxis, velocityJ, zAxis);
+	VectorOperations<double>::normalize(zAxis, zAxis, 3);
+
+	// y-Axis completes RHS
+	double yAxis[3] = {0, 0, 0};
+	VectorOperations<double>::cross(zAxis, xAxis, yAxis);
+
+	//Complete transformation matrix
+	double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]}, {xAxis[1], yAxis[1], zAxis[1]}, {xAxis[2], yAxis[2], zAxis[2]}};
+	QuaternionOperations::fromDcm(dcmTgt,targetQuat);
+
+	//-------------------------------------------------------------------------------------
+	//	Calculation of reference rotation rate
+	//-------------------------------------------------------------------------------------
+	double timeElapsed = now.tv_sec + now.tv_usec * pow(10,-6) - (timeSavedQuaternionNadir.tv_sec +
+			timeSavedQuaternionNadir.tv_usec * pow(timeSavedQuaternionNadir.tv_usec,-6));
+	if (timeElapsed < acsParameters.pointingModeControllerParameters.nadirTimeElapsedMax) {
+	  double qDiff[4] = {0, 0, 0, 0};
+	  VectorOperations<double>::subtract(targetQuat, savedQuaternionNadir, qDiff, 4);
+	  VectorOperations<double>::mulScalar(qDiff, 1/timeElapsed, qDiff, 4);
+
+	  double tgtQuatVec[3] = {targetQuat[0], targetQuat[1], targetQuat[2]},
+			  qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]};
+	  double sum1[3] = {0, 0, 0}, sum2[3] = {0, 0, 0}, sum3[3] = {0, 0, 0}, sum[3] = {0, 0, 0};
+	  VectorOperations<double>::cross(targetQuat, qDiff, sum1);
+	  VectorOperations<double>::mulScalar(tgtQuatVec, qDiff[3], sum2, 3);
+	  VectorOperations<double>::mulScalar(qDiffVec, targetQuat[3], sum3, 3);
+	  VectorOperations<double>::add(sum1, sum2, sum, 3);
+	  VectorOperations<double>::subtract(sum, sum3, sum, 3);
+	  double omegaRefNew[3] = {0, 0, 0};
+	  VectorOperations<double>::mulScalar(sum, -2, omegaRefNew, 3);
+
+	  VectorOperations<double>::mulScalar(omegaRefNew, 2, refSatRate, 3);
+	  VectorOperations<double>::subtract(refSatRate, omegaRefSavedNadir, refSatRate, 3);
+	  omegaRefSavedNadir[0] = omegaRefNew[0];
+	  omegaRefSavedNadir[1] = omegaRefNew[1];
+	  omegaRefSavedNadir[2] = omegaRefNew[2];
+	}
+	else {
+	  refSatRate[0] = 0;
+	  refSatRate[1] = 0;
+	  refSatRate[2] = 0;
+	}
+
+	timeSavedQuaternionNadir = now;
+	savedQuaternionNadir[0] = targetQuat[0];
+	savedQuaternionNadir[1] = targetQuat[1];
+	savedQuaternionNadir[2] = targetQuat[2];
+	savedQuaternionNadir[3] = targetQuat[3];
+}
+
 void Guidance::inertialQuatPtg(double targetQuat[4], double refSatRate[3]) {
   for (int i = 0; i < 4; i++) {
 	targetQuat[i] = acsParameters.inertialModeControllerParameters.refQuatInertial[i];

mission/controller/acs/Guidance.h

@@ -34,6 +34,9 @@ public:
 	void quatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now,
 			double targetQuat[4], double refSatRate[3]);
 
+	void quatNadirPtgFLPVersion(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]);
 
@@ -48,6 +51,9 @@ public:
 private:
 	AcsParameters acsParameters;
 	bool strBlindAvoidFlag = false;
+	timeval timeSavedQuaternionNadir;
+	double savedQuaternionNadir[4] = {0, 0, 0, 0};
+	double omegaRefSavedNadir[3] = {0, 0, 0};
 };
 
 #endif /* ACS_GUIDANCE_H_ */