diff --git a/src/simulation/dynamics/_GeneralModuleFiles/_UnitTestDynamics/test_effectorBranching_integrated.py b/src/simulation/dynamics/_GeneralModuleFiles/_UnitTestDynamics/test_effectorBranching_integrated.py index 80e459c2cb9..15d5a5d5fa1 100644 --- a/src/simulation/dynamics/_GeneralModuleFiles/_UnitTestDynamics/test_effectorBranching_integrated.py +++ b/src/simulation/dynamics/_GeneralModuleFiles/_UnitTestDynamics/test_effectorBranching_integrated.py @@ -32,6 +32,10 @@ path = os.path.dirname(os.path.abspath(filename)) splitPath = path.split('simulation') +from Basilisk import __path__ +bskPath = __path__[0] + + from Basilisk.utilities import SimulationBaseClass, macros, simIncludeThruster from Basilisk.utilities import RigidBodyKinematics as rbk from Basilisk.architecture.bskLogging import BasiliskError @@ -60,11 +64,15 @@ thrusterDynamicEffector, constraintDynamicEffector, dragDynamicEffector, + facetDragDynamicEffector, radiationPressure, facetSRPDynamicEffector, MtbEffector, ) from Basilisk.architecture import messaging +from Basilisk.simulation import tabularAtmosphere +from Basilisk.utilities.readAtmTable import readAtmTable +from Basilisk.simulation import simpleNav # uncomment this line if this test is to be skipped in the global unit test run, adjust message as needed # @pytest.mark.skipif(conditionstring) @@ -97,6 +105,7 @@ ("constraintEffectorOneHub", True), ("constraintEffectorNoHubs", True), # ("dragEffector", False), + ("facetDragDynamicEffector", True), # ("radiationPressure", False), # ("facetSRPDynamicEffector", False), # ("MtbEffector", False), @@ -272,6 +281,28 @@ def effectorBranchingIntegratedTest(show_plots, stateEffector, isParent, dynamic dynamicEff = setup_extPulseTorque() elif dynamicEffector == "thrusterDynamicEffector": dynamicEff, thFactory = setup_thrusterDynamicEffector() + elif dynamicEffector == "facetDragDynamicEffector": + dynamicEff = setup_facetDragDynamicEffector(stateEffProps) + + tabAtmo = tabularAtmosphere.TabularAtmosphere() + tabAtmo.ModelTag = "tabularAtmosphere" + + tabAtmo.addSpacecraftToModel(scObject.scStateOutMsg) + + r_eq = 6378136.6 + dataFileName = bskPath + '/supportData/AtmosphereData/EarthGRAMNominal.txt' + altList, rhoList, tempList = readAtmTable(dataFileName, 'EarthGRAM') + + # assign constants & ref. data to module + tabAtmo.planetRadius = r_eq + tabAtmo.altList = tabularAtmosphere.DoubleVector(altList) + tabAtmo.rhoList = tabularAtmosphere.DoubleVector(rhoList) + tabAtmo.tempList = tabularAtmosphere.DoubleVector(tempList) + + dynamicEff.atmoDensInMsg.subscribeTo(tabAtmo.envOutMsgs[0]) + + unitTestSim.AddModelToTask(unitTaskName, tabAtmo) + elif dynamicEffector == "constraintEffectorOneHub": dynamicEff, scObjectx = setup_constraintEffectorOneHub(scObject, stateEffProps) unitTestSim.AddModelToTask("unitTask", scObjectx) @@ -533,6 +564,34 @@ def setup_thrusterDynamicEffector(): return(thruster, thFactory) +def setup_facetDragDynamicEffector(stateEffProps): + facetDrag = facetDragDynamicEffector.FacetDragDynamicEffector() + facetDrag.ModelTag = "facetDragDynamicEffector" + + facetDrag.setPropName_inertialPosition(stateEffProps.nameOfInertialPositionProperty) + facetDrag.setPropName_inertialVelocity(stateEffProps.nameOfInertialVelocityProperty) + facetDrag.setPropName_inertialAttitude(stateEffProps.nameOfInertialAttitudeProperty) + + panelArea = 10 + panelOffset = 0.3 # Distance from hub COM + panelCd = 5 + + panel_normals = [ + np.array([0, 0, 1]), + np.array([0, 0, -1]) + ] + + # Centered on panel + panel_locations = [ + np.array([0.0, 0.0, panelOffset]), + np.array([0.0, 0.0, panelOffset]) + ] + + for i in range(2): + facetDrag.addFacet(panelArea, panelCd, panel_normals[i], panel_locations[i]) + + return(facetDrag) + def setup_constraintEffector(scObject1): scObject2 = spacecraft.Spacecraft() scObject2.ModelTag = "spacecraftBody2" @@ -800,6 +859,9 @@ def setup_hingedRigidBodyStateEffector(): stateEffProps.r_PB_B = hingedBody.r_HB_B stateEffProps.r_PcP_P = hingedBody.d * s1_hat stateEffProps.inertialPropLogName = "hingedRigidBodyConfigLogOutMsg" + stateEffProps.nameOfInertialPositionProperty = hingedBody.ModelTag + "InertialPosition1" + stateEffProps.nameOfInertialVelocityProperty = hingedBody.ModelTag + "InertialVelocity1" + stateEffProps.nameOfInertialAttitudeProperty = hingedBody.ModelTag + "InertialAttitude1" return(hingedBody, stateEffProps) @@ -867,4 +929,4 @@ class stateEffectorProperties: r_PcP_P = [[0.0], [0.0], [0.0]] # individual COM for linkage that dynEff will be attached to if __name__ == "__main__": - effectorBranchingIntegratedTest(True, "hingedRigidBodies", True, "extForceTorque", True) + effectorBranchingIntegratedTest(True, "hingedRigidBodies", True, "facetDragDynamicEffector", True) diff --git a/src/simulation/dynamics/facetDragEffector/facetDragDynamicEffector.cpp b/src/simulation/dynamics/facetDragEffector/facetDragDynamicEffector.cpp index 8125d7fba48..60a03660ce4 100644 --- a/src/simulation/dynamics/facetDragEffector/facetDragDynamicEffector.cpp +++ b/src/simulation/dynamics/facetDragEffector/facetDragDynamicEffector.cpp @@ -31,7 +31,24 @@ FacetDragDynamicEffector::FacetDragDynamicEffector() this->atmoInData = {}; // Initialize atmosphere data to zero this->windInData = {}; // Initialize wind data to zero this->numFacets = 0; - return; + + // Initialize state pointers + this->hubSigma = nullptr; + this->hubVelocity = nullptr; + + // Initialize property pointers + this->inertialPositionProperty = nullptr; + this->inertialVelocityProperty = nullptr; + this->inertialAttitudeProperty = nullptr; + + // Set default state names for hub attachment + this->stateNameOfSigma = "hubSigma"; + this->stateNameOfVelocity = "hubVelocity"; + + /* This effector can be attached onto a state effector */ + isAttachableToStateEffector = true; + + return; } /*! The destructor.*/ @@ -106,13 +123,76 @@ void FacetDragDynamicEffector::linkInStates(DynParamManager& states){ this->hubVelocity = states.getStateObject(this->stateNameOfVelocity); } +/*! This method is used to link the dragEffector to the state effectors attitude, position and velocity, +which are required for calculating drag forces and torques. +@param properties The parameter manager to collect from +*/ +void FacetDragDynamicEffector::linkInProperties(DynParamManager& properties) +{ + + // updateDragDir only requires these two to do calculations (Keep linking position in case you want to add altitude-dependent drag coefficients or other position-based effects later) + this->inertialAttitudeProperty = properties.getPropertyReference(this->propName_inertialAttitude); + this->inertialVelocityProperty = properties.getPropertyReference(this->propName_inertialVelocity); +} + +/*! This method is used to set the inertialPosition property when facet drag is attached to a state effector rather than the hub + */ +void FacetDragDynamicEffector::setPropName_inertialPosition(std::string value) { + if (!value.empty()) { + this->propName_inertialPosition = value; + } else { + bskLogger.bskLog(BSK_ERROR, "FacetDragDynamicEffector: propName_inertialPosition variable must be a non-empty string"); + } +} + +/*! This method is used to set the inertialVelocity property when facet drag is attached to a state effector rather than the hub + */ +void FacetDragDynamicEffector::setPropName_inertialVelocity(std::string value) { + if (!value.empty()) { + this->propName_inertialVelocity = value; + } else { + bskLogger.bskLog(BSK_ERROR, "FacetDragDynamicEffector: propName_inertialVelocity variable must be a non-empty string"); + } +} + +/*! This method is used to set the inertialAttitude property when facet drag is attached to a state effector rather than the hub + */ +void FacetDragDynamicEffector::setPropName_inertialAttitude(std::string value) { + if (!value.empty()) { + this->propName_inertialAttitude = value; + } else { + bskLogger.bskLog(BSK_ERROR, "FacetDragDynamicEffector: propName_inertialAttitude variable must be a non-empty string"); + } +} + +/*! This method is used to set the inertialAngVelocity property when facet drag is attached to a state effector rather than the hub + */ +void FacetDragDynamicEffector::setPropName_inertialAngVelocity(std::string value) { + if (!value.empty()) { + this->propName_inertialAngVelocity = value; + } else { + bskLogger.bskLog(BSK_ERROR, "FacetDragDynamicEffector: propName_inertialAngVelocity variable must be a non-empty string"); + } +} + /*! This method updates the internal drag direction based on the spacecraft velocity vector. * It accounts for wind velocity if the wind message is linked. */ void FacetDragDynamicEffector::updateDragDir(){ - Eigen::MRPd sigmaBN; - sigmaBN = (Eigen::Vector3d)this->hubSigma->getState(); - Eigen::Matrix3d dcm_BN = sigmaBN.toRotationMatrix().transpose(); + Eigen::MRPd sigmaBN; + Eigen::Vector3d velocity; + // Determine which parent to use based on what's been linked + if (this->inertialAttitudeProperty != nullptr && this->inertialVelocityProperty != nullptr) { + // Attached to state effector: use properties + sigmaBN = (Eigen::Vector3d)(*this->inertialAttitudeProperty); + velocity = (*this->inertialVelocityProperty); + } else { + // Attached to hub: use states + sigmaBN = (Eigen::Vector3d)this->hubSigma->getState(); + velocity = this->hubVelocity->getState(); + } + + Eigen::Matrix3d dcm_BN = sigmaBN.toRotationMatrix().transpose(); Eigen::Vector3d v_BN_N = this->hubVelocity->getState(); @@ -131,7 +211,7 @@ void FacetDragDynamicEffector::updateDragDir(){ this->v_hat_B.setZero(); } - return; + return; } /*! This method WILL implement a more complex flat-plate aerodynamics model with attitude diff --git a/src/simulation/dynamics/facetDragEffector/facetDragDynamicEffector.h b/src/simulation/dynamics/facetDragEffector/facetDragDynamicEffector.h index f4fdd50db7c..524515b0310 100644 --- a/src/simulation/dynamics/facetDragEffector/facetDragDynamicEffector.h +++ b/src/simulation/dynamics/facetDragEffector/facetDragDynamicEffector.h @@ -55,6 +55,7 @@ class FacetDragDynamicEffector: public SysModel, public DynamicEffector { FacetDragDynamicEffector(); ~FacetDragDynamicEffector(); void linkInStates(DynParamManager& states); + void linkInProperties(DynParamManager& properties); // To allow dynamic effector to attach to state effectors void computeForceTorque(double integTime, double timeStep); void Reset(uint64_t CurrentSimNanos); //!< class method void UpdateState(uint64_t CurrentSimNanos); @@ -77,11 +78,36 @@ class FacetDragDynamicEffector: public SysModel, public DynamicEffector { Eigen::Vector3d v_hat_B; //!< -- Drag force direction in the body frame BSKLogger bskLogger; //!< -- BSK Logging + // State names (for hub attachment) + std::string stateNameOfSigma; + std::string stateNameOfVelocity; + + // Property names (for state effector attachment) + std::string propName_inertialPosition; + std::string propName_inertialVelocity; + std::string propName_inertialAttitude; + std::string propName_inertialAngVelocity; + + void setPropName_inertialPosition(std::string value) override; + void setPropName_inertialVelocity(std::string value) override; + void setPropName_inertialAttitude(std::string value) override; + void setPropName_inertialAngVelocity(std::string value) override; + + std::string getPropName_inertialPosition() const { return this->propName_inertialPosition; } + std::string getPropName_inertialVelocity() const { return this->propName_inertialVelocity; } + std::string getPropName_inertialAttitude() const { return this->propName_inertialAttitude; } + std::string getPropName_inertialAngVelocity() const { return this->propName_inertialAngVelocity; } + private: AtmoPropsMsgPayload atmoInData; WindMsgPayload windInData; SpacecraftGeometryData scGeometry; //!< -- Struct to hold spacecraft facet data + // Property pointers (for state effector attachment) + Eigen::MatrixXd *inertialPositionProperty; //!< [m] position relative to inertial frame + Eigen::MatrixXd *inertialVelocityProperty; //!< [m/s] velocity relative to inertial frame + Eigen::MatrixXd *inertialAttitudeProperty; //!< attitude relative to inertial frame + Eigen::MatrixXd *inertialAngVelocity; //!< attitude relative to inertial frame }; #endif