From 25c9bf1a34d7655af6768e708f6dcc00632558bb Mon Sep 17 00:00:00 2001 From: Carlo Cena Date: Thu, 19 Mar 2026 04:00:47 +0100 Subject: [PATCH 1/4] [#1325] Using windMsg in dragDynEff --- .../1317-atmo_rel_vel_dragDynEff.rst | 1 - .../dragEffector/_UnitTest/test_atmoDrag.py | 358 +++++++++--------- .../dragEffector/dragDynamicEffector.cpp | 92 ++--- .../dragEffector/dragDynamicEffector.h | 17 +- .../dragEffector/dragDynamicEffector.i | 2 + .../dragEffector/dragDynamicEffector.rst | 41 +- 6 files changed, 237 insertions(+), 274 deletions(-) delete mode 100644 docs/source/Support/bskReleaseNotesSnippets/1317-atmo_rel_vel_dragDynEff.rst diff --git a/docs/source/Support/bskReleaseNotesSnippets/1317-atmo_rel_vel_dragDynEff.rst b/docs/source/Support/bskReleaseNotesSnippets/1317-atmo_rel_vel_dragDynEff.rst deleted file mode 100644 index fc3925b1e54..00000000000 --- a/docs/source/Support/bskReleaseNotesSnippets/1317-atmo_rel_vel_dragDynEff.rst +++ /dev/null @@ -1 +0,0 @@ -- Added optional configurable atmosphere-relative velocity computation to ``dragDynamicEffector`` for improved drag modeling in rotating planetary atmosphere. diff --git a/src/simulation/dynamics/dragEffector/_UnitTest/test_atmoDrag.py b/src/simulation/dynamics/dragEffector/_UnitTest/test_atmoDrag.py index 13b195fbbe0..422af4bef64 100644 --- a/src/simulation/dynamics/dragEffector/_UnitTest/test_atmoDrag.py +++ b/src/simulation/dynamics/dragEffector/_UnitTest/test_atmoDrag.py @@ -17,7 +17,7 @@ # # Basilisk Scenario Script and Integrated Test # -# Purpose: Test the validity of a simple exponential atmosphere model. +# Purpose: Test the drag dynamic effector, including inertial and atmosphere-relative velocity modes. # Author: Andrew Harris # Creation Date: Jan 18, 2017 # @@ -25,15 +25,16 @@ import inspect import math import os -import pytest import matplotlib.pyplot as plt import numpy as np # print dir(exponentialAtmosphere) +from Basilisk.architecture import messaging from Basilisk.simulation import dragDynamicEffector from Basilisk.simulation import exponentialAtmosphere, simpleNav # import simulation related support from Basilisk.simulation import spacecraft +from Basilisk.simulation import zeroWindModel # import general simulation support files from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros @@ -58,9 +59,9 @@ def test_scenarioDragOrbit(): """This function is called by the py.test environment.""" # each test method requires a single assert method to be called earthCase = "Earth" - marsCase = "Mars" + #marsCase = "Mars" orb1 = "LPO" - orb2 = "LTO" + #orb2 = "LTO" showVal = False testResults = [] testMessage = [] @@ -94,23 +95,9 @@ def cannonballDragComp(dragCoeff, dens, area, vel, att): return dragForce -def cannonballDragCompRel(dragCoeff, dens, area, vel, pos, att, planetOmega): - vAtmo = np.cross(planetOmega, pos) - vRel = vel - vAtmo - vMag = np.linalg.norm(vRel) - if vMag <= 1e-12: - return np.zeros(3), vRel, np.zeros(3) - - dragDir_N = -vRel / vMag - dcm_BN = RigidBodyKinematics.MRP2C(att) - dragDir_B = dcm_BN.dot(dragDir_N) - dragForce = 0.5 * dragCoeff * dens * area * vMag**2.0 * dragDir_B - return dragForce, vRel, dragDir_B - - -def setup_basic_drag_sim(useAtmosphereRelativeVelocity=False, planetOmega=None, - rN=None, vN=None, sigmaBN=None, density=1.0e-12, - dragCoeff=2.2, projArea=10.0): +def setup_basic_drag_sim(rN=None, vN=None, sigmaBN=None, + dragCoeff=2.2, # [-] + projArea=10.0): # [m^2] simTaskName = "simTask" simProcessName = "simProcess" @@ -126,16 +113,13 @@ def setup_basic_drag_sim(useAtmosphereRelativeVelocity=False, planetOmega=None, dragEff.ModelTag = "DragEff" dragEff.coreParams.projectedArea = projArea dragEff.coreParams.dragCoeff = dragCoeff - dragEff.coreParams.comOffset = [1., 0., 0.] - dragEff.setUseAtmosphereRelativeVelocity(useAtmosphereRelativeVelocity) - if planetOmega is not None: - dragEff.setPlanetOmega_N(planetOmega) + dragEff.coreParams.comOffset = [1., 0., 0.] # [m] newAtmo = exponentialAtmosphere.ExponentialAtmosphere() newAtmo.ModelTag = "ExpAtmo" - newAtmo.baseDensity = 1.217 - newAtmo.scaleHeight = 8500.0 - newAtmo.planetRadius = 6371.0 * 1000.0 + newAtmo.baseDensity = 1.217 # [kg/m^3] + newAtmo.scaleHeight = 8500.0 # [m] + newAtmo.planetRadius = 6371.0 * 1000.0 # [m] newAtmo.addSpacecraftToModel(scObject.scStateOutMsg) scObject.addDynamicEffector(dragEff) @@ -147,15 +131,15 @@ def setup_basic_drag_sim(useAtmosphereRelativeVelocity=False, planetOmega=None, dragEff.atmoDensInMsg.subscribeTo(newAtmo.envOutMsgs[0]) if rN is None: - rN = np.array([7000e3, 0.0, 0.0]) + rN = np.array([7000e3, 0.0, 0.0]) # [m] if vN is None: - vN = np.array([0.0, 7500.0, 0.0]) + vN = np.array([0.0, 7500.0, 0.0]) # [m/s] if sigmaBN is None: - sigmaBN = np.array([0.0, 0.0, 0.0]) + sigmaBN = np.array([0.0, 0.0, 0.0]) # [-] MRP - scObject.hub.r_CN_NInit = rN - scObject.hub.v_CN_NInit = vN - scObject.hub.sigma_BNInit = sigmaBN + scObject.hub.r_CN_NInit = rN # [m] + scObject.hub.v_CN_NInit = vN # [m/s] + scObject.hub.sigma_BNInit = sigmaBN # [-] MRP dataLog = scObject.scStateOutMsg.recorder() atmoLog = newAtmo.envOutMsgs[0].recorder() @@ -167,25 +151,20 @@ def setup_basic_drag_sim(useAtmosphereRelativeVelocity=False, planetOmega=None, return scSim, scObject, dragEff, dataLog, atmoLog, dragLog -def test_dragAtmosphereRelativeVelocityDisabled(): - """Verify nominal drag behavior is unchanged when useAtmosphereRelativeVelocity is False.""" - rN = np.array([7000e3, 0.0, 0.0]) - vN = np.array([0.0, 7600.0, 0.0]) - sigmaBN = np.array([0.0, 0.0, 0.0]) +def test_dragInertialVelocity(): + """Verify drag uses inertial velocity when no wind message is linked.""" + rN = np.array([7000e3, 0.0, 0.0]) # [m] + vN = np.array([0.0, 7600.0, 0.0]) # [m/s] + sigmaBN = np.array([0.0, 0.0, 0.0]) # [-] MRP scSim, _, dragEff, dataLog, atmoLog, dragLog = setup_basic_drag_sim( - useAtmosphereRelativeVelocity=False, - planetOmega=np.array([0.0, 0.0, 7.2921159e-5]), - rN=rN, - vN=vN, - sigmaBN=sigmaBN + rN=rN, vN=vN, sigmaBN=sigmaBN ) scSim.InitializeSimulation() scSim.ConfigureStopTime(macros.sec2nano(1.0)) scSim.ExecuteSimulation() - posData = dataLog.r_BN_N velData = dataLog.v_BN_N attData = dataLog.sigma_BN densData = atmoLog.neutralDensity @@ -203,107 +182,6 @@ def test_dragAtmosphereRelativeVelocityDisabled(): assert unitTestSupport.isArrayEqual(dragForce[-1, :], refForce, 3, accuracy) -def test_dragAtmosphereRelativeVelocityEnabled(): - """Verify drag uses v_rel = v_sc - omega x r when enabled.""" - rN = np.array([7000e3, 0.0, 0.0]) - vN = np.array([0.0, 7600.0, 0.0]) - sigmaBN = np.array([0.0, 0.0, 0.0]) - planetOmega = np.array([0.0, 0.0, 7.2921159e-5]) - - scSim, _, dragEff, dataLog, atmoLog, dragLog = setup_basic_drag_sim( - useAtmosphereRelativeVelocity=True, - planetOmega=planetOmega, - rN=rN, - vN=vN, - sigmaBN=sigmaBN - ) - - scSim.InitializeSimulation() - scSim.ConfigureStopTime(macros.sec2nano(1.0)) - scSim.ExecuteSimulation() - - posData = dataLog.r_BN_N - velData = dataLog.v_BN_N - attData = dataLog.sigma_BN - densData = atmoLog.neutralDensity - dragForce = dragLog.forceExternal_B - - refForce, _, _ = cannonballDragCompRel( - dragEff.coreParams.dragCoeff, - densData[-1], - dragEff.coreParams.projectedArea, - velData[-1], - posData[-1], - attData[-1], - planetOmega - ) - - accuracy = 1e-13 - assert unitTestSupport.isArrayEqual(dragForce[-1, :], refForce, 3, accuracy) - - -def test_dragAtmosphereRelativeVelocityPositionStateOptional(): - """Verify position state is only required when atmosphere-relative velocity is enabled.""" - - scSim, _, _, _, _, _ = setup_basic_drag_sim( - useAtmosphereRelativeVelocity=False - ) - scSim.InitializeSimulation() - scSim.ConfigureStopTime(macros.sec2nano(1.0)) - scSim.ExecuteSimulation() - - assert True - - -def test_dragAtmosphereRelativeVelocityNearZero(): - """Verify near-zero relative velocity does not produce invalid normalization behavior.""" - rN = np.array([7000e3, 0.0, 0.0]) - planetOmega = np.array([0.0, 0.0, 7.2921159e-5]) - vAtmo = np.cross(planetOmega, rN) - vN = vAtmo + np.array([1e-15, -1e-15, 1e-15]) - sigmaBN = np.array([0.0, 0.0, 0.0]) - - scSim, _, _, _, _, dragLog = setup_basic_drag_sim( - useAtmosphereRelativeVelocity=True, - planetOmega=planetOmega, - rN=rN, - vN=vN, - sigmaBN=sigmaBN - ) - - scSim.InitializeSimulation() - scSim.ConfigureStopTime(macros.sec2nano(1.0)) - scSim.ExecuteSimulation() - - dragForce = dragLog.forceExternal_B[-1, :] - - assert np.all(np.isfinite(dragForce)) - assert np.linalg.norm(dragForce) < 1e-20 - - -def test_dragAtmosphereRelativeVelocityResetGuardPosition(): - """Verify Reset emits BSK_ERROR when relative velocity is enabled without a valid position state.""" - simTaskName = "simTask" - simProcessName = "simProcess" - - scSim = SimulationBaseClass.SimBaseClass() - dynProcess = scSim.CreateNewProcess(simProcessName) - dynProcess.addTask(scSim.CreateNewTask(simTaskName, macros.sec2nano(1.0))) - - dragEff = dragDynamicEffector.DragDynamicEffector() - dragEff.ModelTag = "DragEff" - dragEff.setUseAtmosphereRelativeVelocity(True) - dragEff.coreParams.projectedArea = 10.0 - dragEff.coreParams.dragCoeff = 2.2 - - # no spacecraft attached here on purpose, so hub position is never linked - - scSim.AddModelToTask(simTaskName, dragEff) - - with pytest.raises(Exception): - scSim.InitializeSimulation() - - def run(show_plots, orbitCase, planetCase): """Call this routine directly to run the tutorial scenario.""" testFailCount = 0 # zero unit test result counter @@ -331,8 +209,8 @@ def run(show_plots, orbitCase, planetCase): atmoTaskName = "atmosphere" newAtmo.ModelTag = "ExpAtmo" - projArea = 10.0 # Set drag area in m^2 - dragCoeff = 2.2 # Set drag ceofficient + projArea = 10.0 # [m^2] + dragCoeff = 2.2 # [-] dragEffector = dragDynamicEffector.DragDynamicEffector() dragEffector.ModelTag = "DragEff" @@ -340,7 +218,7 @@ def run(show_plots, orbitCase, planetCase): dragEffectorTaskName = "drag" dragEffector.coreParams.projectedArea = projArea dragEffector.coreParams.dragCoeff = dragCoeff - dragEffector.coreParams.comOffset = [1., 0., 0.] + dragEffector.coreParams.comOffset = [1., 0., 0.] # [m] dynProcess.addTask(scSim.CreateNewTask(atmoTaskName, simulationTimeStep)) dynProcess.addTask(scSim.CreateNewTask(dragEffectorTaskName, simulationTimeStep)) @@ -374,7 +252,7 @@ def run(show_plots, orbitCase, planetCase): elif planetCase == "Mars": planet = gravFactory.createMars() planet.isCentralBody = True # ensure this is the central gravitational body - mu = planet.mu + mu = planet.mu # [m^3/s^2] # attach gravity model to spacecraft scObject.gravField.gravBodies = spacecraft.GravBodyVector(list(gravFactory.gravBodies.values())) @@ -383,44 +261,44 @@ def run(show_plots, orbitCase, planetCase): oe = orbitalMotion.ClassicElements() if planetCase == "Earth": - r_eq = 6371*1000.0 - refBaseDens = 1.217 - refScaleHeight = 8500.0 + r_eq = 6371*1000.0 # [m] + refBaseDens = 1.217 # [kg/m^3] + refScaleHeight = 8500.0 # [m] elif planetCase == "Mars": - refBaseDens = 0.020 - refScaleHeight = 11100.0 - r_eq = 3389.5 * 1000.0 + refBaseDens = 0.020 # [kg/m^3] + refScaleHeight = 11100.0 # [m] + r_eq = 3389.5 * 1000.0 # [m] else: return 1, "Test failed- did not initialize planets." if orbitCase == "LPO": - orbAltMin = 300.0*1000.0 + orbAltMin = 300.0*1000.0 # [m] orbAltMax = orbAltMin elif orbitCase == "LTO": - orbAltMin = 300*1000.0 - orbAltMax = 800.0 * 1000.0 + orbAltMin = 300*1000.0 # [m] + orbAltMax = 800.0 * 1000.0 # [m] newAtmo.planetRadius = r_eq newAtmo.scaleHeight = refScaleHeight newAtmo.baseDensity = refBaseDens - rMin = r_eq + orbAltMin - rMax = r_eq + orbAltMax - oe.a = (rMin+rMax)/2.0 - oe.e = 1.0 - rMin/oe.a - oe.i = 0.0*macros.D2R + rMin = r_eq + orbAltMin # [m] + rMax = r_eq + orbAltMax # [m] + oe.a = (rMin+rMax)/2.0 # [m] + oe.e = 1.0 - rMin/oe.a # [-] + oe.i = 0.0*macros.D2R # [rad] - oe.Omega = 0.0*macros.D2R - oe.omega = 0.0*macros.D2R - oe.f = 0.0*macros.D2R + oe.Omega = 0.0*macros.D2R # [rad] + oe.omega = 0.0*macros.D2R # [rad] + oe.f = 0.0*macros.D2R # [rad] rN, vN = orbitalMotion.elem2rv(mu, oe) oe = orbitalMotion.rv2elem(mu, rN, vN) # this stores consistent initial orbit elements # with circular or equatorial orbit, some angles are # arbitrary # set the simulation time - n = np.sqrt(mu/oe.a/oe.a/oe.a) - P = 2.*np.pi/n + n = np.sqrt(mu/oe.a/oe.a/oe.a) # [rad/s] + P = 2.*np.pi/n # [s] simulationTime = macros.sec2nano(1*P) @@ -441,8 +319,8 @@ def run(show_plots, orbitCase, planetCase): # # initialize Spacecraft States with initialization variables # - scObject.hub.r_CN_NInit = rN # m - r_CN_N - scObject.hub.v_CN_NInit = vN # m - v_CN_N + scObject.hub.r_CN_NInit = rN # [m] + scObject.hub.v_CN_NInit = vN # [m/s] # # initialize Simulation @@ -470,7 +348,7 @@ def run(show_plots, orbitCase, planetCase): endInd = dragForce.shape[0] refDragForce = np.zeros([endInd,3]) - refDensData = np.zeros([endInd,1]) + #refDensData = np.zeros([endInd,1]) accuracy = 1e-13 # print planetCase # print orbitCase @@ -541,8 +419,8 @@ def run(show_plots, orbitCase, planetCase): ,'--' , color='#555555' ) - plt.xlabel('$i_e$ Cord. [km]') - plt.ylabel('$i_p$ Cord. [km]') + plt.xlabel('$i_e$ Coord. [km]') + plt.ylabel('$i_p$ Coord. [km]') plt.grid() plt.figure() @@ -579,6 +457,138 @@ def run(show_plots, orbitCase, planetCase): return testFailCount, testMessages +def test_drag_wind_velocity_automatic_usage(): + """Verify drag initializes and runs without error when no wind message is linked.""" + # Create a simple simulation + scSim = SimulationBaseClass.SimBaseClass() + dynProcess = scSim.CreateNewProcess("dynamics") + simulationTimeStep = macros.sec2nano(1.0) + dynProcess.addTask(scSim.CreateNewTask("dynamicsTask", simulationTimeStep)) + + # Create spacecraft + scObject = spacecraft.Spacecraft() + scObject.ModelTag = "spacecraft" + + # Create atmosphere model + atmoModel = exponentialAtmosphere.ExponentialAtmosphere() + atmoModel.ModelTag = "atmosphere" + atmoModel.baseDensity = 1.217 # [kg/m^3] + atmoModel.scaleHeight = 8500.0 # [m] + atmoModel.planetRadius = 6371e3 # [m] + + # Add spacecraft to atmosphere model + atmoModel.addSpacecraftToModel(scObject.scStateOutMsg) + + # Create drag effector + dragEff = dragDynamicEffector.DragDynamicEffector() + dragEff.ModelTag = "drag" + dragEff.coreParams.projectedArea = 1.0 # [m^2] + dragEff.coreParams.dragCoeff = 2.2 # [-] + + # Link atmosphere + dragEff.atmoDensInMsg.subscribeTo(atmoModel.envOutMsgs[0]) + + # Add to spacecraft + scObject.addDynamicEffector(dragEff) + + # Setup basic orbit + rN = np.array([7000e3, 0.0, 0.0]) # [m] + vN = np.array([0.0, 7600.0, 0.0]) # [m/s] + sigmaBN = np.array([0.0, 0.0, 0.0]) # [-] MRP + + scObject.hub.r_CN_NInit = rN # [m] + scObject.hub.v_CN_NInit = vN # [m/s] + scObject.hub.sigma_BNInit = sigmaBN # [-] MRP + + # Add models to simulation + scSim.AddModelToTask("dynamicsTask", scObject) + scSim.AddModelToTask("dynamicsTask", atmoModel) + scSim.AddModelToTask("dynamicsTask", dragEff) + + # InitializeSimulation calls Reset internally; should not raise without wind message + scSim.InitializeSimulation() + + +def test_drag_wind_velocity_with_wind_message(): + """Verify drag uses atmosphere-relative velocity when wind message is linked. + + Two simulation steps are needed: step 1 populates windInData via UpdateState/ReadInputs; + step 2's computeForceTorque uses that cached wind velocity. + The force at step 2 is compared against cannonballDragComp using step-1 cached + density/wind and step-2 velocity/attitude. + """ + scSim = SimulationBaseClass.SimBaseClass() + dynProcess = scSim.CreateNewProcess("dynamics") + dynProcess.addTask(scSim.CreateNewTask("dynamicsTask", macros.sec2nano(1.0))) + + scObject = spacecraft.Spacecraft() + scObject.ModelTag = "spacecraft" + + atmoModel = exponentialAtmosphere.ExponentialAtmosphere() + atmoModel.ModelTag = "atmosphere" + atmoModel.baseDensity = 1.217 # [kg/m^3] + atmoModel.scaleHeight = 8500.0 # [m] + atmoModel.planetRadius = 6371e3 # [m] + atmoModel.addSpacecraftToModel(scObject.scStateOutMsg) + + windModel = zeroWindModel.ZeroWindModel() + windModel.ModelTag = "wind" + windModel.addSpacecraftToModel(scObject.scStateOutMsg) + planetStateMsg = messaging.SpicePlanetStateMsg().write(messaging.SpicePlanetStateMsgPayload()) + windModel.planetPosInMsg.subscribeTo(planetStateMsg) + omega_earth = np.array([0.0, 0.0, orbitalMotion.OMEGA_EARTH]) # [rad/s] + windModel.setPlanetOmega_N(omega_earth) + + dragEff = dragDynamicEffector.DragDynamicEffector() + dragEff.ModelTag = "drag" + dragEff.coreParams.projectedArea = 1.0 # [m^2] + dragEff.coreParams.dragCoeff = 2.2 # [-] + dragEff.atmoDensInMsg.subscribeTo(atmoModel.envOutMsgs[0]) + dragEff.windVelInMsg.subscribeTo(windModel.envOutMsgs[0]) + scObject.addDynamicEffector(dragEff) + + scObject.hub.r_CN_NInit = np.array([7000e3, 0.0, 0.0]) # [m] + scObject.hub.v_CN_NInit = np.array([0.0, 7600.0, 0.0]) # [m/s] + scObject.hub.sigma_BNInit = np.array([0.0, 0.0, 0.0]) # [-] MRP + + scSim.AddModelToTask("dynamicsTask", scObject) + scSim.AddModelToTask("dynamicsTask", atmoModel) + scSim.AddModelToTask("dynamicsTask", windModel) + scSim.AddModelToTask("dynamicsTask", dragEff) + + scSim.InitializeSimulation() + + windLog = windModel.envOutMsgs[0].recorder() + scStateLog = scObject.scStateOutMsg.recorder() + atmoLog = atmoModel.envOutMsgs[0].recorder() + dragLog = dragEff.logger("forceExternal_B") + scSim.AddModelToTask("dynamicsTask", windLog) + scSim.AddModelToTask("dynamicsTask", scStateLog) + scSim.AddModelToTask("dynamicsTask", atmoLog) + scSim.AddModelToTask("dynamicsTask", dragLog) + + # 2 steps: step 1 loads wind/atmo into cache; step 2 uses cache in computeForceTorque + scSim.ConfigureStopTime(macros.sec2nano(2.0)) + scSim.ExecuteSimulation() + + # Force at step 2 uses cached step-1 wind/density with step-2 velocity/attitude + dens_step1 = atmoLog.neutralDensity[0] + wind_step1 = np.array(windLog.v_air_N[0]) + v_step2 = np.array(scStateLog.v_BN_N[-1]) + sigma_step2 = np.array(scStateLog.sigma_BN[-1]) + drag_force = np.array(dragLog.forceExternal_B[-1]) + + refForce = cannonballDragComp( + dragEff.coreParams.dragCoeff, + dens_step1, + dragEff.coreParams.projectedArea, + v_step2 - wind_step1, + sigma_step2 + ) + np.testing.assert_allclose(drag_force, refForce, atol=1e-10) + + # close the plots being saved off to avoid over-writing old and new figures + if __name__ == '__main__': run(True,"LPO","Earth") diff --git a/src/simulation/dynamics/dragEffector/dragDynamicEffector.cpp b/src/simulation/dynamics/dragEffector/dragDynamicEffector.cpp index 57c6da25be4..6bf33c9182d 100644 --- a/src/simulation/dynamics/dragEffector/dragDynamicEffector.cpp +++ b/src/simulation/dynamics/dragEffector/dragDynamicEffector.cpp @@ -17,10 +17,7 @@ */ -#include #include "dragDynamicEffector.h" -#include "architecture/utilities/linearAlgebra.h" -#include "architecture/utilities/astroConstants.h" DragDynamicEffector::DragDynamicEffector() { @@ -32,8 +29,8 @@ DragDynamicEffector::DragDynamicEffector() this->torqueExternalPntB_B.fill(0.0); this->v_B.fill(0.0); this->v_hat_B.fill(0.0); - this->useAtmosphereRelativeVelocity = false; - this->planetOmega_N << 0.0, 0.0, OMEGA_EARTH; + this->atmoInData = {}; // Initialize atmosphere data to zero + this->windInData = {}; // Initialize wind data to zero return; } @@ -54,11 +51,6 @@ void DragDynamicEffector::Reset(uint64_t CurrentSimNanos) if (!this->atmoDensInMsg.isLinked()) { bskLogger.bskLog(BSK_ERROR, "dragDynamicEffector.atmoDensInMsg was not linked."); } - if (this->useAtmosphereRelativeVelocity && this->hubPosition == nullptr) { - bskLogger.bskLog(BSK_ERROR, - "dragDynamicEffector requires hub position state when useAtmosphereRelativeVelocity is enabled."); - } - } /*! The DragEffector does not write output messages to the rest of the sim. @@ -70,15 +62,22 @@ void DragDynamicEffector::WriteOutputMessages(uint64_t CurrentClock) } -/*! This method is used to read the incoming density message and update the internal density/ -atmospheric data. +/*! This method is used to read the incoming density message and wind velocity message (if linked) +and update the internal density/atmospheric data. */ bool DragDynamicEffector::ReadInputs() { bool dataGood; - this->atmoInData = this->atmoDensInMsg(); - dataGood = this->atmoDensInMsg.isWritten(); + dataGood = this->atmoDensInMsg.isWritten(); + if (dataGood) { + this->atmoInData = this->atmoDensInMsg(); + } + if (this->windVelInMsg.isLinked() && this->windVelInMsg.isWritten()) { + this->windInData = this->windVelInMsg(); + } else { + this->windInData = {}; + } return(dataGood); } @@ -92,10 +91,6 @@ void DragDynamicEffector::linkInStates(DynParamManager& states){ this->hubSigma = states.getStateObject(this->stateNameOfSigma); this->hubVelocity = states.getStateObject(this->stateNameOfVelocity); - if (this->useAtmosphereRelativeVelocity) { - this->hubPosition = states.getStateObject(this->stateNameOfPosition); - } - if (this->densityCorrectionStateName.empty()) this->densityCorrection = nullptr; else @@ -103,24 +98,25 @@ void DragDynamicEffector::linkInStates(DynParamManager& states){ } /*! 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 DragDynamicEffector::updateDragDir(){ Eigen::MRPd sigmaBN; sigmaBN = (Eigen::Vector3d)this->hubSigma->getState(); Eigen::Matrix3d dcm_BN = sigmaBN.toRotationMatrix().transpose(); - Eigen::Vector3d vRel_N = this->hubVelocity->getState(); // Initializing vRel_N = v_N + Eigen::Vector3d v_BN_N = this->hubVelocity->getState(); - if (this->useAtmosphereRelativeVelocity && this->hubPosition != nullptr) { - Eigen::Vector3d r_N = this->hubPosition->getState(); - Eigen::Vector3d vAtmo_N = this->planetOmega_N.cross(r_N); - vRel_N -= vAtmo_N; + if (this->windVelInMsg.isLinked()) { + Eigen::Map v_air_N(this->windInData.v_air_N); + Eigen::Vector3d v_B_air_N = v_BN_N - v_air_N; + this->v_B = dcm_BN * v_B_air_N; + } else { + this->v_B = dcm_BN * v_BN_N; } - this->v_B = dcm_BN * vRel_N; - double vNorm = this->v_B.norm(); - if (vNorm > 1e-12) { + if (vNorm > 1e-12) { // [m/s] this->v_hat_B = this->v_B / vNorm; } else { this->v_hat_B.setZero(); @@ -142,7 +138,7 @@ double DragDynamicEffector::getDensity() return density; } -/*! This method implements a simple "cannnonball" (attitude-independent) drag model. +/*! This method implements a simple "cannonball" (attitude-independent) drag model. */ void DragDynamicEffector::cannonballDrag(){ //! Begin method steps @@ -151,7 +147,7 @@ void DragDynamicEffector::cannonballDrag(){ this->torqueExternalPntB_B.setZero(); double vMag = this->v_B.norm(); - if (vMag <= 1e-12) { + if (vMag <= 1e-12) { // [m/s] return; } @@ -182,43 +178,3 @@ void DragDynamicEffector::UpdateState(uint64_t CurrentSimNanos) ReadInputs(); return; } - -/*! - * @brief Enables or disables the use of atmosphere-relative velocity for drag computation. - * When enabled, the drag force is computed using v_rel = v_sc - (omega_planet x r_sc). - * Requires hub position state to be available. - * @param useRelVel true to use atmosphere-relative velocity, false to use inertial velocity - */ -void DragDynamicEffector::setUseAtmosphereRelativeVelocity(bool useRelVel) -{ - this->useAtmosphereRelativeVelocity = useRelVel; -} - -/*! - * @brief Returns whether atmosphere-relative velocity is used in drag computation. - * @return true if atmosphere-relative velocity is enabled - */ -bool DragDynamicEffector::getUseAtmosphereRelativeVelocity() const -{ - return this->useAtmosphereRelativeVelocity; -} - -/*! - * @brief Sets the planetary rotation vector expressed in the inertial frame. - * Used to compute the atmosphere velocity when useAtmosphereRelativeVelocity is enabled. - * For Earth, the default is taken from OMEGA_EARTH in astroConstants.h: [0, 0, 7.2921159e-5] rad/s. - * @param omega Planetary rotation vector [rad/s] in inertial frame N - */ -void DragDynamicEffector::setPlanetOmega_N(const Eigen::Vector3d& omega) -{ - this->planetOmega_N = omega; -} - -/*! - * @brief Returns the planetary rotation vector expressed in the inertial frame. - * @return omega_planet [rad/s] in inertial frame N - */ -Eigen::Vector3d DragDynamicEffector::getPlanetOmega_N() const -{ - return this->planetOmega_N; -} diff --git a/src/simulation/dynamics/dragEffector/dragDynamicEffector.h b/src/simulation/dynamics/dragEffector/dragDynamicEffector.h index 8a02bc0e4c3..19049bd3d2a 100644 --- a/src/simulation/dynamics/dragEffector/dragDynamicEffector.h +++ b/src/simulation/dynamics/dragEffector/dragDynamicEffector.h @@ -28,12 +28,12 @@ #include "architecture/_GeneralModuleFiles/sys_model.h" #include "architecture/msgPayloadDefC/AtmoPropsMsgPayload.h" +#include "architecture/msgPayloadDefC/WindMsgPayload.h" #include "architecture/messaging/messaging.h" #include "architecture/utilities/avsEigenMRP.h" #include "architecture/utilities/avsEigenSupport.h" #include "architecture/utilities/bskLogging.h" -#include "architecture/utilities/astroConstants.h" @@ -60,30 +60,25 @@ class DragDynamicEffector: public SysModel, public DynamicEffector { void cannonballDrag(); void updateDragDir(); double getDensity(); - void setUseAtmosphereRelativeVelocity(bool useRelVel); - bool getUseAtmosphereRelativeVelocity() const; - void setPlanetOmega_N(const Eigen::Vector3d& omega); - Eigen::Vector3d getPlanetOmega_N() const; public: DragBaseData coreParams; //!< -- Struct used to hold drag parameters ReadFunctor atmoDensInMsg; //!< -- message used to read density inputs + ReadFunctor windVelInMsg; //!< -- message used to read wind velocity inputs std::string modelType; //!< -- String used to set the type of model used to compute drag StateData *hubSigma; //!< -- Hub/Inertial attitude represented by MRP StateData *hubVelocity; //!< m/s Hub inertial velocity vector std::string densityCorrectionStateName = ""; //!< -- If not '', finds a state with this name to get ``densityCorrection`` StateData *densityCorrection; //!< -- Used density is ``(1 + densityCorrection) * atmoInData.neutralDensity`` - Eigen::Vector3d v_B; //!< m/s local variable to hold the inertial velocity - Eigen::Vector3d v_hat_B; //!< -- Drag force direction in the inertial frame + Eigen::Vector3d v_B; //!< m/s spacecraft velocity expressed in body frame B (relative to air if windVelInMsg is linked, else relative to inertial frame N) + Eigen::Vector3d v_hat_B; //!< -- Drag force direction in the body frame BSKLogger bskLogger; //!< -- BSK Logging private: AtmoPropsMsgPayload atmoInData; - StateData* hubPosition = nullptr; - bool useAtmosphereRelativeVelocity; - Eigen::Vector3d planetOmega_N; + WindMsgPayload windInData; }; -#endif /* THRUSTER_DYNAMIC_EFFECTOR_H */ +#endif /* DRAG_DYNAMIC_EFFECTOR_H */ diff --git a/src/simulation/dynamics/dragEffector/dragDynamicEffector.i b/src/simulation/dynamics/dragEffector/dragDynamicEffector.i index 522131762ad..8a2d46dd446 100644 --- a/src/simulation/dynamics/dragEffector/dragDynamicEffector.i +++ b/src/simulation/dynamics/dragEffector/dragDynamicEffector.i @@ -43,6 +43,8 @@ from Basilisk.architecture.swig_common_model import * %include "architecture/msgPayloadDefC/AtmoPropsMsgPayload.h" struct AtmoPropsMsg_C; +%include "architecture/msgPayloadDefC/WindMsgPayload.h" +struct WindMsg_C; %pythoncode %{ import sys diff --git a/src/simulation/dynamics/dragEffector/dragDynamicEffector.rst b/src/simulation/dynamics/dragEffector/dragDynamicEffector.rst index 368ffdf5aa5..d319a86d751 100644 --- a/src/simulation/dynamics/dragEffector/dragDynamicEffector.rst +++ b/src/simulation/dynamics/dragEffector/dragDynamicEffector.rst @@ -36,43 +36,40 @@ Example setup: scObject.addDynamicEffector(drag) -Atmosphere Relative Velocity ------------------------------ -The drag computation can optionally use the spacecraft velocity relative to a rotating atmosphere instead of the inertial spacecraft velocity. This is useful for modeling atmospheric drag in Low Earth Orbit, where the atmosphere approximately co-rotates with the Earth. - -If ``useAtmosphereRelativeVelocity`` is enabled, the drag model computes +Wind Velocity Input +-------------------- +When ``windVelInMsg`` is linked to a :ref:`windBase`-derived module (e.g. :ref:`zeroWindModel`), +the drag model subtracts the air velocity ``v_air_N`` from the spacecraft inertial velocity before +computing drag, yielding the atmosphere-relative velocity: .. math:: - \mathbf{v}_{rel} = \mathbf{v}_{sc} - (\boldsymbol{\omega}_{planet} \times \mathbf{r}_{sc}) - -where - -- :math:`\mathbf{v}_{sc}` is the spacecraft inertial velocity -- :math:`\mathbf{r}_{sc}` is the spacecraft inertial position -- :math:`\boldsymbol{\omega}_{planet}` is the planetary rotation vector + \mathbf{v}_{rel} = \mathbf{v}_{sc} - \mathbf{v}_{air} -For Earth simulations, the planetary rotation rate is automatically taken from ``OMEGA_EARTH`` in ``astroConstants.h``. -For other bodies it can be configured through ``planetOmega_N``. - -If ``useAtmosphereRelativeVelocity`` is left disabled (default), the drag model uses the spacecraft inertial velocity directly. +If ``windVelInMsg`` is not linked, the inertial spacecraft velocity is used directly. Example setup: .. code-block:: python drag = dragDynamicEffector.DragDynamicEffector() - drag.coreParams.dragCoeff = 2.2 drag.coreParams.projectedArea = 10.0 drag.atmoDensInMsg.subscribeTo(atmo.envOutMsgs[0]) - # Enable atmosphere-relative velocity - drag.setUseAtmosphereRelativeVelocity(True) - drag.setPlanetOmega_N([0.0, 0.0, 7.2921159e-5]) + # Optional: link a wind model for atmosphere-relative velocity + drag.windVelInMsg.subscribeTo(windModel.envOutMsgs[0]) scObject.addDynamicEffector(drag) +Input Message Timing +--------------------- +Both ``atmoDensInMsg`` and ``windVelInMsg`` are refreshed only during ``UpdateState()``. +Because ``Spacecraft::computeForceTorque()`` calls dynamic effectors before the atmosphere +and wind models receive their next ``UpdateState()``, the drag computation always uses +the values from the previous time step (zero-initialized on the first step). +Both inputs are therefore treated as **piecewise-constant** over each integration step. + Message Connection Descriptions ------------------------------- The following table lists all the module input and output messages. The module msg connection is set by the @@ -89,3 +86,7 @@ provides information on what this message is used for. * - atmoDensInMsg - :ref:`AtmoPropsMsgPayload` - atmospheric density input message + * - windVelInMsg + - :ref:`WindMsgPayload` + - (optional) wind velocity input message; when linked, ``v_air_N`` is subtracted from the + spacecraft inertial velocity to obtain the atmosphere-relative velocity From 6b1357f3a07dd0da70996b5187f2c2383af97123 Mon Sep 17 00:00:00 2001 From: Carlo Cena Date: Thu, 19 Mar 2026 04:01:02 +0100 Subject: [PATCH 2/4] [#1325] Using windMsg in facetDragDynEff --- .../_UnitTest/test_unitFacetDrag.py | 203 ++++++++++++++++-- .../facetDragDynamicEffector.cpp | 48 ++++- .../facetDragDynamicEffector.h | 10 +- .../facetDragDynamicEffector.i | 2 + .../facetDragDynamicEffector.rst | 36 +++- 5 files changed, 277 insertions(+), 22 deletions(-) diff --git a/src/simulation/dynamics/facetDragEffector/_UnitTest/test_unitFacetDrag.py b/src/simulation/dynamics/facetDragEffector/_UnitTest/test_unitFacetDrag.py index 90408c142dc..a6e1979d6a2 100644 --- a/src/simulation/dynamics/facetDragEffector/_UnitTest/test_unitFacetDrag.py +++ b/src/simulation/dynamics/facetDragEffector/_UnitTest/test_unitFacetDrag.py @@ -45,10 +45,12 @@ from Basilisk.utilities import RigidBodyKinematics as rbk # import simulation related support +from Basilisk.architecture import messaging from Basilisk.simulation import spacecraft from Basilisk.simulation import exponentialAtmosphere from Basilisk.simulation import facetDragDynamicEffector from Basilisk.simulation import simpleNav +from Basilisk.simulation import zeroWindModel from Basilisk.utilities import unitTestSupport from Basilisk.utilities import simIncludeGravBody @@ -107,18 +109,18 @@ def test_DragCalculation(scAreas, scCoeff, B_normals, B_locations): # setup orbit and simulation time oe = orbitalMotion.ClassicElements() - r_eq = 6371*1000.0 - refBaseDens = 1.217 - refScaleHeight = 8500.0 + r_eq = 6371*1000.0 # [m] + refBaseDens = 1.217 # [kg/m^3] + refScaleHeight = 8500.0 # [m] # Set base density, equitorial radius, scale height in Atmosphere newAtmo.baseDensity = refBaseDens newAtmo.scaleHeight = refScaleHeight newAtmo.planetRadius = r_eq - rN = np.array([r_eq+200.0e3,0,0]) - vN = np.array([0,7.788e3,0]) - sig_BN = np.array([0,0,0]) + rN = np.array([r_eq+200.0e3,0,0]) # [m] + vN = np.array([0,7.788e3,0]) # [m/s] + sig_BN = np.array([0,0,0]) # [-] MRP # initialize Spacecraft States with the initialization variables scObject.hub.r_CN_NInit = rN # m - r_CN_N scObject.hub.v_CN_NInit = vN # m - v_CN_N @@ -242,18 +244,18 @@ def test_ShadowCalculation(scAreas, scCoeff, B_normals, B_locations): # setup orbit and simulation time oe = orbitalMotion.ClassicElements() - r_eq = 6371*1000.0 - refBaseDens = 1.217 - refScaleHeight = 8500.0 + r_eq = 6371*1000.0 # [m] + refBaseDens = 1.217 # [kg/m^3] + refScaleHeight = 8500.0 # [m] # Set base density, equitorial radius, scale height in Atmosphere newAtmo.baseDensity = refBaseDens newAtmo.scaleHeight = refScaleHeight newAtmo.planetRadius = r_eq - rN = np.array([r_eq+200.0e3,0,0]) - vN = np.array([0,7.788e3,0]) - sig_BN = np.array([0,0,0]) + rN = np.array([r_eq+200.0e3,0,0]) # [m] + vN = np.array([0,7.788e3,0]) # [m/s] + sig_BN = np.array([0,0,0]) # [-] MRP # initialize Spacecraft States with the initialization variables scObject.hub.r_CN_NInit = rN # m - r_CN_N @@ -303,6 +305,183 @@ def test_ShadowCalculation(scAreas, scCoeff, B_normals, B_locations): np.testing.assert_allclose(dragDataForce_B[ind,1:4], [0, 0, 0], atol = 1e-11) np.testing.assert_allclose(dragTorqueData[ind,1:4], [0, 0, 0], atol = 1e-11) +def facetDragComp(dens, areas, coeffs, normals, vel, att): + """Reference facet drag force in body frame using inertial velocity.""" + dcm = rbk.MRP2C(att) + vMag = np.linalg.norm(vel) + v_hat_B = dcm.dot(vel) / vMag + force = np.zeros(3) + for area, coeff, normal in zip(areas, coeffs, normals): + proj = normal.dot(v_hat_B) * area + if proj > 0: + force += -0.5 * dens * proj * coeff * vMag**2 * v_hat_B + return force + + +def setup_basic_facet_drag_sim(rN=None, vN=None, sigmaBN=None, + areas=None, coeffs=None, normals=None, locations=None): + simTaskName = "simTask" + simProcessName = "simProcess" + + scSim = SimulationBaseClass.SimBaseClass() + dynProcess = scSim.CreateNewProcess(simProcessName) + simulationTimeStep = macros.sec2nano(1.0) + dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) + + scObject = spacecraft.Spacecraft() + scObject.ModelTag = "spacecraftBody" + + newAtmo = exponentialAtmosphere.ExponentialAtmosphere() + newAtmo.ModelTag = "ExpAtmo" + newAtmo.baseDensity = 1.217 # [kg/m^3] + newAtmo.scaleHeight = 8500.0 # [m] + newAtmo.planetRadius = 6371.0 * 1000.0 # [m] + newAtmo.addSpacecraftToModel(scObject.scStateOutMsg) + + newDrag = facetDragDynamicEffector.FacetDragDynamicEffector() + newDrag.ModelTag = "FacetDrag" + newDrag.atmoDensInMsg.subscribeTo(newAtmo.envOutMsgs[0]) + + if areas is None: + areas = [1.0] + coeffs = [2.2] + normals = [np.array([1, 0, 0])] + locations = [np.array([0, 0, 0])] + for area, coeff, normal, loc in zip(areas, coeffs, normals, locations): + newDrag.addFacet(area, coeff, normal, loc) + + scObject.addDynamicEffector(newDrag) + + scSim.AddModelToTask(simTaskName, scObject) + scSim.AddModelToTask(simTaskName, newAtmo) + scSim.AddModelToTask(simTaskName, newDrag) + + if rN is None: + rN = np.array([7000e3, 0.0, 0.0]) # [m] + if vN is None: + vN = np.array([0.0, 7500.0, 0.0]) # [m/s] + if sigmaBN is None: + sigmaBN = np.array([0.0, 0.0, 0.0]) # [-] MRP + + scObject.hub.r_CN_NInit = rN + scObject.hub.v_CN_NInit = vN + scObject.hub.sigma_BNInit = sigmaBN + + dataLog = scObject.scStateOutMsg.recorder() + atmoLog = newAtmo.envOutMsgs[0].recorder() + dragLog = newDrag.logger("forceExternal_B") + scSim.AddModelToTask(simTaskName, dataLog) + scSim.AddModelToTask(simTaskName, atmoLog) + scSim.AddModelToTask(simTaskName, dragLog) + + return scSim, scObject, newDrag, dataLog, atmoLog, dragLog, areas, coeffs, normals + + +def test_facetDragInertialVelocity(): + """Verify drag uses inertial velocity when no wind message is linked. + + Velocity has a component along the default facet normal [1, 0, 0] to ensure + non-zero projected area and a non-trivial reference force. + """ + rN = np.array([7000e3, 0.0, 0.0]) # [m] + vN = np.array([3000.0, 7000.0, 0.0]) # [m/s] + sigmaBN = np.array([0.0, 0.0, 0.0]) # [-] MRP + + scSim, _, _, dataLog, atmoLog, dragLog, areas, coeffs, normals = setup_basic_facet_drag_sim( + rN=rN, vN=vN, sigmaBN=sigmaBN + ) + + scSim.InitializeSimulation() + scSim.ConfigureStopTime(macros.sec2nano(1.0)) + scSim.ExecuteSimulation() + + velData = dataLog.v_BN_N + attData = dataLog.sigma_BN + densData = atmoLog.neutralDensity + dragForce = dragLog.forceExternal_B + + refForce = facetDragComp(densData[-1], areas, coeffs, normals, velData[-1], attData[-1]) + assert np.allclose(dragForce[-1, :], refForce, atol=1e-13) + + + +def test_facetDragAtmosphereRelativeVelocityWhenWindLinked(): + """Test that drag uses atmosphere-relative velocity when wind message is linked. + + Two simulation steps are needed: step 1 populates windInData via ReadInputs(); + step 2's computeForceTorque uses that cached wind velocity. + The force at step 2 is compared against facetDragComp using step-1 cached + density/wind and step-2 velocity/attitude. + """ + simTaskName = "simTask" + scSim = SimulationBaseClass.SimBaseClass() + scSim.CreateNewProcess("sim").addTask(scSim.CreateNewTask(simTaskName, macros.sec2nano(1.0))) + + scObject = spacecraft.Spacecraft() + scObject.ModelTag = "spacecraftBody" + + newAtmo = exponentialAtmosphere.ExponentialAtmosphere() + newAtmo.ModelTag = "ExpAtmo" + newAtmo.baseDensity = 1.217 # [kg/m^3] + newAtmo.scaleHeight = 8500.0 # [m] + newAtmo.planetRadius = 6371e3 # [m] + newAtmo.addSpacecraftToModel(scObject.scStateOutMsg) + + windMdl = zeroWindModel.ZeroWindModel() + windMdl.ModelTag = "wind" + windMdl.addSpacecraftToModel(scObject.scStateOutMsg) + planetStateMsg = messaging.SpicePlanetStateMsg().write(messaging.SpicePlanetStateMsgPayload()) + windMdl.planetPosInMsg.subscribeTo(planetStateMsg) + omega_earth = np.array([0.0, 0.0, orbitalMotion.OMEGA_EARTH]) # [rad/s] + windMdl.setPlanetOmega_N(omega_earth) + + newDrag = facetDragDynamicEffector.FacetDragDynamicEffector() + newDrag.ModelTag = "FacetDrag" + newDrag.addFacet(1.0, 2.2, np.array([0, 1, 0]), np.array([0, 0, 0.1])) # area [m^2], Cd [-], normal [-], CoP_B [m] + newDrag.atmoDensInMsg.subscribeTo(newAtmo.envOutMsgs[0]) + newDrag.windVelInMsg.subscribeTo(windMdl.envOutMsgs[0]) + scObject.addDynamicEffector(newDrag) + + scObject.hub.r_CN_NInit = np.array([7000e3, 0.0, 0.0]) # [m] + scObject.hub.v_CN_NInit = np.array([0.0, 7600.0, 0.0]) # [m/s] + scObject.hub.sigma_BNInit = np.array([0.0, 0.0, 0.0]) # [-] MRP + + scSim.AddModelToTask(simTaskName, scObject) + scSim.AddModelToTask(simTaskName, newAtmo) + scSim.AddModelToTask(simTaskName, windMdl) + scSim.AddModelToTask(simTaskName, newDrag) + + scSim.InitializeSimulation() + + windLog = windMdl.envOutMsgs[0].recorder() + scLog = scObject.scStateOutMsg.recorder() + atmoLog = newAtmo.envOutMsgs[0].recorder() + dragLog = newDrag.logger("forceExternal_B") + scSim.AddModelToTask(simTaskName, windLog) + scSim.AddModelToTask(simTaskName, scLog) + scSim.AddModelToTask(simTaskName, atmoLog) + scSim.AddModelToTask(simTaskName, dragLog) + + # 2 steps: step 1 loads wind/atmo into cache; step 2 uses cache in computeForceTorque + scSim.ConfigureStopTime(macros.sec2nano(2.0)) + scSim.ExecuteSimulation() + + # Wind velocity at step 2 should match co-rotation at step-2 position + v_air_step2 = np.array(windLog.v_air_N[-1]) + r_step2 = np.array(scLog.r_CN_N[-1]) + np.testing.assert_allclose(v_air_step2, np.cross(omega_earth, r_step2), rtol=1e-10) + + # Force at step 2 uses cached step-1 wind/density with step-2 velocity/attitude + dens_step1 = atmoLog.neutralDensity[0] + wind_step1 = np.array(windLog.v_air_N[0]) + v_step2 = np.array(scLog.v_BN_N[-1]) + sigma_step2 = np.array(scLog.sigma_BN[-1]) + drag_force = np.array(dragLog.forceExternal_B[-1]) + expected_force = facetDragComp(dens_step1, [1.0], [2.2], [np.array([0, 1, 0])], + v_step2 - wind_step1, sigma_step2) + np.testing.assert_allclose(drag_force, expected_force, atol=1e-10) + + if __name__=="__main__": scAreas = [1.0, 1.0] scCoeff = np.array([2.0, 2.0]) diff --git a/src/simulation/dynamics/facetDragEffector/facetDragDynamicEffector.cpp b/src/simulation/dynamics/facetDragEffector/facetDragDynamicEffector.cpp index 588adcd5ebf..8125d7fba48 100644 --- a/src/simulation/dynamics/facetDragEffector/facetDragDynamicEffector.cpp +++ b/src/simulation/dynamics/facetDragEffector/facetDragDynamicEffector.cpp @@ -28,6 +28,8 @@ FacetDragDynamicEffector::FacetDragDynamicEffector() this->torqueExternalPntB_B.fill(0.0); this->v_B.fill(0.0); this->v_hat_B.fill(0.0); + this->atmoInData = {}; // Initialize atmosphere data to zero + this->windInData = {}; // Initialize wind data to zero this->numFacets = 0; return; } @@ -59,15 +61,22 @@ void FacetDragDynamicEffector::WriteOutputMessages(uint64_t CurrentClock) } -/*! This method is used to read the incoming density message and update the internal density/ -atmospheric data. +/*! This method is used to read the incoming density message and wind velocity message (if linked) +and update the internal density/atmospheric data. */ bool FacetDragDynamicEffector::ReadInputs() { bool dataGood; - this->atmoInData = this->atmoDensInMsg(); dataGood = this->atmoDensInMsg.isWritten(); + if (dataGood) { + this->atmoInData = this->atmoDensInMsg(); + } + if (this->windVelInMsg.isLinked() && this->windVelInMsg.isWritten()) { + this->windInData = this->windVelInMsg(); + } else { + this->windInData = {}; + } return(dataGood); } @@ -98,14 +107,29 @@ void FacetDragDynamicEffector::linkInStates(DynParamManager& states){ } /*! 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(); - this->v_B = dcm_BN*this->hubVelocity->getState(); // [m/s] sc velocity - this->v_hat_B = this->v_B / this->v_B.norm(); + Eigen::Vector3d v_BN_N = this->hubVelocity->getState(); + + if (this->windVelInMsg.isLinked()) { + Eigen::Map v_air_N(this->windInData.v_air_N); + Eigen::Vector3d v_B_air_N = v_BN_N - v_air_N; + this->v_B = dcm_BN * v_B_air_N; + } else { + this->v_B = dcm_BN * v_BN_N; + } + + double vNorm = this->v_B.norm(); + if (vNorm > 1e-12) { // [m/s] + this->v_hat_B = this->v_B / vNorm; + } else { + this->v_hat_B.setZero(); + } return; } @@ -125,11 +149,16 @@ void FacetDragDynamicEffector::plateDrag(){ this->forceExternal_B.setZero(); this->torqueExternalPntB_B.setZero(); + double vMag = this->v_B.norm(); + if (vMag <= 1e-12) { // [m/s] + return; + } + for(size_t i = 0; i < this->numFacets; i++){ projectionTerm = this->scGeometry.facetNormals_B[i].dot(this->v_hat_B); projectedArea = this->scGeometry.facetAreas[i] * projectionTerm; if(projectedArea > 0.0){ - facetDragForce = 0.5 * pow(this->v_B.norm(), 2.0) * this->scGeometry.facetCoeffs[i] * projectedArea * this->atmoInData.neutralDensity * (-1.0)*this->v_hat_B; + facetDragForce = 0.5 * pow(vMag, 2.0) * this->scGeometry.facetCoeffs[i] * projectedArea * this->getDensity() * (-1.0)*this->v_hat_B; facetDragTorque = (-1)*facetDragForce.cross(this->scGeometry.facetLocations_B[i]); totalDragForce = totalDragForce + facetDragForce; totalDragTorque = totalDragTorque + facetDragTorque; @@ -161,3 +190,10 @@ void FacetDragDynamicEffector::UpdateState(uint64_t CurrentSimNanos) ReadInputs(); return; } + +/** This method obtains the density from the input data + */ +double FacetDragDynamicEffector::getDensity() +{ + return this->atmoInData.neutralDensity; +} diff --git a/src/simulation/dynamics/facetDragEffector/facetDragDynamicEffector.h b/src/simulation/dynamics/facetDragEffector/facetDragDynamicEffector.h index 2bee2130ec9..f4fdd50db7c 100644 --- a/src/simulation/dynamics/facetDragEffector/facetDragDynamicEffector.h +++ b/src/simulation/dynamics/facetDragEffector/facetDragDynamicEffector.h @@ -28,6 +28,7 @@ #include "architecture/_GeneralModuleFiles/sys_model.h" #include "architecture/msgPayloadDefC/AtmoPropsMsgPayload.h" +#include "architecture/msgPayloadDefC/WindMsgPayload.h" #include "architecture/messaging/messaging.h" #include "architecture/utilities/rigidBodyKinematics.h" @@ -65,19 +66,22 @@ class FacetDragDynamicEffector: public SysModel, public DynamicEffector { void plateDrag(); void updateDragDir(); + double getDensity(); public: uint64_t numFacets; //!< number of facets ReadFunctor atmoDensInMsg; //!< atmospheric density input message + ReadFunctor windVelInMsg; //!< wind velocity input message StateData *hubSigma; //!< -- Hub/Inertial attitude represented by MRP StateData *hubVelocity; //!< m/s Hub inertial velocity vector - Eigen::Vector3d v_B; //!< m/s local variable to hold the inertial velocity - Eigen::Vector3d v_hat_B; //!< class variable + Eigen::Vector3d v_B; //!< m/s spacecraft velocity expressed in body frame B (relative to air if windVelInMsg is linked, else relative to inertial frame N) + Eigen::Vector3d v_hat_B; //!< -- Drag force direction in the body frame BSKLogger bskLogger; //!< -- BSK Logging private: AtmoPropsMsgPayload atmoInData; + WindMsgPayload windInData; SpacecraftGeometryData scGeometry; //!< -- Struct to hold spacecraft facet data }; -#endif +#endif diff --git a/src/simulation/dynamics/facetDragEffector/facetDragDynamicEffector.i b/src/simulation/dynamics/facetDragEffector/facetDragDynamicEffector.i index d7b736e8953..5706e65ec36 100644 --- a/src/simulation/dynamics/facetDragEffector/facetDragDynamicEffector.i +++ b/src/simulation/dynamics/facetDragEffector/facetDragDynamicEffector.i @@ -42,6 +42,8 @@ from Basilisk.architecture.swig_common_model import * %include "architecture/msgPayloadDefC/AtmoPropsMsgPayload.h" struct AtmoPropsMsg_C; +%include "architecture/msgPayloadDefC/WindMsgPayload.h" +struct WindMsg_C; %pythoncode %{ import sys diff --git a/src/simulation/dynamics/facetDragEffector/facetDragDynamicEffector.rst b/src/simulation/dynamics/facetDragEffector/facetDragDynamicEffector.rst index 6121e8af213..c57546a6a85 100644 --- a/src/simulation/dynamics/facetDragEffector/facetDragDynamicEffector.rst +++ b/src/simulation/dynamics/facetDragEffector/facetDragDynamicEffector.rst @@ -10,7 +10,38 @@ drag models. For more information see the :download:`PDF Description `. +Wind Velocity Input +-------------------- +When ``windVelInMsg`` is linked to a :ref:`windBase`-derived module (e.g. :ref:`zeroWindModel`), +the drag model subtracts the air velocity ``v_air_N`` from the spacecraft inertial velocity before +computing drag, yielding the atmosphere-relative velocity: +.. math:: + + \mathbf{v}_{rel} = \mathbf{v}_{sc} - \mathbf{v}_{air} + +If ``windVelInMsg`` is not linked, the inertial spacecraft velocity is used directly. + +Example setup: + +.. code-block:: python + + drag = facetDragDynamicEffector.FacetDragDynamicEffector() + drag.addFacet(10.0, 2.2, [1, 0, 0], [0, 0, 0]) + drag.atmoDensInMsg.subscribeTo(atmo.envOutMsgs[0]) + + # Optional: link a wind model for atmosphere-relative velocity + drag.windVelInMsg.subscribeTo(windModel.envOutMsgs[0]) + + scObject.addDynamicEffector(drag) + +Input Message Timing +--------------------- +Both ``atmoDensInMsg`` and ``windVelInMsg`` are refreshed only during ``UpdateState()``. +Because ``Spacecraft::computeForceTorque()`` calls dynamic effectors before the atmosphere +and wind models receive their next ``UpdateState()``, the drag computation always uses +the values from the previous time step (zero-initialized on the first step). +Both inputs are therefore treated as **piecewise-constant** over each integration step. Message Connection Descriptions ------------------------------- @@ -28,4 +59,7 @@ provides information on what this message is used for. * - atmoDensInMsg - :ref:`AtmoPropsMsgPayload` - input message for atmospheric density information - + * - windVelInMsg + - :ref:`WindMsgPayload` + - (optional) wind velocity input message; when linked, ``v_air_N`` is subtracted from the + spacecraft inertial velocity to obtain the atmosphere-relative velocity From fc4db2a632e226df0a9f2a7e1bb14d3e2f5c95fe Mon Sep 17 00:00:00 2001 From: Carlo Cena Date: Thu, 19 Mar 2026 04:01:35 +0100 Subject: [PATCH 3/4] [#1325] Using windMsg in cannonballDrag --- .../1325-atmo_rel_vel_all_drag.rst | 1 + .../_UnitTest/test_cannonballDrag.py | 114 +++++++++++++----- .../cannonballDrag/cannonballDrag.cpp | 14 ++- .../cannonballDrag/cannonballDrag.h | 6 + .../cannonballDrag/cannonballDrag.i | 2 + .../cannonballDrag/cannonballDrag.rst | 30 ++++- 6 files changed, 134 insertions(+), 33 deletions(-) create mode 100644 docs/source/Support/bskReleaseNotesSnippets/1325-atmo_rel_vel_all_drag.rst diff --git a/docs/source/Support/bskReleaseNotesSnippets/1325-atmo_rel_vel_all_drag.rst b/docs/source/Support/bskReleaseNotesSnippets/1325-atmo_rel_vel_all_drag.rst new file mode 100644 index 00000000000..501275a61fe --- /dev/null +++ b/docs/source/Support/bskReleaseNotesSnippets/1325-atmo_rel_vel_all_drag.rst @@ -0,0 +1 @@ +- ``dragDynamicEffector``, ``facetDragDynamicEffector``, and ``cannonballDrag`` now accept an optional ``windVelInMsg`` (:ref:`WindMsgPayload`) to compute atmosphere-relative drag velocity; supersedes the former ``useAtmosphereRelativeVelocity``/``planetOmega_N`` flags. diff --git a/src/simulation/forceTorque/cannonballDrag/_UnitTest/test_cannonballDrag.py b/src/simulation/forceTorque/cannonballDrag/_UnitTest/test_cannonballDrag.py index b850717c675..ea5aeebd9d5 100644 --- a/src/simulation/forceTorque/cannonballDrag/_UnitTest/test_cannonballDrag.py +++ b/src/simulation/forceTorque/cannonballDrag/_UnitTest/test_cannonballDrag.py @@ -60,19 +60,19 @@ def test_cannonballDrag(): tolerance. """ ... - dt = 1 # s + dt = 1 # [s] scSim = SimulationBaseClass.SimBaseClass() dynProcess = scSim.CreateNewProcess("test") dynProcess.addTask(scSim.CreateNewTask("test", macros.sec2nano(dt))) - density = 2 # kg/m^3 - cd = 1.5 - area = 2.75 # m^2 - r_CP_S = [1, 0, 0] # m + density = 2 # [kg/m^3] + cd = 1.5 # [-] + area = 2.75 # [m^2] + r_CP_S = [1, 0, 0] # [m] - v_SN_N = [0, 0.5, 0] # m/s - sigma_SN = [0.1, 0.2, 0.3] + v_SN_N = [0, 0.5, 0] # [m/s] + sigma_SN = [0.1, 0.2, 0.3] # [-] MRP atmoMsg = messaging.AtmoPropsMsg() atmoMsg.write(messaging.AtmoPropsMsgPayload(neutralDensity=density)) @@ -115,7 +115,7 @@ def test_cannonballDrag(): C_BN = np.array(rbk.MRP2C(sigma_SN)) # maps N components to B=S components v_S = C_BN.dot(v_SN_N) v_mag = np.linalg.norm(v_SN_N) # drag magnitude depends on speed, frame invariant - if v_mag <= 1e-12: + if v_mag <= 1e-12: # [m/s] F_exp_S = np.zeros(3) T_exp_S = np.zeros(3) else: @@ -192,7 +192,7 @@ def cannonballDragBFrame(dragCoeff, density, area, velInertial, sigmaBN): planetData = simIncludeGravBody.BODY_DATA[planetCase] gravitationalParameter = planetData.mu - planetRadius = planetData.radEquator + planetRadius = planetData.radEquator # [m] gravityModel = NBodyGravity.NBodyGravity() gravityModel.ModelTag = "gravity" @@ -204,11 +204,11 @@ def cannonballDragBFrame(dragCoeff, density, area, velInertial, sigmaBN): gravityModel.addGravityTarget(bodyName, mjBody) if planetCase == "earth": - baseDensity = 1.217 - scaleHeight = 8500.0 + baseDensity = 1.217 # [kg/m^3] + scaleHeight = 8500.0 # [m] else: - baseDensity = 0.020 - scaleHeight = 11100.0 + baseDensity = 0.020 # [kg/m^3] + scaleHeight = 11100.0 # [m] atmoModel = exponentialAtmosphere.ExponentialAtmosphere() atmoModel.ModelTag = "ExpAtmo" @@ -220,15 +220,15 @@ def cannonballDragBFrame(dragCoeff, density, area, velInertial, sigmaBN): comStateMsg = mjBody.getCenterOfMass().stateOutMsg atmoModel.addSpacecraftToModel(comStateMsg) - projectedArea = 10.0 - dragCoeff = 2.2 + projectedArea = 10.0 # [m^2] + dragCoeff = 2.2 # [-] dragGeometryMsg = messaging.DragGeometryMsg() dragGeometryMsg.write( messaging.DragGeometryMsgPayload( projectedArea=projectedArea, dragCoeff=dragCoeff, - r_CP_S=[1.0, 0.0, 0.0], + r_CP_S=[1.0, 0.0, 0.0], # [m] ) ) @@ -240,25 +240,25 @@ def cannonballDragBFrame(dragCoeff, density, area, velInertial, sigmaBN): dragModel.dragGeometryInMsg.subscribeTo(dragGeometryMsg) if orbitCase == "LPO": - orbAltMin = 300e3 # m + orbAltMin = 300e3 # [m] orbAltMax = orbAltMin elif orbitCase == "LTO": - orbAltMin = 300e3 # m - orbAltMax = 800e3 # m + orbAltMin = 300e3 # [m] + orbAltMax = 800e3 # [m] rMin = planetRadius + orbAltMin rMax = planetRadius + orbAltMax elements = orbitalMotion.ClassicElements() elements.a = (rMin+rMax)/2.0 - elements.e = 1.0 - rMin/elements.a - elements.i = 0.0 - elements.Omega = 0.0 - elements.omega = 0.0 - elements.f = 0.0 - - meanMotion = np.sqrt(gravitationalParameter / elements.a**3) - orbitPeriod = 2.0 * np.pi / meanMotion + elements.e = 1.0 - rMin/elements.a # [-] + elements.i = 0.0 # [rad] + elements.Omega = 0.0 # [rad] + elements.omega = 0.0 # [rad] + elements.f = 0.0 # [rad] + + meanMotion = np.sqrt(gravitationalParameter / elements.a**3) # [rad/s] + orbitPeriod = 2.0 * np.pi / meanMotion # [s] finalTimeNanos = macros.sec2nano(orbitPeriod) stateRecorder = comStateMsg.recorder() @@ -419,6 +419,66 @@ def cannonballDragBFrame(dragCoeff, density, area, velInertial, sigmaBN): plt.show() + +def test_cannonballDragAtmosphereRelativeVelocityWhenWindLinked(): + """Test that drag uses atmosphere-relative velocity when wind message is linked. + + Uses a static wind message to guarantee isWritten()=True when the drag model runs, + avoiding any task-ordering timing dependency. + """ + dt = 1 # [s] + + scSim = SimulationBaseClass.SimBaseClass() + dynProcess = scSim.CreateNewProcess("test") + dynProcess.addTask(scSim.CreateNewTask("test", macros.sec2nano(dt))) + + density = 1.2 # [kg/m^3] + cd = 2.2 # [-] + area = 1.0 # [m^2] + r_CP_S = [0.0, 0.0, 0.0] # [m] + + v_SN_N = [0.0, 7600.0, 0.0] # [m/s] + sigma_SN = [0.0, 0.0, 0.0] # [-] MRP + v_air_N = [0.0, 510.44, 0.0] # [m/s] known co-rotation wind velocity + + atmoMsg = messaging.AtmoPropsMsg() + atmoMsg.write(messaging.AtmoPropsMsgPayload(neutralDensity=density)) + + dragGeometryMsg = messaging.DragGeometryMsg() + dragGeometryMsg.write(messaging.DragGeometryMsgPayload( + projectedArea=area, dragCoeff=cd, r_CP_S=r_CP_S + )) + + siteFrameMsg = messaging.SCStatesMsg() + siteFrameMsg.write(messaging.SCStatesMsgPayload( + sigma_BN=sigma_SN, v_BN_N=v_SN_N + )) + + windMsg = messaging.WindMsg() + windMsg.write(messaging.WindMsgPayload(v_air_N=v_air_N)) + + drag = cannonballDrag.CannonballDrag() + drag.dragGeometryInMsg.subscribeTo(dragGeometryMsg) + drag.atmoDensInMsg.subscribeTo(atmoMsg) + drag.referenceFrameStateInMsg.subscribeTo(siteFrameMsg) + drag.windVelInMsg.subscribeTo(windMsg) + scSim.AddModelToTask("test", drag) + + scSim.InitializeSimulation() + scSim.ConfigureStopTime(macros.sec2nano(dt)) + scSim.ExecuteSimulation() + + force_S = drag.forceOutMsg.read().force_S + + # Expected: drag uses v_rel = v_SN_N - v_air_N + C_SN = np.array(rbk.MRP2C(sigma_SN)) # identity since sigma=[0,0,0] + v_rel_S = C_SN.dot(np.array(v_SN_N) - np.array(v_air_N)) + v_mag = np.linalg.norm(v_rel_S) + F_exp_S = -0.5 * density * v_mag**2 * cd * area * (v_rel_S / v_mag) + + npt.assert_allclose(force_S, F_exp_S, rtol=0.0, atol=1e-12) + + if __name__ == "__main__": assert couldImportMujoco # test_cannonballDrag() diff --git a/src/simulation/forceTorque/cannonballDrag/cannonballDrag.cpp b/src/simulation/forceTorque/cannonballDrag/cannonballDrag.cpp index 65ad163fe59..231b4a275c5 100644 --- a/src/simulation/forceTorque/cannonballDrag/cannonballDrag.cpp +++ b/src/simulation/forceTorque/cannonballDrag/cannonballDrag.cpp @@ -31,7 +31,7 @@ CannonballDrag::SelfInit() void CannonballDrag::UpdateState(uint64_t CurrentSimNanos) { - const double density = this->atmoDensInMsg().neutralDensity; + double density = this->atmoDensInMsg().neutralDensity; const auto geomPayload = this->dragGeometryInMsg(); const double cd = geomPayload.dragCoeff; @@ -44,8 +44,18 @@ CannonballDrag::UpdateState(uint64_t CurrentSimNanos) sigma_SN = Eigen::Map(siteStatePayload.sigma_BN); const Eigen::Matrix3d dcm_SN = sigma_SN.toRotationMatrix().transpose(); + Eigen::Map v_BN_N(siteStatePayload.v_BN_N); + // inertial velocity of the center of the S frame, expressed in the S frame - const auto v_S = dcm_SN*Eigen::Map(siteStatePayload.v_BN_N); + Eigen::Vector3d v_S; + if (this->windVelInMsg.isLinked()) { + const auto windPayload = this->windVelInMsg(); + Eigen::Map v_air_N(windPayload.v_air_N); + Eigen::Vector3d v_B_air_N = v_BN_N - v_air_N; + v_S = dcm_SN * v_B_air_N; + } else { + v_S = dcm_SN * v_BN_N; + } const auto v = v_S.norm(); const Eigen::Vector3d force_S = -0.5 * cd * area * v * density * v_S; diff --git a/src/simulation/forceTorque/cannonballDrag/cannonballDrag.h b/src/simulation/forceTorque/cannonballDrag/cannonballDrag.h index e6a6f81f3b5..d55653ebc06 100644 --- a/src/simulation/forceTorque/cannonballDrag/cannonballDrag.h +++ b/src/simulation/forceTorque/cannonballDrag/cannonballDrag.h @@ -28,8 +28,10 @@ #include "architecture/msgPayloadDefC/ForceAtSiteMsgPayload.h" #include "architecture/msgPayloadDefC/TorqueAtSiteMsgPayload.h" #include "architecture/msgPayloadDefC/SCStatesMsgPayload.h" +#include "architecture/msgPayloadDefC/WindMsgPayload.h" #include "cMsgCInterface/ForceAtSiteMsg_C.h" #include "cMsgCInterface/TorqueAtSiteMsg_C.h" +#include "simulation/dynamics/_GeneralModuleFiles/stateData.h" /** * @brief Aerodynamic drag model that computes force and torque at a MuJoCo site. @@ -60,6 +62,7 @@ class CannonballDrag : public SysModel /** * @brief Advance the model and publish outputs. * @param CurrentSimNanos Current simulation time in nanoseconds. + * @note If windVelInMsg is linked, wind velocity is used to compute relative velocity. */ void UpdateState(uint64_t CurrentSimNanos) override; @@ -77,6 +80,9 @@ class CannonballDrag : public SysModel /** @brief Atmospheric properties input. */ ReadFunctor atmoDensInMsg; + /** @brief Wind velocity input. */ + ReadFunctor windVelInMsg; + /** @brief State of the reference frame . */ ReadFunctor referenceFrameStateInMsg; diff --git a/src/simulation/forceTorque/cannonballDrag/cannonballDrag.i b/src/simulation/forceTorque/cannonballDrag/cannonballDrag.i index 4435247a4c0..8af0f5c6b88 100644 --- a/src/simulation/forceTorque/cannonballDrag/cannonballDrag.i +++ b/src/simulation/forceTorque/cannonballDrag/cannonballDrag.i @@ -146,6 +146,8 @@ struct SCStatesMsg_C; struct ForceAtSiteMsg_C; %include "architecture/msgPayloadDefC/TorqueAtSiteMsgPayload.h" struct TorqueAtSiteMsg_C; +%include "architecture/msgPayloadDefC/WindMsgPayload.h" +struct WindMsg_C; %pythoncode %{ import sys diff --git a/src/simulation/forceTorque/cannonballDrag/cannonballDrag.rst b/src/simulation/forceTorque/cannonballDrag/cannonballDrag.rst index 4d825394c29..15d97e13a74 100644 --- a/src/simulation/forceTorque/cannonballDrag/cannonballDrag.rst +++ b/src/simulation/forceTorque/cannonballDrag/cannonballDrag.rst @@ -40,6 +40,10 @@ Message Interfaces - :ref:`SCStatesMsgPayload` - Input message providing the inertial state of the site reference frame, including ``sigma_BN`` (MRPs for site frame attitude relative to inertial) and ``v_BN_N`` (inertial velocity of the site frame origin). + * - windVelInMsg + - :ref:`WindMsgPayload` + - (optional) wind velocity input message; when linked, ``v_air_N`` is subtracted from the site frame + inertial velocity before computing the drag force, yielding the atmosphere-relative velocity. * - forceOutMsg - :ref:`ForceAtSiteMsgPayload` - Output message containing aerodynamic drag force expressed in the site reference frame. @@ -62,13 +66,31 @@ At each update step, the module performs the following operations: 1. Reads the atmospheric density from ``atmoDensInMsg``. 2. Reads the drag coefficient, projected area, and center-of-pressure location from ``dragGeometryInMsg``. 3. Reads the site frame attitude and inertial velocity from ``referenceFrameStateInMsg``. -4. Transforms the inertial velocity into the site frame using the provided attitude. -5. Computes the drag force magnitude and direction in the site frame. -6. Computes the resulting torque about the site frame origin. -7. Publishes the force and torque through ``forceOutMsg`` and ``torqueOutMsg`` (and ``forceOutMsgC`` and ``torqueOutMsgC``). +4. Computes the relative velocity: if ``windVelInMsg`` is linked, subtracts ``v_air_N`` from the inertial + velocity; otherwise uses the inertial velocity directly. +5. Transforms the relative velocity into the site frame using the provided attitude. +6. Computes the drag force magnitude and direction in the site frame. +7. Computes the resulting torque about the site frame origin. +8. Publishes the force and torque through ``forceOutMsg`` and ``torqueOutMsg`` (and ``forceOutMsgC`` and ``torqueOutMsgC``). The model assumes quasi-steady aerodynamics and does not account for lift, shadowing, free-molecular effects, or flow angular rates. +Usage Example +------------- +The following example demonstrates how to configure the ``CannonballDrag`` module: + +.. code-block:: python + + # Create cannonball drag module + cannonball = CannonballDrag() + cannonball.ModelTag = "cannonball_drag" + + # Connect input messages + cannonball.dragGeometryInMsg.subscribeTo(dragGeometryMsg) + cannonball.atmoDensInMsg.subscribeTo(atmoPropsMsg) + cannonball.referenceFrameStateInMsg.subscribeTo(siteStateMsg) + cannonball.windVelInMsg.subscribeTo(windMsg) # Optional + Verification and Testing ------------------------ The ``CannonballDrag`` module is verified through automated unit and integration tests that exercise both isolated force computation and coupled orbital dynamics behavior. From cbd65c7718756bd3e3b0c3707ba264dc099e499a Mon Sep 17 00:00:00 2001 From: Carlo Cena Date: Thu, 19 Mar 2026 04:01:58 +0100 Subject: [PATCH 4/4] [#1325] Showing use of wind modules in drag examples --- .../1325-atmo_rel_vel_all_drag.rst | 1 + examples/scenarioAerocapture.py | 24 +++++++-- examples/scenarioDragDeorbit.py | 51 ++++++++++++++++++- examples/scenarioDragRendezvous.py | 29 +++++++++-- examples/scenarioStochasticDragSpacecraft.py | 22 +++++++- 5 files changed, 117 insertions(+), 10 deletions(-) diff --git a/docs/source/Support/bskReleaseNotesSnippets/1325-atmo_rel_vel_all_drag.rst b/docs/source/Support/bskReleaseNotesSnippets/1325-atmo_rel_vel_all_drag.rst index 501275a61fe..22b399c6523 100644 --- a/docs/source/Support/bskReleaseNotesSnippets/1325-atmo_rel_vel_all_drag.rst +++ b/docs/source/Support/bskReleaseNotesSnippets/1325-atmo_rel_vel_all_drag.rst @@ -1 +1,2 @@ - ``dragDynamicEffector``, ``facetDragDynamicEffector``, and ``cannonballDrag`` now accept an optional ``windVelInMsg`` (:ref:`WindMsgPayload`) to compute atmosphere-relative drag velocity; supersedes the former ``useAtmosphereRelativeVelocity``/``planetOmega_N`` flags. +- Updated :ref:`scenarioDragDeorbit`, :ref:`scenarioDragRendezvous`, :ref:`scenarioAerocapture`, and :ref:`scenarioStochasticDragSpacecraft` to illustrate optional use of wind velocity input. diff --git a/examples/scenarioAerocapture.py b/examples/scenarioAerocapture.py index 9ed456d234a..bc1375d024d 100644 --- a/examples/scenarioAerocapture.py +++ b/examples/scenarioAerocapture.py @@ -86,6 +86,7 @@ # Used to get the location of supporting data. from Basilisk import __path__ from Basilisk.simulation import dragDynamicEffector +from Basilisk.simulation import zeroWindModel # import simulation related support from Basilisk.simulation import spacecraft @@ -155,13 +156,16 @@ def sph2rv(xxsph): return rvec_N, uvec_N -def run(show_plots, planetCase): +def run(show_plots, planetCase, useWind=False): """ The scenarios can be run with the followings setups parameters: Args: show_plots (bool): Determines if the script should display plots - planetCase (string): Specify if a `Mars` or `Earth` arrival is simulated + planetCase (string): Specify if a ``Mars`` or ``Earth`` arrival is simulated + useWind (bool): If True and ``planetCase == 'Earth'``, link a ``ZeroWindModel`` via SPICE + so drag is computed against the atmosphere-relative velocity. If False (default), + or for Mars, the inertial spacecraft velocity is used directly. """ @@ -255,6 +259,20 @@ def run(show_plots, planetCase): # attach gravity model to spacecraft gravFactory.addBodiesTo(scObject) + if useWind and planetCase == "Earth": + spiceObject = gravFactory.createSpiceInterface( + time="2020 MAY 21 18:28:03 (UTC)", + ) + spiceObject.zeroBase = "Earth" + scSim.AddModelToTask(simTaskName, spiceObject, -1) + + windModel = zeroWindModel.ZeroWindModel() + windModel.ModelTag = "ZeroWind" + windModel.planetPosInMsg.subscribeTo(spiceObject.planetStateOutMsgs[0]) + windModel.addSpacecraftToModel(scObject.scStateOutMsg) + scSim.AddModelToTask(simTaskName, windModel) + dragEffector.windVelInMsg.subscribeTo(windModel.envOutMsgs[0]) + if planetCase == "Earth": r = 6503 * 1000 u = 11.2 * 1000 @@ -398,4 +416,4 @@ def run(show_plots, planetCase): if __name__ == "__main__": - run(True, "Mars") # planet arrival case, can be Earth or Mars + run(True, "Mars", False) # planet arrival case, can be Earth or Mars diff --git a/examples/scenarioDragDeorbit.py b/examples/scenarioDragDeorbit.py index 3fdb0595e1e..0bdb12cd2be 100644 --- a/examples/scenarioDragDeorbit.py +++ b/examples/scenarioDragDeorbit.py @@ -26,6 +26,12 @@ :ref:`dragDynamicEffector` dynamics module. The simulation is executed until the altitude falls below some threshold, using a terminal event handler. +An optional wind model (:ref:`zeroWindModel`) can be enabled via the ``useWind`` argument (default: ``False``). +When active, SPICE is loaded with the default ``IAU_EARTH`` frame and drag is computed against the +atmosphere-relative velocity rather than the inertial spacecraft velocity. This is a more accurate treatment +for scenarios where Earth's rotation is relevant, but adds a SPICE dependency. When ``useWind=False``, the +inertial spacecraft velocity is used directly, which is the conventional approximation for simple deorbit studies. + The script is found in the folder ``basilisk/examples`` and executed by using:: python3 scenarioDragDeorbit.py @@ -72,6 +78,24 @@ dragEffector.atmoDensInMsg.subscribeTo(atmo.envOutMsgs[0]) +Optionally (``useWind=True``), a :ref:`zeroWindModel` can be linked so drag is computed against the +atmosphere-relative velocity. SPICE is loaded with the default ``IAU_EARTH`` frame so that +``J20002Pfix_dot`` is populated; :ref:`windBase` reads those derivatives to derive the Earth rotation +rate, giving a physically consistent co-rotation:: + + spiceObject = gravFactory.createSpiceInterface( + time="2020 MAY 21 18:28:03 (UTC)", + ) + spiceObject.zeroBase = "Earth" + scSim.AddModelToTask(simTaskName, spiceObject, -1) + + windModel = zeroWindModel.ZeroWindModel() + windModel.ModelTag = "ZeroWind" + windModel.planetPosInMsg.subscribeTo(spiceObject.planetStateOutMsgs[0]) + windModel.addSpacecraftToModel(scObject.scStateOutMsg) + scSim.AddModelToTask(simTaskName, windModel) + dragEffector.windVelInMsg.subscribeTo(windModel.envOutMsgs[0]) + Illustration of Simulation Results ---------------------------------- @@ -142,6 +166,7 @@ exponentialAtmosphere, msisAtmosphere, spacecraft, + zeroWindModel, ) from Basilisk.utilities import ( SimulationBaseClass, @@ -157,7 +182,7 @@ fileName = os.path.basename(os.path.splitext(__file__)[0]) -def run(show_plots, initialAlt=250, deorbitAlt=100, model="exponential"): +def run(show_plots, initialAlt=250, deorbitAlt=100, model="exponential", useWind=False): """ Initialize a satellite with drag and propagate until it falls below a deorbit altitude. Note that an excessively low deorbit_alt can lead to intersection with the Earth prior to deorbit being detected, causing some terms to blow @@ -168,6 +193,8 @@ def run(show_plots, initialAlt=250, deorbitAlt=100, model="exponential"): initialAlt (float): Starting altitude in km deorbitAlt (float): Terminal altitude in km model (str): ["exponential", "msis"] + useWind (bool): If True, link a :ref:`zeroWindModel` so drag is computed against the atmosphere-relative + velocity. If False (default), the inertial spacecraft velocity is used directly. Returns: Dictionary of figure handles @@ -238,6 +265,25 @@ def run(show_plots, initialAlt=250, deorbitAlt=100, model="exponential"): mu = planet.mu gravFactory.addBodiesTo(scObject) + # Optionally link a zero-wind model so drag is computed against the atmosphere-relative velocity. + # SPICE is loaded with the default IAU_EARTH frame so that J20002Pfix_dot is populated and + # WindBase can derive the true Earth rotation rate from it, keeping the co-rotation consistent + # with the actual planet orientation. Omitting this (useWind=False) falls back to using the + # inertial spacecraft velocity directly. + if useWind: + spiceObject = gravFactory.createSpiceInterface( + time="2020 MAY 21 18:28:03 (UTC)", + ) + spiceObject.zeroBase = "Earth" + scSim.AddModelToTask(simTaskName, spiceObject, -1) + + windModel = zeroWindModel.ZeroWindModel() + windModel.ModelTag = "ZeroWind" + windModel.planetPosInMsg.subscribeTo(spiceObject.planetStateOutMsgs[0]) + windModel.addSpacecraftToModel(scObject.scStateOutMsg) + scSim.AddModelToTask(simTaskName, windModel) + dragEffector.windVelInMsg.subscribeTo(windModel.envOutMsgs[0]) + # Set up a circular orbit using classical orbit elements oe = orbitalMotion.ClassicElements() oe.a = planet.radEquator + initialAlt * 1000 # meters @@ -383,5 +429,6 @@ def register_fig(i): show_plots=True, initialAlt=250, deorbitAlt=100, - model="msis" # "msis", "exponential" + model="msis", # "msis", "exponential" + useWind=False ) diff --git a/examples/scenarioDragRendezvous.py b/examples/scenarioDragRendezvous.py index 694b7746445..159eea4f5f1 100644 --- a/examples/scenarioDragRendezvous.py +++ b/examples/scenarioDragRendezvous.py @@ -85,6 +85,7 @@ facetDragDynamicEffector, simpleNav, exponentialAtmosphere, + zeroWindModel, ) from Basilisk.utilities import RigidBodyKinematics as rbk from Basilisk.utilities import SimulationBaseClass @@ -255,7 +256,7 @@ def setup_spacecraft_plant(rN, vN, modelName): def drag_simulator( - altOffset, trueAnomOffset, densMultiplier, ctrlType="lqr", useJ2=False + altOffset, trueAnomOffset, densMultiplier, ctrlType="lqr", useJ2=False, useWind=False ): """ Basilisk simulation of a two-spacecraft rendezvous using relative-attitude driven differential drag. Includes @@ -265,6 +266,11 @@ def drag_simulator( Args: altOffset - double - deputy altitude offset from the chief ('x' hill direction), meters trueAnomOffset - double - deputy true anomaly difference from the chief ('y' direction), degrees + densMultiplier - double - multiplicative scale factor applied to the exponential atmosphere base density + ctrlType (str): Control law type; ``'lqr'`` for static LQR or ``'dtv'`` for desensitized time-varying gain + useJ2 (bool): If True, include J2 spherical harmonic gravity perturbation + useWind (bool): If True, link a ``ZeroWindModel`` via SPICE so drag is computed against the + atmosphere-relative velocity. If False (default), the inertial spacecraft velocity is used directly. """ startTime = time.time() @@ -341,6 +347,22 @@ def drag_simulator( atmosphere.addSpacecraftToModel(chiefSc.scStateOutMsg) chiefDrag.atmoDensInMsg.subscribeTo(atmosphere.envOutMsgs[-1]) + if useWind: + spiceObject = gravFactory.createSpiceInterface( + time="2020 MAY 21 18:28:03 (UTC)", + ) + spiceObject.zeroBase = "Earth" + scSim.AddModelToTask(dynTaskName, spiceObject, 999) + + windModel = zeroWindModel.ZeroWindModel() + windModel.ModelTag = "ZeroWind" + windModel.planetPosInMsg.subscribeTo(spiceObject.planetStateOutMsgs[0]) + windModel.addSpacecraftToModel(depSc.scStateOutMsg) + windModel.addSpacecraftToModel(chiefSc.scStateOutMsg) + scSim.AddModelToTask(dynTaskName, windModel, 919) + depDrag.windVelInMsg.subscribeTo(windModel.envOutMsgs[0]) + chiefDrag.windVelInMsg.subscribeTo(windModel.envOutMsgs[1]) + # Add all dynamics stuff to dynamics task scSim.AddModelToTask(dynTaskName, atmosphere, 920) # scSim.AddModelToTask(dynTaskName, ephemConverter, 921) @@ -470,10 +492,10 @@ def drag_simulator( def run( - show_plots, altOffset, trueAnomOffset, densMultiplier, ctrlType="lqr", useJ2=False + show_plots, altOffset, trueAnomOffset, densMultiplier, ctrlType="lqr", useJ2=False, useWind=False ): results = drag_simulator( - altOffset, trueAnomOffset, densMultiplier, ctrlType=ctrlType, useJ2=useJ2 + altOffset, trueAnomOffset, densMultiplier, ctrlType=ctrlType, useJ2=useJ2, useWind=useWind ) timeData = results["dynTimeData"] @@ -598,4 +620,5 @@ def run( 1, # Density multiplier (nondimensional) ctrlType="lqr", useJ2=False, + useWind=False, ) diff --git a/examples/scenarioStochasticDragSpacecraft.py b/examples/scenarioStochasticDragSpacecraft.py index ca9dcb5e432..88efd6b7033 100644 --- a/examples/scenarioStochasticDragSpacecraft.py +++ b/examples/scenarioStochasticDragSpacecraft.py @@ -72,6 +72,7 @@ from Basilisk.simulation import exponentialAtmosphere from Basilisk.simulation import dragDynamicEffector from Basilisk.simulation import meanRevertingNoiseStateEffector +from Basilisk.simulation import zeroWindModel from Basilisk.utilities import SimulationBaseClass from Basilisk.utilities import macros from Basilisk.utilities import orbitalMotion @@ -80,13 +81,16 @@ fileName = os.path.basename(os.path.splitext(__file__)[0]) -def run(showPlots: bool = False, rngSeed: Optional[int] = None): +def run(showPlots: bool = False, rngSeed: Optional[int] = None, useWind: bool = False): """ Run the spacecraft-based stochastic drag scenario. Args: showPlots: If True, display figures. rngSeed: Optional stochastic integrator seed for reproducibility. + useWind (bool): If True, link a ``ZeroWindModel`` via SPICE so drag is computed against + the atmosphere-relative velocity. If False (default), the inertial spacecraft + velocity is used directly. Returns: Dict of matplotlib figure handles. """ @@ -159,6 +163,20 @@ def run(showPlots: bool = False, rngSeed: Optional[int] = None): scObject.addDynamicEffector(drag) scSim.AddModelToTask(simTaskName, drag) + if useWind: + spiceObject = gravFactory.createSpiceInterface( + time="2020 MAY 21 18:28:03 (UTC)", + ) + spiceObject.zeroBase = "Earth" + scSim.AddModelToTask(simTaskName, spiceObject, -1) + + windModel = zeroWindModel.ZeroWindModel() + windModel.ModelTag = "ZeroWind" + windModel.planetPosInMsg.subscribeTo(spiceObject.planetStateOutMsgs[0]) + windModel.addSpacecraftToModel(scObject.scStateOutMsg) + scSim.AddModelToTask(simTaskName, windModel) + drag.windVelInMsg.subscribeTo(windModel.envOutMsgs[0]) + # Recorders stateRecorder = scObject.scStateOutMsg.recorder() deterministicDensityRecorder = atmo.envOutMsgs[0].recorder() @@ -247,4 +265,4 @@ def plotOrbits(timeAxis, posData, velData, dragForce, deterministicDenseData, oe if __name__ == "__main__": - run(True) + run(True, useWind = False)