From b256bf16213d0f0ac4eaefbb1481b1347ffcdcb1 Mon Sep 17 00:00:00 2001 From: Florian Chaubeyre Date: Thu, 19 Apr 2018 10:36:20 +1000 Subject: [PATCH 01/48] Set torque disturbances as optional - Fix scenario torque provider. --- .../src/main/java/msp/simulator/Main.java | 3 +- .../TorqueOverTimeScenarioProvider.java | 125 ++++++++++++++---- .../simulator/dynamic/torques/Torques.java | 28 ++-- .../java/msp/simulator/user/Dashboard.java | 9 ++ .../java/msp/simulator/test/TestDynamic.java | 99 +++++++++++--- 5 files changed, 213 insertions(+), 51 deletions(-) diff --git a/simulator/src/main/java/msp/simulator/Main.java b/simulator/src/main/java/msp/simulator/Main.java index ec5ad573..baf1b10f 100644 --- a/simulator/src/main/java/msp/simulator/Main.java +++ b/simulator/src/main/java/msp/simulator/Main.java @@ -46,11 +46,12 @@ public static void main(String[] args) { Dashboard.setInitialAttitudeQuaternion(new Quaternion(1, 0, 0, 0)); Dashboard.setInitialSpin(new Vector3D(0.5, 0.5, 0.5)); Dashboard.setInitialRotAcceleration(new Vector3D(0,0,0)); + Dashboard.setTorqueDisturbances(true); Dashboard.setCommandTorqueProvider(TorqueProviderEnum.MEMCACHED); Dashboard.setMemCachedConnection(true, "127.0.0.1:11211"); - //Dashboard.setVtsConnection(true); + Dashboard.setVtsConnection(false); /* *** Creating and launching the simulation. *** */ NumericalSimulator simulator = new NumericalSimulator(); diff --git a/simulator/src/main/java/msp/simulator/dynamic/torques/TorqueOverTimeScenarioProvider.java b/simulator/src/main/java/msp/simulator/dynamic/torques/TorqueOverTimeScenarioProvider.java index 1eee0ca2..512c073e 100644 --- a/simulator/src/main/java/msp/simulator/dynamic/torques/TorqueOverTimeScenarioProvider.java +++ b/simulator/src/main/java/msp/simulator/dynamic/torques/TorqueOverTimeScenarioProvider.java @@ -18,6 +18,13 @@ import org.hipparchus.geometry.euclidean.threed.Vector3D; import org.orekit.time.AbsoluteDate; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import msp.simulator.NumericalSimulator; +import msp.simulator.dynamic.propagation.integration.Integration; +import msp.simulator.satellite.Satellite; +import msp.simulator.satellite.assembly.SatelliteStates; /** * This class implements a simple torque maneuver @@ -61,14 +68,34 @@ public Step(double start, double duration, Vector3D vector){ public double getDuration() {return duration;} public Vector3D getRotVector() {return rotVector;} } - + /** @return The default torque intensity of the scenario. */ public static double getTorqueIntensity() { return maxTorqueIntensity; } - + /* **************************************** */ + /** Logger of the class */ + private static final Logger logger = LoggerFactory.getLogger( + TorqueOverTimeScenarioProvider.class); + + /** Buffered date of the beginning of the step. */ + private AbsoluteDate stepStart; + + /** Satellite states keeping the start date of the step all + * along the step. */ + private SatelliteStates satState; + + /** Buffered date of the next acquisition date. */ + private AbsoluteDate nextAcquisitionDate; + + /** Buffered torque for the current step. */ + private Vector3D stepTorque; + + /** Copy of the fixed integration time step. */ + private final double stepSize = Integration.integrationTimeStep; + /** Absolute start date of the scenario in the simulation. */ private AbsoluteDate startDate; @@ -82,8 +109,8 @@ public static double getTorqueIntensity() { * Constructor with the torque scenario set as default. * @param startDate of the torque scenario over time */ - public TorqueOverTimeScenarioProvider(AbsoluteDate startDate) { - this(startDate, TorqueOverTimeScenarioProvider.TORQUE_SCENARIO); + public TorqueOverTimeScenarioProvider(Satellite satellite, AbsoluteDate startDate) { + this(satellite, startDate, TorqueOverTimeScenarioProvider.TORQUE_SCENARIO); } /** @@ -91,9 +118,17 @@ public TorqueOverTimeScenarioProvider(AbsoluteDate startDate) { * @param startDate Absolute date to start the scenario in the simulation * @param scenario List of the torque steps. */ - public TorqueOverTimeScenarioProvider(AbsoluteDate startDate, ArrayList scenario) { + public TorqueOverTimeScenarioProvider( + Satellite satellite, + AbsoluteDate startDate, + ArrayList scenario) { this.scenario = scenario; this.startDate = startDate; + + this.satState = satellite.getStates(); + this.stepStart = this.satState.getCurrentState().getDate(); + this.nextAcquisitionDate = this.satState.getInitialState().getDate(); + this.stepTorque = Vector3D.ZERO; } /** @@ -109,29 +144,69 @@ public boolean addStep(double startOffset, double duration, Vector3D nRotation) /** {@inheritDoc} */ @Override - public Vector3D getTorque(AbsoluteDate currentDate) { - Vector3D torque; - double offset = currentDate.durationFrom(this.startDate); - boolean success = false; - Step step = null; - /* Iterate the scenario to find the first applicable step. */ - for (int i = 0; i < this.scenario.size(); i++) { - step = this.scenario.get(i); - /* If we find a currently operating step, it's a success. */ - if ( (step.getStart() <= offset) - && - (step.getStart() + step.getDuration() > offset)) - { - success = true; - break; + public Vector3D getTorque(AbsoluteDate date) { + /* Flag to enable the acquisition of the torque for the step. */ + boolean acquisition; + + /* As the torque is considered constant over a step, we only need + * to acquire the torque once at the very beginning of the step. */ + this.stepStart = this.satState.getCurrentState().getDate(); + + acquisition = + (date.compareTo(this.nextAcquisitionDate) == 0) + && + (date.compareTo(this.stepStart) == 0) + ; + + /* Compute the torque command if a new step is detected. */ + if (acquisition) { + + /* Reading the torque command from the scenario. */ + try { + Vector3D torqueCommand; + double offset = date.durationFrom(this.startDate); + boolean success = false; + Step step = null; + + /* Iterate the scenario to find the first applicable step. */ + for (int i = 0; i < this.scenario.size(); i++) { + step = this.scenario.get(i); + /* If we find a currently operating step, it's a success. */ + if ( (step.getStart() <= offset + NumericalSimulator.EPSILON) + && + (step.getStart() + step.getDuration() > offset + + NumericalSimulator.EPSILON)) + { + success = true; + break; + } + } + if (success) { + torqueCommand = step.getRotVector().scalarMultiply(maxTorqueIntensity); + } else { + torqueCommand = Vector3D.ZERO; + } + + /* Then update the buffered data. */ + this.nextAcquisitionDate = this.stepStart.shiftedBy(this.stepSize); + this.stepTorque = torqueCommand; + + } catch (Exception e) { + e.printStackTrace(); } - } - if (success) { - torque = step.getRotVector().scalarMultiply(maxTorqueIntensity); + + /* Debug Information */ + logger.debug("Torque Provider (Acquisition): " + date.toString() +" - " + + this.stepTorque.toString()); + } else { - torque = Vector3D.ZERO; + /* Else the torque is already computed for the current step. */ + logger.debug("------------- Torque Provider: " + date.toString() +" - " + + this.stepTorque.toString()); } - return torque; + + /* Finally returns the torque of the step (updated if needed). */ + return this.stepTorque; } /** diff --git a/simulator/src/main/java/msp/simulator/dynamic/torques/Torques.java b/simulator/src/main/java/msp/simulator/dynamic/torques/Torques.java index 962446e0..acad2188 100644 --- a/simulator/src/main/java/msp/simulator/dynamic/torques/Torques.java +++ b/simulator/src/main/java/msp/simulator/dynamic/torques/Torques.java @@ -37,7 +37,10 @@ public class Torques { /* ******* Public Static Attributes ******* */ /** Set the torque provider in use by the simulator. */ - public static TorqueProviderEnum commandTorqueProvider = TorqueProviderEnum.SCENARIO; + public static TorqueProviderEnum commandTorqueProvider = TorqueProviderEnum.SCENARIO; + + /** Allow the torque disturbances in the simulation. */ + public static boolean allowDisturbances = true; /* **************************************** */ @@ -45,7 +48,10 @@ public class Torques { private static final Logger logger = LoggerFactory.getLogger(Torques.class); /** Instance of Torque Provider. */ - private ArrayList torqueProviders; + private ArrayList torqueProviders; + + /** Private flag to allow torque disturbances in the simulation. */ + private boolean isDisturbances; /** * Build the Main Torque Provider of the dynamic module. @@ -58,7 +64,10 @@ public Torques (Environment environment, Satellite satellite) { /* Build the torque providers in use in the simulation. */ this.torqueProviders = new ArrayList(); - + + /* Set the use of torque disturbances. */ + this.isDisturbances = Torques.allowDisturbances; + /* - Register the command provider. */ switch (Torques.commandTorqueProvider) { case MEMCACHED: @@ -70,17 +79,20 @@ public Torques (Environment environment, Satellite satellite) { case SCENARIO: this.torqueProviders.add( TorqueProviderEnum.SCENARIO.getIndex(), - new TorqueOverTimeScenarioProvider( - satellite.getAssembly().getStates().getInitialState().getDate()) + new TorqueOverTimeScenarioProvider( + satellite, + satellite.getAssembly().getStates().getInitialState().getDate() + ) ); break; default: break; } - /* - Register the disturbances. */ - this.torqueProviders.add(new SimpleTorqueDisturbances()); - + /* - Register the disturbances. */ + if (this.isDisturbances) { + this.torqueProviders.add(new SimpleTorqueDisturbances()); + } } /** diff --git a/simulator/src/main/java/msp/simulator/user/Dashboard.java b/simulator/src/main/java/msp/simulator/user/Dashboard.java index e6f5db6e..681253b1 100644 --- a/simulator/src/main/java/msp/simulator/user/Dashboard.java +++ b/simulator/src/main/java/msp/simulator/user/Dashboard.java @@ -116,6 +116,7 @@ public static void setDefaultConfiguration() { Dashboard.setInitialRotAcceleration(Vector3D.ZERO); Dashboard.setCommandTorqueProvider(TorqueProviderEnum.SCENARIO); Dashboard.setTorqueScenario(new ArrayList()); + Dashboard.setTorqueDisturbances(true); /* **** Structure Settings **** */ Dashboard.setSatBoxSizeWithNoSolarPanel(new double[]{0.01, 0.01, 0.01}); @@ -284,6 +285,14 @@ public static void setInitialSpin(Vector3D spin) { public static void setInitialRotAcceleration(Vector3D accRot) { SatelliteStates.initialRotAcceleration = accRot; } + + /** + * Allow the torque disturbances in the simulation. + * @param flag True if allowed, false otherwise. + */ + public static void setTorqueDisturbances(boolean flag) { + Torques.allowDisturbances = flag; + } /** * Set the user-defined satellite mass. diff --git a/simulator/src/test/java/msp/simulator/test/TestDynamic.java b/simulator/src/test/java/msp/simulator/test/TestDynamic.java index 8e3638dd..7770dfce 100644 --- a/simulator/src/test/java/msp/simulator/test/TestDynamic.java +++ b/simulator/src/test/java/msp/simulator/test/TestDynamic.java @@ -33,7 +33,6 @@ import msp.simulator.dynamic.torques.TorqueOverTimeScenarioProvider.Step; import msp.simulator.dynamic.torques.TorqueProviderEnum; import msp.simulator.user.Dashboard; -import msp.simulator.utils.logs.CustomLoggingTools; /** @@ -44,9 +43,14 @@ public class TestDynamic { /** Instance of the Logger of the class. */ + @SuppressWarnings("unused") private static final Logger logger = LoggerFactory.getLogger(TestDynamic.class); + /** + * Test a simple acceleration during a given duration. + * @throws Exception When simulator's configuration failed. + */ @Test public void testRotationAcceleration() throws Exception { @@ -66,29 +70,20 @@ public void testRotationAcceleration() throws Exception { ArrayList torqueScenario = new ArrayList(); - torqueScenario.add(new Step(0., accDuration + 1, rotVector)); + torqueScenario.add(new Step(0., accDuration, rotVector)); //torqueScenario.add(new Step(5., 3., new Vector3D(-1,0,0))); //torqueScenario.add(new Step(55., 10., new Vector3D(1,2,3))); //torqueScenario.add(new Step(70., 10., new Vector3D(-1,-2,-3))); Dashboard.setTorqueScenario(torqueScenario); + Dashboard.setTorqueDisturbances(false); Dashboard.checkConfiguration(); /**** Creating and launching the simulation. ****/ NumericalSimulator simu = new NumericalSimulator(); simu.initialize(); - - logger.info(CustomLoggingTools.toString( - "Initial State of the satellite", - simu.getSatellite().getStates().getInitialState())); - simu.process(); - - logger.info(CustomLoggingTools.toString( - "Final State of the satellite", - simu.getSatellite().getStates().getCurrentState())); - simu.exit(); /* Extracting final state. */ @@ -106,7 +101,7 @@ public void testRotationAcceleration() throws Exception { Assert.assertArrayEquals( expectedRotAcc, finalState.getAdditionalState("RotAcc"), - 1e-2); + 1e-6); /* Checking Spin */ Assert.assertArrayEquals( @@ -115,7 +110,75 @@ public void testRotationAcceleration() throws Exception { finalState.getAdditionalState(SecondaryStates.key), SecondaryStates.SPIN ), - 1e-2); + 1e-6); + } + + /** + * Test an acceleration then the exact opposite deceleration to assert + * a zero spin at the end of the given time. + * @throws Exception When simulator's configuration failed. + */ + @Test + public void testRotationDeceleration() throws Exception { + + /* **** Data of the test **** */ + long accDuration = 50; + Vector3D rotVector = new Vector3D(1, 0.1, 0.2); + /* ************************** */ + + /**** Configuration of the simulation. ****/ + Dashboard.setDefaultConfiguration(); + Dashboard.setRealTimeProcessing(false); + Dashboard.setSimulationDuration(2 * accDuration); + Dashboard.setIntegrationTimeStep(0.1); + + /* Writing the torque scenario. */ + ArrayList torqueScenario = + new ArrayList(); + + torqueScenario.add(new Step(0., accDuration, rotVector)); + torqueScenario.add(new Step(accDuration, accDuration, rotVector.negate())); + + //torqueScenario.add(new Step(5., 3., new Vector3D(-1,0,0))); + //torqueScenario.add(new Step(55., 10., new Vector3D(1,2,3))); + //torqueScenario.add(new Step(70., 10., new Vector3D(-1,-2,-3))); + + Dashboard.setTorqueScenario(torqueScenario); + Dashboard.setTorqueDisturbances(false); + + Dashboard.checkConfiguration(); + + /**** Creating and launching the simulation. ****/ + NumericalSimulator simu = new NumericalSimulator(); + simu.initialize(); + simu.process(); + simu.exit(); + + /* Extracting final state. */ + SpacecraftState finalState = simu.getSatellite().getStates().getCurrentState(); + + /* Computing the expected final acceleration: negative because of the deceleration. */ + double[] expectedRotAcc = RotAccProvider.computeEulerEquations( + rotVector.scalarMultiply( + - 1 * TorqueOverTimeScenarioProvider.getTorqueIntensity()), + finalState.getAttitude().getSpin(), + simu.getSatellite().getAssembly().getBody().getInertiaMatrix() + ); + + /* Checking Rotational Acceleration. */ + Assert.assertArrayEquals( + expectedRotAcc, + finalState.getAdditionalState("RotAcc"), + 1e-6); + + /* Checking Spin */ + Assert.assertArrayEquals( + Vector3D.ZERO.toArray(), + SecondaryStates.extractState( + finalState.getAdditionalState(SecondaryStates.key), + SecondaryStates.SPIN + ), + 1e-6); } @@ -137,7 +200,7 @@ public void testRotationAcceleration() throws Exception { * + At the time t = dur, i.e. the end state:

* Q = ( cos(Pi/2), sin(Pi/2).n ) * = ( 0, nx, ny, nz) - * @throws Exception when initialization of simulation fails + * @throws Exception When simulator's configuration failed. * */ @Test @@ -159,7 +222,9 @@ public void testRotation() throws Exception { Dashboard.setInitialAttitudeQuaternion(new Quaternion(1, 0, 0, 0)); Dashboard.setInitialSpin(new Vector3D(FastMath.PI / rotationTime, n)); Dashboard.setInitialRotAcceleration(new Vector3D(0,0,0)); - //Dashboard.setVtsConnection(true); + Dashboard.setTorqueDisturbances(false); + + Dashboard.setVtsConnection(false); /* *** Creating and launching the simulation. *** */ NumericalSimulator simu = new NumericalSimulator(); @@ -180,7 +245,7 @@ public void testRotation() throws Exception { double[] expectedAttitudeArray = new double[] {0, n.getX(), n.getY(), n.getZ()} ; /* Approximation error during the propagation. */ - double delta = 1e-3; + double delta = 1e-6; /* Testing the attitude of the satellite after the processing. */ Assert.assertArrayEquals( From 85d28d5d1161fef110d0688b129acd8e0f46ee88 Mon Sep 17 00:00:00 2001 From: Florian Chaubeyre Date: Fri, 20 Apr 2018 10:03:17 +1000 Subject: [PATCH 02/48] Fix transformation from ITRF to Body frame. --- .../msp/simulator/satellite/Satellite.java | 15 +++---- .../satellite/assembly/Assembly.java | 43 ++++++++++++++++--- .../satellite/sensors/Magnetometer.java | 42 +++++++----------- 3 files changed, 59 insertions(+), 41 deletions(-) diff --git a/simulator/src/main/java/msp/simulator/satellite/Satellite.java b/simulator/src/main/java/msp/simulator/satellite/Satellite.java index 0c81e918..f5e5b2d1 100644 --- a/simulator/src/main/java/msp/simulator/satellite/Satellite.java +++ b/simulator/src/main/java/msp/simulator/satellite/Satellite.java @@ -15,6 +15,7 @@ package msp.simulator.satellite; import org.hipparchus.geometry.euclidean.threed.Vector3D; +import org.orekit.time.AbsoluteDate; import org.slf4j.Logger; import org.slf4j.LoggerFactory; @@ -73,9 +74,7 @@ public Satellite(Environment environment) { * externalization of the sensors etc. */ public void executeStepMission() { - - this.getSensors().getGyrometer().getData_rotAcc(); - + AbsoluteDate date = this.getStates().getCurrentState().getDate(); /* Export Sensor Measurements */ if (this.io.isConnectedToMemCached()) { @@ -104,7 +103,7 @@ public void executeStepMission() { "Simulation_Magnetometer_Z", 0, rawMag_z ); - + /* Gyrometer Sensor Measurement */ Vector3D gyroMeasure = this.getSensors().getGyrometer().getData_rotAcc(); byte[] rawGyro_x = MemcachedRawTranscoder.toRawByteArray(gyroMeasure.getX()); @@ -123,11 +122,11 @@ public void executeStepMission() { "Simulation_Gyrometer_Z", 0, rawGyro_z ); - + /* Infrared Sensor Measurement */ - Vector3D nadir_ecef = Vector3D.MINUS_K; - Vector3D nadir_body = this.assembly.getStates().getCurrentState() - .toTransform().transformVector(nadir_ecef); + Vector3D nadir_itrf = Vector3D.MINUS_K; + Vector3D nadir_body = this.assembly.getItrf2body(date) + .transformVector(nadir_itrf); double posXIR = this.sensors.getPosXIRSensor() .calculateInfraredReading(nadir_body); diff --git a/simulator/src/main/java/msp/simulator/satellite/assembly/Assembly.java b/simulator/src/main/java/msp/simulator/satellite/assembly/Assembly.java index d1d58428..ed27dd00 100644 --- a/simulator/src/main/java/msp/simulator/satellite/assembly/Assembly.java +++ b/simulator/src/main/java/msp/simulator/satellite/assembly/Assembly.java @@ -15,17 +15,21 @@ package msp.simulator.satellite.assembly; import org.hipparchus.geometry.euclidean.threed.Vector3D; +import org.orekit.errors.OrekitException; import org.orekit.frames.Frame; import org.orekit.frames.FramesFactory; +import org.orekit.frames.Transform; +import org.orekit.time.AbsoluteDate; import org.slf4j.Logger; import org.slf4j.LoggerFactory; import msp.simulator.environment.Environment; +import msp.simulator.environment.solarSystem.Earth; import msp.simulator.utils.logs.CustomLoggingTools; /** * The assembly describes the entity of the satellite and gathers - * both its physical and state representation. + * both its physical and state representations. * * @author Florian CHAUBEYRE */ @@ -33,12 +37,18 @@ public class Assembly { /** Logger of the class */ private static final Logger logger = LoggerFactory.getLogger(Assembly.class); + + /** Earth instance of the simulation. */ + private Earth earth; /** Instance of the satellite body for the assembly. */ private SatelliteBody satelliteBody; /** Instance of the satellite initial state in space. */ private SatelliteStates satelliteStates; + + /** Satellite frame. */ + private Frame satelliteFrame; /** * Build the satellite as a body and a state vector. @@ -52,6 +62,12 @@ public Assembly(Environment environment) { this.satelliteBody = new SatelliteBody(environment); this.satelliteStates = new SatelliteStates(environment, satelliteBody); + this.satelliteFrame = new Frame ( + FramesFactory.getEME2000(), + this.getStates().getCurrentState().toTransform(), + "SatelliteFrame" + ); + this.earth = environment.getSolarSystem().getEarth(); } /** @@ -79,11 +95,7 @@ public SatelliteStates getStates() { * frame fixed with the axis body. */ public Frame getSatelliteFrame() { - return new Frame ( - FramesFactory.getEME2000(), - this.getStates().getCurrentState().toTransform(), - "SatelliteFrame" - ); + return this.satelliteFrame; } /** @@ -109,5 +121,24 @@ public Vector3D getAngularMomentum() { return angularMomentum; } + /** + * Compute the transformation from the ITRF frame to the + * satellite body frame at the specified date. + * @return The ITRF to Body frame transformation. + */ + public Transform getItrf2body(AbsoluteDate date) { + Transform itrf2body = null; + try { + itrf2body = this.earth.getRotatingFrame().getTransformTo( + this.satelliteFrame, + date + ); + } catch (OrekitException e) { + e.printStackTrace(); + } + + return itrf2body; + } + } diff --git a/simulator/src/main/java/msp/simulator/satellite/sensors/Magnetometer.java b/simulator/src/main/java/msp/simulator/satellite/sensors/Magnetometer.java index 56eb08bc..5b26a0d1 100644 --- a/simulator/src/main/java/msp/simulator/satellite/sensors/Magnetometer.java +++ b/simulator/src/main/java/msp/simulator/satellite/sensors/Magnetometer.java @@ -15,14 +15,11 @@ package msp.simulator.satellite.sensors; import org.hipparchus.geometry.euclidean.threed.Vector3D; -import org.hipparchus.geometry.euclidean.threed.Rotation; import org.hipparchus.util.FastMath; import org.orekit.bodies.GeodeticPoint; import org.orekit.errors.OrekitException; import org.orekit.models.earth.GeoMagneticElements; import org.orekit.propagation.SpacecraftState; -import org.orekit.frames.Transform; - import org.slf4j.Logger; import org.slf4j.LoggerFactory; @@ -39,7 +36,7 @@ * @author Florian CHAUBEYRE */ public class Magnetometer { - + /* ******* Public Static Attributes ******* */ /** This intensity is used to generate a random number to be @@ -62,7 +59,7 @@ public class Magnetometer { /** Assembly of the satellite. */ private Assembly assembly; - + /** Private attribute for the noise intensity. */ private double noiseIntensity; @@ -103,10 +100,10 @@ public GeoMagneticElements retrieveNoisyField() { /* Creating the noisy measure. */ GeoMagneticElements noisyMeasure = new GeoMagneticElements(noisyFieldVector); - + logger.debug("Noisy Geo" + noisyMeasure.toString()); - - + + return noisyMeasure; } @@ -149,35 +146,26 @@ public GeoMagneticElements retrievePerfectField() { * the altitude of the satellite is slightly shifted from the true * one. */ - GeoMagneticElements trueMagField_ecef = this.geomagField.getField().calculateField( + GeoMagneticElements trueField_itrf = this.geomagField.getField().calculateField( FastMath.toDegrees(geodeticPosition.getLatitude()), /* decimal deg */ FastMath.toDegrees(geodeticPosition.getLongitude()), /* decimal deg */ (satState.getA() - this.earth.getRadius()) / 1e3 /* km */ ); - + logger.debug("Magnetometer Measurement: \n" + "Latitude: " + FastMath.toDegrees(geodeticPosition.getLatitude()) + " °\n" + "Longitud: " + FastMath.toDegrees(geodeticPosition.getLongitude()) + " °\n" + "Altitude: " + (satState.getA() - this.earth.getRadius()) / 1e3 + " km\n" + - "True Geo ECEF" + trueMagField_ecef.toString() + "True Geo ITRF" + trueField_itrf.toString() ); + /* Rotate the magnetic field reading into the body frame */ + Vector3D trueField_body = this.assembly.getItrf2body(satState.getDate()) + .transformVector(trueField_itrf.getFieldVector()); - /* Rotate the magnetic field reading into the body frame */ - // Assuming WMM outputs vectors in Earth-centred-Earth-fixed frame - // This might be backwards - Rotation rotation_ecef_to_body = - this.assembly.getStates() - .getCurrentState().getAttitude().getRotation(); - Transform ecef_to_body = new Transform(null, rotation_ecef_to_body); - Vector3D trueMagField_body_vec = - ecef_to_body.transformVector(trueMagField_ecef.getFieldVector()); - GeoMagneticElements trueMagField_body = new GeoMagneticElements( - trueMagField_body_vec); - - return trueMagField_body; + return new GeoMagneticElements(trueField_body); } - + /** * Retrieve the measured data from the magnetometer sensors. *

@@ -190,10 +178,10 @@ public GeoMagneticElements retrievePerfectField() { public Vector3D getData_magField() { /* Retrieve the noisy magnetic field. */ Vector3D data = this.retrieveNoisyField().getFieldVector(); - + /* Convert nTesla into Tesla. */ data = data.scalarMultiply(1e-9); - + return data; } From 2df495d919fe62ad8dac7057210210c5750356ff Mon Sep 17 00:00:00 2001 From: Jmcrobbie Date: Mon, 24 Sep 2018 15:28:33 +1000 Subject: [PATCH 03/48] init --- .../utils/logs/ephemeris/EphemerisGenerator.java | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java b/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java index dd440d68..e11143f9 100644 --- a/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java +++ b/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java @@ -94,8 +94,13 @@ public class EphemerisGenerator { /** Attitude AEM ephemeris. */ private File fileAEM; + /** Vector for the mag field */ + + private File fileAEM_mag; /** Attitude AEM File Writer. */ + private FileWriter writerAEM_mag; + private FileWriter writerAEM; /** OrbitWrapper OEM File Writer */ @@ -143,6 +148,7 @@ public void start() { String common = this.path + this.simuName; this.fileOEM = new File(common + "OEM.txt"); this.fileAEM = new File(common + "AEM.txt"); + this.fileAEM_mag = new File(common + "body_mag" + "AEM.txt"); if (!fileOEM.getParentFile().exists()) { fileOEM.getParentFile().mkdirs(); @@ -150,6 +156,9 @@ public void start() { if (!fileAEM.getParentFile().exists()) { fileAEM.getParentFile().mkdir(); } + if (!fileAEM_mag.getParentFile().exists()) { + fileAEM_mag.getParentFile().mkdir(); + } try { /* Creating the files. */ From aa230215b0ff358adb6c0e2231c190ac885fdde5 Mon Sep 17 00:00:00 2001 From: Jmcrobbie Date: Mon, 24 Sep 2018 16:04:24 +1000 Subject: [PATCH 04/48] Writing Header for Magnetic field vector recordings --- .../utils/logs/ephemeris/EphemerisGenerator.java | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java b/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java index e11143f9..bbb26995 100644 --- a/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java +++ b/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java @@ -94,7 +94,8 @@ public class EphemerisGenerator { /** Attitude AEM ephemeris. */ private File fileAEM; - /** Vector for the mag field */ + + /** Vector for the magnetic field */ private File fileAEM_mag; @@ -148,7 +149,7 @@ public void start() { String common = this.path + this.simuName; this.fileOEM = new File(common + "OEM.txt"); this.fileAEM = new File(common + "AEM.txt"); - this.fileAEM_mag = new File(common + "body_mag" + "AEM.txt"); + this.fileAEM_mag = new File(common + "body_mag-" + "AEM.txt"); if (!fileOEM.getParentFile().exists()) { fileOEM.getParentFile().mkdirs(); @@ -164,15 +165,18 @@ public void start() { /* Creating the files. */ fileOEM.createNewFile(); fileAEM.createNewFile(); - + fileAEM_mag.createNewFile(); /* Creating each associated Writer. */ this.writerOEM = new FileWriter(this.fileOEM); this.writerAEM = new FileWriter(this.fileAEM); - + this.writerAEM_mag = new FileWriter(this.fileAEM_mag); /* Generating the headers. */ this.writerAEM.write(this.getAemHeader(OBJECT_NAME, SIMU_ID)); this.writerOEM.write(this.getOemHeader(OBJECT_NAME, SIMU_ID)); - + this.writerAEM_mag.write(this.getAemHeader(OBJECT_NAME, SIMU_ID)); + this.writerOEM.flush(); + this.writerAEM.flush(); + this.writerAEM_mag.flush(); } catch (IOException e) { e.printStackTrace(); } From 5e833e293fe5f4985481bf8c4410158406360bee Mon Sep 17 00:00:00 2001 From: Jmcrobbie Date: Mon, 24 Sep 2018 16:17:26 +1000 Subject: [PATCH 05/48] Added header generating function --- .../logs/ephemeris/EphemerisGenerator.java | 45 +++++++++++++++++-- 1 file changed, 42 insertions(+), 3 deletions(-) diff --git a/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java b/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java index bbb26995..20503dcb 100644 --- a/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java +++ b/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java @@ -173,7 +173,7 @@ public void start() { /* Generating the headers. */ this.writerAEM.write(this.getAemHeader(OBJECT_NAME, SIMU_ID)); this.writerOEM.write(this.getOemHeader(OBJECT_NAME, SIMU_ID)); - this.writerAEM_mag.write(this.getAemHeader(OBJECT_NAME, SIMU_ID)); + this.writerAEM_mag.write(this.getVectorVisHeader("MAGNETIC_FIELD",OBJECT_NAME, SIMU_ID)); this.writerOEM.flush(); this.writerAEM.flush(); this.writerAEM_mag.flush(); @@ -281,8 +281,47 @@ public void writeStep(Satellite satellite) { e.printStackTrace(); } } - - + /** + * Returns the header of the Attitude file for VTS vector visualization + * @param vector name - name of the vector + * @param objectName For the Satellite Object + * @param simuIdentifier Current Simulation ID + * @return Header as a string + */ +private String getVectorVisHeader(String vectorName, String objectName, String simuIdentifier) { + try { + String headerAEM = new String(); + + AbsoluteDate currentDate = new AbsoluteDate( + new GregorianCalendar(TimeZone.getTimeZone("GMT+00")).getTime(), + TimeScalesFactory.getUTC() + ); + + headerAEM += "CIC_AEM_VERS = 1.0" + LS ; + headerAEM += ("CREATION_DATE = " + + currentDate.toString(TimeScalesFactory.getUTC()) ) + LS ; + headerAEM += ("ORIGINATOR = ") + simuIdentifier + LS ; + headerAEM += (" ") + LS ; + headerAEM += ("META_START") + LS ; + headerAEM += ("") + LS ; + headerAEM += ("OBJECT_NAME = ") + objectName +"-"+ vectorName + LS ; + headerAEM += ("OBJECT_ID = MSP001") + LS ; + headerAEM += ("CENTER_NAME = EARTH") + LS ; + headerAEM += ("REF_FRAME_A = EME2000") + LS ; + headerAEM += ("REF_FRAME_B = SC_BODY_1") + LS ; + headerAEM += ("ATTITUDE_DIR = A2B") + LS ; + headerAEM += ("TIME_SYSTEM = UTC") + LS ; + headerAEM += ("ATTITUDE_TYPE = QUATERNION") + LS ; + headerAEM += ("") + LS ; + headerAEM += ("META_STOP") + LS ; + + return headerAEM; + } + catch (OrekitException e) { + e.printStackTrace(); + } + return null; +} /** * Return the header of an AEM ephemeris. * @param objectName For the Satellite Object From 0ea6e6ce26bfa593c4dbb398c149d5b96cf9ea9a Mon Sep 17 00:00:00 2001 From: Jmcrobbie Date: Mon, 24 Sep 2018 16:47:36 +1000 Subject: [PATCH 06/48] magnetic field writing to log files - untested in vts --- .../logs/ephemeris/EphemerisGenerator.java | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java b/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java index 20503dcb..d2cd2d3b 100644 --- a/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java +++ b/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java @@ -26,6 +26,7 @@ import org.hipparchus.geometry.euclidean.threed.Vector3D; import org.orekit.errors.OrekitException; import org.orekit.frames.FramesFactory; +import org.orekit.models.earth.GeoMagneticElements; import org.orekit.propagation.SpacecraftState; import org.orekit.time.AbsoluteDate; import org.orekit.time.TimeScalesFactory; @@ -214,7 +215,22 @@ public void writeStep(Satellite satellite) { ); int days = (int) (seconds / Constants.JULIAN_DAY); seconds = seconds - days * Constants.JULIAN_DAY; - + /* Writing the current mag field to the log file. */ + Vector3D mag_field = satellite.getSensors().getMagnetometer().retrievePerfectField().getFieldVector(); + buff + .append(days) + .append(" ") /* Column Separator */ + .append(seconds) + .append(" ") + .append(mag_field.getX() * 1e-3) /* Conversion to KM */ + .append(" ") + .append(mag_field.getY() * 1e-3) + .append(" ") + .append(mag_field.getZ() * 1e-3) + ; + this.writerAEM_mag.append(buff.toString()+ LS); + this.writerAEM_mag.flush(); + buff = new StringBuffer(); /* Writing the OEM Ephemeris. */ Vector3D position = newState .getPVCoordinates(FramesFactory.getEME2000()) @@ -231,6 +247,7 @@ public void writeStep(Satellite satellite) { .append(" ") .append(position.getZ() * 1e-3) ; + this.writerOEM.append(buff.toString() + LS); this.writerOEM.flush(); From f024b90f0e24e37cbe7044c7b30023a6bc393109 Mon Sep 17 00:00:00 2001 From: Jmcrobbie Date: Mon, 24 Sep 2018 17:17:43 +1000 Subject: [PATCH 07/48] asdf --- .../logs/ephemeris/EphemerisGenerator.java | 58 +++++++++++++++++-- 1 file changed, 54 insertions(+), 4 deletions(-) diff --git a/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java b/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java index d2cd2d3b..e27c519c 100644 --- a/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java +++ b/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java @@ -99,11 +99,27 @@ public class EphemerisGenerator { /** Vector for the magnetic field */ private File fileAEM_mag; + + /** Vector for the angular momentum */ + + private File fileAEM_angMomentum; + + /** vector for the angular velocity */ + + private File fileAEM_angVelocity; + + /** Vector for applied torque */ + + private File fileAEM_torque; /** Attitude AEM File Writer. */ private FileWriter writerAEM_mag; private FileWriter writerAEM; + + private FileWriter writerAEM_angMomentum; + private FileWriter writerAEM_angVel; + private FileWriter writerAEM_torque; /** OrbitWrapper OEM File Writer */ private FileWriter writerOEM; @@ -151,6 +167,9 @@ public void start() { this.fileOEM = new File(common + "OEM.txt"); this.fileAEM = new File(common + "AEM.txt"); this.fileAEM_mag = new File(common + "body_mag-" + "AEM.txt"); + this.fileAEM_angVelocity = new File(common + "ang_mom-" + "AEM.txt"); + this.fileAEM_angMomentum = new File(common + "ang_vel-" + "AEM.txt"); + this.fileAEM_torque = new File(common + "torque-" + "AEM.txt"); if (!fileOEM.getParentFile().exists()) { fileOEM.getParentFile().mkdirs(); @@ -161,23 +180,47 @@ public void start() { if (!fileAEM_mag.getParentFile().exists()) { fileAEM_mag.getParentFile().mkdir(); } + if (!fileAEM_angVelocity.getParentFile().exists()) { + fileAEM_angVelocity.getParentFile().mkdir(); + } + if (!fileAEM_angMomentum.getParentFile().exists()) { + fileAEM_angMomentum.getParentFile().mkdir(); + } + if (!fileAEM_torque.getParentFile().exists()) { + fileAEM_torque.getParentFile().mkdir(); + } + try { + /* Creating the files. */ fileOEM.createNewFile(); fileAEM.createNewFile(); fileAEM_mag.createNewFile(); + fileAEM_angMomentum.createNewFile(); + fileAEM_angVelocity.createNewFile(); + fileAEM_torque.createNewFile(); + /* Creating each associated Writer. */ this.writerOEM = new FileWriter(this.fileOEM); this.writerAEM = new FileWriter(this.fileAEM); this.writerAEM_mag = new FileWriter(this.fileAEM_mag); + this.writerAEM_angVel = new FileWriter(this.fileAEM_angVelocity); + this.writerAEM_angMomentum = new FileWriter(this.fileAEM_angMomentum); + this.writerAEM_torque = new FileWriter(this.fileAEM_torque); /* Generating the headers. */ this.writerAEM.write(this.getAemHeader(OBJECT_NAME, SIMU_ID)); this.writerOEM.write(this.getOemHeader(OBJECT_NAME, SIMU_ID)); this.writerAEM_mag.write(this.getVectorVisHeader("MAGNETIC_FIELD",OBJECT_NAME, SIMU_ID)); + this.writerAEM_angMomentum.write(this.getVectorVisHeader("ANGULAR-MOMENTUM", OBJECT_NAME,SIMU_ID)); + this.writerAEM_angVel.write(this.getVectorVisHeader("ANGULAR-VELOCITY", OBJECT_NAME,SIMU_ID)); + this.writerAEM_torque.write(this.getVectorVisHeader("TORQUE", OBJECT_NAME,SIMU_ID)); this.writerOEM.flush(); this.writerAEM.flush(); this.writerAEM_mag.flush(); + this.writerAEM_angMomentum.flush(); + this.writerAEM_angVel.flush(); + this.writerAEM_torque.flush(); } catch (IOException e) { e.printStackTrace(); } @@ -203,7 +246,7 @@ public void stop() { public void writeStep(Satellite satellite) { SpacecraftState newState = satellite.getStates().getCurrentState(); try { - StringBuffer buff = new StringBuffer(); + /* Determining the time of the state (in offset). */ AbsoluteDate currentDate = newState.getDate(); @@ -215,6 +258,13 @@ public void writeStep(Satellite satellite) { ); int days = (int) (seconds / Constants.JULIAN_DAY); seconds = seconds - days * Constants.JULIAN_DAY; + + /** Writing to the torque file */ + /** Writing to the angular velocity file */ + /** Writing to the angular momentum file */ + + StringBuffer buff = new StringBuffer(); + /* Writing the current mag field to the log file. */ Vector3D mag_field = satellite.getSensors().getMagnetometer().retrievePerfectField().getFieldVector(); buff @@ -222,11 +272,11 @@ public void writeStep(Satellite satellite) { .append(" ") /* Column Separator */ .append(seconds) .append(" ") - .append(mag_field.getX() * 1e-3) /* Conversion to KM */ + .append(mag_field.getX()) .append(" ") - .append(mag_field.getY() * 1e-3) + .append(mag_field.getY()) .append(" ") - .append(mag_field.getZ() * 1e-3) + .append(mag_field.getZ()) ; this.writerAEM_mag.append(buff.toString()+ LS); this.writerAEM_mag.flush(); From 208d1e4364eac91e89fba993a011fb0e875c8f1e Mon Sep 17 00:00:00 2001 From: Jmcrobbie Date: Mon, 24 Sep 2018 17:37:03 +1000 Subject: [PATCH 08/48] Logging angular velocity and angular momentum --- .../logs/ephemeris/EphemerisGenerator.java | 48 ++++++++++++++++++- 1 file changed, 47 insertions(+), 1 deletion(-) diff --git a/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java b/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java index e27c519c..f90aaafe 100644 --- a/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java +++ b/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java @@ -258,12 +258,58 @@ public void writeStep(Satellite satellite) { ); int days = (int) (seconds / Constants.JULIAN_DAY); seconds = seconds - days * Constants.JULIAN_DAY; + + StringBuffer buff = new StringBuffer(); /** Writing to the torque file */ + buff + .append(days) + .append(" ") /* Column Separator */ + .append(seconds) + .append(" ") + .append(" ") + .append(" ") + ; + this.writerAEM_torque.append(buff+LS); + this.writerAEM_torque.flush(); + + buff = new StringBuffer(); + /** Writing to the angular velocity file */ + Vector3D vel = satellite.getAssembly().getAngularMomentum(); + buff + .append(days) + .append(" ") /* Column Separator */ + .append(seconds) + .append(" ") + .append(vel.getX()) + .append(" ") + .append(vel.getY()) + .append(" ") + .append(vel.getZ()) + ; + this.writerAEM_angVel.append(buff+LS); + this.writerAEM_angVel.flush(); + + buff = new StringBuffer(); + /** Writing to the angular momentum file */ + Vector3D mom = satellite.getAssembly().getAngularMomentum(); + buff + .append(days) + .append(" ") /* Column Separator */ + .append(seconds) + .append(" ") + .append(mom.getX()) + .append(" ") + .append(mom.getY()) + .append(" ") + .append(mom.getZ()) + ; + this.writerAEM_angMomentum.append(buff+LS); + this.writerAEM_angMomentum.flush(); - StringBuffer buff = new StringBuffer(); + buff = new StringBuffer(); /* Writing the current mag field to the log file. */ Vector3D mag_field = satellite.getSensors().getMagnetometer().retrievePerfectField().getFieldVector(); From 94b15b0c4fd8085c9dce37044e683e4fc1218425 Mon Sep 17 00:00:00 2001 From: Jmcrobbie Date: Mon, 24 Sep 2018 17:49:22 +1000 Subject: [PATCH 09/48] Added angular acceleration logging For testing purposes, will implement torque getting. This is viable for MVP --- .../simulator/utils/logs/ephemeris/EphemerisGenerator.java | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java b/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java index f90aaafe..185b2a1f 100644 --- a/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java +++ b/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java @@ -262,13 +262,18 @@ public void writeStep(Satellite satellite) { StringBuffer buff = new StringBuffer(); /** Writing to the torque file */ + Vector3D torque = satellite.getStates().getCurrentState().getAttitude().getRotationAcceleration(); + //TODO Change this to take in the actual torque!!!! buff .append(days) .append(" ") /* Column Separator */ .append(seconds) .append(" ") + .append(torque.getX()) .append(" ") + .append(torque.getY()) .append(" ") + .append(torque.getZ()) ; this.writerAEM_torque.append(buff+LS); this.writerAEM_torque.flush(); @@ -276,7 +281,7 @@ public void writeStep(Satellite satellite) { buff = new StringBuffer(); /** Writing to the angular velocity file */ - Vector3D vel = satellite.getAssembly().getAngularMomentum(); + Vector3D vel = satellite.getStates().getCurrentState().getAttitude().getSpin(); buff .append(days) .append(" ") /* Column Separator */ From 54552d73f1f2852ebe2420905ecd7169f0c98a4f Mon Sep 17 00:00:00 2001 From: Jmcrobbie Date: Mon, 24 Sep 2018 18:01:17 +1000 Subject: [PATCH 10/48] Hackish calculation of torque added to logging --- .../utils/logs/ephemeris/EphemerisGenerator.java | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java b/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java index 185b2a1f..3f0fa31f 100644 --- a/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java +++ b/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java @@ -262,18 +262,24 @@ public void writeStep(Satellite satellite) { StringBuffer buff = new StringBuffer(); /** Writing to the torque file */ - Vector3D torque = satellite.getStates().getCurrentState().getAttitude().getRotationAcceleration(); - //TODO Change this to take in the actual torque!!!! + + /** TODO - rewrite this section to be more elegant and correct*/ + + Vector3D ang_accel = satellite.getStates().getCurrentState().getAttitude().getRotationAcceleration(); + double torqueX = ang_accel.getX()/satellite.getAssembly().getBody().getInertiaMatrix()[0][0]; + double torqueY = ang_accel.getY()/satellite.getAssembly().getBody().getInertiaMatrix()[1][1]; + double torqueZ = ang_accel.getZ()/satellite.getAssembly().getBody().getInertiaMatrix()[2][2]; + buff .append(days) .append(" ") /* Column Separator */ .append(seconds) .append(" ") - .append(torque.getX()) + .append(torqueX) .append(" ") - .append(torque.getY()) + .append(torqueY) .append(" ") - .append(torque.getZ()) + .append(torqueZ) ; this.writerAEM_torque.append(buff+LS); this.writerAEM_torque.flush(); From 52e18f87519dd3a89bc4334fd0ca54fa1936a3b1 Mon Sep 17 00:00:00 2001 From: Jmcrobbie Date: Mon, 24 Sep 2018 20:16:54 +1000 Subject: [PATCH 11/48] Added comment to action review --- .../simulator/utils/logs/ephemeris/EphemerisGenerator.java | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java b/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java index 3f0fa31f..bc7cbf03 100644 --- a/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java +++ b/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java @@ -266,6 +266,13 @@ public void writeStep(Satellite satellite) { /** TODO - rewrite this section to be more elegant and correct*/ Vector3D ang_accel = satellite.getStates().getCurrentState().getAttitude().getRotationAcceleration(); + + /** + * + * WARNING!: THis only works for diagonal inertia matrices. + * TODO Resolve this so it works for non diagonal systems. + * */ + double torqueX = ang_accel.getX()/satellite.getAssembly().getBody().getInertiaMatrix()[0][0]; double torqueY = ang_accel.getY()/satellite.getAssembly().getBody().getInertiaMatrix()[1][1]; double torqueZ = ang_accel.getZ()/satellite.getAssembly().getBody().getInertiaMatrix()[2][2]; From 9f20a3540e5ec5d967225db0df9abef56cb76eb1 Mon Sep 17 00:00:00 2001 From: Jmcrobbie Date: Mon, 24 Sep 2018 21:32:29 +1000 Subject: [PATCH 12/48] Hot-fix working in vts --- .../src/main/java/msp/simulator/Main.java | 4 +-- .../logs/ephemeris/EphemerisGenerator.java | 27 ++++++++++++++++--- 2 files changed, 25 insertions(+), 6 deletions(-) diff --git a/simulator/src/main/java/msp/simulator/Main.java b/simulator/src/main/java/msp/simulator/Main.java index baf1b10f..2c896b1a 100644 --- a/simulator/src/main/java/msp/simulator/Main.java +++ b/simulator/src/main/java/msp/simulator/Main.java @@ -48,8 +48,8 @@ public static void main(String[] args) { Dashboard.setInitialRotAcceleration(new Vector3D(0,0,0)); Dashboard.setTorqueDisturbances(true); - Dashboard.setCommandTorqueProvider(TorqueProviderEnum.MEMCACHED); - Dashboard.setMemCachedConnection(true, "127.0.0.1:11211"); + //Dashboard.setCommandTorqueProvider(TorqueProviderEnum.MEMCACHED); + //Dashboard.setMemCachedConnection(false, "127.0.0.1:11211"); Dashboard.setVtsConnection(false); diff --git a/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java b/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java index bc7cbf03..949ed6f6 100644 --- a/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java +++ b/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java @@ -276,17 +276,22 @@ public void writeStep(Satellite satellite) { double torqueX = ang_accel.getX()/satellite.getAssembly().getBody().getInertiaMatrix()[0][0]; double torqueY = ang_accel.getY()/satellite.getAssembly().getBody().getInertiaMatrix()[1][1]; double torqueZ = ang_accel.getZ()/satellite.getAssembly().getBody().getInertiaMatrix()[2][2]; - + Vector3D torque = new Vector3D(torqueX,torqueY,torqueZ); + if (torque.getNorm() != 0) { + torque = torque.normalize(); + } buff .append(days) .append(" ") /* Column Separator */ .append(seconds) .append(" ") - .append(torqueX) + .append(torque.getX()) + .append(" ") + .append(torque.getY()) .append(" ") - .append(torqueY) + .append(torque.getZ()) .append(" ") - .append(torqueZ) + .append(0) ; this.writerAEM_torque.append(buff+LS); this.writerAEM_torque.flush(); @@ -295,6 +300,10 @@ public void writeStep(Satellite satellite) { /** Writing to the angular velocity file */ Vector3D vel = satellite.getStates().getCurrentState().getAttitude().getSpin(); + vel.normalize(); + if (vel.getNorm() != 0) { + vel = vel.normalize(); + } buff .append(days) .append(" ") /* Column Separator */ @@ -305,6 +314,8 @@ public void writeStep(Satellite satellite) { .append(vel.getY()) .append(" ") .append(vel.getZ()) + .append(" ") + .append(0) ; this.writerAEM_angVel.append(buff+LS); this.writerAEM_angVel.flush(); @@ -313,6 +324,9 @@ public void writeStep(Satellite satellite) { /** Writing to the angular momentum file */ Vector3D mom = satellite.getAssembly().getAngularMomentum(); + if(mom.getNorm()!= 0) { + mom = mom.normalize(); + } buff .append(days) .append(" ") /* Column Separator */ @@ -323,6 +337,8 @@ public void writeStep(Satellite satellite) { .append(mom.getY()) .append(" ") .append(mom.getZ()) + .append(" ") + .append(0) ; this.writerAEM_angMomentum.append(buff+LS); this.writerAEM_angMomentum.flush(); @@ -331,6 +347,7 @@ public void writeStep(Satellite satellite) { /* Writing the current mag field to the log file. */ Vector3D mag_field = satellite.getSensors().getMagnetometer().retrievePerfectField().getFieldVector(); + mag_field = mag_field.normalize(); buff .append(days) .append(" ") /* Column Separator */ @@ -341,6 +358,8 @@ public void writeStep(Satellite satellite) { .append(mag_field.getY()) .append(" ") .append(mag_field.getZ()) + .append(" ") + .append(0) ; this.writerAEM_mag.append(buff.toString()+ LS); this.writerAEM_mag.flush(); From c4407eb68d6c14f19d1d37d76d7383908cef0b2b Mon Sep 17 00:00:00 2001 From: Jmcrobbie Date: Mon, 24 Sep 2018 23:47:19 +1000 Subject: [PATCH 13/48] asdf --- .../msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java | 1 - 1 file changed, 1 deletion(-) diff --git a/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java b/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java index 949ed6f6..8479c966 100644 --- a/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java +++ b/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java @@ -300,7 +300,6 @@ public void writeStep(Satellite satellite) { /** Writing to the angular velocity file */ Vector3D vel = satellite.getStates().getCurrentState().getAttitude().getSpin(); - vel.normalize(); if (vel.getNorm() != 0) { vel = vel.normalize(); } From 61c2a1a569732617dc58cdf845749d49a346a14f Mon Sep 17 00:00:00 2001 From: Rowan Skewes Date: Fri, 28 Sep 2018 15:19:37 +1000 Subject: [PATCH 14/48] Add BDot ephemeris log --- .../logs/ephemeris/EphemerisGenerator.java | 54 +++++++++++++++++-- 1 file changed, 51 insertions(+), 3 deletions(-) diff --git a/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java b/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java index 8479c966..5859b645 100644 --- a/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java +++ b/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java @@ -112,6 +112,10 @@ public class EphemerisGenerator { private File fileAEM_torque; + /** vector for the change in the magnetic field vector */ + + private File fileAEM_bDot; + /** Attitude AEM File Writer. */ private FileWriter writerAEM_mag; @@ -120,6 +124,7 @@ public class EphemerisGenerator { private FileWriter writerAEM_angMomentum; private FileWriter writerAEM_angVel; private FileWriter writerAEM_torque; + private FileWriter writerAEM_bDot; /** OrbitWrapper OEM File Writer */ private FileWriter writerOEM; @@ -170,6 +175,7 @@ public void start() { this.fileAEM_angVelocity = new File(common + "ang_mom-" + "AEM.txt"); this.fileAEM_angMomentum = new File(common + "ang_vel-" + "AEM.txt"); this.fileAEM_torque = new File(common + "torque-" + "AEM.txt"); + this.fileAEM_bDot = new File(common + "b_dot-" + "AEM.txt"); if (!fileOEM.getParentFile().exists()) { fileOEM.getParentFile().mkdirs(); @@ -189,6 +195,9 @@ public void start() { if (!fileAEM_torque.getParentFile().exists()) { fileAEM_torque.getParentFile().mkdir(); } + if (!fileAEM_bDot.getParentFile().exists()) { + fileAEM_bDot.getParentFile().mkdir(); + } try { @@ -200,6 +209,7 @@ public void start() { fileAEM_angMomentum.createNewFile(); fileAEM_angVelocity.createNewFile(); fileAEM_torque.createNewFile(); + fileAEM_bDot.createNewFile(); /* Creating each associated Writer. */ this.writerOEM = new FileWriter(this.fileOEM); @@ -208,6 +218,7 @@ public void start() { this.writerAEM_angVel = new FileWriter(this.fileAEM_angVelocity); this.writerAEM_angMomentum = new FileWriter(this.fileAEM_angMomentum); this.writerAEM_torque = new FileWriter(this.fileAEM_torque); + this.writerAEM_bDot = new FileWriter(this.fileAEM_bDot); /* Generating the headers. */ this.writerAEM.write(this.getAemHeader(OBJECT_NAME, SIMU_ID)); this.writerOEM.write(this.getOemHeader(OBJECT_NAME, SIMU_ID)); @@ -215,12 +226,14 @@ public void start() { this.writerAEM_angMomentum.write(this.getVectorVisHeader("ANGULAR-MOMENTUM", OBJECT_NAME,SIMU_ID)); this.writerAEM_angVel.write(this.getVectorVisHeader("ANGULAR-VELOCITY", OBJECT_NAME,SIMU_ID)); this.writerAEM_torque.write(this.getVectorVisHeader("TORQUE", OBJECT_NAME,SIMU_ID)); + this.writerAEM_bDot.write(this.getVectorVisHeader("B_DOT", OBJECT_NAME,SIMU_ID)); this.writerOEM.flush(); this.writerAEM.flush(); this.writerAEM_mag.flush(); this.writerAEM_angMomentum.flush(); this.writerAEM_angVel.flush(); this.writerAEM_torque.flush(); + this.writerAEM_bDot.flush(); } catch (IOException e) { e.printStackTrace(); } @@ -233,6 +246,11 @@ public void stop() { try { this.writerOEM.close(); this.writerAEM.close(); + this.writerAEM_mag.close(); + this.writerAEM_angMomentum.close(); + this.writerAEM_angVel.close(); + this.writerAEM_torque.close(); + this.writerAEM_bDot.close(); } catch (IOException e) { e.printStackTrace(); } @@ -362,6 +380,32 @@ public void writeStep(Satellite satellite) { ; this.writerAEM_mag.append(buff.toString()+ LS); this.writerAEM_mag.flush(); + + + buff = new StringBuffer(); + + /** Writing to the b dot file */ + /* Ignore the rotation of the magnetic field due to the linear movement of + * the spacecraft + */ + Vector3D b_dot = Vector3D.crossProduct(vel, mag_field); + buff + .append(days) + .append(" ") /* Column Separator */ + .append(seconds) + .append(" ") + .append(b_dot.getX()) + .append(" ") + .append(b_dot.getY()) + .append(" ") + .append(b_dot.getZ()) + .append(" ") + .append(0) + ; + this.writerAEM_bDot.append(buff+LS); + this.writerAEM_bDot.flush(); + + buff = new StringBuffer(); /* Writing the OEM Ephemeris. */ Vector3D position = newState @@ -379,7 +423,7 @@ public void writeStep(Satellite satellite) { .append(" ") .append(position.getZ() * 1e-3) ; - + this.writerOEM.append(buff.toString() + LS); this.writerOEM.flush(); @@ -416,14 +460,18 @@ public void writeStep(Satellite satellite) { "Attitude: [{}, {}, {}, {}] \n" + "Spin : {} \n" + "RotAcc : {}\n" + - "Momentum: {}", + "B Dot : {}\n" + + "Momentum: {}\n" + + "Momentum Norm: {}", newState.getAttitude().getRotation().getQ0(), newState.getAttitude().getRotation().getQ1(), newState.getAttitude().getRotation().getQ2(), newState.getAttitude().getRotation().getQ3(), newState.getAttitude().getSpin().toString(), newState.getAttitude().getRotationAcceleration().toString(), - satellite.getAssembly().getAngularMomentum() + b_dot, + satellite.getAssembly().getAngularMomentum(), + satellite.getAssembly().getAngularMomentum().getNorm() ); } catch (OrekitException | IOException e) { From bd00ccc1cf504437762ff4b6f7cb0738c8c92f84 Mon Sep 17 00:00:00 2001 From: Jmcrobbie Date: Fri, 28 Sep 2018 23:23:58 +1000 Subject: [PATCH 15/48] One line commit :P --- .../main/java/msp/simulator/satellite/sensors/Magnetometer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulator/src/main/java/msp/simulator/satellite/sensors/Magnetometer.java b/simulator/src/main/java/msp/simulator/satellite/sensors/Magnetometer.java index 5b26a0d1..c187f578 100644 --- a/simulator/src/main/java/msp/simulator/satellite/sensors/Magnetometer.java +++ b/simulator/src/main/java/msp/simulator/satellite/sensors/Magnetometer.java @@ -43,7 +43,7 @@ public class Magnetometer { * added to each components of the true magnetic field. * (nanoTesla) */ - public static double defaultMagnetoNoiseIntensity = 1e2 ; + public static double defaultMagnetoNoiseIntensity = 0.5918*1000; /* **************************************** */ From c4686241bd84b749878239bd3f024d06cf703d29 Mon Sep 17 00:00:00 2001 From: Jmcrobbie Date: Fri, 28 Sep 2018 23:47:16 +1000 Subject: [PATCH 16/48] added noise to sim --- .../satellite/sensors/Magnetometer.java | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/simulator/src/main/java/msp/simulator/satellite/sensors/Magnetometer.java b/simulator/src/main/java/msp/simulator/satellite/sensors/Magnetometer.java index c187f578..4d401934 100644 --- a/simulator/src/main/java/msp/simulator/satellite/sensors/Magnetometer.java +++ b/simulator/src/main/java/msp/simulator/satellite/sensors/Magnetometer.java @@ -22,6 +22,7 @@ import org.orekit.propagation.SpacecraftState; import org.slf4j.Logger; import org.slf4j.LoggerFactory; +import org.hipparchus.random.RandomDataGenerator; import msp.simulator.environment.Environment; import msp.simulator.environment.geomagneticField.EarthMagneticField; @@ -36,9 +37,14 @@ * @author Florian CHAUBEYRE */ public class Magnetometer { + /* ******* Public Static Attributes ******* */ - + + + /* Random number generator */ + private RandomDataGenerator randomDataGenerator = new RandomDataGenerator(1000); + /** This intensity is used to generate a random number to be * added to each components of the true magnetic field. * (nanoTesla) @@ -85,13 +91,15 @@ public Magnetometer(Environment environment, Assembly assembly) { */ public GeoMagneticElements retrieveNoisyField() { /* Perfect Measure. */ + + GeoMagneticElements perfectMeasure = this.retrievePerfectField(); /* Normally distributed random noise contribution. */ Vector3D noise = new Vector3D ( new double[] { - 2 * (FastMath.random() - 0.5) * this.noiseIntensity, - 2 * (FastMath.random() - 0.5) * this.noiseIntensity, - 2 * (FastMath.random() - 0.5) * this.noiseIntensity + randomDataGenerator.nextNormal(0, this.noiseIntensity), + randomDataGenerator.nextNormal(0, this.noiseIntensity), + randomDataGenerator.nextNormal(0, this.noiseIntensity) }); /* Disturbing the perfect measurement. */ From 51a73fe9105077af4abb2afa1c82f95d32c3918b Mon Sep 17 00:00:00 2001 From: Jmcrobbie Date: Sat, 29 Sep 2018 00:32:27 +1000 Subject: [PATCH 17/48] updated noise --- .../main/java/msp/simulator/satellite/sensors/Magnetometer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulator/src/main/java/msp/simulator/satellite/sensors/Magnetometer.java b/simulator/src/main/java/msp/simulator/satellite/sensors/Magnetometer.java index 4d401934..b8c36878 100644 --- a/simulator/src/main/java/msp/simulator/satellite/sensors/Magnetometer.java +++ b/simulator/src/main/java/msp/simulator/satellite/sensors/Magnetometer.java @@ -49,7 +49,7 @@ public class Magnetometer { * added to each components of the true magnetic field. * (nanoTesla) */ - public static double defaultMagnetoNoiseIntensity = 0.5918*1000; + public static double defaultMagnetoNoiseIntensity = 4.156*1000; /* **************************************** */ From 9d46dde5c7dce2063a323f75d1449756302dac01 Mon Sep 17 00:00:00 2001 From: Jmcrobbie Date: Wed, 3 Oct 2018 17:48:41 +1000 Subject: [PATCH 18/48] updated intertia tensor --- .../msp/simulator/satellite/assembly/SatelliteBody.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/simulator/src/main/java/msp/simulator/satellite/assembly/SatelliteBody.java b/simulator/src/main/java/msp/simulator/satellite/assembly/SatelliteBody.java index a9a310ff..0c1fa026 100644 --- a/simulator/src/main/java/msp/simulator/satellite/assembly/SatelliteBody.java +++ b/simulator/src/main/java/msp/simulator/satellite/assembly/SatelliteBody.java @@ -43,9 +43,9 @@ public class SatelliteBody extends BoxAndSolarArraySpacecraft { /* TODO(rskew) update inertia matrix. */ /** Inertia matrix of the satellite. */ public static double[][] satInertiaMatrix = /* kg.m^2 */ { - {1191.648 * 1.3e-6, 0 , 0 }, - { 0 , 1169.506 * 1.3e-6, 0 }, - { 0 , 0 , 1203.969 * 1.3e-6 }, + {1.9002 * 1e-3, 0 , 0 }, + { 0 , 1.9156 * 1e-3, 0 }, + { 0 , 0 , 1.9496 * 1e-3 }, }; /** Simple balance inertia matrix (Unit matrix). */ From f92a22b31e3ef60910d8625fc96d77d17497f657 Mon Sep 17 00:00:00 2001 From: Jmcrobbie Date: Wed, 3 Oct 2018 17:49:38 +1000 Subject: [PATCH 19/48] removed TODO message --- .../java/msp/simulator/satellite/assembly/SatelliteBody.java | 1 - 1 file changed, 1 deletion(-) diff --git a/simulator/src/main/java/msp/simulator/satellite/assembly/SatelliteBody.java b/simulator/src/main/java/msp/simulator/satellite/assembly/SatelliteBody.java index 0c1fa026..89705851 100644 --- a/simulator/src/main/java/msp/simulator/satellite/assembly/SatelliteBody.java +++ b/simulator/src/main/java/msp/simulator/satellite/assembly/SatelliteBody.java @@ -40,7 +40,6 @@ public class SatelliteBody extends BoxAndSolarArraySpacecraft { /** Mass of the satellite in kilogram. */ public static double satelliteMass = 1.04; - /* TODO(rskew) update inertia matrix. */ /** Inertia matrix of the satellite. */ public static double[][] satInertiaMatrix = /* kg.m^2 */ { {1.9002 * 1e-3, 0 , 0 }, From 3f8d41421949ffc0024d9ab30a5f35d923b25b81 Mon Sep 17 00:00:00 2001 From: Rowan Skewes Date: Thu, 18 Oct 2018 17:34:16 +1100 Subject: [PATCH 20/48] Remove system path for spymemcached dependency in pom --- simulator/pom.xml | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/simulator/pom.xml b/simulator/pom.xml index f7a240f4..47fb9ec5 100644 --- a/simulator/pom.xml +++ b/simulator/pom.xml @@ -31,8 +31,6 @@ net.spy spymemcached 2.12.3 - system - ${project.basedir}/src/main/resources/jar/spymemcached-2.12.3.jar @@ -56,4 +54,4 @@ msp.simulator - \ No newline at end of file + From 216e07d9485239a621a8679f3674ec4f8470ccad Mon Sep 17 00:00:00 2001 From: Jmcrobbie Date: Sun, 21 Oct 2018 00:50:15 +1100 Subject: [PATCH 21/48] implemented torque from pwm actioned comments --- .../torques/MemCachedTorqueProvider.java | 66 ++++++++++++------- 1 file changed, 44 insertions(+), 22 deletions(-) diff --git a/simulator/src/main/java/msp/simulator/dynamic/torques/MemCachedTorqueProvider.java b/simulator/src/main/java/msp/simulator/dynamic/torques/MemCachedTorqueProvider.java index c7a616b8..7ba42808 100644 --- a/simulator/src/main/java/msp/simulator/dynamic/torques/MemCachedTorqueProvider.java +++ b/simulator/src/main/java/msp/simulator/dynamic/torques/MemCachedTorqueProvider.java @@ -12,9 +12,10 @@ * limitations under the License. */ -package msp.simulator.dynamic.torques; +package msp.simulator.dynamic.torques; + +import java.nio.ByteBuffer; -import java.nio.ByteBuffer; import org.hipparchus.geometry.euclidean.threed.Vector3D; import org.orekit.time.AbsoluteDate; @@ -28,7 +29,7 @@ import msp.simulator.satellite.io.MemcachedRawTranscoder; import msp.simulator.utils.logs.CustomLoggingTools; import net.spy.memcached.MemcachedClient; - +import msp.simulator.satellite.sensors.Magnetometer; /** * * @author Florian CHAUBEYRE @@ -39,12 +40,12 @@ public class MemCachedTorqueProvider implements TorqueProvider { /** Public key to access the MemCached hash table. */ public static String torqueCommandKey = "Simulation_Torque_"; - + public static String pwmCommandKey = "Satellite_PWM_"; /* **************************************** */ /** Private Key to store the public key. */ private String torqueKey; - + private String pwmKey; /** Logger of the class */ private static final Logger logger = LoggerFactory.getLogger( MemCachedTorqueProvider.class); @@ -70,7 +71,24 @@ public class MemCachedTorqueProvider implements TorqueProvider { private Vector3D stepTorque; /** Copy of the fixed integration time step. */ - private final double stepSize = Integration.integrationTimeStep; + private static final double stepSize = Integration.integrationTimeStep; + + /* Copy of the magnetic field*/ + private Vector3D b_field; + /* Copy of the sensor object*/ + private Magnetometer magnetometer; + + private final double[] magnetorquerMaxDipole = {0.2,0.2,0.05}; + + private Vector3D Pwm2Torque(Vector3D pwm) { + double x = pwm.getX() * magnetorquerMaxDipole[0]; + double y = pwm.getY() * magnetorquerMaxDipole[1]; + double z = pwm.getZ() * magnetorquerMaxDipole[2]; + Vector3D dipole = new Vector3D(x,y,z); + Vector3D torque; + torque = Vector3D.crossProduct(dipole,this.b_field); + return torque; + } /** * Create the instance of memcached torque provider. @@ -88,9 +106,12 @@ public MemCachedTorqueProvider(Satellite satellite) { this.satState = satellite.getStates(); this.stepStart = this.satState.getCurrentState().getDate(); this.nextAcquisitionDate = this.satState.getInitialState().getDate(); - this.stepTorque = Vector3D.ZERO; - - this.torqueKey = MemCachedTorqueProvider.torqueCommandKey; + this.stepTorque = Vector3D.ZERO; + this.b_field = satellite.getSensors().getMagnetometer().retrievePerfectField().getFieldVector(); + this.magnetometer = satellite.getSensors().getMagnetometer(); + this.torqueKey = MemCachedTorqueProvider.torqueCommandKey; + this.pwmKey = MemCachedTorqueProvider.pwmCommandKey; + this.memcached = satellite.getIO().getMemcached(); this.memcachedTranscoder = satellite.getIO().getRawTranscoder(); @@ -114,7 +135,7 @@ public MemCachedTorqueProvider(Satellite satellite) { public Vector3D getTorque(AbsoluteDate date) { /* Flag to enable the acquisition of the torque for the step. */ boolean acquisition; - + this.b_field = magnetometer.retrievePerfectField().getFieldVector().scalarMultiply(1E-9); // gets magnetic field (nT) then converts to SI /* As the torque is considered constant over a step, we only need * to acquire the torque once at the very beginning of the step. */ this.stepStart = this.satState.getCurrentState().getDate(); @@ -130,31 +151,32 @@ public Vector3D getTorque(AbsoluteDate date) { /* Reading the torque command from MemCached. */ try { - Vector3D torqueCommand; + Vector3D pwmCommand; - double torque_x = ByteBuffer.wrap( + double pwm_x = ByteBuffer.wrap( this.memcached.get( - this.torqueKey + "X", + this.pwmKey + "X", this.memcachedTranscoder)) .getDouble(); - double torque_y = ByteBuffer.wrap( + double pwm_y = ByteBuffer.wrap( this.memcached.get( - this.torqueKey + "Y", + this.pwmKey + "Y", this.memcachedTranscoder)) .getDouble(); - double torque_z = ByteBuffer.wrap( + double pwm_z = ByteBuffer.wrap( this.memcached.get( - this.torqueKey + "Z", + this.pwmKey + "Z", this.memcachedTranscoder)) .getDouble(); - torqueCommand = new Vector3D( - torque_x, - torque_y, - torque_z - ); + pwmCommand = new Vector3D( + pwm_x, + pwm_y, + pwm_z + ); + Vector3D torqueCommand = this.Pwm2Torque(pwmCommand); /* Checking the data transmission. */ if (torqueCommand.isNaN() || torqueCommand.isInfinite()) { From e3721cabfb5eceb89b3d0f15593476184f97642d Mon Sep 17 00:00:00 2001 From: JMcRobbie Date: Thu, 8 Nov 2018 22:50:03 +1100 Subject: [PATCH 22/48] fixed --- simulator/src/main/java/msp/simulator/Main.java | 8 ++++---- .../utils/logs/ephemeris/EphemerisGenerator.java | 9 +++++---- 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/simulator/src/main/java/msp/simulator/Main.java b/simulator/src/main/java/msp/simulator/Main.java index 2c896b1a..572cd94f 100644 --- a/simulator/src/main/java/msp/simulator/Main.java +++ b/simulator/src/main/java/msp/simulator/Main.java @@ -40,16 +40,16 @@ public static void main(String[] args) { Dashboard.setSimulationDuration(1000000); Dashboard.setIntegrationTimeStep(0.1); - Dashboard.setEphemerisTimeStep(1.0); + Dashboard.setEphemerisTimeStep(0.1); Dashboard.setSatelliteInertiaMatrix(SatelliteBody.satInertiaMatrix); Dashboard.setInitialAttitudeQuaternion(new Quaternion(1, 0, 0, 0)); Dashboard.setInitialSpin(new Vector3D(0.5, 0.5, 0.5)); Dashboard.setInitialRotAcceleration(new Vector3D(0,0,0)); - Dashboard.setTorqueDisturbances(true); + Dashboard.setTorqueDisturbances(false); - //Dashboard.setCommandTorqueProvider(TorqueProviderEnum.MEMCACHED); - //Dashboard.setMemCachedConnection(false, "127.0.0.1:11211"); + Dashboard.setCommandTorqueProvider(TorqueProviderEnum.MEMCACHED); + Dashboard.setMemCachedConnection(true, "127.0.0.1:11211"); Dashboard.setVtsConnection(false); diff --git a/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java b/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java index 5859b645..0bfa389a 100644 --- a/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java +++ b/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java @@ -364,17 +364,18 @@ public void writeStep(Satellite satellite) { /* Writing the current mag field to the log file. */ Vector3D mag_field = satellite.getSensors().getMagnetometer().retrievePerfectField().getFieldVector(); - mag_field = mag_field.normalize(); + Vector3D mag_unit; + mag_unit = mag_field.normalize(); buff .append(days) .append(" ") /* Column Separator */ .append(seconds) .append(" ") - .append(mag_field.getX()) + .append(mag_unit.getX()) .append(" ") - .append(mag_field.getY()) + .append(mag_unit.getY()) .append(" ") - .append(mag_field.getZ()) + .append(mag_unit.getZ()) .append(" ") .append(0) ; From e45d7acee21e30ce5ce7f620d552dbcd7dc5bb6f Mon Sep 17 00:00:00 2001 From: JMcRobbie Date: Thu, 8 Nov 2018 23:06:52 +1100 Subject: [PATCH 23/48] fixed euler equations and logged torques directly for debugging --- .../dynamic/propagation/integration/RotAccProvider.java | 4 ++-- .../simulator/dynamic/torques/MemCachedTorqueProvider.java | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/simulator/src/main/java/msp/simulator/dynamic/propagation/integration/RotAccProvider.java b/simulator/src/main/java/msp/simulator/dynamic/propagation/integration/RotAccProvider.java index 8b0b3c9a..8e4dc34a 100644 --- a/simulator/src/main/java/msp/simulator/dynamic/propagation/integration/RotAccProvider.java +++ b/simulator/src/main/java/msp/simulator/dynamic/propagation/integration/RotAccProvider.java @@ -138,8 +138,8 @@ public static double[] computeEulerEquations( * the Euler Equations of motion for a rotating rigid body. */ rotAcc[0] = (M1 - (I3 - I2) * W2 * W3) / I1 ; - rotAcc[1] = (M2 - (I1 - I3) * W3 * W1) / I1 ; - rotAcc[2] = (M3 - (I2 - I1) * W1 * W2) / I1 ; + rotAcc[1] = (M2 - (I1 - I3) * W3 * W1) / I2 ; + rotAcc[2] = (M3 - (I2 - I1) * W1 * W2) / I3 ; return rotAcc; } diff --git a/simulator/src/main/java/msp/simulator/dynamic/torques/MemCachedTorqueProvider.java b/simulator/src/main/java/msp/simulator/dynamic/torques/MemCachedTorqueProvider.java index 7ba42808..5276e948 100644 --- a/simulator/src/main/java/msp/simulator/dynamic/torques/MemCachedTorqueProvider.java +++ b/simulator/src/main/java/msp/simulator/dynamic/torques/MemCachedTorqueProvider.java @@ -192,7 +192,7 @@ public Vector3D getTorque(AbsoluteDate date) { } /* Debug Information */ - logger.debug("Torque Provider (Acquisition): " + date.toString() +" - " + + logger.info("Torque Provider (Acquisition): " + date.toString() +" - " + this.stepTorque.toString()); } else { From d300debcaf61612f1bf8725f3e44ea85e34c1aac Mon Sep 17 00:00:00 2001 From: JMcRobbie Date: Thu, 8 Nov 2018 23:16:38 +1100 Subject: [PATCH 24/48] removed misleading and useless line from main --- simulator/src/main/java/msp/simulator/Main.java | 1 - 1 file changed, 1 deletion(-) diff --git a/simulator/src/main/java/msp/simulator/Main.java b/simulator/src/main/java/msp/simulator/Main.java index 572cd94f..bfd2ed23 100644 --- a/simulator/src/main/java/msp/simulator/Main.java +++ b/simulator/src/main/java/msp/simulator/Main.java @@ -41,7 +41,6 @@ public static void main(String[] args) { Dashboard.setIntegrationTimeStep(0.1); Dashboard.setEphemerisTimeStep(0.1); - Dashboard.setSatelliteInertiaMatrix(SatelliteBody.satInertiaMatrix); Dashboard.setInitialAttitudeQuaternion(new Quaternion(1, 0, 0, 0)); Dashboard.setInitialSpin(new Vector3D(0.5, 0.5, 0.5)); From 934bc07d67d4f98651ce9dbe57e353e31e0d4798 Mon Sep 17 00:00:00 2001 From: jmcrobbie Date: Thu, 8 Nov 2018 23:56:05 +1100 Subject: [PATCH 25/48] fix --- simulator/src/main/java/msp/simulator/user/Dashboard.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulator/src/main/java/msp/simulator/user/Dashboard.java b/simulator/src/main/java/msp/simulator/user/Dashboard.java index 681253b1..8857cf35 100644 --- a/simulator/src/main/java/msp/simulator/user/Dashboard.java +++ b/simulator/src/main/java/msp/simulator/user/Dashboard.java @@ -121,7 +121,7 @@ public static void setDefaultConfiguration() { /* **** Structure Settings **** */ Dashboard.setSatBoxSizeWithNoSolarPanel(new double[]{0.01, 0.01, 0.01}); Dashboard.setSatelliteMass(1.0); - Dashboard.setSatelliteInertiaMatrix(SatelliteBody.simpleBalancedInertiaMatrix); + Dashboard.setSatelliteInertiaMatrix(SatelliteBody.satInertiaMatrix); /* **** Structure Settings **** */ Dashboard.setMagnetometerNoiseIntensity(1e2); From 59e23a3ae11c23c53ab3091bf38f51e1cdf94272 Mon Sep 17 00:00:00 2001 From: JMcRobbie Date: Fri, 9 Nov 2018 00:50:21 +1100 Subject: [PATCH 26/48] fixed itrf to body conversions --- .../satellite/assembly/Assembly.java | 24 ++++++++----------- 1 file changed, 10 insertions(+), 14 deletions(-) diff --git a/simulator/src/main/java/msp/simulator/satellite/assembly/Assembly.java b/simulator/src/main/java/msp/simulator/satellite/assembly/Assembly.java index ed27dd00..be0e1667 100644 --- a/simulator/src/main/java/msp/simulator/satellite/assembly/Assembly.java +++ b/simulator/src/main/java/msp/simulator/satellite/assembly/Assembly.java @@ -37,7 +37,7 @@ public class Assembly { /** Logger of the class */ private static final Logger logger = LoggerFactory.getLogger(Assembly.class); - + /** Earth instance of the simulation. */ private Earth earth; @@ -46,9 +46,6 @@ public class Assembly { /** Instance of the satellite initial state in space. */ private SatelliteStates satelliteStates; - - /** Satellite frame. */ - private Frame satelliteFrame; /** * Build the satellite as a body and a state vector. @@ -62,11 +59,6 @@ public Assembly(Environment environment) { this.satelliteBody = new SatelliteBody(environment); this.satelliteStates = new SatelliteStates(environment, satelliteBody); - this.satelliteFrame = new Frame ( - FramesFactory.getEME2000(), - this.getStates().getCurrentState().toTransform(), - "SatelliteFrame" - ); this.earth = environment.getSolarSystem().getEarth(); } @@ -95,7 +87,11 @@ public SatelliteStates getStates() { * frame fixed with the axis body. */ public Frame getSatelliteFrame() { - return this.satelliteFrame; + return new Frame ( + FramesFactory.getEME2000(), + this.getStates().getCurrentState().toTransform(), + "SatelliteFrame" + ); } /** @@ -105,7 +101,7 @@ public Frame getSatelliteFrame() { public Vector3D getAngularMomentum() { Vector3D rotationRate = this.satelliteStates .getCurrentState().getAttitude().getSpin(); - + double[][] inertiaMatrix = this.satelliteBody.getInertiaMatrix(); Vector3D row0 = new Vector3D(inertiaMatrix[0]); @@ -130,15 +126,15 @@ public Transform getItrf2body(AbsoluteDate date) { Transform itrf2body = null; try { itrf2body = this.earth.getRotatingFrame().getTransformTo( - this.satelliteFrame, + this.getSatelliteFrame(), date ); } catch (OrekitException e) { e.printStackTrace(); } - + return itrf2body; } -} +} \ No newline at end of file From 65458b0548c1b7cfff2055990c8c3cf1584de70a Mon Sep 17 00:00:00 2001 From: JMcRobbie Date: Fri, 9 Nov 2018 01:03:02 +1100 Subject: [PATCH 27/48] Applied a fix for standinmemcached pop and logged some more info --- .../dynamic/torques/MemCachedTorqueProvider.java | 1 + .../msp/simulator/groundStation/GroundStation.java | 14 +++++++------- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/simulator/src/main/java/msp/simulator/dynamic/torques/MemCachedTorqueProvider.java b/simulator/src/main/java/msp/simulator/dynamic/torques/MemCachedTorqueProvider.java index 5276e948..c6268e66 100644 --- a/simulator/src/main/java/msp/simulator/dynamic/torques/MemCachedTorqueProvider.java +++ b/simulator/src/main/java/msp/simulator/dynamic/torques/MemCachedTorqueProvider.java @@ -86,6 +86,7 @@ private Vector3D Pwm2Torque(Vector3D pwm) { double z = pwm.getZ() * magnetorquerMaxDipole[2]; Vector3D dipole = new Vector3D(x,y,z); Vector3D torque; + logger.info("PWM SIGNAL:"+dipole.toString()); torque = Vector3D.crossProduct(dipole,this.b_field); return torque; } diff --git a/simulator/src/main/java/msp/simulator/groundStation/GroundStation.java b/simulator/src/main/java/msp/simulator/groundStation/GroundStation.java index 83042569..51210c6f 100644 --- a/simulator/src/main/java/msp/simulator/groundStation/GroundStation.java +++ b/simulator/src/main/java/msp/simulator/groundStation/GroundStation.java @@ -158,7 +158,7 @@ public void executeMission(AbsoluteDate date) { byte[] raan = MemcachedRawTranscoder.toRawByteArray( Double.valueOf(tle.getLine2().substring(17, 25))); - byte[] eccentricity = MemcachedRawTranscoder.toRawByteArray( + byte[] eccentricity1e7 = MemcachedRawTranscoder.toRawByteArray( Double.valueOf(tle.getLine2().substring(26, 33))); byte[] argPerigee = MemcachedRawTranscoder.toRawByteArray( @@ -175,7 +175,7 @@ public void executeMission(AbsoluteDate date) { memcached.set("Simulation_TLE_Mean_Motion_First_Deriv" , 0, meanMotionFirstDerivative); memcached.set("Simulation_TLE_Mean_Motion" , 0, meanMotion); memcached.set("Simulation_TLE_Argument_Perigee" , 0, argPerigee); - memcached.set("Simulation_TLE_Eccentricity" , 0, eccentricity); + memcached.set("Simulation_TLE_Eccentricity_1e7" , 0, eccentricity1e7); memcached.set("Simulation_TLE_Mean_Anomaly" , 0, meanAnomaly); memcached.set("Simulation_TLE_Inclination" , 0, inclination); memcached.set("Simulation_TLE_Bstar" , 0, bStar); @@ -213,15 +213,15 @@ public void executeMission(AbsoluteDate date) { + "TLE_Epoch: " + + ByteBuffer.wrap(epoch).getDouble() + "\n" - + "TLE_Eccentricity: " + - + ByteBuffer.wrap(eccentricity).getDouble() + + "TLE_Eccentricity_1e7: " + + + ByteBuffer.wrap(eccentricity1e7).getDouble() + "\n" + "TLE_Argument_Perigee: " + + ByteBuffer.wrap(argPerigee).getDouble() + "\n" - + "Raw : " + tle.getLine1() + + "Raw: " + tle.getLine1() + "\n" - + "Raw : " + tle.getLine2() + + "Raw: " + tle.getLine2() ; logger.info(tleUpdate); @@ -250,4 +250,4 @@ public boolean isTeamWorking(AbsoluteDate date) { return teamIsWorking; } -} +} \ No newline at end of file From d61a40cc21b86bba3153b16d803d453fbe265fd8 Mon Sep 17 00:00:00 2001 From: JMcRobbie Date: Fri, 9 Nov 2018 01:22:35 +1100 Subject: [PATCH 28/48] fixed gyro reads --- .../main/java/msp/simulator/satellite/Satellite.java | 2 +- .../msp/simulator/satellite/sensors/Gyrometer.java | 11 ++++++----- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/simulator/src/main/java/msp/simulator/satellite/Satellite.java b/simulator/src/main/java/msp/simulator/satellite/Satellite.java index f5e5b2d1..2f116da8 100644 --- a/simulator/src/main/java/msp/simulator/satellite/Satellite.java +++ b/simulator/src/main/java/msp/simulator/satellite/Satellite.java @@ -105,7 +105,7 @@ public void executeStepMission() { ); /* Gyrometer Sensor Measurement */ - Vector3D gyroMeasure = this.getSensors().getGyrometer().getData_rotAcc(); + Vector3D gyroMeasure = this.getSensors().getGyrometer().getData_angularVelocity(); byte[] rawGyro_x = MemcachedRawTranscoder.toRawByteArray(gyroMeasure.getX()); byte[] rawGyro_y = MemcachedRawTranscoder.toRawByteArray(gyroMeasure.getY()); byte[] rawGyro_z = MemcachedRawTranscoder.toRawByteArray(gyroMeasure.getZ()); diff --git a/simulator/src/main/java/msp/simulator/satellite/sensors/Gyrometer.java b/simulator/src/main/java/msp/simulator/satellite/sensors/Gyrometer.java index ef1131fd..31bee55f 100644 --- a/simulator/src/main/java/msp/simulator/satellite/sensors/Gyrometer.java +++ b/simulator/src/main/java/msp/simulator/satellite/sensors/Gyrometer.java @@ -34,7 +34,8 @@ public class Gyrometer { /** This intensity is used to generate a random number to be * added to each components of the sensor data. */ - public static double defaultGyroNoiseIntensity = 1e-3; + //public static double defaultGyroNoiseIntensity = 1e-3; + public static double defaultGyroNoiseIntensity = 0; /* **************************************** */ @@ -65,13 +66,13 @@ public Gyrometer(Environment environment, Assembly assembly) { * Retrieve the data from the sensor. * @return Rotational Acceleration */ - public Vector3D getData_rotAcc() { - /* Get the acceleration from the satellite state. */ + public Vector3D getData_angularVelocity() { + /* Get the angular velocity from the satellite state. */ /* Note that these data are already in the satellite * body frame! */ Vector3D data = this.assembly.getStates() - .getCurrentState().getAttitude().getRotationAcceleration(); + .getCurrentState().getAttitude().getSpin(); /* Add the noise contribution. */ Vector3D noise = new Vector3D( @@ -86,4 +87,4 @@ public Vector3D getData_rotAcc() { } -} +} \ No newline at end of file From 0c45c6f2e02f05ab4b55dbc0b1b3ccef2284431b Mon Sep 17 00:00:00 2001 From: JMcRobbie Date: Fri, 9 Nov 2018 03:07:07 +1100 Subject: [PATCH 29/48] fix --- simulator/src/main/java/msp/simulator/Main.java | 2 +- .../msp/simulator/dynamic/torques/MemCachedTorqueProvider.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/simulator/src/main/java/msp/simulator/Main.java b/simulator/src/main/java/msp/simulator/Main.java index bfd2ed23..08391e54 100644 --- a/simulator/src/main/java/msp/simulator/Main.java +++ b/simulator/src/main/java/msp/simulator/Main.java @@ -43,7 +43,7 @@ public static void main(String[] args) { Dashboard.setEphemerisTimeStep(0.1); Dashboard.setInitialAttitudeQuaternion(new Quaternion(1, 0, 0, 0)); - Dashboard.setInitialSpin(new Vector3D(0.5, 0.5, 0.5)); + Dashboard.setInitialSpin(new Vector3D(0.3, 0.3, 0.3)); // tip off 30 degrees per second Dashboard.setInitialRotAcceleration(new Vector3D(0,0,0)); Dashboard.setTorqueDisturbances(false); diff --git a/simulator/src/main/java/msp/simulator/dynamic/torques/MemCachedTorqueProvider.java b/simulator/src/main/java/msp/simulator/dynamic/torques/MemCachedTorqueProvider.java index c6268e66..f36fa666 100644 --- a/simulator/src/main/java/msp/simulator/dynamic/torques/MemCachedTorqueProvider.java +++ b/simulator/src/main/java/msp/simulator/dynamic/torques/MemCachedTorqueProvider.java @@ -78,7 +78,7 @@ public class MemCachedTorqueProvider implements TorqueProvider { /* Copy of the sensor object*/ private Magnetometer magnetometer; - private final double[] magnetorquerMaxDipole = {0.2,0.2,0.05}; + private final double[] magnetorquerMaxDipole = {0.2,0.2,0.02}; private Vector3D Pwm2Torque(Vector3D pwm) { double x = pwm.getX() * magnetorquerMaxDipole[0]; From 6be764d88cde6866e81ac91fa4b54fe3ff77fd1d Mon Sep 17 00:00:00 2001 From: JMcRobbie Date: Fri, 9 Nov 2018 10:43:45 +1100 Subject: [PATCH 30/48] asd --- .../msp/simulator/dynamic/torques/MemCachedTorqueProvider.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulator/src/main/java/msp/simulator/dynamic/torques/MemCachedTorqueProvider.java b/simulator/src/main/java/msp/simulator/dynamic/torques/MemCachedTorqueProvider.java index f36fa666..6fe77313 100644 --- a/simulator/src/main/java/msp/simulator/dynamic/torques/MemCachedTorqueProvider.java +++ b/simulator/src/main/java/msp/simulator/dynamic/torques/MemCachedTorqueProvider.java @@ -78,7 +78,7 @@ public class MemCachedTorqueProvider implements TorqueProvider { /* Copy of the sensor object*/ private Magnetometer magnetometer; - private final double[] magnetorquerMaxDipole = {0.2,0.2,0.02}; + private final double[] magnetorquerMaxDipole = {0.2,0.2,0.2}; private Vector3D Pwm2Torque(Vector3D pwm) { double x = pwm.getX() * magnetorquerMaxDipole[0]; From ecb8ffe418f594c73ce417c4fab1635cc9f4109c Mon Sep 17 00:00:00 2001 From: jmcrobbie Date: Tue, 5 Feb 2019 17:31:00 +1030 Subject: [PATCH 31/48] init --- README.md | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 507deb83..dbd934c6 100644 --- a/README.md +++ b/README.md @@ -16,7 +16,11 @@ Optional, but advised: - Right click on simulator project > Maven > Update Project... > Select all > Force Update of Snapchots/Releases > OK The simulator project is now set up. You can launch the main method and some tests. - +## Command Line Execution of Simulation +### Linux +- cd into /simulator +- mvn compile +- mvn exec:java-D"exec.mainClass"="msp.simulator.Main" ## Simple Test Execution: - In Eclipse, select the test to run in src/test/java/msp/simulator - Run As > JUnit Test From 6e2fe749cf3cb813bcef7ff1fd299b2a698421ca Mon Sep 17 00:00:00 2001 From: jmcrobbie Date: Tue, 5 Feb 2019 17:33:37 +1030 Subject: [PATCH 32/48] asf: --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index dbd934c6..5f8882f6 100644 --- a/README.md +++ b/README.md @@ -19,8 +19,8 @@ The simulator project is now set up. You can launch the main method and some tes ## Command Line Execution of Simulation ### Linux - cd into /simulator -- mvn compile -- mvn exec:java-D"exec.mainClass"="msp.simulator.Main" +- $mvn compile +- $mvn exec:java-D"exec.mainClass"="msp.simulator.Main" ## Simple Test Execution: - In Eclipse, select the test to run in src/test/java/msp/simulator - Run As > JUnit Test From 0ef553aa1bbade8a4c787b26d6dff4f8c32eba9e Mon Sep 17 00:00:00 2001 From: jmcrobbie Date: Tue, 5 Feb 2019 17:36:05 +1030 Subject: [PATCH 33/48] fixed --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 5f8882f6..b0f87af8 100644 --- a/README.md +++ b/README.md @@ -20,7 +20,7 @@ The simulator project is now set up. You can launch the main method and some tes ### Linux - cd into /simulator - $mvn compile -- $mvn exec:java-D"exec.mainClass"="msp.simulator.Main" +- $mvn mvn exec:java -D"exec.mainClass"="msp.simulator.Main" ## Simple Test Execution: - In Eclipse, select the test to run in src/test/java/msp/simulator - Run As > JUnit Test From 68383b5a27e724abda7ed9156299c3576f47c394 Mon Sep 17 00:00:00 2001 From: jmcrobbie Date: Tue, 5 Feb 2019 17:31:00 +1030 Subject: [PATCH 34/48] init --- .../torques/ControllerTorqueProvider.java | 41 +++++++++++++++++++ .../dynamic/torques/TorqueProviderEnum.java | 2 +- 2 files changed, 42 insertions(+), 1 deletion(-) create mode 100644 simulator/src/main/java/msp/simulator/dynamic/torques/ControllerTorqueProvider.java diff --git a/simulator/src/main/java/msp/simulator/dynamic/torques/ControllerTorqueProvider.java b/simulator/src/main/java/msp/simulator/dynamic/torques/ControllerTorqueProvider.java new file mode 100644 index 00000000..897b5e13 --- /dev/null +++ b/simulator/src/main/java/msp/simulator/dynamic/torques/ControllerTorqueProvider.java @@ -0,0 +1,41 @@ +/* Copyright 20017-2018 Melbourne Space Program + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package msp.simulator.dynamic.torques; + +import org.hipparchus.geometry.euclidean.threed.Vector3D; +import org.orekit.time.AbsoluteDate; + +import msp.simulator.satellite.Satellite; + +/** + * + * @author Jack McRobbie + * This class represents the satellites own method + * for providing stabilization and control + */ + + +public class ControllerTorqueProvider implements TorqueProvider{ + public ControllerTorqueProvider(Satellite satellite, AbsoluteDate date) { + + } + + public Vector3D getTorque(AbsoluteDate date) { + Vector3D a = new Vector3D(null); + return a; //TODO + } + + + +} diff --git a/simulator/src/main/java/msp/simulator/dynamic/torques/TorqueProviderEnum.java b/simulator/src/main/java/msp/simulator/dynamic/torques/TorqueProviderEnum.java index 9b860d12..3068667f 100644 --- a/simulator/src/main/java/msp/simulator/dynamic/torques/TorqueProviderEnum.java +++ b/simulator/src/main/java/msp/simulator/dynamic/torques/TorqueProviderEnum.java @@ -23,7 +23,7 @@ public enum TorqueProviderEnum { /* Command torque provider. */ MEMCACHED(0), SCENARIO(0), - + CONTROLLER(0), /* Disturbances. */ GRAVITY(1), ATMOSPHERIC(2), From 268acd9e5a17b59ca3db2644da12f456dd8816e2 Mon Sep 17 00:00:00 2001 From: jmcrobbie Date: Tue, 12 Feb 2019 16:27:45 +1030 Subject: [PATCH 35/48] Begun major refactoring to create ADCS feature! --- simulator/.classpath | 6 +- .../.settings/org.eclipse.jdt.core.prefs | 1 + .../satellite/ADACS/sensors/Gyrometer.java | 90 ++++++++ .../ADACS/sensors/InfraredSensor.java | 91 ++++++++ .../satellite/ADACS/sensors/Magnetometer.java | 203 ++++++++++++++++++ .../satellite/ADACS/sensors/Sensors.java | 129 +++++++++++ .../satellite/ADACS/sensors/package-info.java | 22 ++ .../msp/simulator/satellite/ADCS/ADCS.java | 38 ++++ .../satellite/ADCS/Actuators/Actuators.java | 39 ++++ .../ADCS/Actuators/MagnetoTorquers.java | 40 ++++ .../ADCS/Actuators/package-info.java | 20 ++ .../satellite/ADCS/Controller/B_Dot.java | 32 +++ .../ADCS/Controller/ComputeTorque.java | 37 ++++ .../satellite/ADCS/Controller/Controller.java | 22 ++ .../satellite/ADCS/Estimators/Estimators.java | 22 ++ 15 files changed, 787 insertions(+), 5 deletions(-) create mode 100644 simulator/src/main/java/msp/simulator/satellite/ADACS/sensors/Gyrometer.java create mode 100644 simulator/src/main/java/msp/simulator/satellite/ADACS/sensors/InfraredSensor.java create mode 100644 simulator/src/main/java/msp/simulator/satellite/ADACS/sensors/Magnetometer.java create mode 100644 simulator/src/main/java/msp/simulator/satellite/ADACS/sensors/Sensors.java create mode 100644 simulator/src/main/java/msp/simulator/satellite/ADACS/sensors/package-info.java create mode 100644 simulator/src/main/java/msp/simulator/satellite/ADCS/ADCS.java create mode 100644 simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/Actuators.java create mode 100644 simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/MagnetoTorquers.java create mode 100644 simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/package-info.java create mode 100644 simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/B_Dot.java create mode 100644 simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/ComputeTorque.java create mode 100644 simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/Controller.java create mode 100644 simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/Estimators.java diff --git a/simulator/.classpath b/simulator/.classpath index 7e4ca5cd..a5d95095 100644 --- a/simulator/.classpath +++ b/simulator/.classpath @@ -15,6 +15,7 @@ + @@ -27,10 +28,5 @@ - - - - - diff --git a/simulator/.settings/org.eclipse.jdt.core.prefs b/simulator/.settings/org.eclipse.jdt.core.prefs index 13b3428a..91ca62e2 100644 --- a/simulator/.settings/org.eclipse.jdt.core.prefs +++ b/simulator/.settings/org.eclipse.jdt.core.prefs @@ -10,4 +10,5 @@ org.eclipse.jdt.core.compiler.debug.sourceFile=generate org.eclipse.jdt.core.compiler.problem.assertIdentifier=error org.eclipse.jdt.core.compiler.problem.enumIdentifier=error org.eclipse.jdt.core.compiler.problem.forbiddenReference=warning +org.eclipse.jdt.core.compiler.release=disabled org.eclipse.jdt.core.compiler.source=1.8 diff --git a/simulator/src/main/java/msp/simulator/satellite/ADACS/sensors/Gyrometer.java b/simulator/src/main/java/msp/simulator/satellite/ADACS/sensors/Gyrometer.java new file mode 100644 index 00000000..6e3b25e3 --- /dev/null +++ b/simulator/src/main/java/msp/simulator/satellite/ADACS/sensors/Gyrometer.java @@ -0,0 +1,90 @@ +/* Copyright 20017-2018 Melbourne Space Program + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package msp.simulator.satellite.ADACS.sensors; + +import org.hipparchus.geometry.euclidean.threed.Vector3D; +import org.hipparchus.util.FastMath; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import msp.simulator.environment.Environment; +import msp.simulator.satellite.assembly.Assembly; +import msp.simulator.utils.logs.CustomLoggingTools; + +/** + * Modelize the gyrometer sensor of the satellite. + * + * @author Florian CHAUBEYRE + */ +public class Gyrometer { + + /* ******* Public Static Attributes ******* */ + + /** This intensity is used to generate a random number to be + * added to each components of the sensor data. + */ + //public static double defaultGyroNoiseIntensity = 1e-3; + public static double defaultGyroNoiseIntensity = 0; + + /* **************************************** */ + + /** Logger of the class. */ + private static final Logger logger = LoggerFactory.getLogger( + Gyrometer.class); + + /** Instance of the simulation. */ + private Assembly assembly; + + /** Normal noise disturbing the gyrometer measures. */ + private double gyroNoiseIntensity; + + /** + * Simple constructor of the gyrometer. + * @param environment Instance of the simulation + * @param assembly Instance of the simulation + */ + public Gyrometer(Environment environment, Assembly assembly) { + logger.info(CustomLoggingTools.indentMsg(logger, + " -> Building the Gyrometer...")); + + this.assembly = assembly; + this.gyroNoiseIntensity = defaultGyroNoiseIntensity; + } + + /** + * Retrieve the data from the sensor. + * @return Rotational Acceleration + */ + public Vector3D getData_angularVelocity() { + /* Get the angular velocity from the satellite state. */ + /* Note that these data are already in the satellite + * body frame! + */ + Vector3D data = this.assembly.getStates() + .getCurrentState().getAttitude().getSpin(); + + /* Add the noise contribution. */ + Vector3D noise = new Vector3D( + 2 * (FastMath.random() - 0.5) * this.gyroNoiseIntensity, + 2 * (FastMath.random() - 0.5) * this.gyroNoiseIntensity, + 2 * (FastMath.random() - 0.5) * this.gyroNoiseIntensity + ); + + data = data.add(noise); + + return data; + } + + +} \ No newline at end of file diff --git a/simulator/src/main/java/msp/simulator/satellite/ADACS/sensors/InfraredSensor.java b/simulator/src/main/java/msp/simulator/satellite/ADACS/sensors/InfraredSensor.java new file mode 100644 index 00000000..f1b2b0a2 --- /dev/null +++ b/simulator/src/main/java/msp/simulator/satellite/ADACS/sensors/InfraredSensor.java @@ -0,0 +1,91 @@ +package msp.simulator.satellite.ADACS.sensors; + +import org.hipparchus.geometry.euclidean.threed.*; +import org.hipparchus.util.FastMath; + +/** + * This class represents the infrared sensor of the satellite + * + * @author Braeden BORG + */ +public class InfraredSensor { + + /** + * Constants determined for a ninth-order polynomial that best represents the + * model curve for infrared readings versus angle from Nadir + */ + private static final double p1 = 0.007330053825571, p2 = -0.090570226260328, p3 = 0.445759574332216, + p4 = -1.123946349873612, p5 = 1.594507643864135, p6 = -1.440516312413851, p7 = 1.178825031933260, + p8 = -1.078355217581003, p9 = 0.182686213443513, p10 = 0.987208931556143; + private double infraredReading, angleReading; + private Vector3D sideNormal; + + /** + * Creates an infrared sensor for a satellite side with an infrared reading and + * associated angle to nadir + * + * @param sideNormalVector + * Vector normal to a satellite side + */ + public InfraredSensor(Vector3D sideNormalVector) { + infraredReading = 0; + angleReading = 0; + sideNormal = sideNormalVector; + } + + /** + * Function to compute the infrared reading for a face of the satellite given + * the angle to Nadir + * + * @param angle + * Angle to Nadir vector along an axis plane + * @return result The corresponding infrared reading for the angle to Nadir + */ + private double angleToInfrared(double angle) { + double result, x = angle; + /* + * Uses a ninth-order polynomial of best fit to the model infrared curve + */ + result = p1 * FastMath.pow(x, 9) + p2 * FastMath.pow(x, 8) + p3 * FastMath.pow(x, 7) + p4 * FastMath.pow(x, 6) + + p5 * FastMath.pow(x, 5) + p6 * FastMath.pow(x, 4) + p7 * FastMath.pow(x, 3) + p8 * FastMath.pow(x, 2) + + p9 * x + p10; + + return result; + } + + /** + * Function to determine the angle of a Nadir vector to each reference axes + * + * @param nadir Known Nadir vector + * @param sideNormal Vector normal to a satellite side + * @return Angle to Nadir along an axis plane + */ + private double angleFromNadirVector(Vector3D nadir, Vector3D sideNormal) { + return FastMath.acos(Vector3D.dotProduct(nadir, sideNormal) / nadir.getNorm()); + } + + /** + * Function to initialise the infrared sensor and all of its values for a given + * Nadir vector + * + * @param nadir Known Nadir vector + * @return infraredReading Infrared reading of the sensor based upon its + * relationship with the angle to Nadir + */ + public double calculateInfraredReading(Vector3D nadir) { + angleReading = angleFromNadirVector(nadir, sideNormal); + infraredReading = angleToInfrared(angleReading); + return infraredReading; + } + + /** + * Series of get and set functions for @testing purposes only + */ + public double getAngle() { + return angleReading; + } + + public Vector3D getSideNormal() { + return sideNormal; + } +} diff --git a/simulator/src/main/java/msp/simulator/satellite/ADACS/sensors/Magnetometer.java b/simulator/src/main/java/msp/simulator/satellite/ADACS/sensors/Magnetometer.java new file mode 100644 index 00000000..ca0b1780 --- /dev/null +++ b/simulator/src/main/java/msp/simulator/satellite/ADACS/sensors/Magnetometer.java @@ -0,0 +1,203 @@ +/* Copyright 20017-2018 Melbourne Space Program + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +package msp.simulator.satellite.ADACS.sensors; + +import org.hipparchus.geometry.euclidean.threed.Vector3D; +import org.hipparchus.util.FastMath; +import org.orekit.bodies.GeodeticPoint; +import org.orekit.errors.OrekitException; +import org.orekit.models.earth.GeoMagneticElements; +import org.orekit.propagation.SpacecraftState; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; +import org.hipparchus.random.RandomDataGenerator; + +import msp.simulator.environment.Environment; +import msp.simulator.environment.geomagneticField.EarthMagneticField; +import msp.simulator.environment.solarSystem.Earth; +import msp.simulator.satellite.assembly.Assembly; +import msp.simulator.utils.logs.CustomLoggingTools; + +/** + * This class represents the magnetometer sensor of the + * satellite. + * + * @author Florian CHAUBEYRE + */ +public class Magnetometer { + + + /* ******* Public Static Attributes ******* */ + + + /* Random number generator */ + private RandomDataGenerator randomDataGenerator = new RandomDataGenerator(1000); + + /** This intensity is used to generate a random number to be + * added to each components of the true magnetic field. + * (nanoTesla) + */ + public static double defaultMagnetoNoiseIntensity = 4.156*1000; + + /* **************************************** */ + + /** Logger of the class. */ + private static final Logger logger = LoggerFactory.getLogger( + Magnetometer.class); + + /** Instance of the OreKit Magnetic Field. */ + private EarthMagneticField geomagField; + + /** Instance of the Earth. */ + private Earth earth; + + /** Assembly of the satellite. */ + private Assembly assembly; + + /** Private attribute for the noise intensity. */ + private double noiseIntensity; + + public Magnetometer(Environment environment, Assembly assembly) { + logger.info(CustomLoggingTools.indentMsg(logger, + " -> Building the Magnetometer...")); + + /* Linking the class to the rest of the simulation. */ + this.geomagField = environment.getGeoMagneticField(); + this.earth = environment.getSolarSystem().getEarth(); + this.assembly = assembly; + + /* Initializing the class. */ + this.noiseIntensity = Magnetometer.defaultMagnetoNoiseIntensity; + } + + /** + * Return a measurement disturbed by a random noise. + * The intensity of the noise factor can be modified at + * initialization. + * @return GeoMagneticElements (where field vector is expressed in nT) + * @see #retrievePerfectMeasurement() + */ + public GeoMagneticElements retrieveNoisyField() { + /* Perfect Measure. */ + + + GeoMagneticElements perfectMeasure = this.retrievePerfectField(); + + /* Normally distributed random noise contribution. */ + Vector3D noise = new Vector3D ( new double[] { + randomDataGenerator.nextNormal(0, this.noiseIntensity), + randomDataGenerator.nextNormal(0, this.noiseIntensity), + randomDataGenerator.nextNormal(0, this.noiseIntensity) + }); + + /* Disturbing the perfect measurement. */ + Vector3D noisyFieldVector = + perfectMeasure.getFieldVector().add(noise); + + /* Creating the noisy measure. */ + GeoMagneticElements noisyMeasure = new GeoMagneticElements(noisyFieldVector); + + logger.debug("Noisy Geo" + noisyMeasure.toString()); + + + return noisyMeasure; + } + + + + /** + * Retrieve a perfect measured data from the sensors, i.e. an + * ideal measurement without any noise or interference. + * + * @return GeoMagneticElements at the location of the satellite. + * (where field vector is expressed in nT) + * @see GeoMagneticElements + */ + public GeoMagneticElements retrievePerfectField() { + + SpacecraftState satState = this.assembly.getStates().getCurrentState() ; + + Vector3D positionOnEarth = + satState.getOrbit().getPVCoordinates().getPosition(); + + GeodeticPoint geodeticPosition = null; + + try { + /* The transformation from cartesian to geodetic coordinates is actually + * not straight as it needs to solve some 2-unknowns non-linear equations. + * So it needs a processing algorithm like the one presented by OreKit + * in the following method. + */ + geodeticPosition = earth.getEllipsoid().transform( + positionOnEarth, + satState.getOrbit().getFrame(), + satState.getDate() + ); + } catch (OrekitException e) { + e.printStackTrace(); + } + + /* Calculate the magnetic field at the projected geodetic point. + * Note that the algorithm brings some approximation, for instance + * the altitude of the satellite is slightly shifted from the true + * one. + */ + GeoMagneticElements trueField_itrf = this.geomagField.getField().calculateField( + FastMath.toDegrees(geodeticPosition.getLatitude()), /* decimal deg */ + FastMath.toDegrees(geodeticPosition.getLongitude()), /* decimal deg */ + (satState.getA() - this.earth.getRadius()) / 1e3 /* km */ + ); + + logger.debug("Magnetometer Measurement: \n" + + "Latitude: " + FastMath.toDegrees(geodeticPosition.getLatitude()) + " °\n" + + "Longitud: " + FastMath.toDegrees(geodeticPosition.getLongitude()) + " °\n" + + "Altitude: " + (satState.getA() - this.earth.getRadius()) / 1e3 + " km\n" + + "True Geo ITRF" + trueField_itrf.toString() + ); + + /* Rotate the magnetic field reading into the body frame */ + Vector3D trueField_body = this.assembly.getItrf2body(satState.getDate()) + .transformVector(trueField_itrf.getFieldVector()); + + return new GeoMagneticElements(trueField_body); + } + + /** + * Retrieve the measured data from the magnetometer sensors. + *

+ * WARNING: a noise is always introduced component by component + * on the returned value. Storing the vector before working on + * it may be required. + * + * @return Geomagnetic Field Vector (in Tesla) + */ + public Vector3D getData_magField() { + /* Retrieve the noisy magnetic field. */ + Vector3D data = this.retrieveNoisyField().getFieldVector(); + + /* Convert nTesla into Tesla. */ + data = data.scalarMultiply(1e-9); + + return data; + } + + /** + * @return The noise intensity in nTesla. + */ + public double getNoiseIntensity() { + return noiseIntensity; + } + +} diff --git a/simulator/src/main/java/msp/simulator/satellite/ADACS/sensors/Sensors.java b/simulator/src/main/java/msp/simulator/satellite/ADACS/sensors/Sensors.java new file mode 100644 index 00000000..68f16408 --- /dev/null +++ b/simulator/src/main/java/msp/simulator/satellite/ADACS/sensors/Sensors.java @@ -0,0 +1,129 @@ +/* Copyright 20017-2018 Melbourne Space Program + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +package msp.simulator.satellite.ADACS.sensors; + +import org.hipparchus.geometry.euclidean.threed.Vector3D; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import msp.simulator.environment.Environment; +import msp.simulator.satellite.assembly.Assembly; +import msp.simulator.utils.logs.CustomLoggingTools; + +/** + * + * @author Florian CHAUBEYRE + * @author Braeden BORG + */ +public class Sensors { + + /** Logger of the class. */ + private static final Logger logger = + LoggerFactory.getLogger(Sensors.class); + + /** Instance of environment of the simulation. */ + private Environment environment; + + /** Instance of the satellite assembly of the simulation. */ + private Assembly assembly; + + /* ***** Instances of the different sensors. ***** */ + + /** Instance of magnetometer in the simulation */ + private Magnetometer magnetometer; + + /** Instance of infrared Sensors in the simulation */ + private InfraredSensor + posXIRSensor, + negXIRSensor, + posYIRSensor, + negYIRSensor, + posZIRSensor, + negZIRSensor; + + /** Instance of gyrometer in the simulation. */ + private Gyrometer gyrometer; + + /** + * Constructor of the satellite sensors. + * + * @param environment Instance of the simulation + * @param assembly Instance of the simulation + */ + public Sensors(Environment environment, Assembly assembly) { + logger.info(CustomLoggingTools.indentMsg(logger, + "Building the satellite Sensors...")); + + /* Linking the sensors class to the rest of the simulation. */ + this.environment = environment; + this.assembly = assembly; + + /* TODO: Normalize the construction and the use of the sensors. + * Typically by extending a minimal abstract class Sensor. + */ + + /* Building the sensors. */ + this.magnetometer = new Magnetometer(this.environment, this.assembly); + this.gyrometer = new Gyrometer(this.environment, this.assembly); + + this.posXIRSensor = new InfraredSensor(Vector3D.PLUS_I); + this.negXIRSensor = new InfraredSensor(Vector3D.MINUS_I); + this.posYIRSensor = new InfraredSensor(Vector3D.PLUS_J); + this.negYIRSensor = new InfraredSensor(Vector3D.MINUS_J); + this.posZIRSensor = new InfraredSensor(Vector3D.PLUS_K); + this.negZIRSensor = new InfraredSensor(Vector3D.MINUS_K); + } + + /** + * @return the magnetometer + */ + public Magnetometer getMagnetometer() { + return magnetometer; + } + + /** + * @return the gyrometer + */ + public Gyrometer getGyrometer() { + return gyrometer; + } + + /** + * @return Infrared sensor for a side of the satellite + */ + public InfraredSensor getPosXIRSensor() { + return posXIRSensor; + } + + public InfraredSensor getNegXIRSensor() { + return negXIRSensor; + } + + public InfraredSensor getPosYIRSensor() { + return posYIRSensor; + } + + public InfraredSensor getNegYIRSensor() { + return negYIRSensor; + } + + public InfraredSensor getPosZIRSensor() { + return posZIRSensor; + } + + public InfraredSensor getNegZIRSensor() { + return negZIRSensor; + } +} diff --git a/simulator/src/main/java/msp/simulator/satellite/ADACS/sensors/package-info.java b/simulator/src/main/java/msp/simulator/satellite/ADACS/sensors/package-info.java new file mode 100644 index 00000000..25d4eb29 --- /dev/null +++ b/simulator/src/main/java/msp/simulator/satellite/ADACS/sensors/package-info.java @@ -0,0 +1,22 @@ +/* Copyright 20017-2018 Melbourne Space Program + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/** + * This package gathers all of the sensor modules + * of the satellite. + * + * @author Florian CHAUBEYRE + * @author Braeden BORG + */ +package msp.simulator.satellite.ADACS.sensors; diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/ADCS.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/ADCS.java new file mode 100644 index 00000000..27d75c0e --- /dev/null +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/ADCS.java @@ -0,0 +1,38 @@ +/* Copyright 20017-2018 Melbourne Space Program + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package msp.simulator.satellite.ADCS; + +import java.util.function.Consumer; + +import org.orekit.files.general.EphemerisFile.SatelliteEphemeris; + +import msp.simulator.satellite.ADACS.sensors.Sensors; +import msp.simulator.satellite.ADCS.Controller.Controller; +import msp.simulator.satellite.ADCS.Estimators.Estimators; +import msp.simulator.satellite.actuators.Actuators; +/** + * + * @author Jack McRobbie + */ +public class ADCS { + private Sensors sensors; + private Estimators estimators; + private Controller controllers; + private Actuators actuators; + + public ADCS(SatelliteEphemeris sat) { + + } + +} diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/Actuators.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/Actuators.java new file mode 100644 index 00000000..a19b1fcc --- /dev/null +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/Actuators.java @@ -0,0 +1,39 @@ +/* Copyright 20017-2018 Melbourne Space Program + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +package msp.simulator.satellite.ADCS.Actuators; + +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import msp.simulator.utils.logs.CustomLoggingTools; + +/** + * + * @author Florian CHAUBEYRE + */ +public class Actuators { + + /** Logger of the class */ + private static final Logger logger = LoggerFactory.getLogger(Actuators.class); + + /** + * + */ + public Actuators() { + logger.info(CustomLoggingTools.indentMsg(logger, + "Building the Actuators...")); + } + +} diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/MagnetoTorquers.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/MagnetoTorquers.java new file mode 100644 index 00000000..c010b603 --- /dev/null +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/MagnetoTorquers.java @@ -0,0 +1,40 @@ +/* Copyright 20017-2018 Melbourne Space Program + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +package msp.simulator.satellite.ADCS.Actuators; + +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import msp.simulator.utils.logs.CustomLoggingTools; + +/** + * + * @author Florian CHAUBEYRE + */ +public class MagnetoTorquers { + + + /** Logger of the class */ + private static final Logger logger = LoggerFactory.getLogger(MagnetoTorquers.class); + + /** + * + */ + public MagnetoTorquers() { + logger.info(CustomLoggingTools.indentMsg(logger, + "Building the MagnetoTorquers...")); + } + +} diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/package-info.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/package-info.java new file mode 100644 index 00000000..0ae2ff98 --- /dev/null +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/package-info.java @@ -0,0 +1,20 @@ +/* Copyright 20017-2018 Melbourne Space Program + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/** + * Gather all of the models of actuators of the satellite. + * + * @author Florian CHAUBEYRE + */ +package msp.simulator.satellite.ADCS.Actuators; \ No newline at end of file diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/B_Dot.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/B_Dot.java new file mode 100644 index 00000000..be03b295 --- /dev/null +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/B_Dot.java @@ -0,0 +1,32 @@ +/* Copyright 20017-2018 Melbourne Space Program + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package msp.simulator.satellite.ADCS.Controller; + +import org.hipparchus.geometry.euclidean.threed.Vector3D; + +import msp.simulator.satellite.Satellite; + +/** + * + * @author Jack McRobbie> + */ +public class B_Dot { + private static final Vector3D bdotGains = new Vector3D(-54000,-54000,-54000); + + + public void B_dot(Satellite sat) { + return; + } + +} diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/ComputeTorque.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/ComputeTorque.java new file mode 100644 index 00000000..e58a27c5 --- /dev/null +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/ComputeTorque.java @@ -0,0 +1,37 @@ +/* Copyright 20017-2018 Melbourne Space Program + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package msp.simulator.satellite.ADCS.Controller; + +import org.hipparchus.geometry.euclidean.threed.Vector3D; + +import msp.simulator.satellite.Satellite; +import msp.simulator.satellite.ADCS.*; + +/** + * + * @author Jack McRobbie + */ +public class ComputeTorque { + private Satellite sat; + private ADCS adcs; + + public ComputeTorque(Satellite satellite) { + + } + public Vector3D getTorque() { + return new Vector3D(null); //to be sorted out + + } + +} diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/Controller.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/Controller.java new file mode 100644 index 00000000..43d16c47 --- /dev/null +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/Controller.java @@ -0,0 +1,22 @@ +/* Copyright 20017-2018 Melbourne Space Program + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package msp.simulator.satellite.ADCS.Controller; + +/** + * + * @author Jack McRobbie + */ +public class Controller { + +} diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/Estimators.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/Estimators.java new file mode 100644 index 00000000..bae2b05d --- /dev/null +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/Estimators.java @@ -0,0 +1,22 @@ +/* Copyright 20017-2018 Melbourne Space Program + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package msp.simulator.satellite.ADCS.Estimators; + +/** + * + * @author Florian CHAUBEYRE + */ +public class Estimators { + +} From 9b9a3f0b2a9876f0697629338436856b3b9ca67b Mon Sep 17 00:00:00 2001 From: jmcrobbie Date: Tue, 12 Feb 2019 18:07:10 +1030 Subject: [PATCH 36/48] Constructed Low pass filter class for sensor use --- .../ADCS/Actuators/MagnetoTorquers.java | 15 +++++ .../satellite/ADCS/Controller/B_Dot.java | 4 +- .../BdotEstimator/BdotEstimator.java | 39 ++++++++++++ .../BdotEstimator/LowPassFilter.java | 59 +++++++++++++++++++ .../satellite/ADCS/Estimators/Estimators.java | 9 +++ .../satellite/actuators/MagnetoTorquers.java | 15 +++++ 6 files changed, 140 insertions(+), 1 deletion(-) create mode 100644 simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/BdotEstimator.java create mode 100644 simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/LowPassFilter.java diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/MagnetoTorquers.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/MagnetoTorquers.java index c010b603..3d1ea721 100644 --- a/simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/MagnetoTorquers.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/MagnetoTorquers.java @@ -14,6 +14,7 @@ package msp.simulator.satellite.ADCS.Actuators; +import org.hipparchus.geometry.euclidean.threed.Vector3D; import org.slf4j.Logger; import org.slf4j.LoggerFactory; @@ -22,8 +23,11 @@ /** * * @author Florian CHAUBEYRE + * @author Jack McRobbie */ public class MagnetoTorquers { + private Vector3D orientation; + private Vector3D MaxDipole; /** Logger of the class */ @@ -35,6 +39,17 @@ public class MagnetoTorquers { public MagnetoTorquers() { logger.info(CustomLoggingTools.indentMsg(logger, "Building the MagnetoTorquers...")); + this.orientation = new Vector3D(1,1,1); + this.MaxDipole = new Vector3D(0.1,0.1,0.1); //TODO make configurable from simulation initialization + + } + public Vector3D computeDipole(Vector3D dutyCycle) { + Vector3D dipole = new Vector3D( + dutyCycle.getX() * orientation.getX(), + dutyCycle.getY() * orientation.getY(), + dutyCycle.getZ() * orientation.getZ() + ); + return dipole; } } diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/B_Dot.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/B_Dot.java index be03b295..a8d272e8 100644 --- a/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/B_Dot.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/B_Dot.java @@ -16,6 +16,7 @@ import org.hipparchus.geometry.euclidean.threed.Vector3D; import msp.simulator.satellite.Satellite; +import msp.simulator.satellite.ADCS.Estimators.Estimators; /** * @@ -23,9 +24,10 @@ */ public class B_Dot { private static final Vector3D bdotGains = new Vector3D(-54000,-54000,-54000); - + private Estimators est; public void B_dot(Satellite sat) { + this.est = new Estimators(sat); return; } diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/BdotEstimator.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/BdotEstimator.java new file mode 100644 index 00000000..a80b428b --- /dev/null +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/BdotEstimator.java @@ -0,0 +1,39 @@ +/* Copyright 20017-2018 Melbourne Space Program + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package msp.simulator.satellite.ADCS.Estimators.BdotEstimator; + +import org.hipparchus.geometry.euclidean.threed.Vector3D; + +import msp.simulator.satellite.ADACS.sensors.Magnetometer; +import msp.simulator.satellite.assembly.SatelliteBody; + +/** + * + * @author Jack McRobbie + */ +public class BdotEstimator { + private LowPassFilter lowpassfilter; + private Magnetometer mag; + + public BdotEstimator(SatelliteBody sat) { + lowpassfilter = new LowPassFilter(5,0.25); + } + public Vector3D computeDutyCycle() { + Vector3D dutyCycle = new Vector3D( + ); + + return dutyCycle; + } + +} diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/LowPassFilter.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/LowPassFilter.java new file mode 100644 index 00000000..92c1a60e --- /dev/null +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/LowPassFilter.java @@ -0,0 +1,59 @@ +/* Copyright 20017-2018 Melbourne Space Program + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package msp.simulator.satellite.ADCS.Estimators.BdotEstimator; + +import org.hipparchus.geometry.euclidean.threed.Vector3D; + +/** + * + * @author Jack McRobbie + */ +public class LowPassFilter { + private final String Constructor3d = "Vector3D"; + private final String ConstructorDouble = "double"; + + private double timeConstant; + private double samplePeriodMili; + private double state; + private Vector3D state3d; + private double alpha; + private String flag; + public LowPassFilter(double tc, double spms,double initialState) { + this.timeConstant = tc; + this.samplePeriodMili = spms; + this.alpha = Math.exp(spms/tc); + state = initialState; + this.flag = this.ConstructorDouble; + } + public LowPassFilter(double tc, double spms, Vector3D initState) { + this.timeConstant = tc; + this.samplePeriodMili = spms; + this.alpha = Math.exp(spms/tc); + + /* Utilize multiplicative class constructor returns same as initState */ + state3d = new Vector3D(1.0,initState); + this.flag = this.Constructor3d; + } + public double ProcessSample(double new_sample) { + this.state = this.alpha * this.state + (1 - this.alpha) * new_sample; + return this.state; + } + public double getSamplePeriod() { + return this.samplePeriodMili/1000.0; + } + + public double getTimeConstant() { + return this.timeConstant; + } +} diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/Estimators.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/Estimators.java index bae2b05d..f4f1eeff 100644 --- a/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/Estimators.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/Estimators.java @@ -13,10 +13,19 @@ */ package msp.simulator.satellite.ADCS.Estimators; +import msp.simulator.satellite.Satellite; + /** * * @author Florian CHAUBEYRE */ public class Estimators { + /** + * @param sat + */ + public Estimators(Satellite sat) { + // TODO Auto-generated constructor stub + } + } diff --git a/simulator/src/main/java/msp/simulator/satellite/actuators/MagnetoTorquers.java b/simulator/src/main/java/msp/simulator/satellite/actuators/MagnetoTorquers.java index ea6a3e8a..d65c87e7 100644 --- a/simulator/src/main/java/msp/simulator/satellite/actuators/MagnetoTorquers.java +++ b/simulator/src/main/java/msp/simulator/satellite/actuators/MagnetoTorquers.java @@ -14,6 +14,7 @@ package msp.simulator.satellite.actuators; +import org.hipparchus.geometry.euclidean.threed.Vector3D; import org.slf4j.Logger; import org.slf4j.LoggerFactory; @@ -22,8 +23,11 @@ /** * * @author Florian CHAUBEYRE + * @author Jack McRobbie */ public class MagnetoTorquers { + private Vector3D orientation; + private Vector3D MaxDipole; /** Logger of the class */ @@ -35,6 +39,17 @@ public class MagnetoTorquers { public MagnetoTorquers() { logger.info(CustomLoggingTools.indentMsg(logger, "Building the MagnetoTorquers...")); + this.orientation = new Vector3D(1,1,1); + this.MaxDipole = new Vector3D(0.1,0.1,0.1); //TODO make configurable from simulation initialization + + } + public Vector3D computeDipole(Vector3D dutyCycle) { + Vector3D dipole = new Vector3D( + dutyCycle.getX() * orientation.getX(), + dutyCycle.getY() * orientation.getY(), + dutyCycle.getZ() * orientation.getZ() + ); + return dipole; } } From 39339888e0d81f9c1aa3c196a22db9fa054f0086 Mon Sep 17 00:00:00 2001 From: jmcrobbie Date: Tue, 12 Feb 2019 18:26:16 +1030 Subject: [PATCH 37/48] Tidied up some bugs and continued restructuring --- .../BdotEstimator/BdotEstimator.java | 8 ++---- .../BdotEstimator/FirstOrderDiff.java | 28 +++++++++++++++++++ .../BdotEstimator/LowPassFilter.java | 7 +++++ 3 files changed, 38 insertions(+), 5 deletions(-) create mode 100644 simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/FirstOrderDiff.java diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/BdotEstimator.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/BdotEstimator.java index a80b428b..f541bee5 100644 --- a/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/BdotEstimator.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/BdotEstimator.java @@ -27,13 +27,11 @@ public class BdotEstimator { private Magnetometer mag; public BdotEstimator(SatelliteBody sat) { - lowpassfilter = new LowPassFilter(5,0.25); + Vector3D initalState = new Vector3D(0.0,0.0,0.0); + lowpassfilter = new LowPassFilter(5.0,0.25,initalState); } public Vector3D computeDutyCycle() { - Vector3D dutyCycle = new Vector3D( - ); - - return dutyCycle; + } } diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/FirstOrderDiff.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/FirstOrderDiff.java new file mode 100644 index 00000000..b93a4be3 --- /dev/null +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/FirstOrderDiff.java @@ -0,0 +1,28 @@ +/* Copyright 20017-2018 Melbourne Space Program + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package msp.simulator.satellite.ADCS.Estimators.BdotEstimator; + +/** + * + * @author Jack McRobbie + */ +public class FirstOrderDiff { + private double timestep; + + + public FirstOrderDiff() { + + } + +} diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/LowPassFilter.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/LowPassFilter.java index 92c1a60e..a259e550 100644 --- a/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/LowPassFilter.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/LowPassFilter.java @@ -49,6 +49,13 @@ public double ProcessSample(double new_sample) { this.state = this.alpha * this.state + (1 - this.alpha) * new_sample; return this.state; } + public Vector3D ProcessSample(Vector3D new_sample) { + double x = this.alpha * this.state3d.getX() + (1 - this.alpha) * new_sample.getX(); + double y = this.alpha * this.state3d.getY() + (1 - this.alpha) * new_sample.getY(); + double z = this.alpha * this.state3d.getZ() + (1 - this.alpha) * new_sample.getZ(); + this.state3d = new Vector3D(x,y,z); + return this.state3d; + } public double getSamplePeriod() { return this.samplePeriodMili/1000.0; } From 9cb0fdba8abfe03152d67a5c0e4d61516d6b3ec2 Mon Sep 17 00:00:00 2001 From: jmcrobbie Date: Tue, 12 Feb 2019 18:47:34 +1030 Subject: [PATCH 38/48] Added Bdot estimator class --- .../BdotEstimator/BdotEstimator.java | 18 ++++++++++-- .../BdotEstimator/FirstOrderDiff.java | 28 ------------------- 2 files changed, 16 insertions(+), 30 deletions(-) delete mode 100644 simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/FirstOrderDiff.java diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/BdotEstimator.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/BdotEstimator.java index f541bee5..51b5d823 100644 --- a/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/BdotEstimator.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/BdotEstimator.java @@ -25,13 +25,27 @@ public class BdotEstimator { private LowPassFilter lowpassfilter; private Magnetometer mag; + private Vector3D lastMagFieldReading; + private final double timestep; public BdotEstimator(SatelliteBody sat) { Vector3D initalState = new Vector3D(0.0,0.0,0.0); lowpassfilter = new LowPassFilter(5.0,0.25,initalState); + timestep = 0.1; // TODO make equal to Controller frequency! } - public Vector3D computeDutyCycle() { - + public Vector3D computeBdot() { + Vector3D bDotUnfiltered = this.getFirstOrderDiff(); + Vector3D bdot = lowpassfilter.ProcessSample(bDotUnfiltered); + return bdot; + } + private Vector3D getFirstOrderDiff() { + Vector3D magreading = mag.retrieveNoisyField().getFieldVector(); + double x = magreading.getX() - this.lastMagFieldReading.getX(); + double y = magreading.getY() - this.lastMagFieldReading.getY(); + double z = magreading.getZ() - this.lastMagFieldReading.getZ(); + this.lastMagFieldReading = magreading; + Vector3D result = new Vector3D(x/this.timestep,y/this.timestep,z/this.timestep); + return result; } } diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/FirstOrderDiff.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/FirstOrderDiff.java deleted file mode 100644 index b93a4be3..00000000 --- a/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/FirstOrderDiff.java +++ /dev/null @@ -1,28 +0,0 @@ -/* Copyright 20017-2018 Melbourne Space Program - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ -package msp.simulator.satellite.ADCS.Estimators.BdotEstimator; - -/** - * - * @author Jack McRobbie - */ -public class FirstOrderDiff { - private double timestep; - - - public FirstOrderDiff() { - - } - -} From 76a1b2f9bc1133f6a71da5fe522393e3931f7cd8 Mon Sep 17 00:00:00 2001 From: jmcrobbie Date: Tue, 12 Feb 2019 19:26:56 +1030 Subject: [PATCH 39/48] Began constructing adcs and Controller classes --- .../msp/simulator/satellite/ADCS/ADCS.java | 13 +++++-- .../satellite/ADCS/Controller/B_Dot.java | 17 ++++++--- .../ADCS/Controller/ComputeTorque.java | 37 ------------------- .../satellite/ADCS/Controller/Controller.java | 11 +++++- .../BdotEstimator/BdotEstimator.java | 5 +-- .../satellite/ADCS/Estimators/Estimators.java | 2 +- 6 files changed, 33 insertions(+), 52 deletions(-) delete mode 100644 simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/ComputeTorque.java diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/ADCS.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/ADCS.java index 27d75c0e..93c51ff9 100644 --- a/simulator/src/main/java/msp/simulator/satellite/ADCS/ADCS.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/ADCS.java @@ -15,12 +15,14 @@ import java.util.function.Consumer; -import org.orekit.files.general.EphemerisFile.SatelliteEphemeris; +import org.hipparchus.geometry.euclidean.threed.Vector3D; +import msp.simulator.environment.Environment; +import msp.simulator.satellite.Satellite; import msp.simulator.satellite.ADACS.sensors.Sensors; +import msp.simulator.satellite.ADCS.Actuators.Actuators; import msp.simulator.satellite.ADCS.Controller.Controller; import msp.simulator.satellite.ADCS.Estimators.Estimators; -import msp.simulator.satellite.actuators.Actuators; /** * * @author Jack McRobbie @@ -31,8 +33,11 @@ public class ADCS { private Controller controllers; private Actuators actuators; - public ADCS(SatelliteEphemeris sat) { - + public ADCS(Satellite sat,Environment environment) { + this.sensors = new Sensors(environment, sat.getAssembly()); + this.estimators = new Estimators(sat); + this.controllers = new Controller(sat); } + } diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/B_Dot.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/B_Dot.java index a8d272e8..b188ad1d 100644 --- a/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/B_Dot.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/B_Dot.java @@ -16,7 +16,8 @@ import org.hipparchus.geometry.euclidean.threed.Vector3D; import msp.simulator.satellite.Satellite; -import msp.simulator.satellite.ADCS.Estimators.Estimators; +import msp.simulator.satellite.ADCS.Actuators.Actuators; +import msp.simulator.satellite.ADCS.Estimators.BdotEstimator.BdotEstimator; /** * @@ -24,11 +25,15 @@ */ public class B_Dot { private static final Vector3D bdotGains = new Vector3D(-54000,-54000,-54000); - private Estimators est; + private Actuators magnetorquers; + private BdotEstimator est; - public void B_dot(Satellite sat) { - this.est = new Estimators(sat); - return; + public B_Dot(Satellite sat) { + this.est = new BdotEstimator(sat); + magnetorquers = new Actuators(); + } + public Vector3D computeDipole() { + Vector3D result = new Vector3D(1.0,est.computeBdot(),1.0,this.bdotGains); + return result; } - } diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/ComputeTorque.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/ComputeTorque.java deleted file mode 100644 index e58a27c5..00000000 --- a/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/ComputeTorque.java +++ /dev/null @@ -1,37 +0,0 @@ -/* Copyright 20017-2018 Melbourne Space Program - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ -package msp.simulator.satellite.ADCS.Controller; - -import org.hipparchus.geometry.euclidean.threed.Vector3D; - -import msp.simulator.satellite.Satellite; -import msp.simulator.satellite.ADCS.*; - -/** - * - * @author Jack McRobbie - */ -public class ComputeTorque { - private Satellite sat; - private ADCS adcs; - - public ComputeTorque(Satellite satellite) { - - } - public Vector3D getTorque() { - return new Vector3D(null); //to be sorted out - - } - -} diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/Controller.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/Controller.java index 43d16c47..3aceb465 100644 --- a/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/Controller.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/Controller.java @@ -13,10 +13,19 @@ */ package msp.simulator.satellite.ADCS.Controller; +import msp.simulator.satellite.Satellite; + /** * - * @author Jack McRobbie + * @author Jack McRobbie */ public class Controller { + B_Dot bdot; + Satellite sat; + public Controller(Satellite satellite) { + this.sat = satellite; + bdot = new B_Dot(this.sat); + + } } diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/BdotEstimator.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/BdotEstimator.java index 51b5d823..57ec16d9 100644 --- a/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/BdotEstimator.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/BdotEstimator.java @@ -15,9 +15,8 @@ import org.hipparchus.geometry.euclidean.threed.Vector3D; +import msp.simulator.satellite.Satellite; import msp.simulator.satellite.ADACS.sensors.Magnetometer; -import msp.simulator.satellite.assembly.SatelliteBody; - /** * * @author Jack McRobbie @@ -28,7 +27,7 @@ public class BdotEstimator { private Vector3D lastMagFieldReading; private final double timestep; - public BdotEstimator(SatelliteBody sat) { + public BdotEstimator(Satellite sat) { Vector3D initalState = new Vector3D(0.0,0.0,0.0); lowpassfilter = new LowPassFilter(5.0,0.25,initalState); timestep = 0.1; // TODO make equal to Controller frequency! diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/Estimators.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/Estimators.java index f4f1eeff..671a382c 100644 --- a/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/Estimators.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/Estimators.java @@ -17,7 +17,7 @@ /** * - * @author Florian CHAUBEYRE + * @author Jack McRobbie */ public class Estimators { From 907a029989ee969920436f3f9b3861d428c99b20 Mon Sep 17 00:00:00 2001 From: jmcrobbie Date: Thu, 14 Feb 2019 17:39:41 +1030 Subject: [PATCH 40/48] Continued Constructing ADCS Package --- .../torques/ControllerTorqueProvider.java | 16 +++--- .../simulator/dynamic/torques/Torques.java | 9 ++- .../msp/simulator/satellite/ADCS/ADCS.java | 9 ++- .../satellite/ADCS/Actuators/Actuators.java | 11 +++- .../satellite/ADCS/Controller/B_Dot.java | 7 ++- .../satellite/ADCS/Controller/Controller.java | 5 ++ .../satellite/actuators/Actuators.java | 39 ------------- .../satellite/actuators/MagnetoTorquers.java | 55 ------------------- .../satellite/actuators/package-info.java | 20 ------- 9 files changed, 40 insertions(+), 131 deletions(-) delete mode 100644 simulator/src/main/java/msp/simulator/satellite/actuators/Actuators.java delete mode 100644 simulator/src/main/java/msp/simulator/satellite/actuators/MagnetoTorquers.java delete mode 100644 simulator/src/main/java/msp/simulator/satellite/actuators/package-info.java diff --git a/simulator/src/main/java/msp/simulator/dynamic/torques/ControllerTorqueProvider.java b/simulator/src/main/java/msp/simulator/dynamic/torques/ControllerTorqueProvider.java index 897b5e13..52bb0eb5 100644 --- a/simulator/src/main/java/msp/simulator/dynamic/torques/ControllerTorqueProvider.java +++ b/simulator/src/main/java/msp/simulator/dynamic/torques/ControllerTorqueProvider.java @@ -12,30 +12,28 @@ * limitations under the License. */ package msp.simulator.dynamic.torques; - import org.hipparchus.geometry.euclidean.threed.Vector3D; import org.orekit.time.AbsoluteDate; +import msp.simulator.environment.Environment; import msp.simulator.satellite.Satellite; - +import msp.simulator.satellite.ADCS.ADCS; /** * * @author Jack McRobbie * This class represents the satellites own method * for providing stabilization and control */ - - public class ControllerTorqueProvider implements TorqueProvider{ - public ControllerTorqueProvider(Satellite satellite, AbsoluteDate date) { + private ADCS adcsModule; + + public ControllerTorqueProvider(Satellite satellite, AbsoluteDate date, Environment environment) { + adcsModule = new ADCS(satellite,environment); } - + @Override public Vector3D getTorque(AbsoluteDate date) { Vector3D a = new Vector3D(null); return a; //TODO } - - - } diff --git a/simulator/src/main/java/msp/simulator/dynamic/torques/Torques.java b/simulator/src/main/java/msp/simulator/dynamic/torques/Torques.java index acad2188..8cce5027 100644 --- a/simulator/src/main/java/msp/simulator/dynamic/torques/Torques.java +++ b/simulator/src/main/java/msp/simulator/dynamic/torques/Torques.java @@ -69,7 +69,14 @@ public Torques (Environment environment, Satellite satellite) { this.isDisturbances = Torques.allowDisturbances; /* - Register the command provider. */ - switch (Torques.commandTorqueProvider) { + switch (Torques.commandTorqueProvider) { + case CONTROLLER: + this.torqueProviders.add( + TorqueProviderEnum.CONTROLLER.getIndex(), new ControllerTorqueProvider( + satellite, + satellite.getAssembly().getStates().getInitialState().getDate(), + environment + )); case MEMCACHED: this.torqueProviders.add( TorqueProviderEnum.MEMCACHED.getIndex(), diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/ADCS.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/ADCS.java index 93c51ff9..e0b4ae29 100644 --- a/simulator/src/main/java/msp/simulator/satellite/ADCS/ADCS.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/ADCS.java @@ -31,13 +31,16 @@ public class ADCS { private Sensors sensors; private Estimators estimators; private Controller controllers; - private Actuators actuators; + private Actuators actuators; + public ADCS(Satellite sat,Environment environment) { this.sensors = new Sensors(environment, sat.getAssembly()); this.estimators = new Estimators(sat); this.controllers = new Controller(sat); } - - + public Vector3D ComputeTorque() { + + return this.controllers.getTorque(); + } } diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/Actuators.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/Actuators.java index a19b1fcc..022b591b 100644 --- a/simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/Actuators.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/Actuators.java @@ -27,13 +27,22 @@ public class Actuators { /** Logger of the class */ private static final Logger logger = LoggerFactory.getLogger(Actuators.class); - + private static MagnetoTorquers magnetorquer; /** * */ public Actuators() { + this.magnetorquer = new MagnetoTorquers(); logger.info(CustomLoggingTools.indentMsg(logger, "Building the Actuators...")); } + /** + * @return + */ + public MagnetoTorquers getMagnetorquers() { + // TODO Auto-generated method stub + return this.magnetorquer; + } + } diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/B_Dot.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/B_Dot.java index b188ad1d..0b16aec7 100644 --- a/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/B_Dot.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/B_Dot.java @@ -25,15 +25,16 @@ */ public class B_Dot { private static final Vector3D bdotGains = new Vector3D(-54000,-54000,-54000); - private Actuators magnetorquers; + private Actuators actuators; private BdotEstimator est; public B_Dot(Satellite sat) { this.est = new BdotEstimator(sat); - magnetorquers = new Actuators(); + this.actuators = new Actuators(); } public Vector3D computeDipole() { - Vector3D result = new Vector3D(1.0,est.computeBdot(),1.0,this.bdotGains); + Vector3D dutyCycle = new Vector3D(1.0,est.computeBdot(),1.0,this.bdotGains); + Vector3D result = this.actuators.getMagnetorquers().computeDipole(dutyCycle); return result; } } diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/Controller.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/Controller.java index 3aceb465..d5eb8ea3 100644 --- a/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/Controller.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/Controller.java @@ -13,6 +13,8 @@ */ package msp.simulator.satellite.ADCS.Controller; +import org.hipparchus.geometry.euclidean.threed.Vector3D; + import msp.simulator.satellite.Satellite; /** @@ -27,5 +29,8 @@ public Controller(Satellite satellite) { bdot = new B_Dot(this.sat); } + public Vector3D getDipole() { + return this.bdot.computeDipole(); + } } diff --git a/simulator/src/main/java/msp/simulator/satellite/actuators/Actuators.java b/simulator/src/main/java/msp/simulator/satellite/actuators/Actuators.java deleted file mode 100644 index 0bc5ae3c..00000000 --- a/simulator/src/main/java/msp/simulator/satellite/actuators/Actuators.java +++ /dev/null @@ -1,39 +0,0 @@ -/* Copyright 20017-2018 Melbourne Space Program - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -package msp.simulator.satellite.actuators; - -import org.slf4j.Logger; -import org.slf4j.LoggerFactory; - -import msp.simulator.utils.logs.CustomLoggingTools; - -/** - * - * @author Florian CHAUBEYRE - */ -public class Actuators { - - /** Logger of the class */ - private static final Logger logger = LoggerFactory.getLogger(Actuators.class); - - /** - * - */ - public Actuators() { - logger.info(CustomLoggingTools.indentMsg(logger, - "Building the Actuators...")); - } - -} diff --git a/simulator/src/main/java/msp/simulator/satellite/actuators/MagnetoTorquers.java b/simulator/src/main/java/msp/simulator/satellite/actuators/MagnetoTorquers.java deleted file mode 100644 index d65c87e7..00000000 --- a/simulator/src/main/java/msp/simulator/satellite/actuators/MagnetoTorquers.java +++ /dev/null @@ -1,55 +0,0 @@ -/* Copyright 20017-2018 Melbourne Space Program - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -package msp.simulator.satellite.actuators; - -import org.hipparchus.geometry.euclidean.threed.Vector3D; -import org.slf4j.Logger; -import org.slf4j.LoggerFactory; - -import msp.simulator.utils.logs.CustomLoggingTools; - -/** - * - * @author Florian CHAUBEYRE - * @author Jack McRobbie - */ -public class MagnetoTorquers { - private Vector3D orientation; - private Vector3D MaxDipole; - - - /** Logger of the class */ - private static final Logger logger = LoggerFactory.getLogger(MagnetoTorquers.class); - - /** - * - */ - public MagnetoTorquers() { - logger.info(CustomLoggingTools.indentMsg(logger, - "Building the MagnetoTorquers...")); - this.orientation = new Vector3D(1,1,1); - this.MaxDipole = new Vector3D(0.1,0.1,0.1); //TODO make configurable from simulation initialization - - } - public Vector3D computeDipole(Vector3D dutyCycle) { - Vector3D dipole = new Vector3D( - dutyCycle.getX() * orientation.getX(), - dutyCycle.getY() * orientation.getY(), - dutyCycle.getZ() * orientation.getZ() - ); - return dipole; - } - -} diff --git a/simulator/src/main/java/msp/simulator/satellite/actuators/package-info.java b/simulator/src/main/java/msp/simulator/satellite/actuators/package-info.java deleted file mode 100644 index 9303c151..00000000 --- a/simulator/src/main/java/msp/simulator/satellite/actuators/package-info.java +++ /dev/null @@ -1,20 +0,0 @@ -/* Copyright 20017-2018 Melbourne Space Program - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -/** - * Gather all of the models of actuators of the satellite. - * - * @author Florian CHAUBEYRE - */ -package msp.simulator.satellite.actuators; \ No newline at end of file From 2e4c901c79950d58b18ca5507b093fa7fc6e618a Mon Sep 17 00:00:00 2001 From: jmcrobbie Date: Thu, 14 Feb 2019 18:09:05 +1030 Subject: [PATCH 41/48] Added torque physics to adcs module --- .../src/main/java/msp/simulator/Main.java | 4 +- .../torques/ControllerTorqueProvider.java | 2 +- .../msp/simulator/satellite/ADCS/ADCS.java | 9 ++-- .../ADCS/ADCSPhysics/ADCSPhysics.java | 41 +++++++++++++++++++ .../satellite/ADCS/Actuators/Actuators.java | 1 - .../satellite/ADCS/Controller/Controller.java | 1 - 6 files changed, 50 insertions(+), 8 deletions(-) create mode 100644 simulator/src/main/java/msp/simulator/satellite/ADCS/ADCSPhysics/ADCSPhysics.java diff --git a/simulator/src/main/java/msp/simulator/Main.java b/simulator/src/main/java/msp/simulator/Main.java index 08391e54..c23bb19c 100644 --- a/simulator/src/main/java/msp/simulator/Main.java +++ b/simulator/src/main/java/msp/simulator/Main.java @@ -47,8 +47,8 @@ public static void main(String[] args) { Dashboard.setInitialRotAcceleration(new Vector3D(0,0,0)); Dashboard.setTorqueDisturbances(false); - Dashboard.setCommandTorqueProvider(TorqueProviderEnum.MEMCACHED); - Dashboard.setMemCachedConnection(true, "127.0.0.1:11211"); + Dashboard.setCommandTorqueProvider(TorqueProviderEnum.CONTROLLER); +// Dashboard.setMemCachedConnection(true, "127.0.0.1:11211"); Dashboard.setVtsConnection(false); diff --git a/simulator/src/main/java/msp/simulator/dynamic/torques/ControllerTorqueProvider.java b/simulator/src/main/java/msp/simulator/dynamic/torques/ControllerTorqueProvider.java index 52bb0eb5..1b95ca8b 100644 --- a/simulator/src/main/java/msp/simulator/dynamic/torques/ControllerTorqueProvider.java +++ b/simulator/src/main/java/msp/simulator/dynamic/torques/ControllerTorqueProvider.java @@ -33,7 +33,7 @@ public ControllerTorqueProvider(Satellite satellite, AbsoluteDate date, Environm } @Override public Vector3D getTorque(AbsoluteDate date) { - Vector3D a = new Vector3D(null); + Vector3D a = this.adcsModule.ComputeTorque(); return a; //TODO } } diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/ADCS.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/ADCS.java index e0b4ae29..ac18ca29 100644 --- a/simulator/src/main/java/msp/simulator/satellite/ADCS/ADCS.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/ADCS.java @@ -20,6 +20,7 @@ import msp.simulator.environment.Environment; import msp.simulator.satellite.Satellite; import msp.simulator.satellite.ADACS.sensors.Sensors; +import msp.simulator.satellite.ADCS.ADCSPhysics.ADCSPhysics; import msp.simulator.satellite.ADCS.Actuators.Actuators; import msp.simulator.satellite.ADCS.Controller.Controller; import msp.simulator.satellite.ADCS.Estimators.Estimators; @@ -32,15 +33,17 @@ public class ADCS { private Estimators estimators; private Controller controllers; private Actuators actuators; + private ADCSPhysics physics; public ADCS(Satellite sat,Environment environment) { this.sensors = new Sensors(environment, sat.getAssembly()); this.estimators = new Estimators(sat); - this.controllers = new Controller(sat); + this.controllers = new Controller(sat); + this.physics = new ADCSPhysics(sat, environment); } public Vector3D ComputeTorque() { - - return this.controllers.getTorque(); + Vector3D magneticDipole = this.controllers.getDipole(); + return this.physics.ComputeMagnetorquerTorque(magneticDipole); } } diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/ADCSPhysics/ADCSPhysics.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/ADCSPhysics/ADCSPhysics.java new file mode 100644 index 00000000..ec943fc5 --- /dev/null +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/ADCSPhysics/ADCSPhysics.java @@ -0,0 +1,41 @@ +/* Copyright 20017-2018 Melbourne Space Program + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package msp.simulator.satellite.ADCS.ADCSPhysics; + +import org.hipparchus.geometry.euclidean.threed.Vector3D; + +import msp.simulator.environment.Environment; +import msp.simulator.satellite.Satellite; +import msp.simulator.satellite.ADACS.sensors.Sensors; +import msp.simulator.satellite.ADCS.Actuators.MagnetoTorquers; + +/** + * + * @author Florian CHAUBEYRE + */ +public class ADCSPhysics { + private Satellite satellite; + private Environment environment; + private Sensors sensor; + public ADCSPhysics(Satellite satellite, Environment environemt) { + this.satellite = satellite; + this.environment = environment; + } + public Vector3D ComputeMagnetorquerTorque(Vector3D magneticDipole) { + Vector3D result = Vector3D.crossProduct(magneticDipole, + this.sensor.getMagnetometer().retrievePerfectField().getFieldVector()); + return result; + } + +} diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/Actuators.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/Actuators.java index 022b591b..c47e02f3 100644 --- a/simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/Actuators.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/Actuators.java @@ -44,5 +44,4 @@ public MagnetoTorquers getMagnetorquers() { // TODO Auto-generated method stub return this.magnetorquer; } - } diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/Controller.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/Controller.java index d5eb8ea3..1ca8ec46 100644 --- a/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/Controller.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/Controller.java @@ -32,5 +32,4 @@ public Controller(Satellite satellite) { public Vector3D getDipole() { return this.bdot.computeDipole(); } - } From 1ab8a069ed7fbea8d738bdbc9b744ebe86492d2f Mon Sep 17 00:00:00 2001 From: Jack McRobbie <32558106+JmcRobbie@users.noreply.github.com> Date: Thu, 14 Feb 2019 18:11:54 +1030 Subject: [PATCH 42/48] Update README.md Fixed typo in readme --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index b0f87af8..3edf5333 100644 --- a/README.md +++ b/README.md @@ -20,7 +20,7 @@ The simulator project is now set up. You can launch the main method and some tes ### Linux - cd into /simulator - $mvn compile -- $mvn mvn exec:java -D"exec.mainClass"="msp.simulator.Main" +- $mvn exec:java -D"exec.mainClass"="msp.simulator.Main" ## Simple Test Execution: - In Eclipse, select the test to run in src/test/java/msp/simulator - Run As > JUnit Test From 8661fc297872ff6d9907b67af4f8034b88dbb8f0 Mon Sep 17 00:00:00 2001 From: jmcrobbie Date: Thu, 14 Feb 2019 20:07:31 +1030 Subject: [PATCH 43/48] asdf --- simulator/src/main/java/msp/simulator/Main.java | 1 - .../msp/simulator/dynamic/torques/ControllerTorqueProvider.java | 2 +- simulator/src/main/java/msp/simulator/satellite/ADCS/ADCS.java | 2 +- 3 files changed, 2 insertions(+), 3 deletions(-) diff --git a/simulator/src/main/java/msp/simulator/Main.java b/simulator/src/main/java/msp/simulator/Main.java index c23bb19c..c471986c 100644 --- a/simulator/src/main/java/msp/simulator/Main.java +++ b/simulator/src/main/java/msp/simulator/Main.java @@ -48,7 +48,6 @@ public static void main(String[] args) { Dashboard.setTorqueDisturbances(false); Dashboard.setCommandTorqueProvider(TorqueProviderEnum.CONTROLLER); -// Dashboard.setMemCachedConnection(true, "127.0.0.1:11211"); Dashboard.setVtsConnection(false); diff --git a/simulator/src/main/java/msp/simulator/dynamic/torques/ControllerTorqueProvider.java b/simulator/src/main/java/msp/simulator/dynamic/torques/ControllerTorqueProvider.java index 1b95ca8b..e041351b 100644 --- a/simulator/src/main/java/msp/simulator/dynamic/torques/ControllerTorqueProvider.java +++ b/simulator/src/main/java/msp/simulator/dynamic/torques/ControllerTorqueProvider.java @@ -34,6 +34,6 @@ public ControllerTorqueProvider(Satellite satellite, AbsoluteDate date, Environm @Override public Vector3D getTorque(AbsoluteDate date) { Vector3D a = this.adcsModule.ComputeTorque(); - return a; //TODO + return a; } } diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/ADCS.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/ADCS.java index ac18ca29..48530afe 100644 --- a/simulator/src/main/java/msp/simulator/satellite/ADCS/ADCS.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/ADCS.java @@ -43,7 +43,7 @@ public ADCS(Satellite sat,Environment environment) { this.physics = new ADCSPhysics(sat, environment); } public Vector3D ComputeTorque() { - Vector3D magneticDipole = this.controllers.getDipole(); + Vector3D magneticDipole = this.controllers.getDipole(); return this.physics.ComputeMagnetorquerTorque(magneticDipole); } } From e45ec5d6fe5631d0e6108c0688c3ccbd583b465c Mon Sep 17 00:00:00 2001 From: jmcrobbie Date: Fri, 15 Feb 2019 15:26:40 +1030 Subject: [PATCH 44/48] Detumbling appears to be running correctly --- .../torques/ControllerTorqueProvider.java | 17 +- .../torques/MemCachedTorqueProvider.java | 6 +- .../simulator/dynamic/torques/Torques.java | 5 +- .../msp/simulator/satellite/ADCS/ADCS.java | 14 ++ .../ADCS/ADCSPhysics/ADCSPhysics.java | 19 +- .../satellite/ADCS/Actuators/Actuators.java | 5 +- .../ADCS/Actuators/MagnetoTorquers.java | 31 ++- .../satellite/ADCS/Controller/Controller.java | 10 + .../BdotEstimator/BdotEstimator.java | 10 +- .../msp/simulator/satellite/Satellite.java | 40 ++-- .../satellite/assembly/SatelliteBody.java | 6 +- .../satellite/sensors/Gyrometer.java | 90 -------- .../satellite/sensors/InfraredSensor.java | 91 -------- .../satellite/sensors/Magnetometer.java | 203 ------------------ .../simulator/satellite/sensors/Sensors.java | 129 ----------- .../satellite/sensors/package-info.java | 22 -- .../java/msp/simulator/user/Dashboard.java | 8 +- .../logs/ephemeris/EphemerisGenerator.java | 2 +- 18 files changed, 116 insertions(+), 592 deletions(-) delete mode 100644 simulator/src/main/java/msp/simulator/satellite/sensors/Gyrometer.java delete mode 100644 simulator/src/main/java/msp/simulator/satellite/sensors/InfraredSensor.java delete mode 100644 simulator/src/main/java/msp/simulator/satellite/sensors/Magnetometer.java delete mode 100644 simulator/src/main/java/msp/simulator/satellite/sensors/Sensors.java delete mode 100644 simulator/src/main/java/msp/simulator/satellite/sensors/package-info.java diff --git a/simulator/src/main/java/msp/simulator/dynamic/torques/ControllerTorqueProvider.java b/simulator/src/main/java/msp/simulator/dynamic/torques/ControllerTorqueProvider.java index e041351b..35dc2c83 100644 --- a/simulator/src/main/java/msp/simulator/dynamic/torques/ControllerTorqueProvider.java +++ b/simulator/src/main/java/msp/simulator/dynamic/torques/ControllerTorqueProvider.java @@ -15,9 +15,10 @@ import org.hipparchus.geometry.euclidean.threed.Vector3D; import org.orekit.time.AbsoluteDate; +import msp.simulator.NumericalSimulator; +import msp.simulator.dynamic.torques.TorqueOverTimeScenarioProvider.Step; import msp.simulator.environment.Environment; import msp.simulator.satellite.Satellite; -import msp.simulator.satellite.ADCS.ADCS; /** * * @author Jack McRobbie @@ -25,15 +26,17 @@ * for providing stabilization and control */ public class ControllerTorqueProvider implements TorqueProvider{ - private ADCS adcsModule; - + private Satellite sat; + private Vector3D steptorque; public ControllerTorqueProvider(Satellite satellite, AbsoluteDate date, Environment environment) { - adcsModule = new ADCS(satellite,environment); - + this.sat = satellite; + this.steptorque = Vector3D.ZERO; } + /** {@inheritDoc} */ @Override public Vector3D getTorque(AbsoluteDate date) { - Vector3D a = this.adcsModule.ComputeTorque(); - return a; + this.steptorque = sat.getADCS().ComputeTorque(); + /* Finally returns the torque of the step (updated if needed). */ + return this.steptorque; } } diff --git a/simulator/src/main/java/msp/simulator/dynamic/torques/MemCachedTorqueProvider.java b/simulator/src/main/java/msp/simulator/dynamic/torques/MemCachedTorqueProvider.java index 6fe77313..900708d3 100644 --- a/simulator/src/main/java/msp/simulator/dynamic/torques/MemCachedTorqueProvider.java +++ b/simulator/src/main/java/msp/simulator/dynamic/torques/MemCachedTorqueProvider.java @@ -29,7 +29,7 @@ import msp.simulator.satellite.io.MemcachedRawTranscoder; import msp.simulator.utils.logs.CustomLoggingTools; import net.spy.memcached.MemcachedClient; -import msp.simulator.satellite.sensors.Magnetometer; +import msp.simulator.satellite.ADACS.sensors.*;; /** * * @author Florian CHAUBEYRE @@ -108,8 +108,8 @@ public MemCachedTorqueProvider(Satellite satellite) { this.stepStart = this.satState.getCurrentState().getDate(); this.nextAcquisitionDate = this.satState.getInitialState().getDate(); this.stepTorque = Vector3D.ZERO; - this.b_field = satellite.getSensors().getMagnetometer().retrievePerfectField().getFieldVector(); - this.magnetometer = satellite.getSensors().getMagnetometer(); + this.b_field = satellite.getADCS().getSensors().getMagnetometer().retrievePerfectField().getFieldVector(); + this.magnetometer = satellite.getADCS().getSensors().getMagnetometer(); this.torqueKey = MemCachedTorqueProvider.torqueCommandKey; this.pwmKey = MemCachedTorqueProvider.pwmCommandKey; diff --git a/simulator/src/main/java/msp/simulator/dynamic/torques/Torques.java b/simulator/src/main/java/msp/simulator/dynamic/torques/Torques.java index 8cce5027..9a0262dd 100644 --- a/simulator/src/main/java/msp/simulator/dynamic/torques/Torques.java +++ b/simulator/src/main/java/msp/simulator/dynamic/torques/Torques.java @@ -37,7 +37,7 @@ public class Torques { /* ******* Public Static Attributes ******* */ /** Set the torque provider in use by the simulator. */ - public static TorqueProviderEnum commandTorqueProvider = TorqueProviderEnum.SCENARIO; + public static TorqueProviderEnum commandTorqueProvider = TorqueProviderEnum.CONTROLLER; /** Allow the torque disturbances in the simulation. */ public static boolean allowDisturbances = true; @@ -76,7 +76,8 @@ public Torques (Environment environment, Satellite satellite) { satellite, satellite.getAssembly().getStates().getInitialState().getDate(), environment - )); + )); + break; case MEMCACHED: this.torqueProviders.add( TorqueProviderEnum.MEMCACHED.getIndex(), diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/ADCS.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/ADCS.java index 48530afe..4713e30c 100644 --- a/simulator/src/main/java/msp/simulator/satellite/ADCS/ADCS.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/ADCS.java @@ -16,6 +16,8 @@ import java.util.function.Consumer; import org.hipparchus.geometry.euclidean.threed.Vector3D; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import msp.simulator.environment.Environment; import msp.simulator.satellite.Satellite; @@ -24,11 +26,13 @@ import msp.simulator.satellite.ADCS.Actuators.Actuators; import msp.simulator.satellite.ADCS.Controller.Controller; import msp.simulator.satellite.ADCS.Estimators.Estimators; +import msp.simulator.utils.logs.CustomLoggingTools; /** * * @author Jack McRobbie */ public class ADCS { + private static final Logger logger = LoggerFactory.getLogger(ADCS.class); private Sensors sensors; private Estimators estimators; private Controller controllers; @@ -41,9 +45,19 @@ public ADCS(Satellite sat,Environment environment) { this.estimators = new Estimators(sat); this.controllers = new Controller(sat); this.physics = new ADCSPhysics(sat, environment); + ADCS.logger.info(CustomLoggingTools.indentMsg(ADCS.logger, + "Building the ADCS Module: Success...")); } public Vector3D ComputeTorque() { Vector3D magneticDipole = this.controllers.getDipole(); + logger.info(this.physics.ComputeMagnetorquerTorque(magneticDipole).toString()); return this.physics.ComputeMagnetorquerTorque(magneticDipole); +// return new Vector3D(0.01,0.01,0.01); + } + /** + * @return + */ + public Sensors getSensors() { + return sensors; } } diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/ADCSPhysics/ADCSPhysics.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/ADCSPhysics/ADCSPhysics.java index ec943fc5..f872ce8c 100644 --- a/simulator/src/main/java/msp/simulator/satellite/ADCS/ADCSPhysics/ADCSPhysics.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/ADCSPhysics/ADCSPhysics.java @@ -14,28 +14,39 @@ package msp.simulator.satellite.ADCS.ADCSPhysics; import org.hipparchus.geometry.euclidean.threed.Vector3D; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import msp.simulator.environment.Environment; import msp.simulator.satellite.Satellite; import msp.simulator.satellite.ADACS.sensors.Sensors; import msp.simulator.satellite.ADCS.Actuators.MagnetoTorquers; +import msp.simulator.satellite.assembly.SatelliteBody; +import msp.simulator.utils.logs.CustomLoggingTools; /** * - * @author Florian CHAUBEYRE + * @author Jack McRobbie */ public class ADCSPhysics { + + /** Logger of the class. */ + private static final Logger logger = + LoggerFactory.getLogger(ADCSPhysics.class); + private Satellite satellite; private Environment environment; private Sensors sensor; public ADCSPhysics(Satellite satellite, Environment environemt) { this.satellite = satellite; this.environment = environment; + ADCSPhysics.logger.info(CustomLoggingTools.indentMsg(ADCSPhysics.logger, + " -> Building the ADCS Physics engine: Success.")); } public Vector3D ComputeMagnetorquerTorque(Vector3D magneticDipole) { - Vector3D result = Vector3D.crossProduct(magneticDipole, - this.sensor.getMagnetometer().retrievePerfectField().getFieldVector()); + Vector3D magfield = this.satellite.getADCS().getSensors().getMagnetometer() + .retrievePerfectField().getFieldVector().scalarMultiply(0.000000001); + Vector3D result = Vector3D.crossProduct(magneticDipole,magfield ); return result; } - } diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/Actuators.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/Actuators.java index c47e02f3..5a148b84 100644 --- a/simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/Actuators.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/Actuators.java @@ -27,13 +27,13 @@ public class Actuators { /** Logger of the class */ private static final Logger logger = LoggerFactory.getLogger(Actuators.class); - private static MagnetoTorquers magnetorquer; + private MagnetoTorquers magnetorquer; /** * */ public Actuators() { this.magnetorquer = new MagnetoTorquers(); - logger.info(CustomLoggingTools.indentMsg(logger, + Actuators.logger.info(CustomLoggingTools.indentMsg(Actuators.logger, "Building the Actuators...")); } @@ -41,7 +41,6 @@ public Actuators() { * @return */ public MagnetoTorquers getMagnetorquers() { - // TODO Auto-generated method stub return this.magnetorquer; } } diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/MagnetoTorquers.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/MagnetoTorquers.java index 3d1ea721..737172fc 100644 --- a/simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/MagnetoTorquers.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/MagnetoTorquers.java @@ -44,12 +44,31 @@ public MagnetoTorquers() { } public Vector3D computeDipole(Vector3D dutyCycle) { - Vector3D dipole = new Vector3D( - dutyCycle.getX() * orientation.getX(), - dutyCycle.getY() * orientation.getY(), - dutyCycle.getZ() * orientation.getZ() - ); - return dipole; + return this.constrainDipole(dutyCycle); + } + /** + * @param dutyCycle requested duty cycle for the magnetorquers + */ + private Vector3D constrainDipole(Vector3D dutyCycle) { + double x = dutyCycle.getX(); + double y = dutyCycle.getY(); + double z = dutyCycle.getZ(); + int sign; + if(Math.abs(x)>this.MaxDipole.getX()) { + sign = (0 > dutyCycle.getX())?-1:1; + x = this.MaxDipole.getX() * sign; + } + if(Math.abs(y)>this.MaxDipole.getY()) { + sign = (0 > dutyCycle.getY())?-1:1; + y = this.MaxDipole.getY() * sign; + } + if(Math.abs(z)>this.MaxDipole.getZ()) { + sign = (0 > dutyCycle.getZ())?-1:1; + z = this.MaxDipole.getZ() * sign; + } + Vector3D result = new Vector3D(x,y,z); + logger.info(result.toString()); + return result; } } diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/Controller.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/Controller.java index 1ca8ec46..0ff9e6a9 100644 --- a/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/Controller.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/Controller.java @@ -14,19 +14,29 @@ package msp.simulator.satellite.ADCS.Controller; import org.hipparchus.geometry.euclidean.threed.Vector3D; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import msp.simulator.satellite.Satellite; +import msp.simulator.satellite.ADCS.ADCSPhysics.ADCSPhysics; +import msp.simulator.utils.logs.CustomLoggingTools; /** * * @author Jack McRobbie */ public class Controller { + + private static final Logger logger = + LoggerFactory.getLogger(Controller.class); + B_Dot bdot; Satellite sat; public Controller(Satellite satellite) { this.sat = satellite; bdot = new B_Dot(this.sat); + this.logger.info(CustomLoggingTools.indentMsg(this.logger, + " -> Building the ADCS Controller: Success.")); } public Vector3D getDipole() { diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/BdotEstimator.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/BdotEstimator.java index 57ec16d9..0fcaf256 100644 --- a/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/BdotEstimator.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/BdotEstimator.java @@ -26,11 +26,14 @@ public class BdotEstimator { private Magnetometer mag; private Vector3D lastMagFieldReading; private final double timestep; + private Satellite satellite; public BdotEstimator(Satellite sat) { Vector3D initalState = new Vector3D(0.0,0.0,0.0); lowpassfilter = new LowPassFilter(5.0,0.25,initalState); timestep = 0.1; // TODO make equal to Controller frequency! + satellite = sat; + this.lastMagFieldReading= Vector3D.NEGATIVE_INFINITY; } public Vector3D computeBdot() { Vector3D bDotUnfiltered = this.getFirstOrderDiff(); @@ -38,7 +41,12 @@ public Vector3D computeBdot() { return bdot; } private Vector3D getFirstOrderDiff() { - Vector3D magreading = mag.retrieveNoisyField().getFieldVector(); + this.mag = this.satellite.getADCS().getSensors().getMagnetometer(); + Vector3D magreading = mag.retrieveNoisyField().getFieldVector().scalarMultiply(10^-9); + if(this.lastMagFieldReading == Vector3D.NEGATIVE_INFINITY) { + this.lastMagFieldReading = magreading; + return Vector3D.ZERO; + } double x = magreading.getX() - this.lastMagFieldReading.getX(); double y = magreading.getY() - this.lastMagFieldReading.getY(); double z = magreading.getZ() - this.lastMagFieldReading.getZ(); diff --git a/simulator/src/main/java/msp/simulator/satellite/Satellite.java b/simulator/src/main/java/msp/simulator/satellite/Satellite.java index 2f116da8..532bcb37 100644 --- a/simulator/src/main/java/msp/simulator/satellite/Satellite.java +++ b/simulator/src/main/java/msp/simulator/satellite/Satellite.java @@ -20,11 +20,11 @@ import org.slf4j.LoggerFactory; import msp.simulator.environment.Environment; +import msp.simulator.satellite.ADCS.ADCS; import msp.simulator.satellite.assembly.Assembly; import msp.simulator.satellite.assembly.SatelliteStates; import msp.simulator.satellite.io.IO; import msp.simulator.satellite.io.MemcachedRawTranscoder; -import msp.simulator.satellite.sensors.Sensors; import msp.simulator.utils.logs.CustomLoggingTools; /** @@ -39,14 +39,13 @@ public class Satellite { /** Instance of Assembly of the Satellite. */ private Assembly assembly; - /** Instance of the Sensors of the satellite. */ - private Sensors sensors; - /** Instance of the IO Manager of the satellite. */ private IO io; + + private ADCS adcsModule; /** - * Build the intance of the Satellite in the simulation and connect + * Build the instance of the Satellite in the simulation and connect * the required IO. * @param environment Instance of the Simulation */ @@ -57,8 +56,7 @@ public Satellite(Environment environment) { /* Building the Assembly of the Satellite. */ this.assembly = new Assembly(environment); - /* Building the sensors. */ - this.sensors = new Sensors(environment, assembly); + this.adcsModule = new ADCS(this,environment); /* Build the IO Manager. */ this.io = new IO(); @@ -82,7 +80,7 @@ public void executeStepMission() { /* Magnetometer Measurement of the Geomagnetic field vector. */ /* This import is done once to avoid multiple noise computation. * But it actually does not matter.*/ - Vector3D mmtMeasuredData = this.getSensors().getMagnetometer().getData_magField(); + Vector3D mmtMeasuredData = this.adcsModule.getSensors().getMagnetometer().getData_magField(); /* Note that the double types are converted into an array of bytes * before being send to the Memcached common memory to avoid both @@ -105,7 +103,7 @@ public void executeStepMission() { ); /* Gyrometer Sensor Measurement */ - Vector3D gyroMeasure = this.getSensors().getGyrometer().getData_angularVelocity(); + Vector3D gyroMeasure = this.adcsModule.getSensors().getGyrometer().getData_angularVelocity(); byte[] rawGyro_x = MemcachedRawTranscoder.toRawByteArray(gyroMeasure.getX()); byte[] rawGyro_y = MemcachedRawTranscoder.toRawByteArray(gyroMeasure.getY()); byte[] rawGyro_z = MemcachedRawTranscoder.toRawByteArray(gyroMeasure.getZ()); @@ -128,7 +126,7 @@ public void executeStepMission() { Vector3D nadir_body = this.assembly.getItrf2body(date) .transformVector(nadir_itrf); - double posXIR = this.sensors.getPosXIRSensor() + double posXIR = this.adcsModule.getSensors().getPosXIRSensor() .calculateInfraredReading(nadir_body); byte[] rawIR_xPos = MemcachedRawTranscoder.toRawByteArray(posXIR); this.io.getMemcached().set( @@ -136,7 +134,7 @@ public void executeStepMission() { rawIR_xPos ); - double negXIR = this.sensors.getNegXIRSensor() + double negXIR = this.adcsModule.getSensors().getNegXIRSensor() .calculateInfraredReading(nadir_body); byte[] rawIR_xNeg = MemcachedRawTranscoder.toRawByteArray(negXIR); this.io.getMemcached().set( @@ -144,7 +142,7 @@ public void executeStepMission() { rawIR_xNeg ); - double posYIR = this.sensors.getPosYIRSensor() + double posYIR = this.adcsModule.getSensors().getPosYIRSensor() .calculateInfraredReading(nadir_body); byte[] rawIR_yPos = MemcachedRawTranscoder.toRawByteArray(posYIR); this.io.getMemcached().set( @@ -152,7 +150,7 @@ public void executeStepMission() { rawIR_yPos ); - double negYIR = this.sensors.getNegYIRSensor() + double negYIR = this.adcsModule.getSensors().getNegYIRSensor() .calculateInfraredReading(nadir_body); byte[] rawIR_yNeg = MemcachedRawTranscoder.toRawByteArray(negYIR); this.io.getMemcached().set( @@ -160,7 +158,7 @@ public void executeStepMission() { rawIR_yNeg ); - double posZIR = this.sensors.getPosZIRSensor() + double posZIR = this.adcsModule.getSensors().getPosZIRSensor() .calculateInfraredReading(nadir_body); byte[] rawIR_zPos = MemcachedRawTranscoder.toRawByteArray(posZIR); this.io.getMemcached().set( @@ -168,7 +166,7 @@ public void executeStepMission() { rawIR_zPos ); - double negZIR = this.sensors.getNegZIRSensor() + double negZIR = this.adcsModule.getSensors().getNegZIRSensor() .calculateInfraredReading(nadir_body); byte[] rawIR_zNeg = MemcachedRawTranscoder.toRawByteArray(negZIR); this.io.getMemcached().set( @@ -200,14 +198,7 @@ public SatelliteStates getStates() { return this.getAssembly().getStates(); } - /** - * Return the satellite sensors. - * @return Sensors - * @see msp.simulator.satellite.sensors.Sensors - */ - public Sensors getSensors() { - return this.sensors; - } + /** * Return the satellite IO manager. @@ -216,5 +207,8 @@ public Sensors getSensors() { public IO getIO() { return this.io; } + public ADCS getADCS() { + return adcsModule; + } } diff --git a/simulator/src/main/java/msp/simulator/satellite/assembly/SatelliteBody.java b/simulator/src/main/java/msp/simulator/satellite/assembly/SatelliteBody.java index 89705851..ef1a5804 100644 --- a/simulator/src/main/java/msp/simulator/satellite/assembly/SatelliteBody.java +++ b/simulator/src/main/java/msp/simulator/satellite/assembly/SatelliteBody.java @@ -42,9 +42,9 @@ public class SatelliteBody extends BoxAndSolarArraySpacecraft { /** Inertia matrix of the satellite. */ public static double[][] satInertiaMatrix = /* kg.m^2 */ { - {1.9002 * 1e-3, 0 , 0 }, - { 0 , 1.9156 * 1e-3, 0 }, - { 0 , 0 , 1.9496 * 1e-3 }, + {1, 0 , 0 }, + { 0 , 1, 0 }, + { 0 , 0 , 1 }, }; /** Simple balance inertia matrix (Unit matrix). */ diff --git a/simulator/src/main/java/msp/simulator/satellite/sensors/Gyrometer.java b/simulator/src/main/java/msp/simulator/satellite/sensors/Gyrometer.java deleted file mode 100644 index 31bee55f..00000000 --- a/simulator/src/main/java/msp/simulator/satellite/sensors/Gyrometer.java +++ /dev/null @@ -1,90 +0,0 @@ -/* Copyright 20017-2018 Melbourne Space Program - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ -package msp.simulator.satellite.sensors; - -import org.hipparchus.geometry.euclidean.threed.Vector3D; -import org.hipparchus.util.FastMath; -import org.slf4j.Logger; -import org.slf4j.LoggerFactory; - -import msp.simulator.environment.Environment; -import msp.simulator.satellite.assembly.Assembly; -import msp.simulator.utils.logs.CustomLoggingTools; - -/** - * Modelize the gyrometer sensor of the satellite. - * - * @author Florian CHAUBEYRE - */ -public class Gyrometer { - - /* ******* Public Static Attributes ******* */ - - /** This intensity is used to generate a random number to be - * added to each components of the sensor data. - */ - //public static double defaultGyroNoiseIntensity = 1e-3; - public static double defaultGyroNoiseIntensity = 0; - - /* **************************************** */ - - /** Logger of the class. */ - private static final Logger logger = LoggerFactory.getLogger( - Gyrometer.class); - - /** Instance of the simulation. */ - private Assembly assembly; - - /** Normal noise disturbing the gyrometer measures. */ - private double gyroNoiseIntensity; - - /** - * Simple constructor of the gyrometer. - * @param environment Instance of the simulation - * @param assembly Instance of the simulation - */ - public Gyrometer(Environment environment, Assembly assembly) { - logger.info(CustomLoggingTools.indentMsg(logger, - " -> Building the Gyrometer...")); - - this.assembly = assembly; - this.gyroNoiseIntensity = defaultGyroNoiseIntensity; - } - - /** - * Retrieve the data from the sensor. - * @return Rotational Acceleration - */ - public Vector3D getData_angularVelocity() { - /* Get the angular velocity from the satellite state. */ - /* Note that these data are already in the satellite - * body frame! - */ - Vector3D data = this.assembly.getStates() - .getCurrentState().getAttitude().getSpin(); - - /* Add the noise contribution. */ - Vector3D noise = new Vector3D( - 2 * (FastMath.random() - 0.5) * this.gyroNoiseIntensity, - 2 * (FastMath.random() - 0.5) * this.gyroNoiseIntensity, - 2 * (FastMath.random() - 0.5) * this.gyroNoiseIntensity - ); - - data = data.add(noise); - - return data; - } - - -} \ No newline at end of file diff --git a/simulator/src/main/java/msp/simulator/satellite/sensors/InfraredSensor.java b/simulator/src/main/java/msp/simulator/satellite/sensors/InfraredSensor.java deleted file mode 100644 index 7d90dcb3..00000000 --- a/simulator/src/main/java/msp/simulator/satellite/sensors/InfraredSensor.java +++ /dev/null @@ -1,91 +0,0 @@ -package msp.simulator.satellite.sensors; - -import org.hipparchus.geometry.euclidean.threed.*; -import org.hipparchus.util.FastMath; - -/** - * This class represents the infrared sensor of the satellite - * - * @author Braeden BORG - */ -public class InfraredSensor { - - /** - * Constants determined for a ninth-order polynomial that best represents the - * model curve for infrared readings versus angle from Nadir - */ - private static final double p1 = 0.007330053825571, p2 = -0.090570226260328, p3 = 0.445759574332216, - p4 = -1.123946349873612, p5 = 1.594507643864135, p6 = -1.440516312413851, p7 = 1.178825031933260, - p8 = -1.078355217581003, p9 = 0.182686213443513, p10 = 0.987208931556143; - private double infraredReading, angleReading; - private Vector3D sideNormal; - - /** - * Creates an infrared sensor for a satellite side with an infrared reading and - * associated angle to nadir - * - * @param sideNormalVector - * Vector normal to a satellite side - */ - public InfraredSensor(Vector3D sideNormalVector) { - infraredReading = 0; - angleReading = 0; - sideNormal = sideNormalVector; - } - - /** - * Function to compute the infrared reading for a face of the satellite given - * the angle to Nadir - * - * @param angle - * Angle to Nadir vector along an axis plane - * @return result The corresponding infrared reading for the angle to Nadir - */ - private double angleToInfrared(double angle) { - double result, x = angle; - /* - * Uses a ninth-order polynomial of best fit to the model infrared curve - */ - result = p1 * FastMath.pow(x, 9) + p2 * FastMath.pow(x, 8) + p3 * FastMath.pow(x, 7) + p4 * FastMath.pow(x, 6) - + p5 * FastMath.pow(x, 5) + p6 * FastMath.pow(x, 4) + p7 * FastMath.pow(x, 3) + p8 * FastMath.pow(x, 2) - + p9 * x + p10; - - return result; - } - - /** - * Function to determine the angle of a Nadir vector to each reference axes - * - * @param nadir Known Nadir vector - * @param sideNormal Vector normal to a satellite side - * @return Angle to Nadir along an axis plane - */ - private double angleFromNadirVector(Vector3D nadir, Vector3D sideNormal) { - return FastMath.acos(Vector3D.dotProduct(nadir, sideNormal) / nadir.getNorm()); - } - - /** - * Function to initialise the infrared sensor and all of its values for a given - * Nadir vector - * - * @param nadir Known Nadir vector - * @return infraredReading Infrared reading of the sensor based upon its - * relationship with the angle to Nadir - */ - public double calculateInfraredReading(Vector3D nadir) { - angleReading = angleFromNadirVector(nadir, sideNormal); - infraredReading = angleToInfrared(angleReading); - return infraredReading; - } - - /** - * Series of get and set functions for @testing purposes only - */ - public double getAngle() { - return angleReading; - } - - public Vector3D getSideNormal() { - return sideNormal; - } -} diff --git a/simulator/src/main/java/msp/simulator/satellite/sensors/Magnetometer.java b/simulator/src/main/java/msp/simulator/satellite/sensors/Magnetometer.java deleted file mode 100644 index b8c36878..00000000 --- a/simulator/src/main/java/msp/simulator/satellite/sensors/Magnetometer.java +++ /dev/null @@ -1,203 +0,0 @@ -/* Copyright 20017-2018 Melbourne Space Program - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -package msp.simulator.satellite.sensors; - -import org.hipparchus.geometry.euclidean.threed.Vector3D; -import org.hipparchus.util.FastMath; -import org.orekit.bodies.GeodeticPoint; -import org.orekit.errors.OrekitException; -import org.orekit.models.earth.GeoMagneticElements; -import org.orekit.propagation.SpacecraftState; -import org.slf4j.Logger; -import org.slf4j.LoggerFactory; -import org.hipparchus.random.RandomDataGenerator; - -import msp.simulator.environment.Environment; -import msp.simulator.environment.geomagneticField.EarthMagneticField; -import msp.simulator.environment.solarSystem.Earth; -import msp.simulator.satellite.assembly.Assembly; -import msp.simulator.utils.logs.CustomLoggingTools; - -/** - * This class represents the magnetometer sensor of the - * satellite. - * - * @author Florian CHAUBEYRE - */ -public class Magnetometer { - - - /* ******* Public Static Attributes ******* */ - - - /* Random number generator */ - private RandomDataGenerator randomDataGenerator = new RandomDataGenerator(1000); - - /** This intensity is used to generate a random number to be - * added to each components of the true magnetic field. - * (nanoTesla) - */ - public static double defaultMagnetoNoiseIntensity = 4.156*1000; - - /* **************************************** */ - - /** Logger of the class. */ - private static final Logger logger = LoggerFactory.getLogger( - Magnetometer.class); - - /** Instance of the OreKit Magnetic Field. */ - private EarthMagneticField geomagField; - - /** Instance of the Earth. */ - private Earth earth; - - /** Assembly of the satellite. */ - private Assembly assembly; - - /** Private attribute for the noise intensity. */ - private double noiseIntensity; - - public Magnetometer(Environment environment, Assembly assembly) { - logger.info(CustomLoggingTools.indentMsg(logger, - " -> Building the Magnetometer...")); - - /* Linking the class to the rest of the simulation. */ - this.geomagField = environment.getGeoMagneticField(); - this.earth = environment.getSolarSystem().getEarth(); - this.assembly = assembly; - - /* Initializing the class. */ - this.noiseIntensity = Magnetometer.defaultMagnetoNoiseIntensity; - } - - /** - * Return a measurement disturbed by a random noise. - * The intensity of the noise factor can be modified at - * initialization. - * @return GeoMagneticElements (where field vector is expressed in nT) - * @see #retrievePerfectMeasurement() - */ - public GeoMagneticElements retrieveNoisyField() { - /* Perfect Measure. */ - - - GeoMagneticElements perfectMeasure = this.retrievePerfectField(); - - /* Normally distributed random noise contribution. */ - Vector3D noise = new Vector3D ( new double[] { - randomDataGenerator.nextNormal(0, this.noiseIntensity), - randomDataGenerator.nextNormal(0, this.noiseIntensity), - randomDataGenerator.nextNormal(0, this.noiseIntensity) - }); - - /* Disturbing the perfect measurement. */ - Vector3D noisyFieldVector = - perfectMeasure.getFieldVector().add(noise); - - /* Creating the noisy measure. */ - GeoMagneticElements noisyMeasure = new GeoMagneticElements(noisyFieldVector); - - logger.debug("Noisy Geo" + noisyMeasure.toString()); - - - return noisyMeasure; - } - - - - /** - * Retrieve a perfect measured data from the sensors, i.e. an - * ideal measurement without any noise or interference. - * - * @return GeoMagneticElements at the location of the satellite. - * (where field vector is expressed in nT) - * @see GeoMagneticElements - */ - public GeoMagneticElements retrievePerfectField() { - - SpacecraftState satState = this.assembly.getStates().getCurrentState() ; - - Vector3D positionOnEarth = - satState.getOrbit().getPVCoordinates().getPosition(); - - GeodeticPoint geodeticPosition = null; - - try { - /* The transformation from cartesian to geodetic coordinates is actually - * not straight as it needs to solve some 2-unknowns non-linear equations. - * So it needs a processing algorithm like the one presented by OreKit - * in the following method. - */ - geodeticPosition = earth.getEllipsoid().transform( - positionOnEarth, - satState.getOrbit().getFrame(), - satState.getDate() - ); - } catch (OrekitException e) { - e.printStackTrace(); - } - - /* Calculate the magnetic field at the projected geodetic point. - * Note that the algorithm brings some approximation, for instance - * the altitude of the satellite is slightly shifted from the true - * one. - */ - GeoMagneticElements trueField_itrf = this.geomagField.getField().calculateField( - FastMath.toDegrees(geodeticPosition.getLatitude()), /* decimal deg */ - FastMath.toDegrees(geodeticPosition.getLongitude()), /* decimal deg */ - (satState.getA() - this.earth.getRadius()) / 1e3 /* km */ - ); - - logger.debug("Magnetometer Measurement: \n" + - "Latitude: " + FastMath.toDegrees(geodeticPosition.getLatitude()) + " °\n" + - "Longitud: " + FastMath.toDegrees(geodeticPosition.getLongitude()) + " °\n" + - "Altitude: " + (satState.getA() - this.earth.getRadius()) / 1e3 + " km\n" + - "True Geo ITRF" + trueField_itrf.toString() - ); - - /* Rotate the magnetic field reading into the body frame */ - Vector3D trueField_body = this.assembly.getItrf2body(satState.getDate()) - .transformVector(trueField_itrf.getFieldVector()); - - return new GeoMagneticElements(trueField_body); - } - - /** - * Retrieve the measured data from the magnetometer sensors. - *

- * WARNING: a noise is always introduced component by component - * on the returned value. Storing the vector before working on - * it may be required. - * - * @return Geomagnetic Field Vector (in Tesla) - */ - public Vector3D getData_magField() { - /* Retrieve the noisy magnetic field. */ - Vector3D data = this.retrieveNoisyField().getFieldVector(); - - /* Convert nTesla into Tesla. */ - data = data.scalarMultiply(1e-9); - - return data; - } - - /** - * @return The noise intensity in nTesla. - */ - public double getNoiseIntensity() { - return noiseIntensity; - } - -} diff --git a/simulator/src/main/java/msp/simulator/satellite/sensors/Sensors.java b/simulator/src/main/java/msp/simulator/satellite/sensors/Sensors.java deleted file mode 100644 index c1c4b83b..00000000 --- a/simulator/src/main/java/msp/simulator/satellite/sensors/Sensors.java +++ /dev/null @@ -1,129 +0,0 @@ -/* Copyright 20017-2018 Melbourne Space Program - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -package msp.simulator.satellite.sensors; - -import org.hipparchus.geometry.euclidean.threed.Vector3D; -import org.slf4j.Logger; -import org.slf4j.LoggerFactory; - -import msp.simulator.environment.Environment; -import msp.simulator.satellite.assembly.Assembly; -import msp.simulator.utils.logs.CustomLoggingTools; - -/** - * - * @author Florian CHAUBEYRE - * @author Braeden BORG - */ -public class Sensors { - - /** Logger of the class. */ - private static final Logger logger = - LoggerFactory.getLogger(Sensors.class); - - /** Instance of environment of the simulation. */ - private Environment environment; - - /** Instance of the satellite assembly of the simulation. */ - private Assembly assembly; - - /* ***** Instances of the different sensors. ***** */ - - /** Instance of magnetometer in the simulation */ - private Magnetometer magnetometer; - - /** Instance of infrared Sensors in the simulation */ - private InfraredSensor - posXIRSensor, - negXIRSensor, - posYIRSensor, - negYIRSensor, - posZIRSensor, - negZIRSensor; - - /** Instance of gyrometer in the simulation. */ - private Gyrometer gyrometer; - - /** - * Constructor of the satellite sensors. - * - * @param environment Instance of the simulation - * @param assembly Instance of the simulation - */ - public Sensors(Environment environment, Assembly assembly) { - logger.info(CustomLoggingTools.indentMsg(logger, - "Building the satellite Sensors...")); - - /* Linking the sensors class to the rest of the simulation. */ - this.environment = environment; - this.assembly = assembly; - - /* TODO: Normalize the construction and the use of the sensors. - * Typically by extending a minimal abstract class Sensor. - */ - - /* Building the sensors. */ - this.magnetometer = new Magnetometer(this.environment, this.assembly); - this.gyrometer = new Gyrometer(this.environment, this.assembly); - - this.posXIRSensor = new InfraredSensor(Vector3D.PLUS_I); - this.negXIRSensor = new InfraredSensor(Vector3D.MINUS_I); - this.posYIRSensor = new InfraredSensor(Vector3D.PLUS_J); - this.negYIRSensor = new InfraredSensor(Vector3D.MINUS_J); - this.posZIRSensor = new InfraredSensor(Vector3D.PLUS_K); - this.negZIRSensor = new InfraredSensor(Vector3D.MINUS_K); - } - - /** - * @return the magnetometer - */ - public Magnetometer getMagnetometer() { - return magnetometer; - } - - /** - * @return the gyrometer - */ - public Gyrometer getGyrometer() { - return gyrometer; - } - - /** - * @return Infrared sensor for a side of the satellite - */ - public InfraredSensor getPosXIRSensor() { - return posXIRSensor; - } - - public InfraredSensor getNegXIRSensor() { - return negXIRSensor; - } - - public InfraredSensor getPosYIRSensor() { - return posYIRSensor; - } - - public InfraredSensor getNegYIRSensor() { - return negYIRSensor; - } - - public InfraredSensor getPosZIRSensor() { - return posZIRSensor; - } - - public InfraredSensor getNegZIRSensor() { - return negZIRSensor; - } -} diff --git a/simulator/src/main/java/msp/simulator/satellite/sensors/package-info.java b/simulator/src/main/java/msp/simulator/satellite/sensors/package-info.java deleted file mode 100644 index 7ba0f065..00000000 --- a/simulator/src/main/java/msp/simulator/satellite/sensors/package-info.java +++ /dev/null @@ -1,22 +0,0 @@ -/* Copyright 20017-2018 Melbourne Space Program - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -/** - * This package gathers all of the sensor modules - * of the satellite. - * - * @author Florian CHAUBEYRE - * @author Braeden BORG - */ -package msp.simulator.satellite.sensors; diff --git a/simulator/src/main/java/msp/simulator/user/Dashboard.java b/simulator/src/main/java/msp/simulator/user/Dashboard.java index 8857cf35..3576d5fe 100644 --- a/simulator/src/main/java/msp/simulator/user/Dashboard.java +++ b/simulator/src/main/java/msp/simulator/user/Dashboard.java @@ -40,11 +40,11 @@ import msp.simulator.satellite.assembly.SatelliteBody; import msp.simulator.satellite.assembly.SatelliteStates; import msp.simulator.satellite.io.IO; -import msp.simulator.satellite.sensors.Gyrometer; -import msp.simulator.satellite.sensors.Magnetometer; import msp.simulator.utils.logs.CustomLoggingTools; import msp.simulator.utils.logs.ephemeris.EphemerisGenerator; - +import msp.simulator.satellite.ADACS.*; +import msp.simulator.satellite.ADACS.sensors.Gyrometer; +import msp.simulator.satellite.ADACS.sensors.Magnetometer;;; /** * This class handles the user-configuration * of the numerical simulator. @@ -114,7 +114,7 @@ public static void setDefaultConfiguration() { Dashboard.setInitialAttitudeQuaternion(new Quaternion(1,0,0,0)); Dashboard.setInitialSpin(Vector3D.ZERO); Dashboard.setInitialRotAcceleration(Vector3D.ZERO); - Dashboard.setCommandTorqueProvider(TorqueProviderEnum.SCENARIO); + Dashboard.setCommandTorqueProvider(TorqueProviderEnum.CONTROLLER); Dashboard.setTorqueScenario(new ArrayList()); Dashboard.setTorqueDisturbances(true); diff --git a/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java b/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java index 0bfa389a..d6359828 100644 --- a/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java +++ b/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java @@ -363,7 +363,7 @@ public void writeStep(Satellite satellite) { buff = new StringBuffer(); /* Writing the current mag field to the log file. */ - Vector3D mag_field = satellite.getSensors().getMagnetometer().retrievePerfectField().getFieldVector(); + Vector3D mag_field = satellite.getADCS().getSensors().getMagnetometer().retrievePerfectField().getFieldVector(); Vector3D mag_unit; mag_unit = mag_field.normalize(); buff From 92b8f57f0360a8b6e2a87ce44faacf0a11c304ac Mon Sep 17 00:00:00 2001 From: jmcrobbie Date: Sat, 16 Feb 2019 16:20:19 +1030 Subject: [PATCH 45/48] Fixed silly bug in LPF --- .../ADCS/Estimators/BdotEstimator/BdotEstimator.java | 8 ++++++++ .../ADCS/Estimators/BdotEstimator/LowPassFilter.java | 2 +- .../msp/simulator/satellite/assembly/SatelliteBody.java | 6 +++--- 3 files changed, 12 insertions(+), 4 deletions(-) diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/BdotEstimator.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/BdotEstimator.java index 0fcaf256..044c337b 100644 --- a/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/BdotEstimator.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/BdotEstimator.java @@ -14,9 +14,13 @@ package msp.simulator.satellite.ADCS.Estimators.BdotEstimator; import org.hipparchus.geometry.euclidean.threed.Vector3D; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import msp.simulator.satellite.Satellite; import msp.simulator.satellite.ADACS.sensors.Magnetometer; +import msp.simulator.satellite.ADCS.Actuators.Actuators; +import msp.simulator.utils.logs.CustomLoggingTools; /** * * @author Jack McRobbie @@ -27,6 +31,7 @@ public class BdotEstimator { private Vector3D lastMagFieldReading; private final double timestep; private Satellite satellite; + private static final Logger logger = LoggerFactory.getLogger(BdotEstimator.class); public BdotEstimator(Satellite sat) { Vector3D initalState = new Vector3D(0.0,0.0,0.0); @@ -34,10 +39,13 @@ public BdotEstimator(Satellite sat) { timestep = 0.1; // TODO make equal to Controller frequency! satellite = sat; this.lastMagFieldReading= Vector3D.NEGATIVE_INFINITY; + BdotEstimator.logger.info(CustomLoggingTools.indentMsg(BdotEstimator.logger, + "Building the Bdot Estimator...")); } public Vector3D computeBdot() { Vector3D bDotUnfiltered = this.getFirstOrderDiff(); Vector3D bdot = lowpassfilter.ProcessSample(bDotUnfiltered); + logger.info("Bdot estimate" + bdot.toString()); return bdot; } private Vector3D getFirstOrderDiff() { diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/LowPassFilter.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/LowPassFilter.java index a259e550..7db842a2 100644 --- a/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/LowPassFilter.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/LowPassFilter.java @@ -39,7 +39,7 @@ public LowPassFilter(double tc, double spms,double initialState) { public LowPassFilter(double tc, double spms, Vector3D initState) { this.timeConstant = tc; this.samplePeriodMili = spms; - this.alpha = Math.exp(spms/tc); + this.alpha = Math.exp(-spms/tc); /* Utilize multiplicative class constructor returns same as initState */ state3d = new Vector3D(1.0,initState); diff --git a/simulator/src/main/java/msp/simulator/satellite/assembly/SatelliteBody.java b/simulator/src/main/java/msp/simulator/satellite/assembly/SatelliteBody.java index ef1a5804..2818fb72 100644 --- a/simulator/src/main/java/msp/simulator/satellite/assembly/SatelliteBody.java +++ b/simulator/src/main/java/msp/simulator/satellite/assembly/SatelliteBody.java @@ -42,9 +42,9 @@ public class SatelliteBody extends BoxAndSolarArraySpacecraft { /** Inertia matrix of the satellite. */ public static double[][] satInertiaMatrix = /* kg.m^2 */ { - {1, 0 , 0 }, - { 0 , 1, 0 }, - { 0 , 0 , 1 }, + {0.001, 0 , 0 }, + { 0 , 0.001, 0 }, + { 0 , 0 , 0.001 }, }; /** Simple balance inertia matrix (Unit matrix). */ From bb105ee17cb7eec4fb4b5e628ecf7c560d771895 Mon Sep 17 00:00:00 2001 From: jmcrobbie Date: Sat, 16 Feb 2019 19:00:08 +1030 Subject: [PATCH 46/48] Bugs fixed, bdot implemented and running --- .../main/java/msp/simulator/satellite/ADCS/ADCS.java | 2 +- .../satellite/ADCS/Actuators/MagnetoTorquers.java | 6 +++--- .../simulator/satellite/ADCS/Controller/B_Dot.java | 7 +++++-- .../ADCS/Estimators/BdotEstimator/BdotEstimator.java | 11 +++++------ .../ADCS/Estimators/BdotEstimator/LowPassFilter.java | 2 +- 5 files changed, 15 insertions(+), 13 deletions(-) diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/ADCS.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/ADCS.java index 4713e30c..0bc778b3 100644 --- a/simulator/src/main/java/msp/simulator/satellite/ADCS/ADCS.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/ADCS.java @@ -50,7 +50,7 @@ public ADCS(Satellite sat,Environment environment) { } public Vector3D ComputeTorque() { Vector3D magneticDipole = this.controllers.getDipole(); - logger.info(this.physics.ComputeMagnetorquerTorque(magneticDipole).toString()); + return this.physics.ComputeMagnetorquerTorque(magneticDipole); // return new Vector3D(0.01,0.01,0.01); } diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/MagnetoTorquers.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/MagnetoTorquers.java index 737172fc..2302cfd4 100644 --- a/simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/MagnetoTorquers.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/MagnetoTorquers.java @@ -37,7 +37,7 @@ public class MagnetoTorquers { * */ public MagnetoTorquers() { - logger.info(CustomLoggingTools.indentMsg(logger, + MagnetoTorquers.logger.info(CustomLoggingTools.indentMsg(logger, "Building the MagnetoTorquers...")); this.orientation = new Vector3D(1,1,1); this.MaxDipole = new Vector3D(0.1,0.1,0.1); //TODO make configurable from simulation initialization @@ -54,6 +54,7 @@ private Vector3D constrainDipole(Vector3D dutyCycle) { double y = dutyCycle.getY(); double z = dutyCycle.getZ(); int sign; + Vector3D result = new Vector3D(x,y,z); if(Math.abs(x)>this.MaxDipole.getX()) { sign = (0 > dutyCycle.getX())?-1:1; x = this.MaxDipole.getX() * sign; @@ -66,8 +67,7 @@ private Vector3D constrainDipole(Vector3D dutyCycle) { sign = (0 > dutyCycle.getZ())?-1:1; z = this.MaxDipole.getZ() * sign; } - Vector3D result = new Vector3D(x,y,z); - logger.info(result.toString()); + result = new Vector3D(x,y,z); return result; } diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/B_Dot.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/B_Dot.java index 0b16aec7..2b64a875 100644 --- a/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/B_Dot.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/B_Dot.java @@ -24,7 +24,7 @@ * @author Jack McRobbie> */ public class B_Dot { - private static final Vector3D bdotGains = new Vector3D(-54000,-54000,-54000); + private static final Vector3D bdotGains = new Vector3D(54000,54000,54000); private Actuators actuators; private BdotEstimator est; @@ -33,7 +33,10 @@ public B_Dot(Satellite sat) { this.actuators = new Actuators(); } public Vector3D computeDipole() { - Vector3D dutyCycle = new Vector3D(1.0,est.computeBdot(),1.0,this.bdotGains); + Vector3D dutyCycle = new Vector3D(this.bdotGains.getX()*this.est.computeBdot().getX(), + this.bdotGains.getY()*this.est.computeBdot().getY(), + this.bdotGains.getZ()*this.est.computeBdot().getZ() + ); Vector3D result = this.actuators.getMagnetorquers().computeDipole(dutyCycle); return result; } diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/BdotEstimator.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/BdotEstimator.java index 044c337b..caf0e005 100644 --- a/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/BdotEstimator.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/BdotEstimator.java @@ -35,7 +35,7 @@ public class BdotEstimator { public BdotEstimator(Satellite sat) { Vector3D initalState = new Vector3D(0.0,0.0,0.0); - lowpassfilter = new LowPassFilter(5.0,0.25,initalState); + lowpassfilter = new LowPassFilter(1.0,0.1,initalState); timestep = 0.1; // TODO make equal to Controller frequency! satellite = sat; this.lastMagFieldReading= Vector3D.NEGATIVE_INFINITY; @@ -45,7 +45,6 @@ public BdotEstimator(Satellite sat) { public Vector3D computeBdot() { Vector3D bDotUnfiltered = this.getFirstOrderDiff(); Vector3D bdot = lowpassfilter.ProcessSample(bDotUnfiltered); - logger.info("Bdot estimate" + bdot.toString()); return bdot; } private Vector3D getFirstOrderDiff() { @@ -55,11 +54,11 @@ private Vector3D getFirstOrderDiff() { this.lastMagFieldReading = magreading; return Vector3D.ZERO; } - double x = magreading.getX() - this.lastMagFieldReading.getX(); - double y = magreading.getY() - this.lastMagFieldReading.getY(); - double z = magreading.getZ() - this.lastMagFieldReading.getZ(); + double x = (magreading.getX() - this.lastMagFieldReading.getX())/this.timestep; + double y = (magreading.getY() - this.lastMagFieldReading.getY())/this.timestep; + double z = (magreading.getZ() - this.lastMagFieldReading.getZ())/this.timestep; this.lastMagFieldReading = magreading; - Vector3D result = new Vector3D(x/this.timestep,y/this.timestep,z/this.timestep); + Vector3D result = new Vector3D(x,y,z); return result; } diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/LowPassFilter.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/LowPassFilter.java index 7db842a2..baed4393 100644 --- a/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/LowPassFilter.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/LowPassFilter.java @@ -32,7 +32,7 @@ public class LowPassFilter { public LowPassFilter(double tc, double spms,double initialState) { this.timeConstant = tc; this.samplePeriodMili = spms; - this.alpha = Math.exp(spms/tc); + this.alpha = Math.exp(-spms/tc); state = initialState; this.flag = this.ConstructorDouble; } From 2d8d35559b2e6559df9ee8af6cd869db4db8542c Mon Sep 17 00:00:00 2001 From: jmcrobbie Date: Mon, 25 Feb 2019 11:13:59 +1030 Subject: [PATCH 47/48] detumbling running and some quality of life usability addidtions --- README.md | 1 + simulator/runSim.sh | 2 ++ .../msp/simulator/satellite/ADCS/Controller/B_Dot.java | 2 +- .../simulator/satellite/ADCS/Controller/Controller.java | 7 +++++-- .../ADCS/Estimators/BdotEstimator/BdotEstimator.java | 2 +- 5 files changed, 10 insertions(+), 4 deletions(-) create mode 100644 simulator/runSim.sh diff --git a/README.md b/README.md index b0f87af8..ea965607 100644 --- a/README.md +++ b/README.md @@ -21,6 +21,7 @@ The simulator project is now set up. You can launch the main method and some tes - cd into /simulator - $mvn compile - $mvn mvn exec:java -D"exec.mainClass"="msp.simulator.Main" +- OR simply use bash runSim.sh ## Simple Test Execution: - In Eclipse, select the test to run in src/test/java/msp/simulator - Run As > JUnit Test diff --git a/simulator/runSim.sh b/simulator/runSim.sh new file mode 100644 index 00000000..0e27ab65 --- /dev/null +++ b/simulator/runSim.sh @@ -0,0 +1,2 @@ +mvn compile; +mvn exec:java -D"exec.mainClass"="msp.simulator.Main" diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/B_Dot.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/B_Dot.java index 2b64a875..9337b7cc 100644 --- a/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/B_Dot.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/B_Dot.java @@ -24,7 +24,7 @@ * @author Jack McRobbie> */ public class B_Dot { - private static final Vector3D bdotGains = new Vector3D(54000,54000,54000); + private static final Vector3D bdotGains = new Vector3D(-100000,-100000,-100000); private Actuators actuators; private BdotEstimator est; diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/Controller.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/Controller.java index 0ff9e6a9..694dd7dc 100644 --- a/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/Controller.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/Controller.java @@ -35,11 +35,14 @@ public class Controller { public Controller(Satellite satellite) { this.sat = satellite; bdot = new B_Dot(this.sat); - this.logger.info(CustomLoggingTools.indentMsg(this.logger, + Controller.logger.info(CustomLoggingTools.indentMsg(this.logger, " -> Building the ADCS Controller: Success.")); } public Vector3D getDipole() { - return this.bdot.computeDipole(); + Vector3D result = this.bdot.computeDipole(); + Controller.logger.info(result.toString()); + return result; + } } diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/BdotEstimator.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/BdotEstimator.java index caf0e005..e7ec5215 100644 --- a/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/BdotEstimator.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/BdotEstimator.java @@ -49,7 +49,7 @@ public Vector3D computeBdot() { } private Vector3D getFirstOrderDiff() { this.mag = this.satellite.getADCS().getSensors().getMagnetometer(); - Vector3D magreading = mag.retrieveNoisyField().getFieldVector().scalarMultiply(10^-9); + Vector3D magreading = mag.retrieveNoisyField().getFieldVector().scalarMultiply(0.000000001); if(this.lastMagFieldReading == Vector3D.NEGATIVE_INFINITY) { this.lastMagFieldReading = magreading; return Vector3D.ZERO; From c04d5c7a1468924f397b67c89dbd70a4df2e6eb2 Mon Sep 17 00:00:00 2001 From: jmcrobbie Date: Mon, 25 Feb 2019 11:37:14 +1030 Subject: [PATCH 48/48] reverted some unecessary changes to eclipse autoconfigs --- simulator/.classpath | 6 +++++- simulator/.settings/org.eclipse.jdt.core.prefs | 1 - 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/simulator/.classpath b/simulator/.classpath index a5d95095..7e4ca5cd 100644 --- a/simulator/.classpath +++ b/simulator/.classpath @@ -15,7 +15,6 @@ - @@ -28,5 +27,10 @@ + + + + + diff --git a/simulator/.settings/org.eclipse.jdt.core.prefs b/simulator/.settings/org.eclipse.jdt.core.prefs index 91ca62e2..13b3428a 100644 --- a/simulator/.settings/org.eclipse.jdt.core.prefs +++ b/simulator/.settings/org.eclipse.jdt.core.prefs @@ -10,5 +10,4 @@ org.eclipse.jdt.core.compiler.debug.sourceFile=generate org.eclipse.jdt.core.compiler.problem.assertIdentifier=error org.eclipse.jdt.core.compiler.problem.enumIdentifier=error org.eclipse.jdt.core.compiler.problem.forbiddenReference=warning -org.eclipse.jdt.core.compiler.release=disabled org.eclipse.jdt.core.compiler.source=1.8