From 5edad15294b3b08755710c8bb58ab690b95568b2 Mon Sep 17 00:00:00 2001 From: Matthew W Hume Date: Sun, 6 Aug 2023 17:03:38 -0400 Subject: [PATCH] Add ADIA and ADSRA drift. --- Missions/ProjectApollo/Apollo 11.cfg | 12 ++++----- .../samples/ProjectApollo/src_aux/Mission.cpp | 20 +++++++++++++- .../samples/ProjectApollo/src_csm/saturn.cpp | 3 ++- .../samples/ProjectApollo/src_sys/IMU.h | 1 + .../samples/ProjectApollo/src_sys/imu.cpp | 27 +++++++++++-------- 5 files changed, 44 insertions(+), 19 deletions(-) diff --git a/Missions/ProjectApollo/Apollo 11.cfg b/Missions/ProjectApollo/Apollo 11.cfg index 11580a53c7..bbb4b60e24 100644 --- 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 \ No newline at end of file diff --git a/Orbitersdk/samples/ProjectApollo/src_aux/Mission.cpp b/Orbitersdk/samples/ProjectApollo/src_aux/Mission.cpp index 7837b51610..c05d67449d 100644 --- 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 index 0d1aa21e50..9be6bc34f3 100644 --- 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 index 17fa86b497..090928f156 100644 --- 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 index f1a6831f8f..240dd5d2be 100644 --- 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);