Add ADIA and ADSRA drift.

orbiternassp · Aug 6, 2023 · 5edad15 · 5edad15
1 parent 308cf41
commit 5edad15
Show file tree

Hide file tree

Showing 5 changed files with 44 additions and 19 deletions.
diff --git a/Missions/ProjectApollo/Apollo 11.cfg b/Missions/ProjectApollo/Apollo 11.cfg
@@ -11,10 +11,10 @@ LMPSuitName=E. ALDRIN
 CMNBDX=-1.794576296677
 CMNBDY=-0.595579561842233
 CMNBDZ=-0.196544246486212
-CMADSRAX=0.0
-CMADSRAY=0.0
-CMADSRAZ=0.0
-CMADIAX=0.0
-CMADIAY=0.0
-CMADIAZ=0.0
+CMADSRAX=-6.0
+CMADSRAY=3.0
+CMADSRAZ=5.0
+CMADIAX=15.0
+CMADIAY=5.0
+CMADIAZ=1.0
 CSMCueCard=1 SATURN_BOOST_A14
diff --git a/Orbitersdk/samples/ProjectApollo/src_aux/Mission.cpp b/Orbitersdk/samples/ProjectApollo/src_aux/Mission.cpp
@@ -306,6 +306,24 @@ namespace mission {
 			else if (!_strnicmp(line, "CMNBDZ=", 7)) {
 				sscanf(line + 7, "%lf", &CM_IMUDriftRates.m13);
 			}
+			else if (!_strnicmp(line, "CMADSRAX=", 9)) {
+				sscanf(line + 9, "%lf", &CM_IMUDriftRates.m21);
+			}
+			else if (!_strnicmp(line, "CMADSRAY=", 9)) {
+				sscanf(line + 9, "%lf", &CM_IMUDriftRates.m22);
+			}
+			else if (!_strnicmp(line, "CMADSRAZ=", 9)) {
+				sscanf(line + 9, "%lf", &CM_IMUDriftRates.m23);
+			}
+			else if (!_strnicmp(line, "CMADIAX=", 8)) {
+				sscanf(line + 8, "%lf", &CM_IMUDriftRates.m31);
+			}
+			else if (!_strnicmp(line, "CMADIAY=", 8)) {
+				sscanf(line + 8, "%lf", &CM_IMUDriftRates.m32);
+			}
+			else if (!_strnicmp(line, "CMADIAZ=", 8)) {
+				sscanf(line + 8, "%lf", &CM_IMUDriftRates.m33);
+			}
 		}
 		//LoadIMU_AndPIPA_RatesAndBiases(hFile);
 		hFile.close();
@@ -314,7 +332,7 @@ namespace mission {
 	}
 
 	void Mission::LoadIMU_AndPIPA_RatesAndBiases(std::ifstream &hFile) {
-		char line[256];
+		//char line[256];
 
 		//for (std::string line; std::getline(hFile, line);) {
 		//	if (!line.compare(0,7,"CMNBDX=")) {

diff --git a/Orbitersdk/samples/ProjectApollo/src_csm/saturn.cpp b/Orbitersdk/samples/ProjectApollo/src_csm/saturn.cpp
@@ -1422,7 +1422,8 @@ void Saturn::clbkPreStep(double simt, double simdt, double mjd)
 	//
 	// Subclass specific handling
 	//
-	VECTOR3 ATTITUDEFORTESTING = imu.GetTotalAttitude();
+	//VECTOR3 ATTITUDEFORTESTING = imu.GetTotalAttitude();
+	VECTOR3 ATTITUDEFORTESTING = imu.getPlatformEulerAnglesZYX();
 	VECTOR3 DRIFTRATEFORTESTING = imu.GetNBDriftRates();
 	VECTOR3 IMURESOLVERPHASEERROR = imu.getResolverPhaseError();
 	sprintf(oapiDebugString(), "<%0.10f, %0.10f, %0.10f>, <%0.10f, %0.10f, %0.10f>, <%0.10f, %0.10f, %0.10f>", 

diff --git a/Orbitersdk/samples/ProjectApollo/src_sys/IMU.h b/Orbitersdk/samples/ProjectApollo/src_sys/IMU.h
@@ -55,6 +55,7 @@ class IMU {
 	inline VECTOR3 getResolverPhaseError() { return ResolverPhaseError; };
 	inline VECTOR3 getResolverSineGimbal() { return SineGimbal; };
 	inline VECTOR3 getResolverCosineGimbal() { return CosineGimbal; };
+	VECTOR3 getPlatformEulerAnglesZYX();
 
 	void WireToBuses(e_object *a, e_object *b, GuardedToggleSwitch *s);
 	void WireHeaterToBuses(Boiler *heater, e_object *a, e_object *b);

diff --git a/Orbitersdk/samples/ProjectApollo/src_sys/imu.cpp b/Orbitersdk/samples/ProjectApollo/src_sys/imu.cpp
@@ -59,7 +59,6 @@ IMU::~IMU()
 }
 
 void IMU::Init() 
-
 {
 	Operate = false;
 	TurnedOn = false;
@@ -135,13 +134,13 @@ void IMU::SetDriftRates(const MATRIX3 DriftRateMatrix)
 	imuDriftRates.NBD_Y = DriftRateMatrix.m12 * MERU;
 	imuDriftRates.NBD_Z = DriftRateMatrix.m13 * MERU;
 
-	imuDriftRates.ADSRA_X = DriftRateMatrix.m21;
-	imuDriftRates.ADSRA_Y = DriftRateMatrix.m22;
-	imuDriftRates.ADSRA_Z = DriftRateMatrix.m23;
+	imuDriftRates.ADSRA_X = DriftRateMatrix.m21 * MERU;
+	imuDriftRates.ADSRA_Y = DriftRateMatrix.m22 * MERU;
+	imuDriftRates.ADSRA_Z = DriftRateMatrix.m23 * MERU;
 
-	imuDriftRates.ADIA_X = DriftRateMatrix.m31;
-	imuDriftRates.ADIA_Y = DriftRateMatrix.m32;
-	imuDriftRates.ADIA_Z = DriftRateMatrix.m33;
+	imuDriftRates.ADIA_X = DriftRateMatrix.m31 * MERU;
+	imuDriftRates.ADIA_Y = DriftRateMatrix.m32 * MERU;
+	imuDriftRates.ADIA_Z = DriftRateMatrix.m33 * MERU;
 }
 
 VECTOR3 IMU::GetNBDriftRates()
@@ -319,7 +318,13 @@ bool IMU::IsPowered()
 	return true;
 }
 
-void IMU::WireToBuses(e_object *a, e_object *b, GuardedToggleSwitch *s) 
+VECTOR3 IMU::getPlatformEulerAnglesZYX()
+{
+	VECTOR3 RotationDeviation = getRotationAnglesZYX(Orbiter.AttitudeReference);
+	return(_V(RotationDeviation.x, RotationDeviation.z, RotationDeviation.y));
+}
+
+void IMU::WireToBuses(e_object *a, e_object *b, GuardedToggleSwitch *s)
 
 { 
 	DCPower.WireToBuses(a, b); 
@@ -451,9 +456,9 @@ void IMU::Timestep(double simdt)
 		accel = tmul(Orbiter.AttitudeReference, accel);
 
 		//IMU Drift calculation
-		double DriftX = (imuDriftRates.NBD_X) * simdt;// - (imuDriftRates.ADSRA_X * accel.y / 9.80665) + (imuDriftRates.ADIA_X * accel.x / 9.80665)) * simdt;
-		double DriftY = (imuDriftRates.NBD_Y) * simdt;// - (imuDriftRates.ADSRA_Y * accel.z / 9.80665) + (imuDriftRates.ADIA_Y * accel.y / 9.80665)) * simdt;
-		double DriftZ = (imuDriftRates.NBD_Z) * simdt;// + (imuDriftRates.ADSRA_Z * accel.y / 9.80665) + (imuDriftRates.ADIA_Z * accel.z / 9.80665)) * simdt;
+		double DriftX = (imuDriftRates.NBD_X - (imuDriftRates.ADSRA_X * accel.y / 9.80665) + (imuDriftRates.ADIA_X * accel.x / 9.80665)) * simdt;
+		double DriftY = (imuDriftRates.NBD_Y - (imuDriftRates.ADSRA_Y * accel.z / 9.80665) + (imuDriftRates.ADIA_Y * accel.y / 9.80665)) * simdt;
+		double DriftZ = (imuDriftRates.NBD_Z - (imuDriftRates.ADSRA_Z * accel.y / 9.80665) - (imuDriftRates.ADIA_Z * accel.z / 9.80665)) * simdt;
 
 		// convert drift rates to rotation matrices
 		MATRIX3 DriftXRot = getRotationMatrixX(DriftX);