From a9fa85169f121f7eaec7cb502f6e57cc0186ffe8 Mon Sep 17 00:00:00 2001 From: patrick kenneally Date: Fri, 20 Jun 2025 15:53:20 -0600 Subject: [PATCH 01/24] Address Faceted SRP warnings --- .../facetSRPDynamicEffector.cpp | 26 +++++++------------ .../facetSRPDynamicEffector.h | 4 +-- 2 files changed, 12 insertions(+), 18 deletions(-) diff --git a/src/simulation/dynamics/facetSRPDynamicEffector/facetSRPDynamicEffector.cpp b/src/simulation/dynamics/facetSRPDynamicEffector/facetSRPDynamicEffector.cpp index 070d16a91..adb9122f7 100644 --- a/src/simulation/dynamics/facetSRPDynamicEffector/facetSRPDynamicEffector.cpp +++ b/src/simulation/dynamics/facetSRPDynamicEffector/facetSRPDynamicEffector.cpp @@ -109,7 +109,7 @@ void FacetSRPDynamicEffector::ReadMessages() { if (this->articulatedFacetDataInMsgs.size() == this->numArticulatedFacets) { HingedRigidBodyMsgPayload facetAngleMsg; this->facetArticulationAngleList.clear(); - for (int i = 0; i < this->numArticulatedFacets; i++) { + for (size_t i = 0; i < this->numArticulatedFacets; i++) { if (this->articulatedFacetDataInMsgs[i].isLinked() && this->articulatedFacetDataInMsgs[i].isWritten()) { facetAngleMsg = this->articulatedFacetDataInMsgs[i](); this->facetArticulationAngleList.push_back(facetAngleMsg.theta); @@ -145,29 +145,22 @@ void FacetSRPDynamicEffector::computeForceTorque(double callTime, double timeSte Eigen::Vector3d sHat = r_SB_B / r_SB_B.norm(); // Define local srp force and torque storage vectors - Eigen::Vector3d facetSRPForcePntB_B; - Eigen::Vector3d facetSRPTorquePntB_B; - Eigen::Vector3d totalSRPForcePntB_B; - Eigen::Vector3d totalSRPTorquePntB_B; + Eigen::Vector3d facetSRPForcePntB_B = Eigen::Vector3d::Zero(); + Eigen::Vector3d facetSRPTorquePntB_B = Eigen::Vector3d::Zero(); + Eigen::Vector3d totalSRPForcePntB_B = Eigen::Vector3d::Zero(); + Eigen::Vector3d totalSRPTorquePntB_B = Eigen::Vector3d::Zero(); // Zero storage information this->forceExternal_B.setZero(); this->torqueExternalPntB_B.setZero(); - facetSRPForcePntB_B.setZero(); - facetSRPTorquePntB_B.setZero(); - totalSRPForcePntB_B.setZero(); - totalSRPTorquePntB_B.setZero(); - double projectedArea = 0.0; - double cosTheta = 0.0; // Calculate the SRP pressure acting at the current spacecraft location double numAU = AstU / r_SB_B.norm(); double SRPPressure = (solarRadFlux / speedLight) * numAU * numAU; // Loop through the facets and calculate the SRP force and torque acting on the spacecraft about point B - for (int i = 0; i < this->numFacets; i++) { - double dcmBB0[3][3]; - Eigen::Matrix3d dcm_BB0; + for (size_t i = 0; i < this->numFacets; i++) { + Eigen::Matrix3d dcm_BB0 = Eigen::Matrix3d::Zero(); // Determine the current facet normal vector if the facet articulates if ((this->numArticulatedFacets != 0) && (i >= (this->numFacets - this->numArticulatedFacets)) && @@ -179,6 +172,7 @@ void FacetSRPDynamicEffector::computeForceTorque(double callTime, double timeSte double prv_BB0[3] = {-articulationAngle * scGeometry.facetRotAxes_B[i][0], -articulationAngle * scGeometry.facetRotAxes_B[i][1], -articulationAngle * scGeometry.facetRotAxes_B[i][2]}; + double dcmBB0[3][3]; PRV2C(prv_BB0, dcmBB0); dcm_BB0 = c2DArray2EigenMatrix3d(dcmBB0); @@ -187,8 +181,8 @@ void FacetSRPDynamicEffector::computeForceTorque(double callTime, double timeSte } // Determine the facet projected area - cosTheta = this->scGeometry.facetNormals_B[i].dot(sHat); - projectedArea = this->scGeometry.facetAreas[i] * cosTheta; + double cosTheta = this->scGeometry.facetNormals_B[i].dot(sHat); + double projectedArea = this->scGeometry.facetAreas[i] * cosTheta; // Compute the SRP force and torque acting on the facet only if the facet is in view of the Sun if (projectedArea > 0.0) { diff --git a/src/simulation/dynamics/facetSRPDynamicEffector/facetSRPDynamicEffector.h b/src/simulation/dynamics/facetSRPDynamicEffector/facetSRPDynamicEffector.h index 3ed8ababf..f3a45b6ba 100644 --- a/src/simulation/dynamics/facetSRPDynamicEffector/facetSRPDynamicEffector.h +++ b/src/simulation/dynamics/facetSRPDynamicEffector/facetSRPDynamicEffector.h @@ -62,8 +62,8 @@ class FacetSRPDynamicEffector : public SysModel, public DynamicEffector { void addArticulatedFacet(Message *tmpMsg); void ReadMessages(); - uint64_t numFacets; //!< Total number of spacecraft facets - uint64_t numArticulatedFacets; //!< Number of articulated facets + size_t numFacets; //!< Total number of spacecraft facets + size_t numArticulatedFacets; //!< Number of articulated facets ReadFunctor sunInMsg; //!< Sun spice ephemeris input message private: From 0060904ab9eae5ccb8c0cc5752049e3af39b3493 Mon Sep 17 00:00:00 2001 From: patrick kenneally Date: Fri, 20 Jun 2025 16:13:50 -0600 Subject: [PATCH 02/24] Default initialize the VSCMG config msg payload --- .../msgPayloadDefCpp/VSCMGConfigMsgPayload.h | 64 +++++++++---------- .../dynamics/VSCMGs/vscmgStateEffector.cpp | 3 +- 2 files changed, 33 insertions(+), 34 deletions(-) diff --git a/src/architecture/msgPayloadDefCpp/VSCMGConfigMsgPayload.h b/src/architecture/msgPayloadDefCpp/VSCMGConfigMsgPayload.h index ea4da5102..ed221855a 100755 --- a/src/architecture/msgPayloadDefCpp/VSCMGConfigMsgPayload.h +++ b/src/architecture/msgPayloadDefCpp/VSCMGConfigMsgPayload.h @@ -35,16 +35,16 @@ VSCMGConfigMsgPayload //@endcond { VSCMGModels VSCMGModel; //!< [-], Type of imbalance model to use - Eigen::Vector3d rGB_B; //!< [m], position vector of the VSCMG relative to the spacecraft body frame - Eigen::Vector3d gsHat0_B; //!< module variable - Eigen::Vector3d gsHat_B; //!< [-] spin axis unit vector in body frame - Eigen::Vector3d gtHat0_B; //!< module variable - Eigen::Vector3d gtHat_B; //!< module variable - Eigen::Vector3d ggHat_B; //!< module variable - Eigen::Vector3d w2Hat0_B; //!< [-] initial torque axis unit vector in body frame - Eigen::Vector3d w2Hat_B; //!< module variable - Eigen::Vector3d w3Hat0_B; //!< [-] initial gimbal axis unit vector in body frame - Eigen::Vector3d w3Hat_B; //!< module variable + Eigen::Vector3d rGB_B{}; //!< [m], position vector of the VSCMG relative to the spacecraft body frame + Eigen::Vector3d gsHat0_B{}; //!< module variable + Eigen::Vector3d gsHat_B{}; //!< [-] spin axis unit vector in body frame + Eigen::Vector3d gtHat0_B{}; //!< module variable + Eigen::Vector3d gtHat_B{}; //!< module variable + Eigen::Vector3d ggHat_B{}; //!< module variable + Eigen::Vector3d w2Hat0_B{}; //!< [-] initial torque axis unit vector in body frame + Eigen::Vector3d w2Hat_B{}; //!< module variable + Eigen::Vector3d w3Hat0_B{}; //!< [-] initial gimbal axis unit vector in body frame + Eigen::Vector3d w3Hat_B{}; //!< module variable double massV; //!< [kg] double massG; //!< [kg] double massW; //!< [kg] @@ -69,7 +69,7 @@ VSCMGConfigMsgPayload double rhoW; //!< module variable double U_s; //!< [kg-m], static imbalance double U_d; //!< [kg-m^2], dynamic imbalance - Eigen::Vector3d rGcG_G; //!< module variable + Eigen::Vector3d rGcG_G{}; //!< module variable double d; //!< [m], wheel center of mass offset from wheel frame origin double l; //!< module variable double L; //!< module variable @@ -86,33 +86,33 @@ VSCMGConfigMsgPayload double gammaDot_max; //!< [rad/s], max wheel speed double gimbalLinearFrictionRatio;//!< [%] ratio relative to max speed value up to which the friction behaves linearly - Eigen::Matrix3d IGPntGc_B; //!< module variable - Eigen::Matrix3d IWPntWc_B; //!< module variable - Eigen::Matrix3d IPrimeGPntGc_B; //!< module variable - Eigen::Matrix3d IPrimeWPntWc_B; //!< module variable - Eigen::Vector3d rGcG_B; //!< module variable - Eigen::Vector3d rGcB_B; //!< module variable - Eigen::Vector3d rWcB_B; //!< module variable - Eigen::Vector3d rWcG_B; //!< module variable - Eigen::Matrix3d rTildeGcB_B; //!< module variable - Eigen::Matrix3d rTildeWcB_B; //!< module variable - Eigen::Vector3d rPrimeGcB_B; //!< module variable - Eigen::Vector3d rPrimeWcB_B; //!< module variable - Eigen::Matrix3d rPrimeTildeGcB_B; //!< module variable - Eigen::Matrix3d rPrimeTildeWcB_B; //!< module variable - - Eigen::Vector3d aOmega; //!< [-], parameter used in coupled jitter back substitution - Eigen::Vector3d bOmega; //!< [-], parameter used in coupled jitter back substitution + Eigen::Matrix3d IGPntGc_B=Eigen::Matrix3d::Identity(); //!< module variable + Eigen::Matrix3d IWPntWc_B=Eigen::Matrix3d::Identity(); //!< module variable + Eigen::Matrix3d IPrimeWPntWc_B=Eigen::Matrix3d::Identity(); //!< module variable + Eigen::Matrix3d IPrimeGPntGc_B=Eigen::Matrix3d::Identity(); //!< module variable + Eigen::Vector3d rGcG_B{}; //!< module variable + Eigen::Vector3d rGcB_B{}; //!< module variable + Eigen::Vector3d rWcB_B{}; //!< module variable + Eigen::Vector3d rWcG_B{}; //!< module variable + Eigen::Matrix3d rTildeGcB_B=Eigen::Matrix3d::Identity(); //!< module variable + Eigen::Matrix3d rTildeWcB_B=Eigen::Matrix3d::Identity(); //!< module variable + Eigen::Vector3d rPrimeGcB_B{}; //!< module variable + Eigen::Vector3d rPrimeWcB_B{}; //!< module variable + Eigen::Matrix3d rPrimeTildeGcB_B=Eigen::Matrix3d::Identity(); //!< module variable + Eigen::Matrix3d rPrimeTildeWcB_B=Eigen::Matrix3d::Identity(); //!< module variable + + Eigen::Vector3d aOmega{}; //!< [-], parameter used in coupled jitter back substitution + Eigen::Vector3d bOmega{}; //!< [-], parameter used in coupled jitter back substitution double cOmega; //!< [-], parameter used in coupled jitter back substitution double dOmega; //!< module variable double eOmega; //!< module variable - Eigen::Vector3d agamma; //!< module variable - Eigen::Vector3d bgamma; //!< module variable + Eigen::Vector3d agamma{}; //!< module variable + Eigen::Vector3d bgamma{}; //!< module variable double cgamma; //!< module variable double dgamma; //!< module variable double egamma; //!< module variable - Eigen::Vector3d p; //!< module variable - Eigen::Vector3d q; //!< module variable + Eigen::Vector3d p{}; //!< module variable + Eigen::Vector3d q{}; //!< module variable double s; //!< module variable double gravityTorqueWheel_s; //!< module variable diff --git a/src/simulation/dynamics/VSCMGs/vscmgStateEffector.cpp b/src/simulation/dynamics/VSCMGs/vscmgStateEffector.cpp index f0b4fa5dc..5f58b5dcb 100644 --- a/src/simulation/dynamics/VSCMGs/vscmgStateEffector.cpp +++ b/src/simulation/dynamics/VSCMGs/vscmgStateEffector.cpp @@ -579,10 +579,9 @@ void VSCMGStateEffector::reset(uint64_t CurrenSimNanos) { */ void VSCMGStateEffector::WriteOutputMessages(uint64_t CurrentClock) { this->outputStates = VSCMGSpeedMsgPayload{}; - VSCMGConfigMsgPayload tmpVSCMG; std::vector::iterator it; for (it = VSCMGData.begin(); it != VSCMGData.end(); it++) { - tmpVSCMG = VSCMGConfigMsgPayload{}; + auto tmpVSCMG = VSCMGConfigMsgPayload{}; if (numVSCMGJitter > 0) { double thetaCurrent = this->thetasState->getState()(it - VSCMGData.begin(), 0); it->theta = thetaCurrent; From 7bb618507566e895c5662630be0d2b54299f4513 Mon Sep 17 00:00:00 2001 From: patrick kenneally Date: Fri, 20 Jun 2025 17:16:46 -0600 Subject: [PATCH 03/24] Initialize to zeros eigen vectors --- .../dynamics/_GeneralModuleFiles/gravityEffector.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/simulation/dynamics/_GeneralModuleFiles/gravityEffector.cpp b/src/simulation/dynamics/_GeneralModuleFiles/gravityEffector.cpp index 67daa53a4..3263b5389 100644 --- a/src/simulation/dynamics/_GeneralModuleFiles/gravityEffector.cpp +++ b/src/simulation/dynamics/_GeneralModuleFiles/gravityEffector.cpp @@ -170,8 +170,8 @@ void GravityEffector::linkInStates(DynParamManager& statesIn) { void GravityEffector::computeGravityField(Eigen::Vector3d r_cF_N, Eigen::Vector3d rDot_cF_N) { uint64_t systemClock = (uint64_t)this->timeCorr->data()[0]; - Eigen::Vector3d r_cN_N; // position of s/c CoM wrt N - Eigen::Vector3d r_CN_N; // inertial position of central body if there is one. Big C is + Eigen::Vector3d r_cN_N = Eigen::Vector3d::Zero(); // position of s/c CoM wrt N + Eigen::Vector3d r_CN_N = Eigen::Vector3d::Zero(); // inertial position of central body if there is one. Big C is // central body. Little c is CoM of s/c if (this->centralBody) // If there is a central body @@ -236,8 +236,8 @@ Eigen::Vector3d GravityEffector::getEulerSteppedGravBodyPosition(std::shared_ptr } void GravityEffector::updateEnergyContributions(Eigen::Vector3d r_cF_N, double& orbPotEnergyContr) { - Eigen::Vector3d r_CN_N; // C is central body. position of C wrt N in N - Eigen::Vector3d r_cN_N; // position c wrt N in N + Eigen::Vector3d r_CN_N = Eigen::Vector3d::Zero(); // C is central body. position of C wrt N in N + Eigen::Vector3d r_cN_N = Eigen::Vector3d::Zero(); // position c wrt N in N if (this->centralBody) { // Evaluates true if there is a central body, false // otherwise From 07366378a527d994373616604829350aeae7e1ad Mon Sep 17 00:00:00 2001 From: patrick kenneally Date: Fri, 20 Jun 2025 17:19:07 -0600 Subject: [PATCH 04/24] Default initialize msg payload --- .../environment/_GeneralModuleFiles/atmosphereBase.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/simulation/environment/_GeneralModuleFiles/atmosphereBase.cpp b/src/simulation/environment/_GeneralModuleFiles/atmosphereBase.cpp index 9cafdd7e4..272a1809e 100644 --- a/src/simulation/environment/_GeneralModuleFiles/atmosphereBase.cpp +++ b/src/simulation/environment/_GeneralModuleFiles/atmosphereBase.cpp @@ -84,7 +84,7 @@ void AtmosphereBase::addSpacecraftToModel(Message *tmpScMsg) this->envOutMsgs.push_back(msg); /* create buffer message copies*/ - AtmoPropsMsgPayload msgAtmoBuffer; + AtmoPropsMsgPayload msgAtmoBuffer{}; this->envOutBuffer.push_back(msgAtmoBuffer); return; From 94734b5afc9797dcd8650d853e39ceb74d59e468 Mon Sep 17 00:00:00 2001 From: patrick kenneally Date: Fri, 20 Jun 2025 17:30:26 -0600 Subject: [PATCH 05/24] Default initialize msg payload --- .../environment/_GeneralModuleFiles/magneticFieldBase.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/simulation/environment/_GeneralModuleFiles/magneticFieldBase.cpp b/src/simulation/environment/_GeneralModuleFiles/magneticFieldBase.cpp index bb090f73b..32b1983b8 100644 --- a/src/simulation/environment/_GeneralModuleFiles/magneticFieldBase.cpp +++ b/src/simulation/environment/_GeneralModuleFiles/magneticFieldBase.cpp @@ -77,7 +77,7 @@ void MagneticFieldBase::addSpacecraftToModel(Message *tmpScM this->envOutMsgs.push_back(msg); /* create buffer message copies*/ - MagneticFieldMsgPayload msgMagBuffer; + MagneticFieldMsgPayload msgMagBuffer{}; this->magFieldOutBuffer.push_back(msgMagBuffer); return; From e9058c4a42e78fb3e3dc4bc61f0375deef2d0764 Mon Sep 17 00:00:00 2001 From: patrick kenneally Date: Fri, 20 Jun 2025 17:39:33 -0600 Subject: [PATCH 06/24] Default initialize msg payloads --- src/simulation/environment/eclipse/eclipse.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/simulation/environment/eclipse/eclipse.cpp b/src/simulation/environment/eclipse/eclipse.cpp index 02c368f39..9d020a56c 100644 --- a/src/simulation/environment/eclipse/eclipse.cpp +++ b/src/simulation/environment/eclipse/eclipse.cpp @@ -224,7 +224,7 @@ void Eclipse::addSpacecraftToModel(Message *tmpScMsg) { this->eclipseOutMsgs.push_back(msg); /* expand the sc state buffer vector */ - SCStatesMsgPayload scMsg; + SCStatesMsgPayload scMsg{}; this->scStateBuffer.push_back(scMsg); // Now that we know the number of output messages we can size and zero @@ -239,7 +239,7 @@ void Eclipse::addSpacecraftToModel(Message *tmpScMsg) { void Eclipse::addPlanetToModel(Message *tmpSpMsg) { this->planetInMsgs.push_back(tmpSpMsg->addSubscriber()); - SpicePlanetStateMsgPayload tmpMsg; + SpicePlanetStateMsgPayload tmpMsg{}; this->planetBuffer.push_back(tmpMsg); return; } From 1c3c28caeac9a4eff1512e36dc344a9544fbba3c Mon Sep 17 00:00:00 2001 From: patrick kenneally Date: Fri, 20 Jun 2025 17:45:53 -0600 Subject: [PATCH 07/24] Refactor loop counter to be unsigned size_t --- src/simulation/environment/albedo/albedo.cpp | 9 ++++----- src/simulation/environment/albedo/albedo.h | 4 ++-- 2 files changed, 6 insertions(+), 7 deletions(-) diff --git a/src/simulation/environment/albedo/albedo.cpp b/src/simulation/environment/albedo/albedo.cpp index 3f74b6bef..45f3803da 100644 --- a/src/simulation/environment/albedo/albedo.cpp +++ b/src/simulation/environment/albedo/albedo.cpp @@ -231,9 +231,8 @@ void Albedo::updateState(uint64_t currentSimNanos) { this->readMessages(); this->albOutData.clear(); std::vector::iterator planetIt; - int idx; - for (int instIdx = 0; instIdx < this->albOutMsgs.size(); instIdx++) { - idx = 0; + for (size_t instIdx = 0; instIdx < this->albOutMsgs.size(); instIdx++) { + size_t idx = 0; double tmpTot[4] = {}; double outData[4] = {}; for (planetIt = this->planetMsgData.begin(); planetIt != this->planetMsgData.end(); planetIt++) { @@ -514,8 +513,8 @@ void Albedo::evaluateAlbedoModel(int idx) { /*! This method calculates the albedo at instrument @return void */ -void Albedo::computeAlbedo(int idx, - int instIdx, +void Albedo::computeAlbedo(size_t idx, + size_t instIdx, SpicePlanetStateMsgPayload planetMsg, bool albArray, double outData[]) { diff --git a/src/simulation/environment/albedo/albedo.h b/src/simulation/environment/albedo/albedo.h index 9fcada020..861412389 100644 --- a/src/simulation/environment/albedo/albedo.h +++ b/src/simulation/environment/albedo/albedo.h @@ -76,8 +76,8 @@ class Albedo : public SysModel { void writeMessages(uint64_t currentSimNanos); //!< writes the outpus messages void getPlanetRadius(std::string planetSpiceName); //!< gets the planet's radius void evaluateAlbedoModel(int idx); //!< evaluates the ALB model - void computeAlbedo(int idx, - int instIdx, + void computeAlbedo(size_t idx, + size_t instIdx, SpicePlanetStateMsgPayload planetMsg, bool AlbArray, double outData[]); //!< computes the albedo at instrument's location From 0278e5f6b3319f486a5061f6a56dc0bfbec42bc4 Mon Sep 17 00:00:00 2001 From: patrick kenneally Date: Fri, 20 Jun 2025 17:46:19 -0600 Subject: [PATCH 08/24] Default initialize msg payloads --- src/simulation/environment/albedo/albedo.cpp | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/src/simulation/environment/albedo/albedo.cpp b/src/simulation/environment/albedo/albedo.cpp index 45f3803da..24599a65a 100644 --- a/src/simulation/environment/albedo/albedo.cpp +++ b/src/simulation/environment/albedo/albedo.cpp @@ -176,7 +176,7 @@ void Albedo::addPlanetandAlbedoAverageModel(Message this->planetInMsgs.push_back(planetSpiceMsg->addSubscriber()); /* expand the planet state buffer vector */ - SpicePlanetStateMsgPayload plMsg; + SpicePlanetStateMsgPayload plMsg{}; this->planetMsgData.push_back(plMsg); } @@ -198,7 +198,7 @@ void Albedo::addPlanetandAlbedoAverageModel(Message this->planetInMsgs.push_back(planetSpiceMsg->addSubscriber()); /* expand the planet state buffer vector */ - SpicePlanetStateMsgPayload plMsg; + SpicePlanetStateMsgPayload plMsg{}; this->planetMsgData.push_back(plMsg); } @@ -220,7 +220,7 @@ void Albedo::addPlanetandAlbedoDataModel(Message *pl this->planetInMsgs.push_back(planetSpiceMsg->addSubscriber()); /* expand the planet state buffer vector */ - SpicePlanetStateMsgPayload plMsg; + SpicePlanetStateMsgPayload plMsg{}; this->planetMsgData.push_back(plMsg); } @@ -308,8 +308,7 @@ void Albedo::readMessages() { @return void */ void Albedo::writeMessages(uint64_t currentSimNanos) { - AlbedoMsgPayload localMessage; - memset(&localMessage, 0x0, sizeof(localMessage)); + AlbedoMsgPayload localMessage{}; //! - Write albedo output messages for each instrument for (long unsigned int idx = 0; idx < this->albOutMsgs.size(); idx++) { @@ -424,7 +423,7 @@ void Albedo::evaluateAlbedoModel(int idx) { auto albAvg = this->ALB_avgs.at(idx); if (albAvg < 0.0) { // set the albedo average automatically based on the planet's name - SpicePlanetStateMsgPayload planetInfo; + SpicePlanetStateMsgPayload planetInfo{}; planetInfo = this->planetInMsgs.at(idx)(); std::string plName(planetInfo.PlanetName); this->ALB_avgs.at(idx) = getAlbedoAverage(plName); From e502d42445af7aaade7e8e8abf3962e3038edede Mon Sep 17 00:00:00 2001 From: patrick kenneally Date: Fri, 20 Jun 2025 17:49:04 -0600 Subject: [PATCH 09/24] Default initialize msg payloads --- src/simulation/environment/groundMapping/groundMapping.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/simulation/environment/groundMapping/groundMapping.cpp b/src/simulation/environment/groundMapping/groundMapping.cpp index 747500d11..5bba19027 100644 --- a/src/simulation/environment/groundMapping/groundMapping.cpp +++ b/src/simulation/environment/groundMapping/groundMapping.cpp @@ -90,7 +90,7 @@ void GroundMapping::addPointToModel(Eigen::Vector3d &r_LP_P_init) { this->accessOutMsgs.push_back(msg); /* Expand the access buffer vectors */ - AccessMsgPayload accMsg; + AccessMsgPayload accMsg{}; this->accessMsgBuffer.push_back(accMsg); /* Create ground state output message */ @@ -99,7 +99,7 @@ void GroundMapping::addPointToModel(Eigen::Vector3d &r_LP_P_init) { this->currentGroundStateOutMsgs.push_back(msg_2); /* Expand the ground state buffer vectors */ - GroundStateMsgPayload groundMsg; + GroundStateMsgPayload groundMsg{}; this->currentGroundStateMsgBuffer.push_back(groundMsg); } From 9666c186e3e244db65c0aea67546579077c3a91e Mon Sep 17 00:00:00 2001 From: patrick kenneally Date: Fri, 20 Jun 2025 17:50:00 -0600 Subject: [PATCH 10/24] Default initialize msg payloads --- .../environment/spacecraftLocation/spacecraftLocation.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/simulation/environment/spacecraftLocation/spacecraftLocation.cpp b/src/simulation/environment/spacecraftLocation/spacecraftLocation.cpp index c3a15e792..00149c415 100644 --- a/src/simulation/environment/spacecraftLocation/spacecraftLocation.cpp +++ b/src/simulation/environment/spacecraftLocation/spacecraftLocation.cpp @@ -92,14 +92,14 @@ void SpacecraftLocation::addSpacecraftToModel(Message *tmpSc this->accessOutMsgs.push_back(msg); /* expand the buffer vector */ - AccessMsgPayload accMsg; + AccessMsgPayload accMsg{}; this->accessMsgBuffer.push_back(accMsg); } /*! Read module messages */ bool SpacecraftLocation::ReadMessages() { - SCStatesMsgPayload scMsg; + SCStatesMsgPayload scMsg{}; /* clear out the vector of spacecraft states. This is created freshly below. */ this->scStatesBuffer.clear(); From 9d65018fd27084ac629b555ed7a520b22725249d Mon Sep 17 00:00:00 2001 From: patrick kenneally Date: Fri, 20 Jun 2025 17:53:29 -0600 Subject: [PATCH 11/24] Refactor to use size_t type for counter --- src/simulation/environment/spiceInterface/spiceInterface.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/simulation/environment/spiceInterface/spiceInterface.cpp b/src/simulation/environment/spiceInterface/spiceInterface.cpp index 79e687da0..3b11df44b 100755 --- a/src/simulation/environment/spiceInterface/spiceInterface.cpp +++ b/src/simulation/environment/spiceInterface/spiceInterface.cpp @@ -122,7 +122,7 @@ void SpiceInterface::reset(uint64_t CurrenSimNanos) this->timeDataInit = true; std::vector::iterator planit; - int c = 0; // celestial object counter + size_t c = 0; // celestial object counter int autoFrame; // flag to set the frame automatically SpiceChar *name = new SpiceChar[this->charBufferSize]; SpiceBoolean frmFound; @@ -381,7 +381,7 @@ void SpiceInterface::pullSpiceData(std::vector *spic -# Convert the pos/vel over to meters. -# Time stamp the message appropriately */ - int c = 0; // celestial body counter + size_t c = 0; // celestial body counter for(planit = spiceData->begin(); planit != spiceData->end(); planit++) { double lighttime; From a1628f6336a0e699b6cdb52d36a6d29a446f6764 Mon Sep 17 00:00:00 2001 From: patrick kenneally Date: Fri, 20 Jun 2025 17:53:55 -0600 Subject: [PATCH 12/24] Default initialize msg payloads --- src/simulation/environment/spiceInterface/spiceInterface.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/simulation/environment/spiceInterface/spiceInterface.cpp b/src/simulation/environment/spiceInterface/spiceInterface.cpp index 3b11df44b..2e6687a74 100755 --- a/src/simulation/environment/spiceInterface/spiceInterface.cpp +++ b/src/simulation/environment/spiceInterface/spiceInterface.cpp @@ -165,7 +165,7 @@ void SpiceInterface::initTimeData() /* set epoch information. If provided, then the epoch message information should be used. */ if (this->epochInMsg.isLinked()) { // Read in the epoch message and set the internal time structure - EpochMsgPayload epochMsg; + EpochMsgPayload epochMsg{}; epochMsg = this->epochInMsg(); if (!this->epochInMsg.isWritten()) { bskLogger.bskLog(BSK_ERROR, "The input epoch message name was set, but the message was never written. Not using the input message."); @@ -215,7 +215,7 @@ void SpiceInterface::computeGPSData() */ void SpiceInterface::writeOutputMessages(uint64_t CurrentClock) { - SpiceTimeMsgPayload OutputData; + SpiceTimeMsgPayload OutputData{}; //! - Set the members of the time output message structure and write OutputData.J2000Current = this->J2000Current; From 9158b89fd96319f80c7ac48d5eac7892ee115ebd Mon Sep 17 00:00:00 2001 From: patrick kenneally Date: Fri, 20 Jun 2025 18:04:09 -0600 Subject: [PATCH 13/24] Default initialize msg payloads --- .../instrument/mappingInstrument/mappingInstrument.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/simulation/onboardDataHandling/instrument/mappingInstrument/mappingInstrument.cpp b/src/simulation/onboardDataHandling/instrument/mappingInstrument/mappingInstrument.cpp index 42f8e5738..dd7cc918c 100644 --- a/src/simulation/onboardDataHandling/instrument/mappingInstrument/mappingInstrument.cpp +++ b/src/simulation/onboardDataHandling/instrument/mappingInstrument/mappingInstrument.cpp @@ -55,7 +55,7 @@ void MappingInstrument::updateState(uint64_t currentSimNanos) { this->dataNodeOutMsgBuffer.at(c) = DataNodeUsageMsgPayload{}; /* Read the access message */ - AccessMsgPayload accessMsg; + AccessMsgPayload accessMsg{}; accessMsg = this->accessInMsgs.at(c)(); /* Check for access, set the data rate */ @@ -90,8 +90,7 @@ void MappingInstrument::addMappingPoint(Message *tmpAccessMsg, this->dataNodeOutMsgs.push_back(msg); /* Expand the data node usage buffer vectors */ - DataNodeUsageMsgPayload dataNodeUsageMsg; - this->dataNodeOutMsgBuffer.push_back(dataNodeUsageMsg); + this->dataNodeOutMsgBuffer.emplace_back(); return; } From 53dc6bdcf2b980c8ab2df788472ce854df720ca1 Mon Sep 17 00:00:00 2001 From: patrick kenneally Date: Fri, 20 Jun 2025 18:06:05 -0600 Subject: [PATCH 14/24] Refactor to use size_t for index --- .../onboardDataHandling/storageUnit/partitionedStorageUnit.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/simulation/onboardDataHandling/storageUnit/partitionedStorageUnit.cpp b/src/simulation/onboardDataHandling/storageUnit/partitionedStorageUnit.cpp index f950eb2bc..a876f22f1 100644 --- a/src/simulation/onboardDataHandling/storageUnit/partitionedStorageUnit.cpp +++ b/src/simulation/onboardDataHandling/storageUnit/partitionedStorageUnit.cpp @@ -70,7 +70,7 @@ void PartitionedStorageUnit::addPartition(std::string dataName){ */ void PartitionedStorageUnit::setDataBuffer(std::vector partitionNames, std::vector data){ - for (int i = 0; i < partitionNames.size(); i++) + for (size_t i = 0; i < partitionNames.size(); i++) { PartitionedStorageUnit::DataStorageUnitBase::setDataBuffer(partitionNames[i], data[i]); } From e7fc64d56953fb9c24278849bbaa67ded7c1dc08 Mon Sep 17 00:00:00 2001 From: patrick kenneally Date: Fri, 20 Jun 2025 18:06:59 -0600 Subject: [PATCH 15/24] Default initialize msg payload --- .../prescribedLinearTranslation/prescribedLinearTranslation.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/simulation/deviceInterface/prescribedLinearTranslation/prescribedLinearTranslation.cpp b/src/simulation/deviceInterface/prescribedLinearTranslation/prescribedLinearTranslation.cpp index ce3113d17..f7551bd99 100755 --- a/src/simulation/deviceInterface/prescribedLinearTranslation/prescribedLinearTranslation.cpp +++ b/src/simulation/deviceInterface/prescribedLinearTranslation/prescribedLinearTranslation.cpp @@ -47,7 +47,7 @@ The prescribed translational states are then written to the output message. */ void PrescribedLinearTranslation::updateState(uint64_t callTime) { // Read the input message - LinearTranslationRigidBodyMsgPayload linearTranslationRigidBodyIn; + LinearTranslationRigidBodyMsgPayload linearTranslationRigidBodyIn{}; if (this->linearTranslationRigidBodyInMsg.isWritten()) { linearTranslationRigidBodyIn = this->linearTranslationRigidBodyInMsg(); } From b74c1017791d9b9204b8a24edbb64f25b3f4e332 Mon Sep 17 00:00:00 2001 From: patrick kenneally Date: Fri, 20 Jun 2025 18:08:17 -0600 Subject: [PATCH 16/24] Default initialize msg payload --- .../deviceInterface/singleAxisProfiler/singleAxisProfiler.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/simulation/deviceInterface/singleAxisProfiler/singleAxisProfiler.cpp b/src/simulation/deviceInterface/singleAxisProfiler/singleAxisProfiler.cpp index d7ac0c76c..e789f169e 100755 --- a/src/simulation/deviceInterface/singleAxisProfiler/singleAxisProfiler.cpp +++ b/src/simulation/deviceInterface/singleAxisProfiler/singleAxisProfiler.cpp @@ -40,7 +40,7 @@ states. The prescribed rotational states are then written to the output message. */ void SingleAxisProfiler::updateState(uint64_t callTime) { // Read the input message - StepperMotorMsgPayload stepperMotorIn; + StepperMotorMsgPayload stepperMotorIn{}; if (this->stepperMotorInMsg.isWritten()) { stepperMotorIn = StepperMotorMsgPayload(); stepperMotorIn = this->stepperMotorInMsg(); From 9ad75255e25d5b13d262a7b2551bab8f45a23d50 Mon Sep 17 00:00:00 2001 From: patrick kenneally Date: Fri, 20 Jun 2025 18:15:18 -0600 Subject: [PATCH 17/24] Default initialize msg payload --- src/simulation/navigation/pinholeCamera/pinholeCamera.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/simulation/navigation/pinholeCamera/pinholeCamera.cpp b/src/simulation/navigation/pinholeCamera/pinholeCamera.cpp index e0938ba0e..81f8bfc61 100644 --- a/src/simulation/navigation/pinholeCamera/pinholeCamera.cpp +++ b/src/simulation/navigation/pinholeCamera/pinholeCamera.cpp @@ -81,8 +81,7 @@ void PinholeCamera::addLandmark(Eigen::Vector3d& pos, Eigen::Vector3d& normal) { this->landmarkOutMsgs.push_back(msg); /* Expand the landmark buffer vectors */ - LandmarkMsgPayload lmkMsg; - this->landmarkMsgBuffer.push_back(lmkMsg); + this->landmarkMsgBuffer.emplace_back(); } /*! Loop through landmarks From 894b66fb63cb1bcdd0ac7c2cc868221059291ab6 Mon Sep 17 00:00:00 2001 From: patrick kenneally Date: Fri, 20 Jun 2025 18:17:53 -0600 Subject: [PATCH 18/24] Refactor to use size_t for index --- src/simulation/vizard/cielimInterface/cielimInterface.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/simulation/vizard/cielimInterface/cielimInterface.cpp b/src/simulation/vizard/cielimInterface/cielimInterface.cpp index 3c423646d..a89e8dcfd 100644 --- a/src/simulation/vizard/cielimInterface/cielimInterface.cpp +++ b/src/simulation/vizard/cielimInterface/cielimInterface.cpp @@ -63,7 +63,7 @@ void CielimInterface::reset(uint64_t currentSimNanos) { spiceStatus.dataFresh = true; spiceStatus.lastTimeTag = 0xFFFFFFFFFFFFFFFF; this->spiceBodyMessageStatus.clear(); - for (int c = 0; c < this->celestialBodiesList.size(); ++c) { + for (size_t c = 0; c < this->celestialBodiesList.size(); ++c) { /*! set default zero translation and rotation states */ SpicePlanetStateMsgPayload logMsg = {}; for (int i = 0; i < 3; ++i) { @@ -192,7 +192,7 @@ void CielimInterface::writeProtobuffer(uint64_t currentSimNanos) { } /*! Write spice output msgs */ - for (int k = 0; k < this->spiceBodyMessageStatus.size(); ++k) { + for (size_t k = 0; k < this->spiceBodyMessageStatus.size(); ++k) { if (this->spiceBodyMessageStatus[k].dataFresh) { cielimMessage::CelestialBody *spice = visPayload.add_celestialbodies(); spice->set_bodyname(this->celestialBodiesList.at(k).name); From 85f99cc05ca4f2977dc0cfbb583da231cfbc6d0e Mon Sep 17 00:00:00 2001 From: patrick kenneally Date: Fri, 20 Jun 2025 18:20:54 -0600 Subject: [PATCH 19/24] Refactor to use size_t for index --- src/simulation/vizard/vizInterface/vizInterface.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/simulation/vizard/vizInterface/vizInterface.cpp b/src/simulation/vizard/vizInterface/vizInterface.cpp index 6c556ffdf..120180473 100755 --- a/src/simulation/vizard/vizInterface/vizInterface.cpp +++ b/src/simulation/vizard/vizInterface/vizInterface.cpp @@ -929,12 +929,12 @@ void VizInterface::WriteProtobuffer(uint64_t currentSimNanos) scp->set_logotexture(scIt->logoTexture); /* set spacecraft osculating orbit line color */ - for (int i=0; ioscOrbitLineColor.size(); i++){ + for (size_t i=0; ioscOrbitLineColor.size(); i++){ scp->add_oscorbitlinecolor(scIt->oscOrbitLineColor[i]); } /* set spacecraft true orbit line color */ - for (int i=0; itrueTrajectoryLineColor.size(); i++){ + for (size_t i=0; itrueTrajectoryLineColor.size(); i++){ scp->add_truetrajectorylinecolor(scIt->trueTrajectoryLineColor[i]); } From 49004d2a70c99a52adbb740a9ce9e23daabf3365 Mon Sep 17 00:00:00 2001 From: patrick kenneally Date: Fri, 20 Jun 2025 19:19:19 -0600 Subject: [PATCH 20/24] Refactor to return state vector size() as size_t --- src/fswAlgorithms/_GeneralModuleFiles/srukfInterface.cpp | 2 +- src/fswAlgorithms/_GeneralModuleFiles/srukfInterface.h | 2 +- src/fswAlgorithms/_GeneralModuleFiles/stateModels.cpp | 4 ++-- src/fswAlgorithms/_GeneralModuleFiles/stateModels.h | 2 +- .../smallBodyNavigation/smallBodyNavUKF/smallBodyNavUKF.cpp | 2 +- 5 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/fswAlgorithms/_GeneralModuleFiles/srukfInterface.cpp b/src/fswAlgorithms/_GeneralModuleFiles/srukfInterface.cpp index cedd19f64..a1391bac0 100644 --- a/src/fswAlgorithms/_GeneralModuleFiles/srukfInterface.cpp +++ b/src/fswAlgorithms/_GeneralModuleFiles/srukfInterface.cpp @@ -47,7 +47,7 @@ void SRukfInterface::reset(uint64_t currentSimNanos) { this->wM(0) = this->lambda / ((double)this->state.size() + this->lambda); this->wC(0) = this->lambda / ((double)this->state.size() + this->lambda) + (1 - this->alpha * this->alpha + this->beta); - for (auto i = 1; i < this->numberSigmaPoints; ++i) { + for (size_t i = 1; i < this->numberSigmaPoints; ++i) { this->wM(i) = 1.0 / (2.0 * ((double)this->state.size() + this->lambda)); this->wC(i) = this->wM(i); } diff --git a/src/fswAlgorithms/_GeneralModuleFiles/srukfInterface.h b/src/fswAlgorithms/_GeneralModuleFiles/srukfInterface.h index 35b40da81..991afbbcf 100644 --- a/src/fswAlgorithms/_GeneralModuleFiles/srukfInterface.h +++ b/src/fswAlgorithms/_GeneralModuleFiles/srukfInterface.h @@ -60,7 +60,7 @@ class SRukfInterface: public KalmanFilter { Eigen::MatrixXd sBar; //!< [-] Time updated covariance std::array sigmaPoints; //!< [-] sigma point vector - int numberSigmaPoints=0;//!< [-] number of sigma points + size_t numberSigmaPoints=0;//!< [-] number of sigma points Eigen::MatrixXd cholProcessNoise; //!< [-] cholesky of Qnoise Eigen::MatrixXd cholMeasNoise; //!< [-] cholesky of Measurement noise diff --git a/src/fswAlgorithms/_GeneralModuleFiles/stateModels.cpp b/src/fswAlgorithms/_GeneralModuleFiles/stateModels.cpp index 3307bf148..92741016a 100644 --- a/src/fswAlgorithms/_GeneralModuleFiles/stateModels.cpp +++ b/src/fswAlgorithms/_GeneralModuleFiles/stateModels.cpp @@ -37,8 +37,8 @@ Eigen::VectorXd State::getValues() const { /*! Set the positional components of your state (cartesian position, attitude, etc) @param Eigen::VectorXd positionComponents */ -long FilterStateVector::size() const{ - long totalSize = 0; +size_t FilterStateVector::size() const{ + size_t totalSize = 0; if (this->hasPosition()){ totalSize += this->getPositionStates().size(); } diff --git a/src/fswAlgorithms/_GeneralModuleFiles/stateModels.h b/src/fswAlgorithms/_GeneralModuleFiles/stateModels.h index b6cc9f5d9..1774826c6 100644 --- a/src/fswAlgorithms/_GeneralModuleFiles/stateModels.h +++ b/src/fswAlgorithms/_GeneralModuleFiles/stateModels.h @@ -52,7 +52,7 @@ class FilterStateVector{ Eigen::MatrixXd stm; public: - long size() const; + size_t size() const; FilterStateVector add(const FilterStateVector &vector) const; FilterStateVector addVector(const Eigen::VectorXd &vector) const; FilterStateVector scale(const double scalar) const; diff --git a/src/fswAlgorithms/smallBodyNavigation/smallBodyNavUKF/smallBodyNavUKF.cpp b/src/fswAlgorithms/smallBodyNavigation/smallBodyNavUKF/smallBodyNavUKF.cpp index 5b311a448..1da9469ec 100644 --- a/src/fswAlgorithms/smallBodyNavigation/smallBodyNavUKF/smallBodyNavUKF.cpp +++ b/src/fswAlgorithms/smallBodyNavigation/smallBodyNavUKF/smallBodyNavUKF.cpp @@ -64,7 +64,7 @@ void SmallBodyNavUKF::reset(uint64_t currentSimNanos) { /* compute UT weights to be used in the UT */ this->wm_sigma(0) = this->kappa / (this->kappa + this->numStates); this->wc_sigma(0) = this->wm_sigma(0) + 1 - pow(this->alpha, 2) + this->beta; - for (int i = 0; i < this->numStates; i++) { + for (uint64_t i = 0; i < this->numStates; i++) { /* Assign weigths */ this->wm_sigma(i + 1) = 1 / (2 * (this->numStates + this->kappa)); this->wm_sigma(numStates + i + 1) = this->wm_sigma(i + 1); From 36dcc7672210a6c695bade768ffe6422954558d0 Mon Sep 17 00:00:00 2001 From: patrick kenneally Date: Fri, 20 Jun 2025 18:33:02 -0600 Subject: [PATCH 21/24] Refactor to use size_t for index --- .../pointCloudTriangulation/pointCloudTriangulation.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fswAlgorithms/pointCloudProcessing/pointCloudTriangulation/pointCloudTriangulation.cpp b/src/fswAlgorithms/pointCloudProcessing/pointCloudTriangulation/pointCloudTriangulation.cpp index 1b78e85af..2342d5123 100644 --- a/src/fswAlgorithms/pointCloudProcessing/pointCloudTriangulation/pointCloudTriangulation.cpp +++ b/src/fswAlgorithms/pointCloudProcessing/pointCloudTriangulation/pointCloudTriangulation.cpp @@ -221,7 +221,7 @@ Eigen::Vector3d PointCloudTriangulation::triangulation(std::vector Date: Fri, 20 Jun 2025 18:34:37 -0600 Subject: [PATCH 22/24] Refactor to use size_t for index --- .../cameraTriangulation/cameraTriangulation.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/fswAlgorithms/pointCloudProcessing/cameraTriangulation/cameraTriangulation.cpp b/src/fswAlgorithms/pointCloudProcessing/cameraTriangulation/cameraTriangulation.cpp index d75bdb58b..e8266a094 100644 --- a/src/fswAlgorithms/pointCloudProcessing/cameraTriangulation/cameraTriangulation.cpp +++ b/src/fswAlgorithms/pointCloudProcessing/cameraTriangulation/cameraTriangulation.cpp @@ -204,7 +204,7 @@ std::pair CameraTriangulation::triangulation( Eigen::VectorXd y(3 * numLocations); Eigen::Matrix3d covarianceSumTerm = Eigen::Matrix3d::Zero(); - for (int c = 0; c < numLocations; ++c) { + for (size_t c = 0; c < numLocations; ++c) { // update dcm in case they are different for each image point if (dcmCamera.size() != 1) { dcm_CF = dcmCamera.at(c); @@ -228,7 +228,7 @@ std::pair CameraTriangulation::triangulation( if (this->uncertaintyImageMeasurement != 0.) { // Choose index for "companion measurement" // According to https://doi.org/10.2514/1.G006989, this can be done arbitrarily, so simply choose next index - int c2 = c + 1; + size_t c2 = c + 1; if (c2 > numLocations - 1) { // if c is already the last measurement, choose c2 = 0 c2 = 0; From c7ee98a635151e502a65d5efc127a061be507147 Mon Sep 17 00:00:00 2001 From: patrick kenneally Date: Fri, 20 Jun 2025 18:47:06 -0600 Subject: [PATCH 23/24] Pre-commit formatting clean up --- .../msgPayloadDefCpp/VSCMGConfigMsgPayload.h | 181 +++--- .../_GeneralModuleFiles/srukfInterface.h | 38 +- .../_GeneralModuleFiles/stateModels.cpp | 167 ++--- .../_GeneralModuleFiles/stateModels.h | 32 +- .../smallBodyNavUKF/smallBodyNavUKF.h | 79 ++- .../prescribedLinearTranslation.cpp | 285 ++++---- .../singleAxisProfiler/singleAxisProfiler.cpp | 8 +- .../_GeneralModuleFiles/gravityEffector.cpp | 2 +- .../facetSRPDynamicEffector.h | 4 +- .../spiceInterface/spiceInterface.cpp | 227 +++---- .../storageUnit/partitionedStorageUnit.cpp | 20 +- .../vizard/vizInterface/vizInterface.cpp | 615 +++++++++--------- 12 files changed, 810 insertions(+), 848 deletions(-) mode change 100755 => 100644 src/architecture/msgPayloadDefCpp/VSCMGConfigMsgPayload.h mode change 100755 => 100644 src/simulation/deviceInterface/prescribedLinearTranslation/prescribedLinearTranslation.cpp mode change 100755 => 100644 src/simulation/deviceInterface/singleAxisProfiler/singleAxisProfiler.cpp mode change 100755 => 100644 src/simulation/environment/spiceInterface/spiceInterface.cpp mode change 100755 => 100644 src/simulation/vizard/vizInterface/vizInterface.cpp diff --git a/src/architecture/msgPayloadDefCpp/VSCMGConfigMsgPayload.h b/src/architecture/msgPayloadDefCpp/VSCMGConfigMsgPayload.h old mode 100755 new mode 100644 index ed221855a..7b359ea51 --- a/src/architecture/msgPayloadDefCpp/VSCMGConfigMsgPayload.h +++ b/src/architecture/msgPayloadDefCpp/VSCMGConfigMsgPayload.h @@ -23,103 +23,100 @@ #include #include - -/*! @brief enumeration definiting the types of VSCMG modes */ +/*! @brief enumeration definiting the types of VSCMG modes */ enum VSCMGModels { vscmgBalancedWheels, vscmgJitterSimple, vscmgJitterFullyCoupled }; - /*! @brief Structure used to define the individual VSCMG configuration data message*/ typedef struct -//@cond DOXYGEN_IGNORE -VSCMGConfigMsgPayload + //@cond DOXYGEN_IGNORE + VSCMGConfigMsgPayload //@endcond { - VSCMGModels VSCMGModel; //!< [-], Type of imbalance model to use - Eigen::Vector3d rGB_B{}; //!< [m], position vector of the VSCMG relative to the spacecraft body frame - Eigen::Vector3d gsHat0_B{}; //!< module variable - Eigen::Vector3d gsHat_B{}; //!< [-] spin axis unit vector in body frame - Eigen::Vector3d gtHat0_B{}; //!< module variable - Eigen::Vector3d gtHat_B{}; //!< module variable - Eigen::Vector3d ggHat_B{}; //!< module variable - Eigen::Vector3d w2Hat0_B{}; //!< [-] initial torque axis unit vector in body frame - Eigen::Vector3d w2Hat_B{}; //!< module variable - Eigen::Vector3d w3Hat0_B{}; //!< [-] initial gimbal axis unit vector in body frame - Eigen::Vector3d w3Hat_B{}; //!< module variable - double massV; //!< [kg] - double massG; //!< [kg] - double massW; //!< [kg] - double theta; //!< [rad], wheel angle - double Omega; //!< [rad/s], wheel speed - double gamma; //!< [s], gimbal angle - double gammaDot; //!< [rad/s], gimbal rate - double IW1; //!< [kg-m^2], spin axis gsHat rotor moment of inertia - double IW2; //!< [kg-m^2], gtHat axis rotor moment of inertia - double IW3; //!< [kg-m^2], ggHat axis rotor moment of inertia - double IW13; //!< [kg-m^2], x-z inertia of wheel about wheel center in wheel frame (imbalance) - double IG1; //!< [kg-m^2] - double IG2; //!< [kg-m^2] - double IG3; //!< [kg-m^2] - double IG12; //!< [kg-m^2] - double IG13; //!< [kg-m^2] - double IG23; //!< [kg-m^2] - double IV1; //!< [kg-m^2] - double IV2; //!< [kg-m^2] - double IV3; //!< [kg-m^2] - double rhoG; //!< module variable - double rhoW; //!< module variable - double U_s; //!< [kg-m], static imbalance - double U_d; //!< [kg-m^2], dynamic imbalance - Eigen::Vector3d rGcG_G{}; //!< module variable - double d; //!< [m], wheel center of mass offset from wheel frame origin - double l; //!< module variable - double L; //!< module variable - double u_s_current; //!< [N-m], current motor torque - double u_s_max = -1.0; //!< [N-m], Max torque - double u_s_min; //!< [N-m], Min torque - double u_s_f; //!< [N-m], Coulomb friction torque magnitude - double Omega_max = -1.0; //!< [rad/s], max wheel speed - double wheelLinearFrictionRatio;//!< [%] ratio relative to max speed value up to which the friction behaves linearly - double u_g_current; //!< [N-m], current motor torque - double u_g_max = -1.0; //!< [N-m], Max torque - double u_g_min; //!< [N-m], Min torque - double u_g_f; //!< [N-m], Coulomb friction torque magnitude - double gammaDot_max; //!< [rad/s], max wheel speed - double gimbalLinearFrictionRatio;//!< [%] ratio relative to max speed value up to which the friction behaves linearly - - Eigen::Matrix3d IGPntGc_B=Eigen::Matrix3d::Identity(); //!< module variable - Eigen::Matrix3d IWPntWc_B=Eigen::Matrix3d::Identity(); //!< module variable - Eigen::Matrix3d IPrimeWPntWc_B=Eigen::Matrix3d::Identity(); //!< module variable - Eigen::Matrix3d IPrimeGPntGc_B=Eigen::Matrix3d::Identity(); //!< module variable - Eigen::Vector3d rGcG_B{}; //!< module variable - Eigen::Vector3d rGcB_B{}; //!< module variable - Eigen::Vector3d rWcB_B{}; //!< module variable - Eigen::Vector3d rWcG_B{}; //!< module variable - Eigen::Matrix3d rTildeGcB_B=Eigen::Matrix3d::Identity(); //!< module variable - Eigen::Matrix3d rTildeWcB_B=Eigen::Matrix3d::Identity(); //!< module variable - Eigen::Vector3d rPrimeGcB_B{}; //!< module variable - Eigen::Vector3d rPrimeWcB_B{}; //!< module variable - Eigen::Matrix3d rPrimeTildeGcB_B=Eigen::Matrix3d::Identity(); //!< module variable - Eigen::Matrix3d rPrimeTildeWcB_B=Eigen::Matrix3d::Identity(); //!< module variable - - Eigen::Vector3d aOmega{}; //!< [-], parameter used in coupled jitter back substitution - Eigen::Vector3d bOmega{}; //!< [-], parameter used in coupled jitter back substitution - double cOmega; //!< [-], parameter used in coupled jitter back substitution - double dOmega; //!< module variable - double eOmega; //!< module variable - Eigen::Vector3d agamma{}; //!< module variable - Eigen::Vector3d bgamma{}; //!< module variable - double cgamma; //!< module variable - double dgamma; //!< module variable - double egamma; //!< module variable - Eigen::Vector3d p{}; //!< module variable - Eigen::Vector3d q{}; //!< module variable - double s; //!< module variable - - double gravityTorqueWheel_s; //!< module variable - double gravityTorqueGimbal_g; //!< module variable -}VSCMGConfigMsgPayload; - - - + VSCMGModels VSCMGModel; //!< [-], Type of imbalance model to use + Eigen::Vector3d rGB_B{}; //!< [m], position vector of the VSCMG relative to the spacecraft body frame + Eigen::Vector3d gsHat0_B{}; //!< module variable + Eigen::Vector3d gsHat_B{}; //!< [-] spin axis unit vector in body frame + Eigen::Vector3d gtHat0_B{}; //!< module variable + Eigen::Vector3d gtHat_B{}; //!< module variable + Eigen::Vector3d ggHat_B{}; //!< module variable + Eigen::Vector3d w2Hat0_B{}; //!< [-] initial torque axis unit vector in body frame + Eigen::Vector3d w2Hat_B{}; //!< module variable + Eigen::Vector3d w3Hat0_B{}; //!< [-] initial gimbal axis unit vector in body frame + Eigen::Vector3d w3Hat_B{}; //!< module variable + double massV; //!< [kg] + double massG; //!< [kg] + double massW; //!< [kg] + double theta; //!< [rad], wheel angle + double Omega; //!< [rad/s], wheel speed + double gamma; //!< [s], gimbal angle + double gammaDot; //!< [rad/s], gimbal rate + double IW1; //!< [kg-m^2], spin axis gsHat rotor moment of inertia + double IW2; //!< [kg-m^2], gtHat axis rotor moment of inertia + double IW3; //!< [kg-m^2], ggHat axis rotor moment of inertia + double IW13; //!< [kg-m^2], x-z inertia of wheel about wheel center in wheel frame (imbalance) + double IG1; //!< [kg-m^2] + double IG2; //!< [kg-m^2] + double IG3; //!< [kg-m^2] + double IG12; //!< [kg-m^2] + double IG13; //!< [kg-m^2] + double IG23; //!< [kg-m^2] + double IV1; //!< [kg-m^2] + double IV2; //!< [kg-m^2] + double IV3; //!< [kg-m^2] + double rhoG; //!< module variable + double rhoW; //!< module variable + double U_s; //!< [kg-m], static imbalance + double U_d; //!< [kg-m^2], dynamic imbalance + Eigen::Vector3d rGcG_G{}; //!< module variable + double d; //!< [m], wheel center of mass offset from wheel frame origin + double l; //!< module variable + double L; //!< module variable + double u_s_current; //!< [N-m], current motor torque + double u_s_max = -1.0; //!< [N-m], Max torque + double u_s_min; //!< [N-m], Min torque + double u_s_f; //!< [N-m], Coulomb friction torque magnitude + double Omega_max = -1.0; //!< [rad/s], max wheel speed + double + wheelLinearFrictionRatio; //!< [%] ratio relative to max speed value up to which the friction behaves linearly + double u_g_current; //!< [N-m], current motor torque + double u_g_max = -1.0; //!< [N-m], Max torque + double u_g_min; //!< [N-m], Min torque + double u_g_f; //!< [N-m], Coulomb friction torque magnitude + double gammaDot_max; //!< [rad/s], max wheel speed + double + gimbalLinearFrictionRatio; //!< [%] ratio relative to max speed value up to which the friction behaves linearly + + Eigen::Matrix3d IGPntGc_B = Eigen::Matrix3d::Identity(); //!< module variable + Eigen::Matrix3d IWPntWc_B = Eigen::Matrix3d::Identity(); //!< module variable + Eigen::Matrix3d IPrimeWPntWc_B = Eigen::Matrix3d::Identity(); //!< module variable + Eigen::Matrix3d IPrimeGPntGc_B = Eigen::Matrix3d::Identity(); //!< module variable + Eigen::Vector3d rGcG_B{}; //!< module variable + Eigen::Vector3d rGcB_B{}; //!< module variable + Eigen::Vector3d rWcB_B{}; //!< module variable + Eigen::Vector3d rWcG_B{}; //!< module variable + Eigen::Matrix3d rTildeGcB_B = Eigen::Matrix3d::Identity(); //!< module variable + Eigen::Matrix3d rTildeWcB_B = Eigen::Matrix3d::Identity(); //!< module variable + Eigen::Vector3d rPrimeGcB_B{}; //!< module variable + Eigen::Vector3d rPrimeWcB_B{}; //!< module variable + Eigen::Matrix3d rPrimeTildeGcB_B = Eigen::Matrix3d::Identity(); //!< module variable + Eigen::Matrix3d rPrimeTildeWcB_B = Eigen::Matrix3d::Identity(); //!< module variable + + Eigen::Vector3d aOmega{}; //!< [-], parameter used in coupled jitter back substitution + Eigen::Vector3d bOmega{}; //!< [-], parameter used in coupled jitter back substitution + double cOmega; //!< [-], parameter used in coupled jitter back substitution + double dOmega; //!< module variable + double eOmega; //!< module variable + Eigen::Vector3d agamma{}; //!< module variable + Eigen::Vector3d bgamma{}; //!< module variable + double cgamma; //!< module variable + double dgamma; //!< module variable + double egamma; //!< module variable + Eigen::Vector3d p{}; //!< module variable + Eigen::Vector3d q{}; //!< module variable + double s; //!< module variable + + double gravityTorqueWheel_s; //!< module variable + double gravityTorqueGimbal_g; //!< module variable +} VSCMGConfigMsgPayload; #endif diff --git a/src/fswAlgorithms/_GeneralModuleFiles/srukfInterface.h b/src/fswAlgorithms/_GeneralModuleFiles/srukfInterface.h index 991afbbcf..b1a3a5bb4 100644 --- a/src/fswAlgorithms/_GeneralModuleFiles/srukfInterface.h +++ b/src/fswAlgorithms/_GeneralModuleFiles/srukfInterface.h @@ -20,22 +20,22 @@ #ifndef SRUKF_INTERFACE_HPP #define SRUKF_INTERFACE_HPP -#include -#include -#include -#include -#include "architecture/utilities/avsEigenSupport.h" -#include "architecture/utilities/macroDefinitions.h" #include "architecture/_GeneralModuleFiles/sys_model.h" #include "architecture/messaging/messaging.h" +#include "architecture/utilities/avsEigenSupport.h" +#include "architecture/utilities/macroDefinitions.h" #include "fswAlgorithms/_GeneralModuleFiles/filterInterfaceDefinitions.h" #include "fswAlgorithms/_GeneralModuleFiles/kalmanFilter.h" #include "fswAlgorithms/_GeneralModuleFiles/measurementModels.h" #include "fswAlgorithms/_GeneralModuleFiles/stateModels.h" +#include +#include +#include +#include /*! @brief Square Root unscented Kalman Filter base class */ -class SRukfInterface: public KalmanFilter { -public: +class SRukfInterface : public KalmanFilter { + public: SRukfInterface() = default; ~SRukfInterface() = default; void reset(uint64_t currentSimNanos) final; @@ -45,7 +45,7 @@ class SRukfInterface: public KalmanFilter { void setBeta(double beta); double getBeta() const; -private: + private: void timeUpdate(double updateTime) final; void measurementUpdate(const MeasurementModel &measurement) final; @@ -58,20 +58,20 @@ class SRukfInterface: public KalmanFilter { Eigen::MatrixXd forwardSubstitution(const Eigen::MatrixXd &L, const Eigen::MatrixXd &b) const; Eigen::MatrixXd choleskyDecomposition(const Eigen::MatrixXd &input) const; - Eigen::MatrixXd sBar; //!< [-] Time updated covariance - std::array sigmaPoints; //!< [-] sigma point vector - size_t numberSigmaPoints=0;//!< [-] number of sigma points - Eigen::MatrixXd cholProcessNoise; //!< [-] cholesky of Qnoise - Eigen::MatrixXd cholMeasNoise; //!< [-] cholesky of Measurement noise + Eigen::MatrixXd sBar; //!< [-] Time updated covariance + std::array sigmaPoints; //!< [-] sigma point vector + size_t numberSigmaPoints = 0; //!< [-] number of sigma points + Eigen::MatrixXd cholProcessNoise; //!< [-] cholesky of Qnoise + Eigen::MatrixXd cholMeasNoise; //!< [-] cholesky of Measurement noise - double beta=0; - double alpha=0; - double lambda=0; - double eta=0; + double beta = 0; + double alpha = 0; + double lambda = 0; + double eta = 0; Eigen::VectorXd wM; Eigen::VectorXd wC; - Eigen::MatrixXd sBarInitial; //!< [-] Time updated covariance at previous time + Eigen::MatrixXd sBarInitial; //!< [-] Time updated covariance at previous time }; #endif /* SRUKF_INTERFACE_HPP */ diff --git a/src/fswAlgorithms/_GeneralModuleFiles/stateModels.cpp b/src/fswAlgorithms/_GeneralModuleFiles/stateModels.cpp index 92741016a..450430cd1 100644 --- a/src/fswAlgorithms/_GeneralModuleFiles/stateModels.cpp +++ b/src/fswAlgorithms/_GeneralModuleFiles/stateModels.cpp @@ -23,35 +23,31 @@ /*! Set the values of a given state @param Eigen::VectorXd */ -void State::setValues(const Eigen::VectorXd &componentValues){ - this->values = componentValues; -} +void State::setValues(const Eigen::VectorXd &componentValues) { this->values = componentValues; } /*! Get the values of a given state @return Eigen::VectorXd */ -Eigen::VectorXd State::getValues() const { - return this->values; -} +Eigen::VectorXd State::getValues() const { return this->values; } /*! Set the positional components of your state (cartesian position, attitude, etc) @param Eigen::VectorXd positionComponents */ -size_t FilterStateVector::size() const{ +size_t FilterStateVector::size() const { size_t totalSize = 0; - if (this->hasPosition()){ + if (this->hasPosition()) { totalSize += this->getPositionStates().size(); } - if (this->hasVelocity()){ + if (this->hasVelocity()) { totalSize += this->getVelocityStates().size(); } - if (this->hasAcceleration()){ + if (this->hasAcceleration()) { totalSize += this->getAccelerationStates().size(); } - if (this->hasBias()){ + if (this->hasBias()) { totalSize += this->getBiasStates().size(); } - if (this->hasConsider()){ + if (this->hasConsider()) { totalSize += this->getConsiderStates().size(); } return totalSize; @@ -65,34 +61,36 @@ FilterStateVector FilterStateVector::addVector(const Eigen::VectorXd &vector) co assert(vector.size() == this->size()); long lastIndex = 0; FilterStateVector sum; - if (this->hasPosition()){ + if (this->hasPosition()) { PositionState positionState; positionState.setValues(this->getPositionStates() + vector.segment(0, this->getPositionStates().size())); sum.setPosition(positionState); lastIndex += this->getPositionStates().size(); } - if (this->hasVelocity()){ + if (this->hasVelocity()) { VelocityState velocityState; - velocityState.setValues(this->getVelocityStates() + vector.segment(lastIndex, this->getVelocityStates().size())); + velocityState.setValues(this->getVelocityStates() + + vector.segment(lastIndex, this->getVelocityStates().size())); sum.setVelocity(velocityState); lastIndex += this->getVelocityStates().size(); } - if (this->hasAcceleration()){ + if (this->hasAcceleration()) { AccelerationState accelerationState; - accelerationState.setValues(this->getAccelerationStates() + vector.segment( - lastIndex,this->getAccelerationStates().size())); + accelerationState.setValues(this->getAccelerationStates() + + vector.segment(lastIndex, this->getAccelerationStates().size())); sum.setAcceleration(accelerationState); lastIndex += this->getAccelerationStates().size(); } - if (this->hasBias()){ + if (this->hasBias()) { BiasState biasState; biasState.setValues(this->getBiasStates() + vector.segment(lastIndex, this->getBiasStates().size())); sum.setBias(biasState); lastIndex += this->getBiasStates().size(); } - if (this->hasConsider()){ + if (this->hasConsider()) { ConsiderState considerState; - considerState.setValues(this->getConsiderStates() + vector.segment(lastIndex, this->getConsiderStates().size())); + considerState.setValues(this->getConsiderStates() + + vector.segment(lastIndex, this->getConsiderStates().size())); sum.setConsider(considerState); } return sum; @@ -104,27 +102,27 @@ FilterStateVector FilterStateVector::addVector(const Eigen::VectorXd &vector) co */ FilterStateVector FilterStateVector::add(const FilterStateVector &vector) const { FilterStateVector sum; - if (this->hasPosition() && vector.hasPosition()){ + if (this->hasPosition() && vector.hasPosition()) { PositionState sumPosition; sumPosition.setValues(this->getPositionStates() + vector.getPositionStates()); sum.setPosition(sumPosition); } - if (this->hasVelocity() && vector.hasVelocity()){ + if (this->hasVelocity() && vector.hasVelocity()) { VelocityState sumVelocity; sumVelocity.setValues(this->getVelocityStates() + vector.getVelocityStates()); sum.setVelocity(sumVelocity); } - if (this->hasAcceleration() && vector.hasAcceleration()){ + if (this->hasAcceleration() && vector.hasAcceleration()) { AccelerationState sumAcceleration; sumAcceleration.setValues(this->getAccelerationStates() + vector.getAccelerationStates()); sum.setAcceleration(sumAcceleration); } - if (this->hasBias() && vector.hasBias()){ + if (this->hasBias() && vector.hasBias()) { BiasState sumBias; sumBias.setValues(this->getBiasStates() + vector.getBiasStates()); sum.setBias(sumBias); } - if (this->hasConsider() && vector.hasConsider()){ + if (this->hasConsider() && vector.hasConsider()) { ConsiderState sumConsider; sumConsider.setValues(this->getConsiderStates() + vector.getConsiderStates()); sum.setConsider(sumConsider); @@ -139,32 +137,32 @@ FilterStateVector FilterStateVector::add(const FilterStateVector &vector) const */ FilterStateVector FilterStateVector::scale(const double scalar) const { FilterStateVector scaledVector; - if (this->hasPosition()){ + if (this->hasPosition()) { PositionState scaledPosition; - scaledPosition.setValues(this->getPositionStates()*scalar); + scaledPosition.setValues(this->getPositionStates() * scalar); scaledVector.setPosition(scaledPosition); } - if (this->hasVelocity()){ + if (this->hasVelocity()) { VelocityState scaledVelocity; - scaledVelocity.setValues(this->getVelocityStates()*scalar); + scaledVelocity.setValues(this->getVelocityStates() * scalar); scaledVector.setVelocity(scaledVelocity); } - if (this->hasAcceleration()){ + if (this->hasAcceleration()) { AccelerationState scaledAcceleration; - scaledAcceleration.setValues(this->getAccelerationStates()*scalar); + scaledAcceleration.setValues(this->getAccelerationStates() * scalar); scaledVector.setAcceleration(scaledAcceleration); } - if (this->hasBias()){ + if (this->hasBias()) { BiasState scaledBias; - scaledBias.setValues(this->getBiasStates()*scalar); + scaledBias.setValues(this->getBiasStates() * scalar); scaledVector.setBias(scaledBias); } - if (this->hasConsider()){ + if (this->hasConsider()) { ConsiderState scaledConsider; - scaledConsider.setValues(this->getConsiderStates()*scalar); + scaledConsider.setValues(this->getConsiderStates() * scalar); scaledVector.setConsider(scaledConsider); } - scaledVector.attachStm(this->stm*scalar); + scaledVector.attachStm(this->stm * scalar); return scaledVector; } @@ -176,23 +174,23 @@ Eigen::VectorXd FilterStateVector::returnValues() const { Eigen::VectorXd stateVectorValues; stateVectorValues.setZero(numerOfStates); long lastIndex = 0; - if (this->hasPosition()){ + if (this->hasPosition()) { stateVectorValues.segment(0, this->getPositionStates().size()) = this->getPositionStates(); lastIndex += this->getPositionStates().size(); } - if (this->hasVelocity()){ + if (this->hasVelocity()) { stateVectorValues.segment(lastIndex, this->getVelocityStates().size()) = this->getVelocityStates(); lastIndex += this->getVelocityStates().size(); } - if (this->hasAcceleration()){ + if (this->hasAcceleration()) { stateVectorValues.segment(lastIndex, this->getAccelerationStates().size()) = this->getAccelerationStates(); lastIndex += this->getAccelerationStates().size(); } - if (this->hasBias()){ + if (this->hasBias()) { stateVectorValues.segment(lastIndex, this->getBiasStates().size()) = this->getBiasStates(); lastIndex += this->getBiasStates().size(); } - if (this->hasConsider()){ + if (this->hasConsider()) { stateVectorValues.segment(lastIndex, this->getConsiderStates().size()) = this->getConsiderStates(); } return stateVectorValues; @@ -201,154 +199,113 @@ Eigen::VectorXd FilterStateVector::returnValues() const { /*! Check if the state vector has a position state @return bool */ -bool FilterStateVector::hasPosition() const { - return this->position.has_value(); -} +bool FilterStateVector::hasPosition() const { return this->position.has_value(); } /*! Set the positional components of your state (cartesian position, attitude, etc) @param Eigen::VectorXd positionComponents */ -void FilterStateVector::setPosition(const PositionState &positionState){ - this->position = positionState; -} +void FilterStateVector::setPosition(const PositionState &positionState) { this->position = positionState; } /*! Get the positional components of your state (cartesian position, attitude, etc) @return Eigen::VectorXd */ -Eigen::VectorXd FilterStateVector::getPositionStates() const { - return this->getPosition().getValues(); -} - +Eigen::VectorXd FilterStateVector::getPositionStates() const { return this->getPosition().getValues(); } /*! Get the positional state class of your state vector(cartesian position, attitude, etc) @return PositionState */ -PositionState FilterStateVector::getPosition() const { - return this->position.value(); -} +PositionState FilterStateVector::getPosition() const { return this->position.value(); } /*! Check if the state vector has a velocity state @return bool */ -bool FilterStateVector::hasVelocity() const { - return this->velocity.has_value(); -} +bool FilterStateVector::hasVelocity() const { return this->velocity.has_value(); } /*! Set the velocity components of your state (cartesian velocity, angular rate, etc) @param Eigen::VectorXd velocityComponents */ -void FilterStateVector::setVelocity(const VelocityState &velocityState){ - this->velocity = velocityState; -} +void FilterStateVector::setVelocity(const VelocityState &velocityState) { this->velocity = velocityState; } /*! Get the velocity class of your state (cartesian velocity, angular rate, etc) @return Eigen::VectorXd */ -VelocityState FilterStateVector::getVelocity() const { - return this->velocity.value(); -} +VelocityState FilterStateVector::getVelocity() const { return this->velocity.value(); } /*! Get the velocity components of your state (cartesian velocity, angular rate, etc) @return Eigen::VectorXd */ -Eigen::VectorXd FilterStateVector::getVelocityStates() const { - return this->getVelocity().getValues(); -} +Eigen::VectorXd FilterStateVector::getVelocityStates() const { return this->getVelocity().getValues(); } /*! Check if the state vector has a acceleration state @return bool */ -bool FilterStateVector::hasAcceleration() const { - return this->acceleration.has_value(); -} +bool FilterStateVector::hasAcceleration() const { return this->acceleration.has_value(); } /*! Set the acceleration class of your state (cartesian acceleration, angular acceleration, etc) @param Eigen::VectorXd velocityComponents */ -void FilterStateVector::setAcceleration(const AccelerationState &accelerationState){ +void FilterStateVector::setAcceleration(const AccelerationState &accelerationState) { this->acceleration = accelerationState; } /*! Get the velocity class of your state (cartesian acceleration, angular acceleration, etc) @return Eigen::VectorXd */ -AccelerationState FilterStateVector::getAcceleration() const { - return this->acceleration.value(); -} +AccelerationState FilterStateVector::getAcceleration() const { return this->acceleration.value(); } /*! Get the acceleration components of your state (cartesian acceleration, angular acceleration, etc) @return Eigen::VectorXd */ -Eigen::VectorXd FilterStateVector::getAccelerationStates() const { - return this->getAcceleration().getValues(); -} +Eigen::VectorXd FilterStateVector::getAccelerationStates() const { return this->getAcceleration().getValues(); } /*! Check if the state vector has a bias state @return bool */ -bool FilterStateVector::hasBias() const { - return this->bias.has_value(); -} +bool FilterStateVector::hasBias() const { return this->bias.has_value(); } /*! Set the bias class of your state (cartesian bias, angular bias, etc) @param Eigen::VectorXd velocityComponents */ -void FilterStateVector::setBias(const BiasState &biasState){ - this->bias = biasState; -} +void FilterStateVector::setBias(const BiasState &biasState) { this->bias = biasState; } /*! Get the velocity class of your state (cartesian bias, angular bias, etc) @return Eigen::VectorXd */ -BiasState FilterStateVector::getBias() const { - return this->bias.value(); -} +BiasState FilterStateVector::getBias() const { return this->bias.value(); } /*! Get the bias components of your state (cartesian bias, angular bias, etc) @return Eigen::VectorXd */ -Eigen::VectorXd FilterStateVector::getBiasStates() const { - return this->getBias().getValues(); -} +Eigen::VectorXd FilterStateVector::getBiasStates() const { return this->getBias().getValues(); } /*! Check if the state vector has a considerParameters state @return bool */ -bool FilterStateVector::hasConsider() const { - return this->considerParameters.has_value(); -} +bool FilterStateVector::hasConsider() const { return this->considerParameters.has_value(); } /*! Set the considerParameters class of your state (cartesian considerParameters, angular considerParameters, etc) @param Eigen::VectorXd velocityComponents */ -void FilterStateVector::setConsider(const ConsiderState &considerParametersState){ +void FilterStateVector::setConsider(const ConsiderState &considerParametersState) { this->considerParameters = considerParametersState; } /*! Get the velocity class of your state (cartesian considerParameters, angular considerParameters, etc) @return Eigen::VectorXd */ -ConsiderState FilterStateVector::getConsider() const { - return this->considerParameters.value(); -} +ConsiderState FilterStateVector::getConsider() const { return this->considerParameters.value(); } /*! Get the considerParameters components of your state (cartesian considerParameters, angular considerParameters, etc) @return Eigen::VectorXd */ -Eigen::VectorXd FilterStateVector::getConsiderStates() const { - return this->getConsider().getValues(); -} +Eigen::VectorXd FilterStateVector::getConsiderStates() const { return this->getConsider().getValues(); } /*! Attach the state transition matrix of your state for simultaneous propagation @param Eigen::MatrixXd stm */ -void FilterStateVector::attachStm(const Eigen::MatrixXd& stm){ - this->stm = stm; -} +void FilterStateVector::attachStm(const Eigen::MatrixXd &stm) { this->stm = stm; } /*! Detach the state transition matrix of your state for simultaneous propagation @return Eigen::MatrixXd stm */ -Eigen::MatrixXd FilterStateVector::detachStm() const{ - return this->stm; -} +Eigen::MatrixXd FilterStateVector::detachStm() const { return this->stm; } diff --git a/src/fswAlgorithms/_GeneralModuleFiles/stateModels.h b/src/fswAlgorithms/_GeneralModuleFiles/stateModels.h index 1774826c6..bfc3d1fd7 100644 --- a/src/fswAlgorithms/_GeneralModuleFiles/stateModels.h +++ b/src/fswAlgorithms/_GeneralModuleFiles/stateModels.h @@ -18,7 +18,6 @@ */ - #ifndef FILTER_STATE_MODELS_H #define FILTER_STATE_MODELS_H @@ -26,24 +25,24 @@ #include /*! @brief State class */ -class State{ -private: +class State { + private: Eigen::VectorXd values; -public: - void setValues(const Eigen::VectorXd& componentValues); + + public: + void setValues(const Eigen::VectorXd &componentValues); Eigen::VectorXd getValues() const; }; -class PositionState : public State{}; -class VelocityState : public State{}; -class AccelerationState : public State{}; -class BiasState : public State{}; -class ConsiderState : public State{}; - +class PositionState : public State {}; +class VelocityState : public State {}; +class AccelerationState : public State {}; +class BiasState : public State {}; +class ConsiderState : public State {}; /*! @brief State models used to map a state vector to a measurement */ -class FilterStateVector{ -private: +class FilterStateVector { + private: std::optional position; std::optional velocity; std::optional acceleration; @@ -51,7 +50,7 @@ class FilterStateVector{ std::optional considerParameters; Eigen::MatrixXd stm; -public: + public: size_t size() const; FilterStateVector add(const FilterStateVector &vector) const; FilterStateVector addVector(const Eigen::VectorXd &vector) const; @@ -79,11 +78,8 @@ class FilterStateVector{ Eigen::VectorXd getConsiderStates() const; bool hasConsider() const; - void attachStm(const Eigen::MatrixXd& stm); + void attachStm(const Eigen::MatrixXd &stm); Eigen::MatrixXd detachStm() const; - - - }; #endif diff --git a/src/fswAlgorithms/smallBodyNavigation/smallBodyNavUKF/smallBodyNavUKF.h b/src/fswAlgorithms/smallBodyNavigation/smallBodyNavUKF/smallBodyNavUKF.h index eba4e75c9..7b5957d1d 100644 --- a/src/fswAlgorithms/smallBodyNavigation/smallBodyNavUKF/smallBodyNavUKF.h +++ b/src/fswAlgorithms/smallBodyNavigation/smallBodyNavUKF/smallBodyNavUKF.h @@ -17,78 +17,77 @@ */ - #ifndef SMALLBODYNAVUKF_H #define SMALLBODYNAVUKF_H #include "architecture/_GeneralModuleFiles/sys_model.h" +#include "architecture/messaging/messaging.h" #include "architecture/msgPayloadDefC/EphemerisMsgPayload.h" #include "architecture/msgPayloadDefC/NavTransMsgPayload.h" #include "architecture/msgPayloadDefC/SmallBodyNavUKFMsgPayload.h" -#include "architecture/utilities/bskLogging.h" -#include "architecture/messaging/messaging.h" -#include "architecture/utilities/orbitalMotion.h" #include "architecture/utilities/avsEigenSupport.h" +#include "architecture/utilities/bskLogging.h" #include "architecture/utilities/macroDefinitions.h" +#include "architecture/utilities/orbitalMotion.h" - -/*! @brief This module estimates relative spacecraft position, velocity with respect to the body, and the non-Keplerian acceleration perturbing the spacecraft motion, using an unscented Kalman filter (UKF) +/*! @brief This module estimates relative spacecraft position, velocity with respect to the body, and the non-Keplerian + * acceleration perturbing the spacecraft motion, using an unscented Kalman filter (UKF) */ -class SmallBodyNavUKF: public SysModel { -public: +class SmallBodyNavUKF : public SysModel { + public: SmallBodyNavUKF(); - void reset(uint64_t currentSimNanos); //!< Resets module + void reset(uint64_t currentSimNanos); //!< Resets module void updateState(uint64_t currentSimNanos); //!< Updates state -private: - void readMessages(); //!< Reads input messages + private: + void readMessages(); //!< Reads input messages void writeMessages(uint64_t currentSimNanos); //!< Writes output messages - void processUT(uint64_t currentSimNanos); //!< Process unscented transform - void measurementUT(); //!< Measurements unscented transform - void kalmanUpdate(); //!< Computes the state and covariance update + void processUT(uint64_t currentSimNanos); //!< Process unscented transform + void measurementUT(); //!< Measurements unscented transform + void kalmanUpdate(); //!< Computes the state and covariance update -public: - ReadFunctor navTransInMsg; //!< Translational nav input message + public: + ReadFunctor navTransInMsg; //!< Translational nav input message ReadFunctor asteroidEphemerisInMsg; //!< Small body ephemeris input message - Message smallBodyNavUKFOutMsg; //!< Small body nav UKF output msg - states and covariances + Message + smallBodyNavUKFOutMsg; //!< Small body nav UKF output msg - states and covariances BSKLogger bskLogger; //!< -- BSK Logging - double mu_ast; //!< Gravitational constant of the small body - Eigen::MatrixXd P_proc; //!< Process noise covariance - Eigen::MatrixXd R_meas; //!< Measurement noise covariance + double mu_ast; //!< Gravitational constant of the small body + Eigen::MatrixXd P_proc; //!< Process noise covariance + Eigen::MatrixXd R_meas; //!< Measurement noise covariance Eigen::VectorXd x_hat_k; //!< Current state estimate - Eigen::MatrixXd P_k; //!< Current state estimation covariance + Eigen::MatrixXd P_k; //!< Current state estimation covariance double alpha; //!< UKF hyper-parameter - double beta; //!< UKF hyper-parameter + double beta; //!< UKF hyper-parameter double kappa; //!< UKF hyper-parameter - Eigen::Matrix3d dcm_AN; //!< Small body dcm + Eigen::Matrix3d dcm_AN; //!< Small body dcm Eigen::Vector3d omega_AN_A; //!< Small body angular velocity -private: - NavTransMsgPayload navTransInMsgBuffer; //!< Message buffer for input translational nav message + private: + NavTransMsgPayload navTransInMsgBuffer; //!< Message buffer for input translational nav message EphemerisMsgPayload asteroidEphemerisInMsgBuffer; //!< Message buffer for asteroid ephemeris - uint64_t prevTime; //!< Previous time, ns - uint64_t numStates; //!< Number of states - uint64_t numMeas; //!< Number of measurements - uint64_t numSigmas; //!< Number of sigma points - Eigen::VectorXd x_hat_k1_; //!< Apriori state estimate for time k+1 - Eigen::VectorXd x_hat_k1; //!< Update state estimate for time k+1 - Eigen::VectorXd wm_sigma; //!< Mean sigma weights for UT - Eigen::VectorXd wc_sigma; //!< Covariance sigma weights for UT - Eigen::VectorXd y_hat_k1_; //!< Apriori measurement estimate for time k+1 - Eigen::MatrixXd P_k1_; //!< Apriori state covariance estimate for time k+1 - Eigen::MatrixXd P_k1; //!< Update state covariance estimate for time k+1 + uint64_t prevTime; //!< Previous time, ns + uint64_t numStates; //!< Number of states + uint64_t numMeas; //!< Number of measurements + uint64_t numSigmas; //!< Number of sigma points + Eigen::VectorXd x_hat_k1_; //!< Apriori state estimate for time k+1 + Eigen::VectorXd x_hat_k1; //!< Update state estimate for time k+1 + Eigen::VectorXd wm_sigma; //!< Mean sigma weights for UT + Eigen::VectorXd wc_sigma; //!< Covariance sigma weights for UT + Eigen::VectorXd y_hat_k1_; //!< Apriori measurement estimate for time k+1 + Eigen::MatrixXd P_k1_; //!< Apriori state covariance estimate for time k+1 + Eigen::MatrixXd P_k1; //!< Update state covariance estimate for time k+1 Eigen::MatrixXd X_sigma_k1_; //!< Apriori state sigma points for time k+1 - Eigen::MatrixXd R_k1_; //!< Apriori measurements covariance estimate for time k+1 + Eigen::MatrixXd R_k1_; //!< Apriori measurements covariance estimate for time k+1 Eigen::MatrixXd Y_sigma_k1_; //!< Apriori measurements sigma points for time k+1 - Eigen::MatrixXd H; //!< State-measurements cross-correlation matrix - Eigen::MatrixXd K; //!< Kalman gain matrix + Eigen::MatrixXd H; //!< State-measurements cross-correlation matrix + Eigen::MatrixXd K; //!< Kalman gain matrix }; - #endif diff --git a/src/simulation/deviceInterface/prescribedLinearTranslation/prescribedLinearTranslation.cpp b/src/simulation/deviceInterface/prescribedLinearTranslation/prescribedLinearTranslation.cpp old mode 100755 new mode 100644 index f7551bd99..3157df04a --- a/src/simulation/deviceInterface/prescribedLinearTranslation/prescribedLinearTranslation.cpp +++ b/src/simulation/deviceInterface/prescribedLinearTranslation/prescribedLinearTranslation.cpp @@ -29,7 +29,8 @@ */ void PrescribedLinearTranslation::reset(uint64_t callTime) { if (!this->linearTranslationRigidBodyInMsg.isLinked()) { - this->bskLogger->bskLog(BSK_ERROR, "prescribedLinearTranslation.linearTranslationRigidBodyInMsg wasn't connected."); + this->bskLogger->bskLog(BSK_ERROR, + "prescribedLinearTranslation.linearTranslationRigidBodyInMsg wasn't connected."); } this->tInit = 0.0; @@ -115,13 +116,13 @@ void PrescribedLinearTranslation::computeBangBangParametersNoSmoothing() { this->t_b1 = this->tInit + (totalTransTime / 2.0); // Define the parabolic constants for the first and second half of the translation - this->a = 0.5 * (this->transPosRef - this->transPosInit) - / ((this->t_b1 - this->tInit) * (this->t_b1 - this->tInit)); - this->b = -0.5 * (this->transPosRef - this->transPosInit) - / ((this->t_b1 - this->t_f) * (this->t_b1 - this->t_f)); + this->a = + 0.5 * (this->transPosRef - this->transPosInit) / ((this->t_b1 - this->tInit) * (this->t_b1 - this->tInit)); + this->b = -0.5 * (this->transPosRef - this->transPosInit) / ((this->t_b1 - this->t_f) * (this->t_b1 - this->t_f)); } -/*! This method computes the required parameters for the translation with a non-smoothed bang-coast-bang acceleration profile. +/*! This method computes the required parameters for the translation with a non-smoothed bang-coast-bang acceleration + profile. @return void */ void PrescribedLinearTranslation::computeBangCoastBangParametersNoSmoothing() { @@ -131,8 +132,9 @@ void PrescribedLinearTranslation::computeBangCoastBangParametersNoSmoothing() { this->t_b1 = this->tInit + this->coastOptionBangDuration; // Determine the hub-relative position at time t_b1 - this->transPos_tb1 = sign * 0.5 * this->transAccelMax * this->coastOptionBangDuration - * this->coastOptionBangDuration + this->transPosInit; + this->transPos_tb1 = + sign * 0.5 * this->transAccelMax * this->coastOptionBangDuration * this->coastOptionBangDuration + + this->transPosInit; this->transVel_tb1 = sign * this->transAccelMax * this->coastOptionBangDuration; // Determine the distance traveled during the coast period @@ -166,46 +168,48 @@ void PrescribedLinearTranslation::computeSmoothedBangBangParameters() { // Determine the hub-relative position and velocity at time t_s1 this->transVel_ts1 = sign * 0.5 * this->transAccelMax * this->smoothingDuration; - this->transPos_ts1 = sign * (3.0 / 20.0) * this->transAccelMax * this->smoothingDuration * this->smoothingDuration - + this->transPosInit; + this->transPos_ts1 = sign * (3.0 / 20.0) * this->transAccelMax * this->smoothingDuration * this->smoothingDuration + + this->transPosInit; // Determine the duration of the bang segment bangDuration double aTerm = sign * 0.5 * this->transAccelMax; double bTerm = (sign * this->transAccelMax * this->smoothingDuration + this->transVel_ts1) / aTerm; - double cTerm = (sign * (2.0 / 5.0) * this->transAccelMax * this->smoothingDuration * this->smoothingDuration - + this->transVel_ts1 * this->smoothingDuration + this->transPos_ts1 - - 0.5 * (this->transPosRef + this->transPosInit)) / aTerm; - double bangDuration = (- bTerm + sqrt(bTerm * bTerm - 4.0 * cTerm)) / 2.0; + double cTerm = (sign * (2.0 / 5.0) * this->transAccelMax * this->smoothingDuration * this->smoothingDuration + + this->transVel_ts1 * this->smoothingDuration + this->transPos_ts1 - + 0.5 * (this->transPosRef + this->transPosInit)) / + aTerm; + double bangDuration = (-bTerm + sqrt(bTerm * bTerm - 4.0 * cTerm)) / 2.0; // Determine the time at the end of the first bang segment t_b1 this->t_b1 = this->t_s1 + bangDuration; // Determine the hub-relative position and velocity at time t_b1 this->transVel_tb1 = sign * this->transAccelMax * bangDuration + this->transVel_ts1; - this->transPos_tb1 = sign * 0.5 * this->transAccelMax * bangDuration * bangDuration - + this->transVel_ts1 * bangDuration + this->transPos_ts1; + this->transPos_tb1 = sign * 0.5 * this->transAccelMax * bangDuration * bangDuration + + this->transVel_ts1 * bangDuration + this->transPos_ts1; // Determine the time at the end of the second smoothing segment t_s2 this->t_s2 = this->t_b1 + 2.0 * this->smoothingDuration; // Determine the hub-relative position and velocity at time t_s2 this->transVel_ts2 = this->transVel_tb1; - this->transPos_ts2 = sign * (4.0 / 5.0) * this->transAccelMax * this->smoothingDuration * this->smoothingDuration - + this->transVel_tb1 * 2.0 * this->smoothingDuration + this->transPos_tb1; + this->transPos_ts2 = sign * (4.0 / 5.0) * this->transAccelMax * this->smoothingDuration * this->smoothingDuration + + this->transVel_tb1 * 2.0 * this->smoothingDuration + this->transPos_tb1; // Determine the time at the end of the second bang segment t_b2 this->t_b2 = this->t_s2 + bangDuration; // Determine the hub-relative position and velocity at time t_b2 - this->transVel_tb2 = - sign * this->transAccelMax * bangDuration + this->transVel_ts2; - this->transPos_tb2 = - sign * 0.5 * this->transAccelMax * bangDuration * bangDuration - + this->transVel_ts2 * bangDuration + this->transPos_ts2; + this->transVel_tb2 = -sign * this->transAccelMax * bangDuration + this->transVel_ts2; + this->transPos_tb2 = -sign * 0.5 * this->transAccelMax * bangDuration * bangDuration + + this->transVel_ts2 * bangDuration + this->transPos_ts2; // Determine the time when the translation is complete t_f this->t_f = this->t_b2 + this->smoothingDuration; } -/*! This method computes the required parameters for the translation with a smoothed bang-coast-bang acceleration profile. +/*! This method computes the required parameters for the translation with a smoothed bang-coast-bang acceleration + profile. @return void */ void PrescribedLinearTranslation::computeSmoothedBangCoastBangParameters() { @@ -216,25 +220,25 @@ void PrescribedLinearTranslation::computeSmoothedBangCoastBangParameters() { // Determine the hub-relative position and velocity at time t_s1 this->transVel_ts1 = sign * 0.5 * this->transAccelMax * this->smoothingDuration; - this->transPos_ts1 = sign * (3.0 / 20.0) * this->transAccelMax * this->smoothingDuration * this->smoothingDuration - + this->transPosInit; + this->transPos_ts1 = sign * (3.0 / 20.0) * this->transAccelMax * this->smoothingDuration * this->smoothingDuration + + this->transPosInit; // Determine the time at the end of the first bang segment t_b1 this->t_b1 = this->t_s1 + this->coastOptionBangDuration; // Determine the hub-relative position and velocity at time t_b1 this->transVel_tb1 = sign * this->transAccelMax * this->coastOptionBangDuration + this->transVel_ts1; - this->transPos_tb1 = sign * 0.5 * this->transAccelMax * this->coastOptionBangDuration - * this->coastOptionBangDuration + this->transVel_ts1 * this->coastOptionBangDuration - + this->transPos_ts1; + this->transPos_tb1 = + sign * 0.5 * this->transAccelMax * this->coastOptionBangDuration * this->coastOptionBangDuration + + this->transVel_ts1 * this->coastOptionBangDuration + this->transPos_ts1; // Determine the time at the end of the second smoothing segment t_s2 this->t_s2 = this->t_b1 + this->smoothingDuration; // Determine the hub-relative position and velocity at time t_s2 this->transVel_ts2 = sign * 0.5 * this->transAccelMax * this->smoothingDuration + this->transVel_tb1; - this->transPos_ts2 = sign * (7.0 / 20.0) * this->transAccelMax * this->smoothingDuration * this->smoothingDuration - + this->transVel_tb1 * this->smoothingDuration + this->transPos_tb1; + this->transPos_ts2 = sign * (7.0 / 20.0) * this->transAccelMax * this->smoothingDuration * this->smoothingDuration + + this->transVel_tb1 * this->smoothingDuration + this->transPos_tb1; // Compute the time at the end of the coast segment t_c double deltaPosCoast = (this->transPosRef - this->transPosInit) - 2 * (this->transPos_ts2 - this->transPosInit); @@ -248,18 +252,19 @@ void PrescribedLinearTranslation::computeSmoothedBangCoastBangParameters() { this->t_s3 = this->t_c + this->smoothingDuration; // Determine the hub-relative position and velocity at time t_s3 - this->transVel_ts3 = - sign * 0.5 * this->transAccelMax * this->smoothingDuration + this->transVel_tc; - this->transPos_ts3 = - sign * (3.0 / 20.0) * this->transAccelMax * this->smoothingDuration * this->smoothingDuration - + this->transVel_tc * this->smoothingDuration + this->transPos_tc; + this->transVel_ts3 = -sign * 0.5 * this->transAccelMax * this->smoothingDuration + this->transVel_tc; + this->transPos_ts3 = + -sign * (3.0 / 20.0) * this->transAccelMax * this->smoothingDuration * this->smoothingDuration + + this->transVel_tc * this->smoothingDuration + this->transPos_tc; // Determine the time at the end of the second bang segment t_b2 this->t_b2 = this->t_s3 + this->coastOptionBangDuration; // Determine the hub-relative position and velocity at time t_b2 - this->transVel_tb2 = - sign * this->transAccelMax * this->coastOptionBangDuration + this->transVel_ts3; - this->transPos_tb2 = - sign * 0.5 * this->transAccelMax * this->coastOptionBangDuration - * this->coastOptionBangDuration + this->transVel_ts3 * this->coastOptionBangDuration - + this->transPos_ts3; + this->transVel_tb2 = -sign * this->transAccelMax * this->coastOptionBangDuration + this->transVel_ts3; + this->transPos_tb2 = + -sign * 0.5 * this->transAccelMax * this->coastOptionBangDuration * this->coastOptionBangDuration + + this->transVel_ts3 * this->coastOptionBangDuration + this->transPos_ts3; // Determine the time when the translation is complete t_f this->t_f = this->t_b2 + this->smoothingDuration; @@ -270,7 +275,7 @@ void PrescribedLinearTranslation::computeSmoothedBangCoastBangParameters() { */ void PrescribedLinearTranslation::computeCurrentState(double t) { if (this->coastOptionBangDuration > 0.0) { - if(this->smoothingDuration > 0.0) { + if (this->smoothingDuration > 0.0) { if (this->isInFirstSmoothedSegment(t)) { this->computeFirstSmoothedSegment(t); } else if (this->isInFirstBangSegment(t)) { @@ -366,7 +371,8 @@ bool PrescribedLinearTranslation::isInFirstSmoothedSegment(double t) const { return (t <= this->t_s1 && this->t_f - this->tInit != 0.0); } -/*! This method determines if the current time is within the second smoothing segment for the smoothed profiler options.. +/*! This method determines if the current time is within the second smoothing segment for the smoothed profiler + options.. @return bool @param t [s] Current simulation time */ @@ -386,7 +392,8 @@ bool PrescribedLinearTranslation::isInThirdSmoothedSegment(double t) const { } } -/*! This method determines if the current time is within the fourth smoothing segment for the smoothed bang-coast-bang option. +/*! This method determines if the current time is within the fourth smoothing segment for the smoothed bang-coast-bang + option. @return bool @param t [s] Current simulation time */ @@ -401,7 +408,7 @@ bool PrescribedLinearTranslation::isInFourthSmoothedSegment(double t) const { bool PrescribedLinearTranslation::isInCoastSegment(double t) const { if (this->smoothingDuration > 0.0) { return (t > this->t_s2 && t <= this->t_c && this->t_f - this->tInit != 0.0); - } else{ + } else { return (t > this->t_b1 && t <= this->t_c && this->t_f - this->tInit != 0.0); } } @@ -416,8 +423,8 @@ void PrescribedLinearTranslation::computeFirstBangSegment(double t) { if (this->smoothingDuration > 0.0) { this->transVel = this->transAccel * (t - this->t_s1) + this->transVel_ts1; - this->transPos = 0.5 * this->transAccel * (t - this->t_s1) * (t - this->t_s1) - + this->transVel_ts1 * (t - this->t_s1) + this->transPos_ts1; + this->transPos = 0.5 * this->transAccel * (t - this->t_s1) * (t - this->t_s1) + + this->transVel_ts1 * (t - this->t_s1) + this->transPos_ts1; } else { this->transVel = this->transAccel * (t - this->tInit); this->transPos = this->a * (t - this->tInit) * (t - this->tInit) + this->transPosInit; @@ -430,17 +437,17 @@ void PrescribedLinearTranslation::computeFirstBangSegment(double t) { */ void PrescribedLinearTranslation::computeSecondBangSegment(double t) { double sign = (this->transPosRef - this->transPosInit) / abs(this->transPosRef - this->transPosInit); - this->transAccel = - sign * this->transAccelMax; + this->transAccel = -sign * this->transAccelMax; if (this->smoothingDuration > 0.0) { if (this->coastOptionBangDuration > 0.0) { this->transVel = this->transAccel * (t - this->t_s3) + this->transVel_ts3; - this->transPos = 0.5 * this->transAccel * (t - this->t_s3) * (t - this->t_s3) - + this->transVel_ts3 * (t - this->t_s3) + this->transPos_ts3; + this->transPos = 0.5 * this->transAccel * (t - this->t_s3) * (t - this->t_s3) + + this->transVel_ts3 * (t - this->t_s3) + this->transPos_ts3; } else { this->transVel = this->transAccel * (t - this->t_s2) + this->transVel_ts2; - this->transPos = 0.5 * this->transAccel * (t - this->t_s2) * (t - this->t_s2) - + this->transVel_ts2 * (t - this->t_s2) + this->transPos_ts2; + this->transPos = 0.5 * this->transAccel * (t - this->t_s2) * (t - this->t_s2) + + this->transVel_ts2 * (t - this->t_s2) + this->transPos_ts2; } } else { this->transVel = this->transAccel * (t - this->t_f); @@ -456,16 +463,16 @@ void PrescribedLinearTranslation::computeFirstSmoothedSegment(double t) { double sign = (this->transPosRef - this->transPosInit) / abs(this->transPosRef - this->transPosInit); double term1 = (3.0 * (t - this->tInit) * (t - this->tInit)) / (this->smoothingDuration * this->smoothingDuration); - double term2 = (2.0 * (t - this->tInit) * (t - this->tInit) * (t - this->tInit)) - / (this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); - double term3 = ((t - this->tInit) * (t - this->tInit) * (t - this->tInit)) - / (this->smoothingDuration * this->smoothingDuration); - double term4 = ((t - this->tInit) * (t - this->tInit) * (t - this->tInit) * (t - this->tInit)) - / (2.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); - double term5 = ((t - this->tInit) * (t - this->tInit) * (t - this->tInit) * (t - this->tInit)) - / (4.0 * this->smoothingDuration * this->smoothingDuration); - double term6 = ((t - this->tInit) * (t - this->tInit) * (t - this->tInit) * (t - this->tInit) * (t - this->tInit)) - / (10.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + double term2 = (2.0 * (t - this->tInit) * (t - this->tInit) * (t - this->tInit)) / + (this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + double term3 = ((t - this->tInit) * (t - this->tInit) * (t - this->tInit)) / + (this->smoothingDuration * this->smoothingDuration); + double term4 = ((t - this->tInit) * (t - this->tInit) * (t - this->tInit) * (t - this->tInit)) / + (2.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + double term5 = ((t - this->tInit) * (t - this->tInit) * (t - this->tInit) * (t - this->tInit)) / + (4.0 * this->smoothingDuration * this->smoothingDuration); + double term6 = ((t - this->tInit) * (t - this->tInit) * (t - this->tInit) * (t - this->tInit) * (t - this->tInit)) / + (10.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); this->transAccel = sign * this->transAccelMax * (term1 - term2); this->transVel = sign * this->transAccelMax * (term3 - term4); @@ -489,36 +496,36 @@ void PrescribedLinearTranslation::computeSecondSmoothedSegment(double t) { if (this->coastOptionBangDuration > 0.0) { term1 = (3.0 * (t - this->t_b1) * (t - this->t_b1)) / (this->smoothingDuration * this->smoothingDuration); - term2 = (2.0 * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) - / (this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); - term3 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) - / (this->smoothingDuration * this->smoothingDuration); - term4 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) - / (2.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + term2 = (2.0 * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) / + (this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + term3 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) / + (this->smoothingDuration * this->smoothingDuration); + term4 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) / + (2.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); term5 = 0.5 * (t - this->t_b1) * (t - this->t_b1); - term6 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) - / (4.0 * this->smoothingDuration * this->smoothingDuration); - term7 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) - / (10.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + term6 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) / + (4.0 * this->smoothingDuration * this->smoothingDuration); + term7 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) / + (10.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); } else { term1 = (3.0 * (t - this->t_b1) * (t - this->t_b1)) / (2.0 * this->smoothingDuration * this->smoothingDuration); - term2 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) - / (2.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); - term3 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) - / (2.0 * this->smoothingDuration * this->smoothingDuration); - term4 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) - / (8.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + term2 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) / + (2.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + term3 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) / + (2.0 * this->smoothingDuration * this->smoothingDuration); + term4 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) / + (8.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); term5 = 0.5 * (t - this->t_b1) * (t - this->t_b1); - term6 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) - / (8.0 * this->smoothingDuration * this->smoothingDuration); - term7 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) - / (40.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + term6 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) / + (8.0 * this->smoothingDuration * this->smoothingDuration); + term7 = ((t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1) * (t - this->t_b1)) / + (40.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); } this->transAccel = sign * this->transAccelMax * (1.0 - term1 + term2); this->transVel = sign * this->transAccelMax * ((t - this->t_b1) - term3 + term4) + this->transVel_tb1; - this->transPos = sign * this->transAccelMax * (term5 - term6 + term7) - + this->transVel_tb1 * (t - this->t_b1) + this->transPos_tb1; + this->transPos = sign * this->transAccelMax * (term5 - term6 + term7) + this->transVel_tb1 * (t - this->t_b1) + + this->transPos_tb1; } /*! This method computes the third smoothing segment scalar translational states for the smoothed profiler options. @@ -538,63 +545,65 @@ void PrescribedLinearTranslation::computeThirdSmoothedSegment(double t) { if (this->coastOptionBangDuration > 0.0) { term1 = (3.0 * (t - this->t_c) * (t - this->t_c)) / (this->smoothingDuration * this->smoothingDuration); - term2 = (2.0 * (t - this->t_c) * (t - this->t_c) * (t - this->t_c)) - / (this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); - term3 = ((t - this->t_c) * (t - this->t_c) * (t - this->t_c)) - / (this->smoothingDuration * this->smoothingDuration); - term4 = ((t - this->t_c) * (t - this->t_c) * (t - this->t_c) * (t - this->t_c)) - / (2.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); - term5 = ((t - this->t_c) * (t - this->t_c) * (t - this->t_c) * (t - this->t_c)) - / (4.0 * this->smoothingDuration * this->smoothingDuration); - term6 = ((t - this->t_c) * (t - this->t_c) * (t - this->t_c) * (t - this->t_c) * (t - this->t_c)) - / (10.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); - - this->transAccel = - sign * this->transAccelMax * (term1 - term2); - this->transVel = - sign * this->transAccelMax * (term3 - term4) + this->transVel_tc; - this->transPos = - sign * this->transAccelMax * (term5 - term6) + this->transVel_tc * (t - this->t_c) + this->transPos_tc; + term2 = (2.0 * (t - this->t_c) * (t - this->t_c) * (t - this->t_c)) / + (this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + term3 = + ((t - this->t_c) * (t - this->t_c) * (t - this->t_c)) / (this->smoothingDuration * this->smoothingDuration); + term4 = ((t - this->t_c) * (t - this->t_c) * (t - this->t_c) * (t - this->t_c)) / + (2.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + term5 = ((t - this->t_c) * (t - this->t_c) * (t - this->t_c) * (t - this->t_c)) / + (4.0 * this->smoothingDuration * this->smoothingDuration); + term6 = ((t - this->t_c) * (t - this->t_c) * (t - this->t_c) * (t - this->t_c) * (t - this->t_c)) / + (10.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + + this->transAccel = -sign * this->transAccelMax * (term1 - term2); + this->transVel = -sign * this->transAccelMax * (term3 - term4) + this->transVel_tc; + this->transPos = + -sign * this->transAccelMax * (term5 - term6) + this->transVel_tc * (t - this->t_c) + this->transPos_tc; } else { term1 = (3.0 * (t - this->t_b2) * (t - this->t_b2)) / (this->smoothingDuration * this->smoothingDuration); - term2 = (2.0 * (t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2)) - / (this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); - term3 = ((t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2)) - / (this->smoothingDuration * this->smoothingDuration); - term4 = ((t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2)) - / (2.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); - term5 = - 0.5 * (t - this->t_b2) * (t - this->t_b2); - term6 = ((t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2)) - / (4.0 * this->smoothingDuration * this->smoothingDuration); - term7 = ((t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2)) - / (10.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); - - this->transAccel = sign * this->transAccelMax * ( - 1.0 + term1 - term2); - this->transVel = sign * this->transAccelMax * ( - (t - this->t_b2) + term3 - term4) + this->transVel_tb2; - this->transPos = sign * this->transAccelMax * (term5 + term6 - term7) + this->transVel_tb2 * (t - this->t_b2) - + this->transPos_tb2; + term2 = (2.0 * (t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2)) / + (this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + term3 = ((t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2)) / + (this->smoothingDuration * this->smoothingDuration); + term4 = ((t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2)) / + (2.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + term5 = -0.5 * (t - this->t_b2) * (t - this->t_b2); + term6 = ((t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2)) / + (4.0 * this->smoothingDuration * this->smoothingDuration); + term7 = ((t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2) * (t - this->t_b2)) / + (10.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + + this->transAccel = sign * this->transAccelMax * (-1.0 + term1 - term2); + this->transVel = sign * this->transAccelMax * (-(t - this->t_b2) + term3 - term4) + this->transVel_tb2; + this->transPos = sign * this->transAccelMax * (term5 + term6 - term7) + this->transVel_tb2 * (t - this->t_b2) + + this->transPos_tb2; } } -/*! This method computes the fourth smoothing segment scalar translational states for the smoothed bang-coast-bang option. +/*! This method computes the fourth smoothing segment scalar translational states for the smoothed bang-coast-bang + option. @return void @param t [s] Current simulation time */ void PrescribedLinearTranslation::computeFourthSmoothedSegment(double t) { double term1 = (3.0 * (this->t_f - t) * (this->t_f - t)) / (this->smoothingDuration * this->smoothingDuration); - double term2 = (2.0 * (this->t_f - t) * (this->t_f - t) * (this->t_f - t)) - / (this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); - double term3 = ((this->t_f - t) * (this->t_f - t) * (this->t_f - t)) - / (this->smoothingDuration * this->smoothingDuration); - double term4 = ((this->t_f - t) * (this->t_f - t) * (this->t_f - t) * (this->t_f - t)) - / (2.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); - double term5 = ((this->t_f - t) * (this->t_f - t) * (this->t_f - t) * (this->t_f - t)) - / (4.0 * this->smoothingDuration * this->smoothingDuration); - double term6 = ((this->t_f - t) * (this->t_f - t) * (this->t_f - t) * (this->t_f - t) * (this->t_f - t)) - / (10.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + double term2 = (2.0 * (this->t_f - t) * (this->t_f - t) * (this->t_f - t)) / + (this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + double term3 = + ((this->t_f - t) * (this->t_f - t) * (this->t_f - t)) / (this->smoothingDuration * this->smoothingDuration); + double term4 = ((this->t_f - t) * (this->t_f - t) * (this->t_f - t) * (this->t_f - t)) / + (2.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); + double term5 = ((this->t_f - t) * (this->t_f - t) * (this->t_f - t) * (this->t_f - t)) / + (4.0 * this->smoothingDuration * this->smoothingDuration); + double term6 = ((this->t_f - t) * (this->t_f - t) * (this->t_f - t) * (this->t_f - t) * (this->t_f - t)) / + (10.0 * this->smoothingDuration * this->smoothingDuration * this->smoothingDuration); double sign = (this->transPosRef - this->transPosInit) / abs(this->transPosRef - this->transPosInit); - this->transAccel = - sign * this->transAccelMax * (term1 - term2); + this->transAccel = -sign * this->transAccelMax * (term1 - term2); this->transVel = sign * this->transAccelMax * (term3 - term4); - this->transPos = - sign * this->transAccelMax * (term5 - term6) + this->transPosRef; + this->transPos = -sign * this->transAccelMax * (term5 - term6) + this->transPosRef; } /*! This method computes the coast segment scalar translational states @@ -668,57 +677,41 @@ void PrescribedLinearTranslation::setSmoothingDuration(const double smoothingDur @return void @param transAccelMax [m/s^2] Bang segment linear angular acceleration */ -void PrescribedLinearTranslation::setTransAccelMax(const double transAccelMax) { - this->transAccelMax = transAccelMax; -} +void PrescribedLinearTranslation::setTransAccelMax(const double transAccelMax) { this->transAccelMax = transAccelMax; } /*! Setter method for the translating body axis of translation. @return void @param transHat_M Translating body axis of translation (unit vector) */ -void PrescribedLinearTranslation::setTransHat_M(const Eigen::Vector3d &transHat_M) { - this->transHat_M = transHat_M; -} +void PrescribedLinearTranslation::setTransHat_M(const Eigen::Vector3d &transHat_M) { this->transHat_M = transHat_M; } /*! Setter method for the initial translating body hub-relative position. @return void @param transPosInit [m] Initial translating body position relative to the hub */ -void PrescribedLinearTranslation::setTransPosInit(const double transPosInit) { - this->transPosInit = transPosInit; -} +void PrescribedLinearTranslation::setTransPosInit(const double transPosInit) { this->transPosInit = transPosInit; } /*! Getter method for the coast option bang duration. @return double */ -double PrescribedLinearTranslation::getCoastOptionBangDuration() const { - return this->coastOptionBangDuration; -} +double PrescribedLinearTranslation::getCoastOptionBangDuration() const { return this->coastOptionBangDuration; } /*! Getter method for the duration the acceleration is smoothed until reaching the given maximum acceleration value. @return double */ -double PrescribedLinearTranslation::getSmoothingDuration() const { - return this->smoothingDuration; -} +double PrescribedLinearTranslation::getSmoothingDuration() const { return this->smoothingDuration; } /*! Getter method for the bang segment scalar linear acceleration. @return double */ -double PrescribedLinearTranslation::getTransAccelMax() const { - return this->transAccelMax; -} +double PrescribedLinearTranslation::getTransAccelMax() const { return this->transAccelMax; } /*! Getter method for the translating body axis of translation. @return const Eigen::Vector3d */ -const Eigen::Vector3d &PrescribedLinearTranslation::getTransHat_M() const { - return this->transHat_M; -} +const Eigen::Vector3d &PrescribedLinearTranslation::getTransHat_M() const { return this->transHat_M; } /*! Getter method for the initial translating body position. @return double */ -double PrescribedLinearTranslation::getTransPosInit() const { - return this->transPosInit; -} +double PrescribedLinearTranslation::getTransPosInit() const { return this->transPosInit; } diff --git a/src/simulation/deviceInterface/singleAxisProfiler/singleAxisProfiler.cpp b/src/simulation/deviceInterface/singleAxisProfiler/singleAxisProfiler.cpp old mode 100755 new mode 100644 index e789f169e..b0872238b --- a/src/simulation/deviceInterface/singleAxisProfiler/singleAxisProfiler.cpp +++ b/src/simulation/deviceInterface/singleAxisProfiler/singleAxisProfiler.cpp @@ -89,13 +89,9 @@ Eigen::Vector3d SingleAxisProfiler::computeSigma_FM(double theta) { @return void @param rotHat_M Spinning body rotation axis (unit vector) */ -void SingleAxisProfiler::setRotHat_M(const Eigen::Vector3d &rotHat_M) { - this->rotHat_M = rotHat_M / rotHat_M.norm(); -} +void SingleAxisProfiler::setRotHat_M(const Eigen::Vector3d &rotHat_M) { this->rotHat_M = rotHat_M / rotHat_M.norm(); } /*! Getter method for the spinning body rotation axis. @return const Eigen::Vector3d */ -const Eigen::Vector3d &SingleAxisProfiler::getRotHat_M() const { - return this->rotHat_M; -} +const Eigen::Vector3d &SingleAxisProfiler::getRotHat_M() const { return this->rotHat_M; } diff --git a/src/simulation/dynamics/_GeneralModuleFiles/gravityEffector.cpp b/src/simulation/dynamics/_GeneralModuleFiles/gravityEffector.cpp index 3263b5389..b3d51f984 100644 --- a/src/simulation/dynamics/_GeneralModuleFiles/gravityEffector.cpp +++ b/src/simulation/dynamics/_GeneralModuleFiles/gravityEffector.cpp @@ -172,7 +172,7 @@ void GravityEffector::computeGravityField(Eigen::Vector3d r_cF_N, Eigen::Vector3 uint64_t systemClock = (uint64_t)this->timeCorr->data()[0]; Eigen::Vector3d r_cN_N = Eigen::Vector3d::Zero(); // position of s/c CoM wrt N Eigen::Vector3d r_CN_N = Eigen::Vector3d::Zero(); // inertial position of central body if there is one. Big C is - // central body. Little c is CoM of s/c + // central body. Little c is CoM of s/c if (this->centralBody) // If there is a central body { diff --git a/src/simulation/dynamics/facetSRPDynamicEffector/facetSRPDynamicEffector.h b/src/simulation/dynamics/facetSRPDynamicEffector/facetSRPDynamicEffector.h index f3a45b6ba..05a3e7e98 100644 --- a/src/simulation/dynamics/facetSRPDynamicEffector/facetSRPDynamicEffector.h +++ b/src/simulation/dynamics/facetSRPDynamicEffector/facetSRPDynamicEffector.h @@ -62,8 +62,8 @@ class FacetSRPDynamicEffector : public SysModel, public DynamicEffector { void addArticulatedFacet(Message *tmpMsg); void ReadMessages(); - size_t numFacets; //!< Total number of spacecraft facets - size_t numArticulatedFacets; //!< Number of articulated facets + size_t numFacets; //!< Total number of spacecraft facets + size_t numArticulatedFacets; //!< Number of articulated facets ReadFunctor sunInMsg; //!< Sun spice ephemeris input message private: diff --git a/src/simulation/environment/spiceInterface/spiceInterface.cpp b/src/simulation/environment/spiceInterface/spiceInterface.cpp old mode 100755 new mode 100644 index 2e6687a74..b720fcf53 --- a/src/simulation/environment/spiceInterface/spiceInterface.cpp +++ b/src/simulation/environment/spiceInterface/spiceInterface.cpp @@ -17,19 +17,18 @@ */ #include "simulation/environment/spiceInterface/spiceInterface.h" -#include #include "../libs/cspice/include/SpiceUsr.h" -#include -#include "architecture/utilities/simDefinitions.h" #include "architecture/utilities/macroDefinitions.h" #include "architecture/utilities/rigidBodyKinematics.h" +#include "architecture/utilities/simDefinitions.h" +#include +#include /*! This constructor initializes the variables that spice uses. Most of them are not intended to be changed, but a couple are user configurable. */ -SpiceInterface::SpiceInterface() -{ +SpiceInterface::SpiceInterface() { SPICEDataPath = ""; SPICELoaded = false; charBufferSize = 512; @@ -47,11 +46,19 @@ SpiceInterface::SpiceInterface() referenceBase = "j2000"; zeroBase = "SSB"; - timeOutPicture = "MON DD,YYYY HR:MN:SC.#### (UTC) ::UTC"; + timeOutPicture = "MON DD,YYYY HR:MN:SC.#### (UTC) ::UTC"; //! - set default epoch time information char string[255]; - snprintf(string, 255, "%4d/%02d/%02d, %02d:%02d:%04.1f (UTC)", EPOCH_YEAR, EPOCH_MONTH, EPOCH_DAY, EPOCH_HOUR, EPOCH_MIN, EPOCH_SEC); + snprintf(string, + 255, + "%4d/%02d/%02d, %02d:%02d:%04.1f (UTC)", + EPOCH_YEAR, + EPOCH_MONTH, + EPOCH_DAY, + EPOCH_HOUR, + EPOCH_MIN, + EPOCH_SEC); this->UTCCalInit = string; return; @@ -59,58 +66,50 @@ SpiceInterface::SpiceInterface() /*! The only needed activity in the destructor is to delete the spice I/O buffer that was allocated in the constructor*/ -SpiceInterface::~SpiceInterface() -{ - for (long unsigned int c=0; cplanetStateOutMsgs.size(); c++) { +SpiceInterface::~SpiceInterface() { + for (long unsigned int c = 0; c < this->planetStateOutMsgs.size(); c++) { delete this->planetStateOutMsgs.at(c); } - for (long unsigned int c=0; cscStateOutMsgs.size(); c++) { + for (long unsigned int c = 0; c < this->scStateOutMsgs.size(); c++) { delete this->scStateOutMsgs.at(c); } - for (long unsigned int c=0; cattRefStateOutMsgs.size(); c++) { + for (long unsigned int c = 0; c < this->attRefStateOutMsgs.size(); c++) { delete this->attRefStateOutMsgs.at(c); } - for (long unsigned int c=0; ctransRefStateOutMsgs.size(); c++) { + for (long unsigned int c = 0; c < this->transRefStateOutMsgs.size(); c++) { delete this->transRefStateOutMsgs.at(c); } - delete [] this->spiceBuffer; -// if(this->SPICELoaded) -// { -// this->clearKeeper(); -// } + delete[] this->spiceBuffer; + // if(this->SPICELoaded) + // { + // this->clearKeeper(); + // } return; } -void SpiceInterface::clearKeeper() -{ - kclear_c(); -} - +void SpiceInterface::clearKeeper() { kclear_c(); } /*! Reset the module to origina configuration values. @return void */ -void SpiceInterface::reset(uint64_t CurrenSimNanos) -{ +void SpiceInterface::reset(uint64_t CurrenSimNanos) { //! - Bail if the SPICEDataPath is not present - if(this->SPICEDataPath == "") - { + if (this->SPICEDataPath == "") { bskLogger.bskLog(BSK_ERROR, "SPICE data path was not set. No SPICE."); return; } //!- Load the SPICE kernels if they haven't already been loaded - if(!this->SPICELoaded) - { - if(loadSpiceKernel((char *)"naif0012.tls", this->SPICEDataPath.c_str())) { + if (!this->SPICELoaded) { + if (loadSpiceKernel((char *)"naif0012.tls", this->SPICEDataPath.c_str())) { bskLogger.bskLog(BSK_ERROR, "Unable to load %s", "naif0012.tls"); } - if(loadSpiceKernel((char *)"pck00010.tpc", this->SPICEDataPath.c_str())) { + if (loadSpiceKernel((char *)"pck00010.tpc", this->SPICEDataPath.c_str())) { bskLogger.bskLog(BSK_ERROR, "Unable to load %s", "pck00010.tpc"); } - if(loadSpiceKernel((char *)"de-403-masses.tpc", this->SPICEDataPath.c_str())) { + if (loadSpiceKernel((char *)"de-403-masses.tpc", this->SPICEDataPath.c_str())) { bskLogger.bskLog(BSK_ERROR, "Unable to load %s", "de-403-masses.tpc"); } - if(loadSpiceKernel((char *)"de430.bsp", this->SPICEDataPath.c_str())) { + if (loadSpiceKernel((char *)"de430.bsp", this->SPICEDataPath.c_str())) { bskLogger.bskLog(BSK_ERROR, "Unable to load %s", "de430.tpc"); } this->SPICELoaded = true; @@ -122,13 +121,12 @@ void SpiceInterface::reset(uint64_t CurrenSimNanos) this->timeDataInit = true; std::vector::iterator planit; - size_t c = 0; // celestial object counter + size_t c = 0; // celestial object counter int autoFrame; // flag to set the frame automatically SpiceChar *name = new SpiceChar[this->charBufferSize]; SpiceBoolean frmFound; SpiceInt frmCode; - for(planit = this->planetData.begin(); planit != planetData.end(); planit++) - { + for (planit = this->planetData.begin(); planit != planetData.end(); planit++) { autoFrame = 1; planit->computeOrient = 0; // turn off by default if (this->planetFrames.size() > 0) { @@ -146,20 +144,18 @@ void SpiceInterface::reset(uint64_t CurrenSimNanos) } c++; } - delete [] name; + delete[] name; // - Call Update state so that the spice bodies are inputted into the messaging system on reset this->updateState(CurrenSimNanos); } - /*! This method is used to initialize the zero-time that will be used to calculate all system time values in the Update method. It also creates the output message for time data @return void */ -void SpiceInterface::initTimeData() -{ +void SpiceInterface::initTimeData() { double EpochDelteET; /* set epoch information. If provided, then the epoch message information should be used. */ @@ -168,11 +164,21 @@ void SpiceInterface::initTimeData() EpochMsgPayload epochMsg{}; epochMsg = this->epochInMsg(); if (!this->epochInMsg.isWritten()) { - bskLogger.bskLog(BSK_ERROR, "The input epoch message name was set, but the message was never written. Not using the input message."); + bskLogger.bskLog(BSK_ERROR, + "The input epoch message name was set, but the message was never written. Not using the " + "input message."); } else { // Set the epoch information from the input message char string[255]; - snprintf(string, 255, "%4d/%02d/%02d, %02d:%02d:%04.1f (UTC)", epochMsg.year, epochMsg.month, epochMsg.day, epochMsg.hours, epochMsg.minutes, epochMsg.seconds); + snprintf(string, + 255, + "%4d/%02d/%02d, %02d:%02d:%04.1f (UTC)", + epochMsg.year, + epochMsg.month, + epochMsg.day, + epochMsg.hours, + epochMsg.minutes, + epochMsg.seconds); this->UTCCalInit = string; } } @@ -183,7 +189,6 @@ void SpiceInterface::initTimeData() str2et_c(this->UTCCalInit.c_str(), &this->J2000ETInit); //! - Take the JD epoch and get the elapsed time for it deltet_c(this->JDGPSEpoch, "ET", &EpochDelteET); - } /*! This method computes the GPS time data for the current elapsed time. It uses @@ -191,20 +196,19 @@ void SpiceInterface::initTimeData() compute the GPS time (week, seconds, rollovers) @return void */ -void SpiceInterface::computeGPSData() -{ +void SpiceInterface::computeGPSData() { double JDDifference; //! - The difference between the epochs in julian date terms is the total JDDifference = this->J2000Current - this->JDGPSEpoch; //! - Scale the elapsed by a week's worth of seconds to get week - this->GPSWeek = (uint16_t) (JDDifference/(7*86400)); + this->GPSWeek = (uint16_t)(JDDifference / (7 * 86400)); //! - Subtract out the GPS week scaled up to seconds to get time in week - this->GPSSeconds = JDDifference - this->GPSWeek*7*86400; + this->GPSSeconds = JDDifference - this->GPSWeek * 7 * 86400; //! - Maximum GPS week is 1024 so get rollovers and subtract out those weeks - this->GPSRollovers = this->GPSWeek/1024; - this->GPSWeek = (uint16_t)(this->GPSWeek-this->GPSRollovers*1024); + this->GPSRollovers = this->GPSWeek / 1024; + this->GPSWeek = (uint16_t)(this->GPSWeek - this->GPSRollovers * 1024); } /*! This method takes the values computed in the model and outputs them. @@ -213,8 +217,7 @@ void SpiceInterface::computeGPSData() @return void @param CurrentClock The current simulation time (used for time stamping) */ -void SpiceInterface::writeOutputMessages(uint64_t CurrentClock) -{ +void SpiceInterface::writeOutputMessages(uint64_t CurrentClock) { SpiceTimeMsgPayload OutputData{}; //! - Set the members of the time output message structure and write @@ -226,14 +229,12 @@ void SpiceInterface::writeOutputMessages(uint64_t CurrentClock) this->spiceTimeOutMsg.write(&OutputData, this->moduleID, CurrentClock); //! - Iterate through all of the planets that are on and write their outputs - for (long unsigned int c=0; cplanetStateOutMsgs.size(); c++) - { + for (long unsigned int c = 0; c < this->planetStateOutMsgs.size(); c++) { this->planetStateOutMsgs[c]->write(&this->planetData[c], this->moduleID, CurrentClock); } //! - Iterate through all of the spacecraft that are on and write their outputs - for (long unsigned int c=0; cscStateOutMsgs.size(); c++) - { + for (long unsigned int c = 0; c < this->scStateOutMsgs.size(); c++) { SCStatesMsgPayload scStateMsgData = {}; v3Copy(this->scData[c].PositionVector, scStateMsgData.r_BN_N); v3Copy(this->scData[c].PositionVector, scStateMsgData.r_CN_N); @@ -250,7 +251,6 @@ void SpiceInterface::writeOutputMessages(uint64_t CurrentClock) v3Copy(this->scData[c].PositionVector, transRefMsgData.r_RN_N); v3Copy(this->scData[c].VelocityVector, transRefMsgData.v_RN_N); this->transRefStateOutMsgs[c]->write(&transRefMsgData, this->moduleID, CurrentClock); - } } @@ -260,15 +260,13 @@ void SpiceInterface::writeOutputMessages(uint64_t CurrentClock) @return void @param currentSimNanos The current clock time for the simulation */ -void SpiceInterface::updateState(uint64_t currentSimNanos) -{ +void SpiceInterface::updateState(uint64_t currentSimNanos) { //! - Increment the J2000 elapsed time based on init value and Current sim - this->J2000Current = this->J2000ETInit + currentSimNanos*NANO2SEC; + this->J2000Current = this->J2000ETInit + currentSimNanos * NANO2SEC; //! - Compute the current Julian Date string and cast it over to the double - et2utc_c(this->J2000Current, "J", 14, this->charBufferSize - 1, reinterpret_cast - (this->spiceBuffer)); - std::string localString = reinterpret_cast (&this->spiceBuffer[3]); + et2utc_c(this->J2000Current, "J", 14, this->charBufferSize - 1, reinterpret_cast(this->spiceBuffer)); + std::string localString = reinterpret_cast(&this->spiceBuffer[3]); this->julianDateCurrent = std::stod(localString); //! Get GPS and Planet data and then write the message outputs this->computeGPSData(); @@ -283,7 +281,7 @@ void SpiceInterface::addPlanetNames(std::vector planetNames) { std::vector::iterator it; /* clear the planet state message and payload vectors */ - for (long unsigned int c=0; cplanetStateOutMsgs.size(); c++) { + for (long unsigned int c = 0; c < this->planetStateOutMsgs.size(); c++) { delete this->planetStateOutMsgs.at(c); } this->planetStateOutMsgs.clear(); @@ -296,9 +294,10 @@ void SpiceInterface::addPlanetNames(std::vector planetNames) { SpicePlanetStateMsgPayload newPlanet = {}; m33SetIdentity(newPlanet.J20002Pfix); - if(it->size() >= MAX_BODY_NAME_LENGTH) - { - bskLogger.bskLog(BSK_WARNING, "spiceInterface: Warning, your planet name is too long for me. Ignoring: %s", (*it).c_str()); + if (it->size() >= MAX_BODY_NAME_LENGTH) { + bskLogger.bskLog(BSK_WARNING, + "spiceInterface: Warning, your planet name is too long for me. Ignoring: %s", + (*it).c_str()); continue; } strcpy(newPlanet.PlanetName, it->c_str()); @@ -318,13 +317,13 @@ void SpiceInterface::addSpacecraftNames(std::vector spacecraftNames SpiceInt frmCode; /* clear the spacecraft state message and payload vectors */ - for (long unsigned int c=0; cscStateOutMsgs.size(); c++) { + for (long unsigned int c = 0; c < this->scStateOutMsgs.size(); c++) { delete this->scStateOutMsgs.at(c); } - for (long unsigned int c=0; cattRefStateOutMsgs.size(); c++) { + for (long unsigned int c = 0; c < this->attRefStateOutMsgs.size(); c++) { delete this->attRefStateOutMsgs.at(c); } - for (long unsigned int c=0; ctransRefStateOutMsgs.size(); c++) { + for (long unsigned int c = 0; c < this->transRefStateOutMsgs.size(); c++) { delete this->transRefStateOutMsgs.at(c); } this->scStateOutMsgs.clear(); @@ -348,9 +347,10 @@ void SpiceInterface::addSpacecraftNames(std::vector spacecraftNames SpicePlanetStateMsgPayload newSpacecraft = {}; m33SetIdentity(newSpacecraft.J20002Pfix); - if(it->size() >= MAX_BODY_NAME_LENGTH) - { - bskLogger.bskLog(BSK_WARNING, "spiceInterface: Warning, your spacecraft name is too long for me. Ignoring: %s", (*it).c_str()); + if (it->size() >= MAX_BODY_NAME_LENGTH) { + bskLogger.bskLog(BSK_WARNING, + "spiceInterface: Warning, your spacecraft name is too long for me. Ignoring: %s", + (*it).c_str()); continue; } strcpy(newSpacecraft.PlanetName, it->c_str()); @@ -360,18 +360,16 @@ void SpiceInterface::addSpacecraftNames(std::vector spacecraftNames newSpacecraft.computeOrient = frmFound; this->scData.push_back(newSpacecraft); } - delete [] name; + delete[] name; return; } - /*! This method gets the state of each spice item that has been added to the module and saves the information off into the array. @return void */ -void SpiceInterface::pullSpiceData(std::vector *spiceData) -{ +void SpiceInterface::pullSpiceData(std::vector *spiceData) { std::vector::iterator planit; /*! - Loop over the vector of Spice objects and compute values. @@ -381,15 +379,19 @@ void SpiceInterface::pullSpiceData(std::vector *spic -# Convert the pos/vel over to meters. -# Time stamp the message appropriately */ - size_t c = 0; // celestial body counter - for(planit = spiceData->begin(); planit != spiceData->end(); planit++) - { + size_t c = 0; // celestial body counter + for (planit = spiceData->begin(); planit != spiceData->end(); planit++) { double lighttime; double localState[6]; std::string planetFrame = ""; - spkezr_c(planit->PlanetName, this->J2000Current, this->referenceBase.c_str(), - "NONE", this->zeroBase.c_str(), localState, &lighttime); + spkezr_c(planit->PlanetName, + this->J2000Current, + this->referenceBase.c_str(), + "NONE", + this->zeroBase.c_str(), + localState, + &lighttime); v3Copy(&localState[0], planit->PositionVector); v3Copy(&localState[3], planit->VelocityVector); v3Scale(1000., planit->PositionVector, planit->PositionVector); @@ -402,21 +404,24 @@ void SpiceInterface::pullSpiceData(std::vector *spic /* use specific planet frame if specified */ if (this->planetFrames.size() > 0) { if (c < this->planetFrames.size()) { - if (this->planetFrames[c].length() > 0){ + if (this->planetFrames[c].length() > 0) { /* use custom planet frame name */ planetFrame = this->planetFrames[c]; } } } - if(planit->computeOrient) - { - //pxform_c ( referenceBase.c_str(), planetFrame.c_str(), J2000Current, - // planit->second.J20002Pfix); + if (planit->computeOrient) { + // pxform_c ( referenceBase.c_str(), planetFrame.c_str(), J2000Current, + // planit->second.J20002Pfix); double aux[6][6]; - sxform_c(this->referenceBase.c_str(), planetFrame.c_str(), this->J2000Current, aux); //returns attitude of planet (i.e. IAU_EARTH) wrt "j2000". note j2000 is actually ICRF in Spice. + sxform_c( + this->referenceBase.c_str(), + planetFrame.c_str(), + this->J2000Current, + aux); // returns attitude of planet (i.e. IAU_EARTH) wrt "j2000". note j2000 is actually ICRF in Spice. m66Get33Matrix(0, 0, aux, planit->J20002Pfix); @@ -434,8 +439,7 @@ void SpiceInterface::pullSpiceData(std::vector *spic @param kernelName The name of the kernel we are loading @param dataPath The path to the data area on the filesystem */ -int SpiceInterface::loadSpiceKernel(char *kernelName, const char *dataPath) -{ +int SpiceInterface::loadSpiceKernel(char *kernelName, const char *dataPath) { char *fileName = new char[this->charBufferSize]; SpiceChar *name = new SpiceChar[this->charBufferSize]; @@ -452,7 +456,7 @@ int SpiceInterface::loadSpiceKernel(char *kernelName, const char *dataPath) erract_c("SET", this->charBufferSize, name); delete[] fileName; delete[] name; - if(failed_c()) { + if (failed_c()) { return 1; } return 0; @@ -466,8 +470,7 @@ int SpiceInterface::loadSpiceKernel(char *kernelName, const char *dataPath) @param kernelName The name of the kernel we are unloading @param dataPath The path to the data area on the filesystem */ -int SpiceInterface::unloadSpiceKernel(char *kernelName, const char *dataPath) -{ +int SpiceInterface::unloadSpiceKernel(char *kernelName, const char *dataPath) { char *fileName = new char[this->charBufferSize]; SpiceChar *name = new SpiceChar[this->charBufferSize]; @@ -480,29 +483,29 @@ int SpiceInterface::unloadSpiceKernel(char *kernelName, const char *dataPath) unload_c(fileName); delete[] fileName; delete[] name; - if(failed_c()) { + if (failed_c()) { return 1; } return 0; } -std::string SpiceInterface::getCurrentTimeString() -{ - char *spiceOutputBuffer; - int64_t allowedOutputLength; - - allowedOutputLength = (int64_t)this->timeOutPicture.size() - 5; - - if (allowedOutputLength < 0) - { - bskLogger.bskLog(BSK_ERROR, "The output format string is not long enough. It should be much larger than 5 characters. It is currently: %s", this->timeOutPicture.c_str()); - return(""); - } - - spiceOutputBuffer = new char[allowedOutputLength]; - timout_c(this->J2000Current, this->timeOutPicture.c_str(), (SpiceInt) allowedOutputLength, - spiceOutputBuffer); - std::string returnTimeString = spiceOutputBuffer; - delete[] spiceOutputBuffer; - return(returnTimeString); +std::string SpiceInterface::getCurrentTimeString() { + char *spiceOutputBuffer; + int64_t allowedOutputLength; + + allowedOutputLength = (int64_t)this->timeOutPicture.size() - 5; + + if (allowedOutputLength < 0) { + bskLogger.bskLog(BSK_ERROR, + "The output format string is not long enough. It should be much larger than 5 characters. It " + "is currently: %s", + this->timeOutPicture.c_str()); + return (""); + } + + spiceOutputBuffer = new char[allowedOutputLength]; + timout_c(this->J2000Current, this->timeOutPicture.c_str(), (SpiceInt)allowedOutputLength, spiceOutputBuffer); + std::string returnTimeString = spiceOutputBuffer; + delete[] spiceOutputBuffer; + return (returnTimeString); } diff --git a/src/simulation/onboardDataHandling/storageUnit/partitionedStorageUnit.cpp b/src/simulation/onboardDataHandling/storageUnit/partitionedStorageUnit.cpp index a876f22f1..d2ed813e5 100644 --- a/src/simulation/onboardDataHandling/storageUnit/partitionedStorageUnit.cpp +++ b/src/simulation/onboardDataHandling/storageUnit/partitionedStorageUnit.cpp @@ -19,15 +19,13 @@ #include - -#include "partitionedStorageUnit.h" #include "architecture/utilities/bskLogging.h" - +#include "partitionedStorageUnit.h" /*! The constructor creates a partitionedStorageUnit instance with zero stored data @return void; */ -PartitionedStorageUnit::PartitionedStorageUnit(){ +PartitionedStorageUnit::PartitionedStorageUnit() { this->storageCapacity = 0; this->storedDataSum = 0; return; @@ -36,15 +34,13 @@ PartitionedStorageUnit::PartitionedStorageUnit(){ /*! Destructor. @return void */ -PartitionedStorageUnit::~PartitionedStorageUnit(){ - return; -} +PartitionedStorageUnit::~PartitionedStorageUnit() { return; } /*! Custom reset function. @param currentClock @return void */ -void PartitionedStorageUnit::customreset(uint64_t currentClock){ +void PartitionedStorageUnit::customreset(uint64_t currentClock) { if (this->storageCapacity <= 0) { bskLogger.bskLog(BSK_INFORMATION, "The storageCapacity variable must be set to a positive value."); } @@ -55,7 +51,7 @@ void PartitionedStorageUnit::customreset(uint64_t currentClock){ @param dataName @return void */ -void PartitionedStorageUnit::addPartition(std::string dataName){ +void PartitionedStorageUnit::addPartition(std::string dataName) { dataInstance tmpDataInstance; strncpy(tmpDataInstance.dataInstanceName, dataName.c_str(), sizeof(tmpDataInstance.dataInstanceName)); tmpDataInstance.dataInstanceSum = 0; @@ -68,10 +64,8 @@ void PartitionedStorageUnit::addPartition(std::string dataName){ @param data //Vector of data to be added to each partition in partitionNames @return void */ -void PartitionedStorageUnit::setDataBuffer(std::vector partitionNames, std::vector data){ - - for (size_t i = 0; i < partitionNames.size(); i++) - { +void PartitionedStorageUnit::setDataBuffer(std::vector partitionNames, std::vector data) { + for (size_t i = 0; i < partitionNames.size(); i++) { PartitionedStorageUnit::DataStorageUnitBase::setDataBuffer(partitionNames[i], data[i]); } } diff --git a/src/simulation/vizard/vizInterface/vizInterface.cpp b/src/simulation/vizard/vizInterface/vizInterface.cpp old mode 100755 new mode 100644 index 120180473..d9e661c8a --- a/src/simulation/vizard/vizInterface/vizInterface.cpp +++ b/src/simulation/vizard/vizInterface/vizInterface.cpp @@ -15,27 +15,26 @@ */ +#include #include #include -#include -#include "vizInterface.h" +#include "architecture/utilities/astroConstants.h" #include "architecture/utilities/linearAlgebra.h" #include "architecture/utilities/rigidBodyKinematics.h" +#include "vizInterface.h" #include #include -#include "architecture/utilities/astroConstants.h" -void message_buffer_deallocate(void *data, void *hint); +void message_buffer_deallocate(void* data, void* hint); /*! VizInterface Constructor */ -VizInterface::VizInterface() -{ +VizInterface::VizInterface() { this->opNavMode = 0; this->saveFile = false; this->liveStream = false; - this->FrameNumber= -1; + this->FrameNumber = -1; this->firstPass = 0; @@ -48,29 +47,27 @@ VizInterface::VizInterface() /*! VizInterface Destructor */ -VizInterface::~VizInterface() -{ - for (size_t c=0; copnavImageOutMsgs.size(); c++) { +VizInterface::~VizInterface() { + for (size_t c = 0; c < this->opnavImageOutMsgs.size(); c++) { delete this->opnavImageOutMsgs.at(c); } return; } - /*! A Reset method to put the module back into a clean state @param currentSimNanos The current sim time in nanoseconds */ -void VizInterface::reset(uint64_t currentSimNanos) -{ - if (this->opNavMode > 0 || this->liveStream){ +void VizInterface::reset(uint64_t currentSimNanos) { + if (this->opNavMode > 0 || this->liveStream) { /* setup zeroMQ */ - for (size_t camCounter =0; camCountercameraConfInMsgs.size(); camCounter++) { + for (size_t camCounter = 0; camCounter < this->cameraConfInMsgs.size(); camCounter++) { this->bskImagePtrs[camCounter] = NULL; } this->context = zmq_ctx_new(); this->requester_socket = zmq_socket(this->context, ZMQ_REQ); - zmq_connect(this->requester_socket, (this->comProtocol + "://" + this->comAddress + ":" + this->comPortNumber).c_str()); + zmq_connect(this->requester_socket, + (this->comProtocol + "://" + this->comAddress + ":" + this->comPortNumber).c_str()); void* message = malloc(4 * sizeof(char)); memcpy(message, "PING", 4); @@ -83,14 +80,13 @@ void VizInterface::reset(uint64_t currentSimNanos) zmq_msg_init_data(&request, message, 4, message_buffer_deallocate, NULL); zmq_msg_send(&request, this->requester_socket, 0); char buffer[4]; - zmq_recv (this->requester_socket, buffer, 4, 0); - zmq_send (this->requester_socket, "PING", 4, 0); + zmq_recv(this->requester_socket, buffer, 4, 0); + zmq_send(this->requester_socket, "PING", 4, 0); bskLogger.bskLog(BSK_INFORMATION, "Basilisk-Vizard connection made"); } std::vector::iterator scIt; - for (scIt = this->scData.begin(); scIt != this->scData.end(); scIt++) - { + for (scIt = this->scData.begin(); scIt != this->scData.end(); scIt++) { /* Check spacecraft input message */ if (scIt->scStateInMsg.isLinked()) { scIt->scStateInMsgStatus.dataFresh = false; @@ -106,8 +102,7 @@ void VizInterface::reset(uint64_t currentSimNanos) cssStatus.lastTimeTag = 0xFFFFFFFFFFFFFFFF; scIt->cssConfLogInMsgStatus.clear(); scIt->cssInMessage.clear(); - for (size_t idx = 0; idx < (size_t) scIt->cssInMsgs.size(); idx++) - { + for (size_t idx = 0; idx < (size_t)scIt->cssInMsgs.size(); idx++) { if (!scIt->cssInMsgs.at(idx).isLinked()) { bskLogger.bskLog(BSK_ERROR, "vizInterface: CSS(%zu) msg not linked.", idx); } @@ -124,8 +119,7 @@ void VizInterface::reset(uint64_t currentSimNanos) rwStatus.lastTimeTag = 0xFFFFFFFFFFFFFFFF; scIt->rwInMsgStatus.clear(); scIt->rwInMessage.clear(); - for (size_t idx = 0; idx < (size_t) scIt->rwInMsgs.size(); idx++) - { + for (size_t idx = 0; idx < (size_t)scIt->rwInMsgs.size(); idx++) { if (!scIt->rwInMsgs.at(idx).isLinked()) { bskLogger.bskLog(BSK_ERROR, "vizInterface: RW(%zu) msg not linked.", idx); } @@ -142,7 +136,7 @@ void VizInterface::reset(uint64_t currentSimNanos) thrStatus.lastTimeTag = 0xFFFFFFFFFFFFFFFF; int thrCounter = 0; - for (thrCounter = 0; thrCounter < (int) scIt->thrInMsgs.size(); thrCounter++) { + for (thrCounter = 0; thrCounter < (int)scIt->thrInMsgs.size(); thrCounter++) { if (scIt->thrInMsgs.at(thrCounter).isLinked()) { scIt->thrMsgStatus.push_back(thrStatus); THROutputMsgPayload logMsg; @@ -152,8 +146,10 @@ void VizInterface::reset(uint64_t currentSimNanos) } } if (scIt->thrInfo.size() != scIt->thrInMsgs.size()) { - bskLogger.bskLog(BSK_ERROR, "vizInterface: thrInfo vector (%d) must be the same size as thrInMsgs (%d)" - , (int) scIt->thrInfo.size(), (int) scIt->thrInMsgs.size()); + bskLogger.bskLog(BSK_ERROR, + "vizInterface: thrInfo vector (%d) must be the same size as thrInMsgs (%d)", + (int)scIt->thrInfo.size(), + (int)scIt->thrInMsgs.size()); } } } @@ -173,7 +169,7 @@ void VizInterface::reset(uint64_t currentSimNanos) spiceStatus.lastTimeTag = 0xFFFFFFFFFFFFFFFF; this->spiceInMsgStatus.clear(); this->spiceMessage.clear(); - for (long unsigned int c = 0; cgravBodyInformation.size(); c++) { + for (long unsigned int c = 0; c < this->gravBodyInformation.size(); c++) { /* set default zero translation and rotation states */ SpicePlanetStateMsgPayload logMsg = {}; m33SetIdentity(logMsg.J20002Pfix); @@ -184,12 +180,12 @@ void VizInterface::reset(uint64_t currentSimNanos) } } - this->FrameNumber=-1; + this->FrameNumber = -1; if (this->saveFile) { - this->outputStream = new std::ofstream(this->protoFilename, std::ios::out |std::ios::binary); + this->outputStream = new std::ofstream(this->protoFilename, std::ios::out | std::ios::binary); } - this->settings.dataFresh = true; // reset flag to transmit Vizard settings + this->settings.dataFresh = true; // reset flag to transmit Vizard settings this->epochMsgBuffer.year = EPOCH_YEAR; this->epochMsgBuffer.month = EPOCH_MONTH; @@ -204,17 +200,16 @@ void VizInterface::reset(uint64_t currentSimNanos) /*! A method in which the module reads the content of all available bsk messages */ -void VizInterface::ReadBSKMessages() -{ +void VizInterface::ReadBSKMessages() { std::vector::iterator scIt; - for (scIt = this->scData.begin(); scIt != this->scData.end(); scIt++) - { + for (scIt = this->scData.begin(); scIt != this->scData.end(); scIt++) { /* Read BSK spacecraft state msg */ - if (scIt->scStateInMsg.isLinked()){ + if (scIt->scStateInMsg.isLinked()) { SCStatesMsgPayload localSCStateArray; localSCStateArray = scIt->scStateInMsg(); - if(scIt->scStateInMsg.isWritten() && scIt->scStateInMsg.timeWritten() != scIt->scStateInMsgStatus.lastTimeTag){ + if (scIt->scStateInMsg.isWritten() && + scIt->scStateInMsg.timeWritten() != scIt->scStateInMsgStatus.lastTimeTag) { scIt->scStateInMsgStatus.lastTimeTag = scIt->scStateInMsg.timeWritten(); scIt->scStateInMsgStatus.dataFresh = true; } @@ -223,52 +218,52 @@ void VizInterface::ReadBSKMessages() /* Read BSK RW constellation msg */ { - for (size_t idx=0;idx< (size_t) scIt->rwInMsgs.size(); idx++) { - if (scIt->rwInMsgs[idx].isLinked()){ - RWConfigLogMsgPayload localRWArray; - localRWArray = scIt->rwInMsgs.at(idx)(); - - if(scIt->rwInMsgs.at(idx).isWritten() && - scIt->rwInMsgs.at(idx).timeWritten() != scIt->rwInMsgStatus[idx].lastTimeTag) { - scIt->rwInMsgStatus[idx].lastTimeTag = scIt->rwInMsgs.at(idx).timeWritten(); - scIt->rwInMsgStatus[idx].dataFresh = true; - scIt->rwInMessage[idx] = localRWArray; + for (size_t idx = 0; idx < (size_t)scIt->rwInMsgs.size(); idx++) { + if (scIt->rwInMsgs[idx].isLinked()) { + RWConfigLogMsgPayload localRWArray; + localRWArray = scIt->rwInMsgs.at(idx)(); + + if (scIt->rwInMsgs.at(idx).isWritten() && + scIt->rwInMsgs.at(idx).timeWritten() != scIt->rwInMsgStatus[idx].lastTimeTag) { + scIt->rwInMsgStatus[idx].lastTimeTag = scIt->rwInMsgs.at(idx).timeWritten(); + scIt->rwInMsgStatus[idx].dataFresh = true; + scIt->rwInMessage[idx] = localRWArray; + } } } } - } /* Read incoming Thruster constellation msg */ { - for (size_t idx=0;idx< (size_t) scIt->thrInMsgs.size(); idx++){ - if (scIt->thrInMsgs[idx].isLinked()){ - THROutputMsgPayload localThrusterArray; - localThrusterArray = scIt->thrInMsgs.at(idx)(); - if(scIt->thrInMsgs.at(idx).isWritten() && - scIt->thrInMsgs.at(idx).timeWritten() != scIt->thrMsgStatus[idx].lastTimeTag){ - scIt->thrMsgStatus[idx].lastTimeTag = scIt->thrInMsgs.at(idx).timeWritten(); - scIt->thrMsgStatus[idx].dataFresh = true; - scIt->thrOutputMessage[idx] = localThrusterArray; + for (size_t idx = 0; idx < (size_t)scIt->thrInMsgs.size(); idx++) { + if (scIt->thrInMsgs[idx].isLinked()) { + THROutputMsgPayload localThrusterArray; + localThrusterArray = scIt->thrInMsgs.at(idx)(); + if (scIt->thrInMsgs.at(idx).isWritten() && + scIt->thrInMsgs.at(idx).timeWritten() != scIt->thrMsgStatus[idx].lastTimeTag) { + scIt->thrMsgStatus[idx].lastTimeTag = scIt->thrInMsgs.at(idx).timeWritten(); + scIt->thrMsgStatus[idx].dataFresh = true; + scIt->thrOutputMessage[idx] = localThrusterArray; + } } } } - } /* Read CSS constellation log msg */ { - for (size_t idx=0;idx< (size_t) scIt->cssInMsgs.size(); idx++) { - if (scIt->cssInMsgs[idx].isLinked()){ - CSSConfigLogMsgPayload localCSSMsg; - localCSSMsg = scIt->cssInMsgs.at(idx)(); - if(scIt->cssInMsgs.at(idx).isWritten() && - scIt->cssInMsgs.at(idx).timeWritten() != scIt->cssConfLogInMsgStatus[idx].lastTimeTag){ - scIt->cssConfLogInMsgStatus[idx].lastTimeTag = scIt->cssInMsgs.at(idx).timeWritten(); - scIt->cssConfLogInMsgStatus[idx].dataFresh = true; - scIt->cssInMessage[idx] = localCSSMsg; + for (size_t idx = 0; idx < (size_t)scIt->cssInMsgs.size(); idx++) { + if (scIt->cssInMsgs[idx].isLinked()) { + CSSConfigLogMsgPayload localCSSMsg; + localCSSMsg = scIt->cssInMsgs.at(idx)(); + if (scIt->cssInMsgs.at(idx).isWritten() && + scIt->cssInMsgs.at(idx).timeWritten() != scIt->cssConfLogInMsgStatus[idx].lastTimeTag) { + scIt->cssConfLogInMsgStatus[idx].lastTimeTag = scIt->cssInMsgs.at(idx).timeWritten(); + scIt->cssConfLogInMsgStatus[idx].dataFresh = true; + scIt->cssInMessage[idx] = localCSSMsg; + } } } } - } /* read in true trajectory line color if connected */ if (scIt->trueTrajectoryLineColorInMsg.isLinked()) { @@ -276,7 +271,7 @@ void VizInterface::ReadBSKMessages() ColorMsgPayload colorMsg; colorMsg = scIt->trueTrajectoryLineColorInMsg(); scIt->trueTrajectoryLineColor.clear(); - for (int i=0; i<4; i++) { + for (int i = 0; i < 4; i++) { scIt->trueTrajectoryLineColor.push_back(colorMsg.colorRGBA[i]); } } @@ -284,11 +279,11 @@ void VizInterface::ReadBSKMessages() /* read in generic sensor cmd value */ { - for (size_t idx=0;idx< (size_t) scIt->genericSensorList.size(); idx++) { - if (scIt->genericSensorList[idx]->genericSensorCmdInMsg.isLinked()){ + for (size_t idx = 0; idx < (size_t)scIt->genericSensorList.size(); idx++) { + if (scIt->genericSensorList[idx]->genericSensorCmdInMsg.isLinked()) { DeviceCmdMsgPayload deviceCmdMsgBuffer; deviceCmdMsgBuffer = scIt->genericSensorList[idx]->genericSensorCmdInMsg(); - if(scIt->genericSensorList[idx]->genericSensorCmdInMsg.isWritten()){ + if (scIt->genericSensorList[idx]->genericSensorCmdInMsg.isWritten()) { scIt->genericSensorList[idx]->genericSensorCmd = deviceCmdMsgBuffer.deviceCmd; } } @@ -297,12 +292,12 @@ void VizInterface::ReadBSKMessages() /* read in light on/off cmd value */ { - for (size_t idx=0;idx< (size_t) scIt->lightList.size(); idx++) { - if (scIt->lightList[idx]->onOffCmdInMsg.isLinked()){ + for (size_t idx = 0; idx < (size_t)scIt->lightList.size(); idx++) { + if (scIt->lightList[idx]->onOffCmdInMsg.isLinked()) { DeviceCmdMsgPayload onOffCmdMsgBuffer; onOffCmdMsgBuffer = scIt->lightList[idx]->onOffCmdInMsg(); - if(scIt->lightList[idx]->onOffCmdInMsg.isWritten()){ - scIt->lightList[idx]->lightOn = (int) onOffCmdMsgBuffer.deviceCmd; + if (scIt->lightList[idx]->onOffCmdInMsg.isWritten()) { + scIt->lightList[idx]->lightOn = (int)onOffCmdMsgBuffer.deviceCmd; } } } @@ -310,21 +305,24 @@ void VizInterface::ReadBSKMessages() /* read in transceiver state values */ { - for (size_t idx=0;idx< (size_t) scIt->transceiverList.size(); idx++) { + for (size_t idx = 0; idx < (size_t)scIt->transceiverList.size(); idx++) { if (scIt->transceiverList[idx]->transceiverStateInMsgs.size() > 0) { scIt->transceiverList[idx]->transceiverState = 0; - for (size_t idxTr=0; idxTr < (size_t) scIt->transceiverList[idx]->transceiverStateInMsgs.size(); idxTr++) { - if (scIt->transceiverList[idx]->transceiverStateInMsgs[idxTr].isLinked()){ + for (size_t idxTr = 0; idxTr < (size_t)scIt->transceiverList[idx]->transceiverStateInMsgs.size(); + idxTr++) { + if (scIt->transceiverList[idx]->transceiverStateInMsgs[idxTr].isLinked()) { DataNodeUsageMsgPayload stateMsgBuffer; stateMsgBuffer = scIt->transceiverList[idx]->transceiverStateInMsgs[idxTr](); - if(scIt->transceiverList[idx]->transceiverStateInMsgs[idxTr].isWritten()){ + if (scIt->transceiverList[idx]->transceiverStateInMsgs[idxTr].isWritten()) { /* state 0->off, 1->sending, 2->receiving, 3->sending and receiving */ if (stateMsgBuffer.baudRate < 0.0) { /* sending data */ - scIt->transceiverList[idx]->transceiverState = scIt->transceiverList[idx]->transceiverState | 1; + scIt->transceiverList[idx]->transceiverState = + scIt->transceiverList[idx]->transceiverState | 1; } else if (stateMsgBuffer.baudRate > 0.0) { /* receiving data */ - scIt->transceiverList[idx]->transceiverState = scIt->transceiverList[idx]->transceiverState | 2; + scIt->transceiverList[idx]->transceiverState = + scIt->transceiverList[idx]->transceiverState | 2; } } } @@ -335,30 +333,30 @@ void VizInterface::ReadBSKMessages() /* read in generic storage state values */ { - for (size_t idx=0;idx< (size_t) scIt->genericStorageList.size(); idx++) { + for (size_t idx = 0; idx < (size_t)scIt->genericStorageList.size(); idx++) { /* read in battery device state */ - if (scIt->genericStorageList[idx]->batteryStateInMsg.isLinked()){ + if (scIt->genericStorageList[idx]->batteryStateInMsg.isLinked()) { PowerStorageStatusMsgPayload deviceStateMsgBuffer; deviceStateMsgBuffer = scIt->genericStorageList[idx]->batteryStateInMsg(); - if(scIt->genericStorageList[idx]->batteryStateInMsg.isWritten()){ + if (scIt->genericStorageList[idx]->batteryStateInMsg.isWritten()) { scIt->genericStorageList[idx]->currentValue = deviceStateMsgBuffer.storageLevel; scIt->genericStorageList[idx]->maxValue = deviceStateMsgBuffer.storageCapacity; } } /* read in data storage device state */ - if (scIt->genericStorageList[idx]->dataStorageStateInMsg.isLinked()){ + if (scIt->genericStorageList[idx]->dataStorageStateInMsg.isLinked()) { DataStorageStatusMsgPayload deviceStateMsgBuffer; deviceStateMsgBuffer = scIt->genericStorageList[idx]->dataStorageStateInMsg(); - if(scIt->genericStorageList[idx]->dataStorageStateInMsg.isWritten()){ + if (scIt->genericStorageList[idx]->dataStorageStateInMsg.isWritten()) { scIt->genericStorageList[idx]->currentValue = deviceStateMsgBuffer.storageLevel; scIt->genericStorageList[idx]->maxValue = deviceStateMsgBuffer.storageCapacity; } } /* read in fuel tank device state */ - if (scIt->genericStorageList[idx]->fuelTankStateInMsg.isLinked()){ + if (scIt->genericStorageList[idx]->fuelTankStateInMsg.isLinked()) { FuelTankMsgPayload deviceStateMsgBuffer; deviceStateMsgBuffer = scIt->genericStorageList[idx]->fuelTankStateInMsg(); - if(scIt->genericStorageList[idx]->fuelTankStateInMsg.isWritten()){ + if (scIt->genericStorageList[idx]->fuelTankStateInMsg.isWritten()) { scIt->genericStorageList[idx]->currentValue = deviceStateMsgBuffer.fuelMass; scIt->genericStorageList[idx]->maxValue = deviceStateMsgBuffer.maxFuelMass; } @@ -369,17 +367,18 @@ void VizInterface::ReadBSKMessages() /* read in MSM charge values */ { /* read in MSM charge states */ - if (scIt->msmInfo.msmChargeInMsg.isLinked()){ - if(scIt->msmInfo.msmChargeInMsg.isWritten()){ + if (scIt->msmInfo.msmChargeInMsg.isLinked()) { + if (scIt->msmInfo.msmChargeInMsg.isWritten()) { ChargeMsmMsgPayload msmChargeMsgBuffer; msmChargeMsgBuffer = scIt->msmInfo.msmChargeInMsg(); if (msmChargeMsgBuffer.q.size() == scIt->msmInfo.msmList.size()) { - for (size_t idx=0;idx< (size_t) scIt->msmInfo.msmList.size(); idx++) { + for (size_t idx = 0; idx < (size_t)scIt->msmInfo.msmList.size(); idx++) { scIt->msmInfo.msmList[idx]->currentValue = msmChargeMsgBuffer.q[idx]; } } else { - bskLogger.bskLog(BSK_ERROR, "vizInterface: the number of charged in MSM message and the number of msm vizInterface spheres must be the same."); - + bskLogger.bskLog(BSK_ERROR, + "vizInterface: the number of charged in MSM message and the number of msm " + "vizInterface spheres must be the same."); } } } @@ -389,11 +388,11 @@ void VizInterface::ReadBSKMessages() /*! Read incoming camera config msg */ for (size_t camCounter = 0; camCounter < this->cameraConfInMsgs.size(); camCounter++) { - if (this->cameraConfInMsgs[camCounter].isLinked()){ + if (this->cameraConfInMsgs[camCounter].isLinked()) { CameraConfigMsgPayload localCameraConfigArray; localCameraConfigArray = this->cameraConfInMsgs[camCounter](); - if(this->cameraConfInMsgs[camCounter].isWritten() && - this->cameraConfInMsgs[camCounter].timeWritten() != this->cameraConfMsgStatus[camCounter].lastTimeTag){ + if (this->cameraConfInMsgs[camCounter].isWritten() && + this->cameraConfInMsgs[camCounter].timeWritten() != this->cameraConfMsgStatus[camCounter].lastTimeTag) { this->cameraConfMsgStatus[camCounter].lastTimeTag = this->cameraConfInMsgs[camCounter].timeWritten(); this->cameraConfMsgStatus[camCounter].dataFresh = true; } @@ -405,8 +404,7 @@ void VizInterface::ReadBSKMessages() if (this->epochInMsg.isLinked()) { EpochMsgPayload epochMsg_Buffer; epochMsg_Buffer = this->epochInMsg(); - if(this->epochInMsg.isWritten() && - this->epochInMsg.timeWritten() != this->epochMsgStatus.lastTimeTag){ + if (this->epochInMsg.isWritten() && this->epochInMsg.timeWritten() != this->epochMsgStatus.lastTimeTag) { this->epochMsgStatus.lastTimeTag = this->epochInMsg.timeWritten(); this->epochMsgStatus.dataFresh = true; this->epochMsgBuffer = epochMsg_Buffer; @@ -415,21 +413,20 @@ void VizInterface::ReadBSKMessages() /*! Read BSK Spice constellation msg */ { - for(size_t i=0; i < this->spiceInMsgs.size(); i++) - { - if (this->spiceInMsgs.at(i).isLinked()){ - // If the spice msg is not linked then the default zero planet emphemeris is used - SpicePlanetStateMsgPayload localSpiceArray; - localSpiceArray = this->spiceInMsgs.at(i)(); - if(this->spiceInMsgs.at(i).isWritten() && - this->spiceInMsgs.at(i).timeWritten() != this->spiceInMsgStatus[i].lastTimeTag){ - this->spiceInMsgStatus[i].lastTimeTag = this->spiceInMsgs.at(i).timeWritten(); - this->spiceInMsgStatus[i].dataFresh = true; - this->spiceMessage[i] = localSpiceArray; + for (size_t i = 0; i < this->spiceInMsgs.size(); i++) { + if (this->spiceInMsgs.at(i).isLinked()) { + // If the spice msg is not linked then the default zero planet emphemeris is used + SpicePlanetStateMsgPayload localSpiceArray; + localSpiceArray = this->spiceInMsgs.at(i)(); + if (this->spiceInMsgs.at(i).isWritten() && + this->spiceInMsgs.at(i).timeWritten() != this->spiceInMsgStatus[i].lastTimeTag) { + this->spiceInMsgStatus[i].lastTimeTag = this->spiceInMsgs.at(i).timeWritten(); + this->spiceInMsgStatus[i].dataFresh = true; + this->spiceMessage[i] = localSpiceArray; + } } } } - } return; } @@ -437,8 +434,7 @@ void VizInterface::ReadBSKMessages() /*! The method in which the vizInterface writes a protobuffer with the information from the simulation. @param currentSimNanos The current sim time in nanoseconds */ -void VizInterface::WriteProtobuffer(uint64_t currentSimNanos) -{ +void VizInterface::WriteProtobuffer(uint64_t currentSimNanos) { vizProtobufferMessage::VizMessage* message = new vizProtobufferMessage::VizMessage; /*! Send the Vizard settings once */ @@ -448,33 +444,47 @@ void VizInterface::WriteProtobuffer(uint64_t currentSimNanos) // define the viz ambient light setting vizSettings->set_ambient(this->settings.ambient); - if (this->settings.ambient > 8.0 || - (this->settings.ambient < 0.0 && this->settings.ambient != -1.0)) { - bskLogger.bskLog(BSK_WARNING, "vizInterface: The Vizard ambient light value must be within [0,8]. A value of %f was received.", this->settings.ambient); + if (this->settings.ambient > 8.0 || (this->settings.ambient < 0.0 && this->settings.ambient != -1.0)) { + bskLogger.bskLog( + BSK_WARNING, + "vizInterface: The Vizard ambient light value must be within [0,8]. A value of %f was received.", + this->settings.ambient); } // define if osculating orbit lines should be shown vizSettings->set_orbitlineson(this->settings.orbitLinesOn); - if (abs(this->settings.orbitLinesOn)>2) { - bskLogger.bskLog(BSK_WARNING, "vizInterface: The Vizard orbitLinesOn flag must be either -1, 0, 1 or 2. A value of %d was received.", this->settings.orbitLinesOn); + if (abs(this->settings.orbitLinesOn) > 2) { + bskLogger.bskLog( + BSK_WARNING, + "vizInterface: The Vizard orbitLinesOn flag must be either -1, 0, 1 or 2. A value of %d was received.", + this->settings.orbitLinesOn); } // define if true orbit lines should be shown vizSettings->set_truetrajectorylineson(this->settings.trueTrajectoryLinesOn); - if (abs(this->settings.trueTrajectoryLinesOn)>2) { - bskLogger.bskLog(BSK_WARNING, "vizInterface: The Vizard trueTrajectoryLinesOn flag must be either -1, 0, 1 or 2. A value of %d was received.", this->settings.trueTrajectoryLinesOn); + if (abs(this->settings.trueTrajectoryLinesOn) > 2) { + bskLogger.bskLog(BSK_WARNING, + "vizInterface: The Vizard trueTrajectoryLinesOn flag must be either -1, 0, 1 or 2. A " + "value of %d was received.", + this->settings.trueTrajectoryLinesOn); } // define if spacecraft axes should be shown vizSettings->set_spacecraftcson(this->settings.spacecraftCSon); - if (abs(this->settings.spacecraftCSon)>1) { - bskLogger.bskLog(BSK_WARNING, "vizInterface: The Vizard spacecraftCSon flag must be either -1, 0 or 1. A value of %d was received.", this->settings.spacecraftCSon); + if (abs(this->settings.spacecraftCSon) > 1) { + bskLogger.bskLog( + BSK_WARNING, + "vizInterface: The Vizard spacecraftCSon flag must be either -1, 0 or 1. A value of %d was received.", + this->settings.spacecraftCSon); } // define if planet axes should be shown vizSettings->set_planetcson(this->settings.planetCSon); - if (abs(this->settings.planetCSon)>1) { - bskLogger.bskLog(BSK_WARNING, "vizInterface: The Vizard planetCSon flag must be either -1, 0 or 1. A value of %d was received.", this->settings.planetCSon); + if (abs(this->settings.planetCSon) > 1) { + bskLogger.bskLog( + BSK_WARNING, + "vizInterface: The Vizard planetCSon flag must be either -1, 0 or 1. A value of %d was received.", + this->settings.planetCSon); } // define the skyBox variable @@ -487,7 +497,7 @@ void VizInterface::WriteProtobuffer(uint64_t currentSimNanos) vizProtobufferMessage::VizMessage::PointLine* pl = vizSettings->add_pointlines(); pl->set_tobodyname(this->settings.pointLineList[idx].toBodyName); pl->set_frombodyname(this->settings.pointLineList[idx].fromBodyName); - for (int i=0; i<4; i++){ + for (int i = 0; i < 4; i++) { pl->add_linecolor(this->settings.pointLineList[idx].lineColor[i]); } } @@ -496,15 +506,15 @@ void VizInterface::WriteProtobuffer(uint64_t currentSimNanos) for (size_t idx = 0; idx < this->settings.coneList.size(); idx++) { vizProtobufferMessage::VizMessage::KeepOutInCone* cone = vizSettings->add_keepoutincones(); cone->set_iskeepin(this->settings.coneList[idx].isKeepIn); - for (int i=0; i<3; i++) { + for (int i = 0; i < 3; i++) { cone->add_position(this->settings.coneList[idx].position_B[i]); cone->add_normalvector(this->settings.coneList[idx].normalVector_B[i]); } - cone->set_incidenceangle(this->settings.coneList[idx].incidenceAngle*R2D); // Unity expects degrees + cone->set_incidenceangle(this->settings.coneList[idx].incidenceAngle * R2D); // Unity expects degrees cone->set_coneheight(this->settings.coneList[idx].coneHeight); cone->set_tobodyname(this->settings.coneList[idx].toBodyName); cone->set_frombodyname(this->settings.coneList[idx].fromBodyName); - for (int i=0; i<4; i++){ + for (int i = 0; i < 4; i++) { cone->add_conecolor(this->settings.coneList[idx].coneColor[i]); } cone->set_conename(this->settings.coneList[idx].coneName); @@ -512,44 +522,65 @@ void VizInterface::WriteProtobuffer(uint64_t currentSimNanos) // define if camera boresight line should be shown vizSettings->set_viewcameraboresighthud(this->settings.viewCameraBoresightHUD); - if (abs(this->settings.viewCameraBoresightHUD)>1) { - bskLogger.bskLog(BSK_WARNING, "vizInterface: The Vizard viewCameraBoresightHUD flag must be either -1, 0 or 1. A value of %d was received.", this->settings.viewCameraBoresightHUD); + if (abs(this->settings.viewCameraBoresightHUD) > 1) { + bskLogger.bskLog(BSK_WARNING, + "vizInterface: The Vizard viewCameraBoresightHUD flag must be either -1, 0 or 1. A value " + "of %d was received.", + this->settings.viewCameraBoresightHUD); } // define if camera cone should be shown vizSettings->set_viewcameraconehud(this->settings.viewCameraConeHUD); - if (abs(this->settings.viewCameraConeHUD)>1) { - bskLogger.bskLog(BSK_WARNING, "vizInterface: The Vizard viewCameraConeHUD flag must be either -1, 0 or 1. A value of %d was received.", this->settings.viewCameraConeHUD); + if (abs(this->settings.viewCameraConeHUD) > 1) { + bskLogger.bskLog(BSK_WARNING, + "vizInterface: The Vizard viewCameraConeHUD flag must be either -1, 0 or 1. A value of " + "%d was received.", + this->settings.viewCameraConeHUD); } // define if coordinate system labels should be shown vizSettings->set_showcslabels(this->settings.showCSLabels); - if (abs(this->settings.showCSLabels)>1) { - bskLogger.bskLog(BSK_WARNING, "vizInterface: The Vizard showCSLabels flag must be either -1, 0 or 1. A value of %d was received.", this->settings.showCSLabels); + if (abs(this->settings.showCSLabels) > 1) { + bskLogger.bskLog( + BSK_WARNING, + "vizInterface: The Vizard showCSLabels flag must be either -1, 0 or 1. A value of %d was received.", + this->settings.showCSLabels); } // define if celestial body labels should be shown vizSettings->set_showcelestialbodylabels(this->settings.showCelestialBodyLabels); - if (abs(this->settings.showCelestialBodyLabels)>1) { - bskLogger.bskLog(BSK_WARNING, "vizInterface: The Vizard showCelestialBodyLabels flag must be either -1, 0 or 1. A value of %d was received.", this->settings.showCelestialBodyLabels); + if (abs(this->settings.showCelestialBodyLabels) > 1) { + bskLogger.bskLog(BSK_WARNING, + "vizInterface: The Vizard showCelestialBodyLabels flag must be either -1, 0 or 1. A " + "value of %d was received.", + this->settings.showCelestialBodyLabels); } // define if spacecraft labels should be shown vizSettings->set_showspacecraftlabels(this->settings.showSpacecraftLabels); - if (abs(this->settings.showSpacecraftLabels)>1) { - bskLogger.bskLog(BSK_WARNING, "vizInterface: The Vizard showSpacecraftLabels flag must be either -1, 0 or 1. A value of %d was received.", this->settings.showSpacecraftLabels); + if (abs(this->settings.showSpacecraftLabels) > 1) { + bskLogger.bskLog(BSK_WARNING, + "vizInterface: The Vizard showSpacecraftLabels flag must be either -1, 0 or 1. A value " + "of %d was received.", + this->settings.showSpacecraftLabels); } // define if camera labels should be shown vizSettings->set_showcameralabels(this->settings.showCameraLabels); - if (abs(this->settings.showCameraLabels)>1) { - bskLogger.bskLog(BSK_WARNING, "vizInterface: The Vizard showCameraLabels flag must be either -1, 0 or 1. A value of %d was received.", this->settings.showCameraLabels); + if (abs(this->settings.showCameraLabels) > 1) { + bskLogger.bskLog(BSK_WARNING, + "vizInterface: The Vizard showCameraLabels flag must be either -1, 0 or 1. A value of %d " + "was received.", + this->settings.showCameraLabels); } // define the GUI scaling factor vizSettings->set_customguiscale(this->settings.customGUIScale); - if (abs(this->settings.customGUIScale)>3.0) { - bskLogger.bskLog(BSK_WARNING, "vizInterface: The Vizard customGUIScale flag must be either -1 or [0.5, 3] A value of %d was received.", this->settings.customGUIScale); + if (abs(this->settings.customGUIScale) > 3.0) { + bskLogger.bskLog(BSK_WARNING, + "vizInterface: The Vizard customGUIScale flag must be either -1 or [0.5, 3] A value of " + "%d was received.", + this->settings.customGUIScale); } // define default spacecraft sprite behavior @@ -557,35 +588,47 @@ void VizInterface::WriteProtobuffer(uint64_t currentSimNanos) // define if spacecraft should be shown as sprites vizSettings->set_showspacecraftassprites(this->settings.showSpacecraftAsSprites); - if (abs(this->settings.showSpacecraftAsSprites)>1) { - bskLogger.bskLog(BSK_WARNING, "vizInterface: The Vizard showSpacecraftAsSprites flag must be either -1, 0 or 1. A value of %d was received.", this->settings.showSpacecraftAsSprites); + if (abs(this->settings.showSpacecraftAsSprites) > 1) { + bskLogger.bskLog(BSK_WARNING, + "vizInterface: The Vizard showSpacecraftAsSprites flag must be either -1, 0 or 1. A " + "value of %d was received.", + this->settings.showSpacecraftAsSprites); } // define if celestial objects should be shown as sprites vizSettings->set_showcelestialbodiesassprites(this->settings.showCelestialBodiesAsSprites); - if (abs(this->settings.showCelestialBodiesAsSprites)>1) { - bskLogger.bskLog(BSK_WARNING, "vizInterface: The Vizard showCelestialBodiesAsSprites flag must be either -1, 0 or 1. A value of %d was received.", this->settings.showCelestialBodiesAsSprites); + if (abs(this->settings.showCelestialBodiesAsSprites) > 1) { + bskLogger.bskLog(BSK_WARNING, + "vizInterface: The Vizard showCelestialBodiesAsSprites flag must be either -1, 0 or 1. A " + "value of %d was received.", + this->settings.showCelestialBodiesAsSprites); } // define if the time should be shown using a 24h clock vizSettings->set_show24hrclock(this->settings.show24hrClock); - if (abs(this->settings.show24hrClock)>1) { - bskLogger.bskLog(BSK_WARNING, "vizInterface: The Vizard show24hrClock flag must be either -1, 0 or 1. A value of %d was received.", this->settings.show24hrClock); + if (abs(this->settings.show24hrClock) > 1) { + bskLogger.bskLog( + BSK_WARNING, + "vizInterface: The Vizard show24hrClock flag must be either -1, 0 or 1. A value of %d was received.", + this->settings.show24hrClock); } // define if the data frame rate should be shown vizSettings->set_showdataratedisplay(this->settings.showDataRateDisplay); - if (abs(this->settings.showDataRateDisplay)>1) { - bskLogger.bskLog(BSK_WARNING, "vizInterface: The Vizard showDataRateDisplay flag must be either -1, 0 or 1. A value of %d was received.", this->settings.showDataRateDisplay); + if (abs(this->settings.showDataRateDisplay) > 1) { + bskLogger.bskLog(BSK_WARNING, + "vizInterface: The Vizard showDataRateDisplay flag must be either -1, 0 or 1. A value of " + "%d was received.", + this->settings.showDataRateDisplay); } // define the keyboard driven camera rates - vizSettings->set_keyboardangularrate(this->settings.keyboardAngularRate*R2D); + vizSettings->set_keyboardangularrate(this->settings.keyboardAngularRate * R2D); vizSettings->set_keyboardzoomrate(this->settings.keyboardZoomRate); // add default thrust plume color if (this->settings.defaultThrusterColor[0] >= 0) { - for (int i=0; i<4; i++){ + for (int i = 0; i < 4; i++) { vizSettings->add_defaultthrustercolor(this->settings.defaultThrusterColor[i]); } } @@ -607,7 +650,8 @@ void VizInterface::WriteProtobuffer(uint64_t currentSimNanos) vizSettings->set_showlocationlabels(this->settings.showLocationLabels); vizSettings->set_atmospheresoff(this->settings.atmospheresOff); vizSettings->set_scviewtoplanetviewboundarymultiplier(this->settings.scViewToPlanetViewBoundaryMultiplier); - vizSettings->set_planetviewtohelioviewboundarymultiplier(this->settings.planetViewToHelioViewBoundaryMultiplier); + vizSettings->set_planetviewtohelioviewboundarymultiplier( + this->settings.planetViewToHelioViewBoundaryMultiplier); vizSettings->set_sunintensity(this->settings.sunIntensity); vizSettings->set_attenuatesunlightwithdistance(this->settings.attenuateSunLightWithDistance); vizSettings->set_showlightlabels(this->settings.showLightLabels); @@ -642,47 +686,45 @@ void VizInterface::WriteProtobuffer(uint64_t currentSimNanos) il->set_showmultispherelabels(this->settings.instrumentGuiSettingsList[idx].showMultiSphereLabels); } - // define scene object custom object shapes for (size_t idx = 0; idx < this->settings.customModelList.size(); idx++) { vizProtobufferMessage::VizMessage::CustomModel* cm = vizSettings->add_custommodels(); - CustomModel *cmp = &(this->settings.customModelList[idx]); + CustomModel* cmp = &(this->settings.customModelList[idx]); cm->set_modelpath(cmp->modelPath); - for (size_t i=0; isimBodiesToModify.size(); i++) { + for (size_t i = 0; i < cmp->simBodiesToModify.size(); i++) { cm->add_simbodiestomodify(cmp->simBodiesToModify[i]); } - for (size_t i=0; i<3;i++) { + for (size_t i = 0; i < 3; i++) { cm->add_offset(cmp->offset[i]); - cm->add_rotation(cmp->rotation[i]*R2D); // Unity expects degrees + cm->add_rotation(cmp->rotation[i] * R2D); // Unity expects degrees cm->add_scale(cmp->scale[i]); } cm->set_customtexturepath(cmp->customTexturePath); cm->set_normalmappath(cmp->normalMapPath); cm->set_shader(cmp->shader); - for (size_t i=0; icolor.size(); i++) { + for (size_t i = 0; i < cmp->color.size(); i++) { cm->add_color(cmp->color[i]); } - } // define camera settings - for (size_t idx = 0; idx < this->settings.stdCameraList.size(); idx++){ + for (size_t idx = 0; idx < this->settings.stdCameraList.size(); idx++) { vizProtobufferMessage::VizMessage::StandardCameraSettings* sc = vizSettings->add_standardcamerasettings(); - StdCameraSettings *scp = &(this->settings.stdCameraList[idx]); + StdCameraSettings* scp = &(this->settings.stdCameraList[idx]); sc->set_spacecraftname(scp->spacecraftName); sc->set_setmode(scp->setMode); if (scp->fieldOfView < 0) sc->set_fieldofview(-1.0); else { - sc->set_fieldofview(scp->fieldOfView*R2D); // Unity expects degrees + sc->set_fieldofview(scp->fieldOfView * R2D); // Unity expects degrees } sc->set_bodytarget(scp->bodyTarget); sc->set_setview(scp->setView); - for (size_t i=0; i<3; i++){ + for (size_t i = 0; i < 3; i++) { sc->add_pointingvector(scp->pointingVector_B[i]); } if (v3Norm(scp->position_B) > 0.00001) { - for (size_t i=0; i<3; i++) { + for (size_t i = 0; i < 3; i++) { sc->add_position(scp->position_B[i]); } } @@ -703,7 +745,7 @@ void VizInterface::WriteProtobuffer(uint64_t currentSimNanos) vizProtobufferMessage::VizMessage::PointLine* pl = liveVizSettings->add_targetlines(); pl->set_tobodyname(this->liveSettings.targetLineList[idx].toBodyName); pl->set_frombodyname(this->liveSettings.targetLineList[idx].fromBodyName); - for (int i=0; i<4; i++){ + for (int i = 0; i < 4; i++) { pl->add_linecolor(this->liveSettings.targetLineList[idx].lineColor[i]); } } @@ -713,7 +755,7 @@ void VizInterface::WriteProtobuffer(uint64_t currentSimNanos) /*! Write timestamp output msg */ vizProtobufferMessage::VizMessage::TimeStamp* time = new vizProtobufferMessage::VizMessage::TimeStamp; time->set_framenumber(this->FrameNumber); - time->set_simtimeelapsed((double) currentSimNanos); + time->set_simtimeelapsed((double)currentSimNanos); message->set_allocated_currenttime(time); /*! write epoch msg */ @@ -730,84 +772,80 @@ void VizInterface::WriteProtobuffer(uint64_t currentSimNanos) } /*! write the Locations protobuffer messages */ - std::vector::iterator glIt; + std::vector::iterator glIt; for (glIt = locations.begin(); glIt != locations.end(); glIt++) { vizProtobufferMessage::VizMessage::Location* glp = message->add_locations(); glp->set_stationname((*glIt)->stationName); glp->set_parentbodyname((*glIt)->parentBodyName); - glp->set_fieldofview((*glIt)->fieldOfView*R2D); + glp->set_fieldofview((*glIt)->fieldOfView * R2D); glp->set_range((*glIt)->range); - for (int i=0; i<3; i++) { + for (int i = 0; i < 3; i++) { glp->add_r_gp_p((*glIt)->r_GP_P[i]); glp->add_ghat_p((*glIt)->gHat_P[i]); } - for (int i=0; i<4; i++) { + for (int i = 0; i < 4; i++) { glp->add_color((*glIt)->color[i]); } } std::vector::iterator scIt; - for (scIt = scData.begin(); scIt != scData.end(); scIt++) - { + for (scIt = scData.begin(); scIt != scData.end(); scIt++) { /*! Write spacecraft state output msg */ - if (scIt->scStateInMsg.isLinked() && scIt->scStateInMsgStatus.dataFresh){ + if (scIt->scStateInMsg.isLinked() && scIt->scStateInMsgStatus.dataFresh) { vizProtobufferMessage::VizMessage::Spacecraft* scp = message->add_spacecraft(); scp->set_spacecraftname(scIt->spacecraftName); scp->set_parentspacecraftname(scIt->parentSpacecraftName); - for (int i=0; i<3; i++){ + for (int i = 0; i < 3; i++) { scp->add_position(scIt->scStateMsgBuffer.r_BN_N[i]); scp->add_velocity(scIt->scStateMsgBuffer.v_BN_N[i]); scp->add_rotation(scIt->scStateMsgBuffer.sigma_BN[i]); } -// scIt->scStateInMsgID.dataFresh = false; + // scIt->scStateInMsgID.dataFresh = false; /* Write the SC sprite string */ scp->set_spacecraftsprite(scIt->spacecraftSprite); /*! Write RW output msg */ - for (size_t idx =0; idx < (size_t) scIt->rwInMsgs.size(); idx++) - { - if (scIt->rwInMsgs[idx].isLinked() && scIt->rwInMsgStatus[idx].dataFresh){ + for (size_t idx = 0; idx < (size_t)scIt->rwInMsgs.size(); idx++) { + if (scIt->rwInMsgs[idx].isLinked() && scIt->rwInMsgStatus[idx].dataFresh) { vizProtobufferMessage::VizMessage::ReactionWheel* rwheel = scp->add_reactionwheels(); rwheel->set_wheelspeed(scIt->rwInMessage[idx].Omega); rwheel->set_maxspeed(scIt->rwInMessage[idx].Omega_max); rwheel->set_wheeltorque(scIt->rwInMessage[idx].u_current); rwheel->set_maxtorque(scIt->rwInMessage[idx].u_max); - for (int i=0; i<3; i++){ + for (int i = 0; i < 3; i++) { rwheel->add_position(scIt->rwInMessage[idx].rWB_B[i]); rwheel->add_spinaxisvector(scIt->rwInMessage[idx].gsHat_B[i]); } - //rwInMsgID[idx].dataFresh = false; + // rwInMsgID[idx].dataFresh = false; } } /*! Write Thr output msg */ - for (size_t idx =0; idx < (size_t) scIt->thrInMsgs.size(); idx++) - { - if (scIt->thrInMsgs[idx].isLinked() && scIt->thrMsgStatus[idx].dataFresh){ + for (size_t idx = 0; idx < (size_t)scIt->thrInMsgs.size(); idx++) { + if (scIt->thrInMsgs[idx].isLinked() && scIt->thrMsgStatus[idx].dataFresh) { vizProtobufferMessage::VizMessage::Thruster* thr = scp->add_thrusters(); thr->set_maxthrust(scIt->thrOutputMessage[idx].maxThrust); thr->set_currentthrust(scIt->thrOutputMessage[idx].thrustForce); thr->set_thrustertag(scIt->thrInfo[idx].thrTag); if (scIt->thrInfo[idx].color[0] >= 0) { - for (int i=0; i<4; i++) { + for (int i = 0; i < 4; i++) { thr->add_color(scIt->thrInfo[idx].color[i]); } } - for (int i=0; i<3; i++){ + for (int i = 0; i < 3; i++) { thr->add_position(scIt->thrOutputMessage[idx].thrusterLocation[i]); thr->add_thrustvector(scIt->thrOutputMessage[idx].thrusterDirection[i]); } - //thrMsgID[idx].dataFresh = false; + // thrMsgID[idx].dataFresh = false; } } // Write CSS output msg - for (size_t idx =0; idx < (size_t) scIt->cssInMsgs.size(); idx++) - { - if (scIt->cssInMsgs[idx].isLinked() && scIt->cssConfLogInMsgStatus[idx].dataFresh){ + for (size_t idx = 0; idx < (size_t)scIt->cssInMsgs.size(); idx++) { + if (scIt->cssInMsgs[idx].isLinked() && scIt->cssConfLogInMsgStatus[idx].dataFresh) { vizProtobufferMessage::VizMessage::CoarseSunSensor* css = scp->add_css(); - for (int j=0; j<3; j++){ + for (int j = 0; j < 3; j++) { css->add_normalvector(scIt->cssInMessage[idx].nHat_B[j]); css->add_position(scIt->cssInMessage[idx].r_B[j]); } @@ -815,60 +853,60 @@ void VizInterface::WriteProtobuffer(uint64_t currentSimNanos) css->set_maxmsmt(scIt->cssInMessage[idx].maxSignal); css->set_minmsmt(scIt->cssInMessage[idx].minSignal); css->set_cssgroupid(scIt->cssInMessage[idx].CSSGroupID); - css->set_fieldofview(scIt->cssInMessage[idx].fov*2*R2D); /* must be edge to edge fov in degrees */ + css->set_fieldofview(scIt->cssInMessage[idx].fov * 2 * + R2D); /* must be edge to edge fov in degrees */ - //cssConfLogInMsgId[idx].dataFresh = false; + // cssConfLogInMsgId[idx].dataFresh = false; } } // Write generic sensor messages - for (size_t idx =0; idx < (size_t) scIt->genericSensorList.size(); idx++) { + for (size_t idx = 0; idx < (size_t)scIt->genericSensorList.size(); idx++) { vizProtobufferMessage::VizMessage::GenericSensor* gs = scp->add_genericsensors(); - for (int j=0; j<3; j++) { + for (int j = 0; j < 3; j++) { gs->add_position(scIt->genericSensorList[idx]->r_SB_B[j]); gs->add_normalvector(scIt->genericSensorList[idx]->normalVector[j]); } - for (uint64_t j=0; jgenericSensorList[idx]->fieldOfView.size(); j++) { - gs->add_fieldofview(scIt->genericSensorList[idx]->fieldOfView[j]*R2D); + for (uint64_t j = 0; j < scIt->genericSensorList[idx]->fieldOfView.size(); j++) { + gs->add_fieldofview(scIt->genericSensorList[idx]->fieldOfView[j] * R2D); } gs->set_ishidden(scIt->genericSensorList[idx]->isHidden); gs->set_size(scIt->genericSensorList[idx]->size); gs->set_label(scIt->genericSensorList[idx]->label); - for (uint64_t j=0; jgenericSensorList[idx]->color.size(); j++) { + for (uint64_t j = 0; j < scIt->genericSensorList[idx]->color.size(); j++) { gs->add_color(scIt->genericSensorList[idx]->color[j]); } - gs->set_activitystatus((int) scIt->genericSensorList[idx]->genericSensorCmd); + gs->set_activitystatus((int)scIt->genericSensorList[idx]->genericSensorCmd); } // Write Ellipsoid messages - for (size_t idx =0; idx < (size_t) scIt->ellipsoidList.size(); idx++) { + for (size_t idx = 0; idx < (size_t)scIt->ellipsoidList.size(); idx++) { vizProtobufferMessage::VizMessage::Ellipsoid* el = scp->add_ellipsoids(); el->set_ison(scIt->ellipsoidList[idx]->isOn); el->set_usebodyframe(scIt->ellipsoidList[idx]->useBodyFrame); - for (int j=0; j<3; j++) { + for (int j = 0; j < 3; j++) { el->add_position(scIt->ellipsoidList[idx]->position[j]); el->add_semimajoraxes(scIt->ellipsoidList[idx]->semiMajorAxes[j]); } - for (uint64_t j=0; jellipsoidList[idx]->color.size(); j++) { + for (uint64_t j = 0; j < scIt->ellipsoidList[idx]->color.size(); j++) { el->add_color(scIt->ellipsoidList[idx]->color[j]); } el->set_showgridlines(scIt->ellipsoidList[idx]->showGridLines); } - // Write transceiver messages - for (size_t idx =0; idx < (size_t) scIt->transceiverList.size(); idx++) { + for (size_t idx = 0; idx < (size_t)scIt->transceiverList.size(); idx++) { vizProtobufferMessage::VizMessage::Transceiver* tr = scp->add_transceivers(); - for (int j=0; j<3; j++) { + for (int j = 0; j < 3; j++) { tr->add_position(scIt->transceiverList[idx]->r_SB_B[j]); tr->add_normalvector(scIt->transceiverList[idx]->normalVector[j]); } - tr->set_fieldofview(scIt->transceiverList[idx]->fieldOfView*R2D); + tr->set_fieldofview(scIt->transceiverList[idx]->fieldOfView * R2D); tr->set_ishidden(scIt->transceiverList[idx]->isHidden); tr->set_label(scIt->transceiverList[idx]->label); - for (uint64_t j=0; jtransceiverList[idx]->color.size(); j++) { + for (uint64_t j = 0; j < scIt->transceiverList[idx]->color.size(); j++) { tr->add_color(scIt->transceiverList[idx]->color[j]); } tr->set_transmitstatus(scIt->transceiverList[idx]->transceiverState); @@ -876,28 +914,27 @@ void VizInterface::WriteProtobuffer(uint64_t currentSimNanos) } // Write generic storage device messages - for (size_t idx =0; idx < (size_t) scIt->genericStorageList.size(); idx++) { + for (size_t idx = 0; idx < (size_t)scIt->genericStorageList.size(); idx++) { vizProtobufferMessage::VizMessage::GenericStorage* gsd = scp->add_storagedevices(); gsd->set_label(scIt->genericStorageList[idx]->label); gsd->set_currentvalue(scIt->genericStorageList[idx]->currentValue); gsd->set_maxvalue(scIt->genericStorageList[idx]->maxValue); gsd->set_units(scIt->genericStorageList[idx]->units); - for (uint64_t j=0; jgenericStorageList[idx]->color.size(); j++) { + for (uint64_t j = 0; j < scIt->genericStorageList[idx]->color.size(); j++) { gsd->add_color(scIt->genericStorageList[idx]->color[j]); } - for (uint64_t j=0; jgenericStorageList[idx]->thresholds.size(); j++) { + for (uint64_t j = 0; j < scIt->genericStorageList[idx]->thresholds.size(); j++) { gsd->add_thresholds(scIt->genericStorageList[idx]->thresholds[j]); } - } // Write light device messages - for (size_t idx =0; idx < (size_t) scIt->lightList.size(); idx++) { + for (size_t idx = 0; idx < (size_t)scIt->lightList.size(); idx++) { vizProtobufferMessage::VizMessage::Light* ld = scp->add_lights(); ld->set_label(scIt->lightList[idx]->label); - for (uint64_t j=0; j<3; j++) { + for (uint64_t j = 0; j < 3; j++) { ld->add_position(scIt->lightList[idx]->position[j]); ld->add_normalvector(scIt->lightList[idx]->normalVector[j]); } @@ -908,12 +945,12 @@ void VizInterface::WriteProtobuffer(uint64_t currentSimNanos) scIt->lightList[idx]->lightOn = -1; } ld->set_lighton(scIt->lightList[idx]->lightOn); - ld->set_fieldofview(scIt->lightList[idx]->fieldOfView*R2D); + ld->set_fieldofview(scIt->lightList[idx]->fieldOfView * R2D); ld->set_range(scIt->lightList[idx]->range); ld->set_intensity(scIt->lightList[idx]->intensity); ld->set_showlightmarker(scIt->lightList[idx]->showLightMarker); ld->set_markerdiameter(scIt->lightList[idx]->markerDiameter); - for (uint64_t j=0; jlightList[idx]->color.size(); j++) { + for (uint64_t j = 0; j < scIt->lightList[idx]->color.size(); j++) { ld->add_color(scIt->lightList[idx]->color[j]); } ld->set_gammasaturation(scIt->lightList[idx]->gammaSaturation); @@ -929,68 +966,70 @@ void VizInterface::WriteProtobuffer(uint64_t currentSimNanos) scp->set_logotexture(scIt->logoTexture); /* set spacecraft osculating orbit line color */ - for (size_t i=0; ioscOrbitLineColor.size(); i++){ + for (size_t i = 0; i < scIt->oscOrbitLineColor.size(); i++) { scp->add_oscorbitlinecolor(scIt->oscOrbitLineColor[i]); } /* set spacecraft true orbit line color */ - for (size_t i=0; itrueTrajectoryLineColor.size(); i++){ + for (size_t i = 0; i < scIt->trueTrajectoryLineColor.size(); i++) { scp->add_truetrajectorylinecolor(scIt->trueTrajectoryLineColor[i]); } // Write Multi-Sphere-Model messages - for (size_t idx =0; idx < (size_t) scIt->msmInfo.msmList.size(); idx++) { + for (size_t idx = 0; idx < (size_t)scIt->msmInfo.msmList.size(); idx++) { vizProtobufferMessage::VizMessage::MultiSphere* msmp = scp->add_multispheres(); msmp->set_ison(scIt->msmInfo.msmList[idx]->isOn); - for (uint64_t j=0; j<3; j++) { + for (uint64_t j = 0; j < 3; j++) { msmp->add_position(scIt->msmInfo.msmList[idx]->position[j]); } msmp->set_radius(scIt->msmInfo.msmList[idx]->radius); msmp->set_currentvalue(scIt->msmInfo.msmList[idx]->currentValue); msmp->set_maxvalue(scIt->msmInfo.msmList[idx]->maxValue); - for (uint64_t j=0; jmsmInfo.msmList[idx]->positiveColor.size(); j++) { + for (uint64_t j = 0; j < scIt->msmInfo.msmList[idx]->positiveColor.size(); j++) { msmp->add_positivecolor(scIt->msmInfo.msmList[idx]->positiveColor[j]); } - for (uint64_t j=0; jmsmInfo.msmList[idx]->negativeColor.size(); j++) { + for (uint64_t j = 0; j < scIt->msmInfo.msmList[idx]->negativeColor.size(); j++) { msmp->add_negativecolor(scIt->msmInfo.msmList[idx]->negativeColor[j]); } msmp->set_neutralopacity(scIt->msmInfo.msmList[idx]->neutralOpacity); } - } } /*! Write camera output msg */ - for (size_t camCounter = 0; camCounter < this->cameraConfInMsgs.size(); camCounter++){ - if ((this->cameraConfInMsgs[camCounter].isLinked() && this->cameraConfMsgStatus[camCounter].dataFresh) - || this->cameraConfigBuffers[camCounter].cameraID >= 0){ - /*! This corrective rotation allows unity to place the camera as is expected by the python setting. Unity has a -x pointing camera, with z vertical on the sensor, and y horizontal which is not the OpNav frame: z point, x horizontal, y vertical (down) */ + for (size_t camCounter = 0; camCounter < this->cameraConfInMsgs.size(); camCounter++) { + if ((this->cameraConfInMsgs[camCounter].isLinked() && this->cameraConfMsgStatus[camCounter].dataFresh) || + this->cameraConfigBuffers[camCounter].cameraID >= 0) { + /*! This corrective rotation allows unity to place the camera as is expected by the python setting. Unity + * has a -x pointing camera, with z vertical on the sensor, and y horizontal which is not the OpNav frame: z + * point, x horizontal, y vertical (down) */ double sigma_CuC[3], unityCameraMRP[3]; /*! Cu is the unity Camera frame */ - v3Set(1./3, 1./3, -1./3, sigma_CuC); + v3Set(1. / 3, 1. / 3, -1. / 3, sigma_CuC); addMRP(this->cameraConfigBuffers[camCounter].sigma_CB, sigma_CuC, unityCameraMRP); vizProtobufferMessage::VizMessage::CameraConfig* camera = message->add_cameras(); - for (int j=0; j<3; j++){ - if (j < 2){ + for (int j = 0; j < 3; j++) { + if (j < 2) { camera->add_resolution(this->cameraConfigBuffers[camCounter].resolution[j]); } camera->add_cameradir_b(unityCameraMRP[j]); camera->add_camerapos_b(this->cameraConfigBuffers[camCounter].cameraPos_B[j]); } - camera->set_renderrate(this->cameraConfigBuffers[camCounter].renderRate); // Unity expects nano-seconds between images + camera->set_renderrate( + this->cameraConfigBuffers[camCounter].renderRate); // Unity expects nano-seconds between images camera->set_cameraid(this->cameraConfigBuffers[camCounter].cameraID); - camera->set_fieldofview(this->cameraConfigBuffers[camCounter].fieldOfView*R2D); // Unity expects degrees + camera->set_fieldofview(this->cameraConfigBuffers[camCounter].fieldOfView * R2D); // Unity expects degrees camera->set_skybox(this->cameraConfigBuffers[camCounter].skyBox); camera->set_parentname(this->cameraConfigBuffers[camCounter].parentName); camera->set_postprocessingon(this->cameraConfigBuffers[camCounter].postProcessingOn); camera->set_ppfocusdistance(this->cameraConfigBuffers[camCounter].ppFocusDistance); camera->set_ppaperture(this->cameraConfigBuffers[camCounter].ppAperture); - camera->set_ppfocallength(this->cameraConfigBuffers[camCounter].ppFocalLength*1000.); // Unity expects mm + camera->set_ppfocallength(this->cameraConfigBuffers[camCounter].ppFocalLength * 1000.); // Unity expects mm camera->set_ppmaxblursize(this->cameraConfigBuffers[camCounter].ppMaxBlurSize); camera->set_updatecameraparameters(this->cameraConfigBuffers[camCounter].updateCameraParameters); camera->set_rendermode(this->cameraConfigBuffers[camCounter].renderMode); if (this->cameraConfigBuffers[camCounter].depthMapClippingPlanes[0] > 0.0) { - for (int j=0; j<2; j++) { + for (int j = 0; j < 2; j++) { camera->add_depthmapclippingplanes(this->cameraConfigBuffers[camCounter].depthMapClippingPlanes[j]); } } @@ -998,62 +1037,64 @@ void VizInterface::WriteProtobuffer(uint64_t currentSimNanos) } /*! Write spice output msgs */ - for(size_t k=0; kgravBodyInformation.size(); k++) - { - if (this->spiceInMsgStatus[k].dataFresh){ + for (size_t k = 0; k < this->gravBodyInformation.size(); k++) { + if (this->spiceInMsgStatus[k].dataFresh) { vizProtobufferMessage::VizMessage::CelestialBody* spice = message->add_celestialbodies(); spice->set_bodyname(this->gravBodyInformation.at(k).bodyName); - spice->set_mu(this->gravBodyInformation.at(k).mu/1e9); /* must be in km^3/s^2 */ - spice->set_radiuseq(this->gravBodyInformation.at(k).radEquator/1000.); /* must be in km */ + spice->set_mu(this->gravBodyInformation.at(k).mu / 1e9); /* must be in km^3/s^2 */ + spice->set_radiuseq(this->gravBodyInformation.at(k).radEquator / 1000.); /* must be in km */ spice->set_radiusratio(this->gravBodyInformation.at(k).radiusRatio); spice->set_modeldictionarykey(this->gravBodyInformation.at(k).modelDictionaryKey); - for (int i=0; i<3; i++){ + for (int i = 0; i < 3; i++) { spice->add_position(this->spiceMessage[k].PositionVector[i]); spice->add_velocity(this->spiceMessage[k].VelocityVector[i]); - for (int j=0; j<3; j++){ + for (int j = 0; j < 3; j++) { spice->add_rotation(this->spiceMessage[k].J20002Pfix[i][j]); } } -// spiceInMsgID[k].dataFresh = false; + // spiceInMsgID[k].dataFresh = false; } } { google::protobuf::uint8 varIntBuffer[4]; - uint32_t byteCount = (uint32_t) message->ByteSizeLong(); - google::protobuf::uint8 *end = google::protobuf::io::CodedOutputStream::WriteVarint32ToArray(byteCount, varIntBuffer); - unsigned long varIntBytes = (unsigned long) (end - varIntBuffer); + uint32_t byteCount = (uint32_t)message->ByteSizeLong(); + google::protobuf::uint8* end = + google::protobuf::io::CodedOutputStream::WriteVarint32ToArray(byteCount, varIntBuffer); + unsigned long varIntBytes = (unsigned long)(end - varIntBuffer); if (this->saveFile) { - this->outputStream->write(reinterpret_cast (varIntBuffer), (int) varIntBytes); + this->outputStream->write(reinterpret_cast(varIntBuffer), (int)varIntBytes); } /*! Enter in lock-step with the vizard to simulate a camera */ - /*!--OpNavMode set to 1 is to stay in lock-step with the viz at all time steps. It is a slower run, but provides visual capabilities during OpNav */ - /*!--OpNavMode set to 2 is a faster mode in which the viz only steps forward to the BSK time step if an image is requested. This is a faster run but nothing can be visualized post-run */ + /*!--OpNavMode set to 1 is to stay in lock-step with the viz at all time steps. It is a slower run, but provides + * visual capabilities during OpNav */ + /*!--OpNavMode set to 2 is a faster mode in which the viz only steps forward to the BSK time step if an image is + * requested. This is a faster run but nothing can be visualized post-run */ bool opNavModeStatus = false; if (this->opNavMode == 2) { for (size_t camCounter = 0; camCounter < this->cameraConfInMsgs.size(); camCounter++) { - if ((currentSimNanos%this->cameraConfigBuffers[camCounter].renderRate == 0 && this->cameraConfigBuffers[camCounter].isOn == 1) ||this->firstPass < 11) { + if ((currentSimNanos % this->cameraConfigBuffers[camCounter].renderRate == 0 && + this->cameraConfigBuffers[camCounter].isOn == 1) || + this->firstPass < 11) { opNavModeStatus = true; } } } - if (this->opNavMode == 1 - ||(this->opNavMode == 2 && opNavModeStatus) - || this->liveStream - ){ + if (this->opNavMode == 1 || (this->opNavMode == 2 && opNavModeStatus) || this->liveStream) { // Receive pong - /*! - The viz needs 10 images before placing the planets, wait for 11 protobuffers to have been created before attempting to go into opNavMode 2 */ - if (this->firstPass < 11){ + /*! - The viz needs 10 images before placing the planets, wait for 11 protobuffers to have been created + * before attempting to go into opNavMode 2 */ + if (this->firstPass < 11) { this->firstPass++; } zmq_msg_t receive_buffer; zmq_msg_init(&receive_buffer); - zmq_msg_recv (&receive_buffer, requester_socket, 0); + zmq_msg_recv(&receive_buffer, requester_socket, 0); /*! - send protobuffer raw over zmq_socket */ void* serialized_message = malloc(byteCount); - message->SerializeToArray(serialized_message, (int) byteCount); + message->SerializeToArray(serialized_message, (int)byteCount); /*! - Normal sim step by sending protobuffers */ zmq_msg_t request_header; @@ -1067,20 +1108,17 @@ void VizInterface::WriteProtobuffer(uint64_t currentSimNanos) zmq_msg_init_data(&request_header, header_message, 10, message_buffer_deallocate, NULL); zmq_msg_init(&empty_frame1); zmq_msg_init(&empty_frame2); - zmq_msg_init_data(&request_buffer, serialized_message,byteCount, message_buffer_deallocate, NULL); + zmq_msg_init_data(&request_buffer, serialized_message, byteCount, message_buffer_deallocate, NULL); zmq_msg_send(&request_header, this->requester_socket, ZMQ_SNDMORE); zmq_msg_send(&empty_frame1, this->requester_socket, ZMQ_SNDMORE); zmq_msg_send(&empty_frame2, this->requester_socket, ZMQ_SNDMORE); zmq_msg_send(&request_buffer, this->requester_socket, 0); - - for (size_t camCounter =0; camCountercameraConfInMsgs.size(); camCounter++) { + for (size_t camCounter = 0; camCounter < this->cameraConfInMsgs.size(); camCounter++) { /*! - If the camera is requesting periodic images, request them */ - if (this->opNavMode > 0 && - currentSimNanos%this->cameraConfigBuffers[camCounter].renderRate == 0 && - this->cameraConfigBuffers[camCounter].isOn == 1) - { + if (this->opNavMode > 0 && currentSimNanos % this->cameraConfigBuffers[camCounter].renderRate == 0 && + this->cameraConfigBuffers[camCounter].isOn == 1) { this->requestImage(camCounter, currentSimNanos); } } @@ -1093,38 +1131,32 @@ void VizInterface::WriteProtobuffer(uint64_t currentSimNanos) zmq_msg_send(&request_life, this->requester_socket, 0); return; } - } /*! Write protobuffer to file */ - if (!this->saveFile || !message->SerializeToOstream(this->outputStream)) { + if (!this->saveFile || !message->SerializeToOstream(this->outputStream)) { return; } } delete message; google::protobuf::ShutdownProtobufLibrary(); - } /*! Update this module at the task rate @param currentSimNanos The current sim time */ -void VizInterface::updateState(uint64_t currentSimNanos) -{ - - this->FrameNumber+=1; +void VizInterface::updateState(uint64_t currentSimNanos) { + this->FrameNumber += 1; ReadBSKMessages(); - if(currentSimNanos > 0) { + if (currentSimNanos > 0) { WriteProtobuffer(currentSimNanos); } - } /*! Method to add a Vizard instrument camera module to vizInterface @param tmpMsg Camera configuration msg */ -void VizInterface::addCamMsgToModule(Message *tmpMsg) -{ +void VizInterface::addCamMsgToModule(Message* tmpMsg) { /* add the message reader to the vector of input messages */ this->cameraConfInMsgs.push_back(tmpMsg->addSubscriber()); @@ -1139,20 +1171,18 @@ void VizInterface::addCamMsgToModule(Message *tmpMsg) this->cameraConfigBuffers.push_back(tmpCamConfigMsg); /* create output message */ - Message *msg; + Message* msg; msg = new Message; this->opnavImageOutMsgs.push_back(msg); /* create image pointer */ - void *imgPtr = NULL; + void* imgPtr = NULL; this->bskImagePtrs.push_back(imgPtr); } - /*! Requests an image from Vizard and stores it in the image output message */ -void VizInterface::requestImage(size_t camCounter, uint64_t currentSimNanos) -{ +void VizInterface::requestImage(size_t camCounter, uint64_t currentSimNanos) { char buffer[10]; zmq_recv(this->requester_socket, buffer, 10, 0); /*! -- Send request */ @@ -1176,18 +1206,18 @@ void VizInterface::requestImage(size_t camCounter, uint64_t currentSimNanos) zmq_msg_recv(&length, this->requester_socket, 0); zmq_msg_recv(&image, this->requester_socket, 0); - int32_t *lengthPoint= (int32_t *)zmq_msg_data(&length); - void *imagePoint= zmq_msg_data(&image); + int32_t* lengthPoint = (int32_t*)zmq_msg_data(&length); + void* imagePoint = zmq_msg_data(&image); int32_t length_unswapped = *lengthPoint; /*! -- Endianness switch for the length of the buffer */ - int32_t imageBufferLength =((length_unswapped>>24)&0xff) | // move byte 3 to byte 0 - ((length_unswapped<<8)&0xff0000) | // move byte 1 to byte 2 - ((length_unswapped>>8)&0xff00) | // move byte 2 to byte 1 - ((length_unswapped<<24)&0xff000000); // byte 0 to byte 3 + int32_t imageBufferLength = ((length_unswapped >> 24) & 0xff) | // move byte 3 to byte 0 + ((length_unswapped << 8) & 0xff0000) | // move byte 1 to byte 2 + ((length_unswapped >> 8) & 0xff00) | // move byte 2 to byte 1 + ((length_unswapped << 24) & 0xff000000); // byte 0 to byte 3 /*!-Copy the image buffer pointer, so that it does not get freed by ZMQ*/ - this->bskImagePtrs[camCounter] = malloc(imageBufferLength*sizeof(char)); - memcpy(this->bskImagePtrs[camCounter], imagePoint, imageBufferLength*sizeof(char)); + this->bskImagePtrs[camCounter] = malloc(imageBufferLength * sizeof(char)); + memcpy(this->bskImagePtrs[camCounter], imagePoint, imageBufferLength * sizeof(char)); /*! -- Write out the image information to the Image message */ CameraImageMsgPayload imageData = {}; @@ -1197,7 +1227,9 @@ void VizInterface::requestImage(size_t camCounter, uint64_t currentSimNanos) imageData.imageBufferLength = imageBufferLength; imageData.cameraID = this->cameraConfigBuffers[camCounter].cameraID; imageData.imageType = 4; - if (imageBufferLength>0){imageData.valid = 1;} + if (imageBufferLength > 0) { + imageData.valid = 1; + } this->opnavImageOutMsgs[camCounter]->write(&imageData, this->moduleID, currentSimNanos); /*! -- Clean the messages to avoid memory leaks */ @@ -1205,13 +1237,8 @@ void VizInterface::requestImage(size_t camCounter, uint64_t currentSimNanos) zmq_msg_close(&image); } - - /*! A cleaning method to ensure the message buffers are wiped clean. @param data The current sim time in nanoseconds @param hint */ -void message_buffer_deallocate(void *data, void *hint) -{ - free (data); -} +void message_buffer_deallocate(void* data, void* hint) { free(data); } From e16ac4923290266563a155023b9488d1745887d8 Mon Sep 17 00:00:00 2001 From: patrick kenneally Date: Fri, 20 Jun 2025 19:07:25 -0600 Subject: [PATCH 24/24] Refactor to use uint32_t for loop indexs --- .../spacecraftReconfig/spacecraftReconfig.cpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/src/fswAlgorithms/formationFlying/spacecraftReconfig/spacecraftReconfig.cpp b/src/fswAlgorithms/formationFlying/spacecraftReconfig/spacecraftReconfig.cpp index ca826e83a..fbc738990 100644 --- a/src/fswAlgorithms/formationFlying/spacecraftReconfig/spacecraftReconfig.cpp +++ b/src/fswAlgorithms/formationFlying/spacecraftReconfig/spacecraftReconfig.cpp @@ -170,8 +170,7 @@ void SpacecraftReconfig::UpdateManeuver(NavTransMsgPayload chiefTransMsgBuffer, this->thrustOnFlag = 1; // thrustOnFlag is ON this->burnArrayInfoOutMsgBuffer.burnArray[0].flag = 2; // first burn is regarded as finished by setting this to 2 - int i = 0; - for (i = 0; i < thrustConfigMsgBuffer.numThrusters; ++i) { + for (uint32_t i = 0; i < thrustConfigMsgBuffer.numThrusters; ++i) { thrustOnMsgBuffer->OnTimeRequest[i] = this->burnArrayInfoOutMsgBuffer.burnArray[0].thrustOnTime / thrustConfigMsgBuffer.numThrusters; } @@ -199,8 +198,7 @@ void SpacecraftReconfig::UpdateManeuver(NavTransMsgPayload chiefTransMsgBuffer, this->burnArrayInfoOutMsgBuffer.burnArray[1].flag == 1) { this->thrustOnFlag = 1; this->burnArrayInfoOutMsgBuffer.burnArray[1].flag = 2; - int i = 0; - for (i = 0; i < thrustConfigMsgBuffer.numThrusters; ++i) { + for (uint32_t i = 0; i < thrustConfigMsgBuffer.numThrusters; ++i) { thrustOnMsgBuffer->OnTimeRequest[i] = this->burnArrayInfoOutMsgBuffer.burnArray[1].thrustOnTime / thrustConfigMsgBuffer.numThrusters; } @@ -234,8 +232,7 @@ void SpacecraftReconfig::UpdateManeuver(NavTransMsgPayload chiefTransMsgBuffer, this->burnArrayInfoOutMsgBuffer.burnArray[2].flag == 1) { this->thrustOnFlag = 1; this->burnArrayInfoOutMsgBuffer.burnArray[2].flag = 2; - int i = 0; - for (i = 0; i < thrustConfigMsgBuffer.numThrusters; ++i) { + for (uint32_t i = 0; i < thrustConfigMsgBuffer.numThrusters; ++i) { thrustOnMsgBuffer->OnTimeRequest[i] = this->burnArrayInfoOutMsgBuffer.burnArray[2].thrustOnTime / thrustConfigMsgBuffer.numThrusters; }