diff --git a/README.md b/README.md index 507deb83..fafd3e9d 100644 --- a/README.md +++ b/README.md @@ -16,7 +16,13 @@ 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 mvn exec:java -D"exec.mainClass"="msp.simulator.Main" +- OR simply use bash runSim.sh +- $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 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 + 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/Main.java b/simulator/src/main/java/msp/simulator/Main.java index ec5ad573..c471986c 100644 --- a/simulator/src/main/java/msp/simulator/Main.java +++ b/simulator/src/main/java/msp/simulator/Main.java @@ -40,17 +40,16 @@ public static void main(String[] args) { Dashboard.setSimulationDuration(1000000); Dashboard.setIntegrationTimeStep(0.1); - Dashboard.setEphemerisTimeStep(1.0); - Dashboard.setSatelliteInertiaMatrix(SatelliteBody.satInertiaMatrix); + 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); - Dashboard.setCommandTorqueProvider(TorqueProviderEnum.MEMCACHED); - Dashboard.setMemCachedConnection(true, "127.0.0.1:11211"); + Dashboard.setCommandTorqueProvider(TorqueProviderEnum.CONTROLLER); - //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/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/ControllerTorqueProvider.java b/simulator/src/main/java/msp/simulator/dynamic/torques/ControllerTorqueProvider.java new file mode 100644 index 00000000..35dc2c83 --- /dev/null +++ b/simulator/src/main/java/msp/simulator/dynamic/torques/ControllerTorqueProvider.java @@ -0,0 +1,42 @@ +/* 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.NumericalSimulator; +import msp.simulator.dynamic.torques.TorqueOverTimeScenarioProvider.Step; +import msp.simulator.environment.Environment; +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{ + private Satellite sat; + private Vector3D steptorque; + public ControllerTorqueProvider(Satellite satellite, AbsoluteDate date, Environment environment) { + this.sat = satellite; + this.steptorque = Vector3D.ZERO; + } + /** {@inheritDoc} */ + @Override + public Vector3D getTorque(AbsoluteDate date) { + 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 c7a616b8..900708d3 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.ADACS.sensors.*;; /** * * @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,25 @@ 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.2}; + + 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; + logger.info("PWM SIGNAL:"+dipole.toString()); + torque = Vector3D.crossProduct(dipole,this.b_field); + return torque; + } /** * Create the instance of memcached torque provider. @@ -88,9 +107,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.getADCS().getSensors().getMagnetometer().retrievePerfectField().getFieldVector(); + this.magnetometer = satellite.getADCS().getSensors().getMagnetometer(); + this.torqueKey = MemCachedTorqueProvider.torqueCommandKey; + this.pwmKey = MemCachedTorqueProvider.pwmCommandKey; + this.memcached = satellite.getIO().getMemcached(); this.memcachedTranscoder = satellite.getIO().getRawTranscoder(); @@ -114,7 +136,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 +152,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()) { @@ -170,7 +193,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 { 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/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), 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..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,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.CONTROLLER; + + /** 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,9 +64,20 @@ 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) { + switch (Torques.commandTorqueProvider) { + case CONTROLLER: + this.torqueProviders.add( + TorqueProviderEnum.CONTROLLER.getIndex(), new ControllerTorqueProvider( + satellite, + satellite.getAssembly().getStates().getInitialState().getDate(), + environment + )); + break; case MEMCACHED: this.torqueProviders.add( TorqueProviderEnum.MEMCACHED.getIndex(), @@ -70,17 +87,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/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 diff --git a/simulator/src/main/java/msp/simulator/satellite/sensors/Gyrometer.java b/simulator/src/main/java/msp/simulator/satellite/ADACS/sensors/Gyrometer.java similarity index 88% rename from simulator/src/main/java/msp/simulator/satellite/sensors/Gyrometer.java rename to simulator/src/main/java/msp/simulator/satellite/ADACS/sensors/Gyrometer.java index ef1131fd..6e3b25e3 100644 --- a/simulator/src/main/java/msp/simulator/satellite/sensors/Gyrometer.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADACS/sensors/Gyrometer.java @@ -11,7 +11,7 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -package msp.simulator.satellite.sensors; +package msp.simulator.satellite.ADACS.sensors; import org.hipparchus.geometry.euclidean.threed.Vector3D; import org.hipparchus.util.FastMath; @@ -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 diff --git a/simulator/src/main/java/msp/simulator/satellite/sensors/InfraredSensor.java b/simulator/src/main/java/msp/simulator/satellite/ADACS/sensors/InfraredSensor.java similarity index 98% rename from simulator/src/main/java/msp/simulator/satellite/sensors/InfraredSensor.java rename to simulator/src/main/java/msp/simulator/satellite/ADACS/sensors/InfraredSensor.java index 7d90dcb3..f1b2b0a2 100644 --- a/simulator/src/main/java/msp/simulator/satellite/sensors/InfraredSensor.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADACS/sensors/InfraredSensor.java @@ -1,4 +1,4 @@ -package msp.simulator.satellite.sensors; +package msp.simulator.satellite.ADACS.sensors; import org.hipparchus.geometry.euclidean.threed.*; import org.hipparchus.util.FastMath; diff --git a/simulator/src/main/java/msp/simulator/satellite/sensors/Magnetometer.java b/simulator/src/main/java/msp/simulator/satellite/ADACS/sensors/Magnetometer.java similarity index 83% rename from simulator/src/main/java/msp/simulator/satellite/sensors/Magnetometer.java rename to simulator/src/main/java/msp/simulator/satellite/ADACS/sensors/Magnetometer.java index 56eb08bc..ca0b1780 100644 --- a/simulator/src/main/java/msp/simulator/satellite/sensors/Magnetometer.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADACS/sensors/Magnetometer.java @@ -12,19 +12,17 @@ * limitations under the License. */ -package msp.simulator.satellite.sensors; +package msp.simulator.satellite.ADACS.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; +import org.hipparchus.random.RandomDataGenerator; import msp.simulator.environment.Environment; import msp.simulator.environment.geomagneticField.EarthMagneticField; @@ -40,13 +38,18 @@ */ public class Magnetometer { - /* ******* Public Static Attributes ******* */ + /* ******* 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 = 1e2 ; + public static double defaultMagnetoNoiseIntensity = 4.156*1000; /* **************************************** */ @@ -62,7 +65,7 @@ public class Magnetometer { /** Assembly of the satellite. */ private Assembly assembly; - + /** Private attribute for the noise intensity. */ private double noiseIntensity; @@ -88,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. */ @@ -103,10 +108,10 @@ public GeoMagneticElements retrieveNoisyField() { /* Creating the noisy measure. */ GeoMagneticElements noisyMeasure = new GeoMagneticElements(noisyFieldVector); - + logger.debug("Noisy Geo" + noisyMeasure.toString()); - - + + return noisyMeasure; } @@ -149,35 +154,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 +186,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; } diff --git a/simulator/src/main/java/msp/simulator/satellite/sensors/Sensors.java b/simulator/src/main/java/msp/simulator/satellite/ADACS/sensors/Sensors.java similarity index 98% rename from simulator/src/main/java/msp/simulator/satellite/sensors/Sensors.java rename to simulator/src/main/java/msp/simulator/satellite/ADACS/sensors/Sensors.java index c1c4b83b..68f16408 100644 --- a/simulator/src/main/java/msp/simulator/satellite/sensors/Sensors.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADACS/sensors/Sensors.java @@ -12,7 +12,7 @@ * limitations under the License. */ -package msp.simulator.satellite.sensors; +package msp.simulator.satellite.ADACS.sensors; import org.hipparchus.geometry.euclidean.threed.Vector3D; import org.slf4j.Logger; diff --git a/simulator/src/main/java/msp/simulator/satellite/sensors/package-info.java b/simulator/src/main/java/msp/simulator/satellite/ADACS/sensors/package-info.java similarity index 94% rename from simulator/src/main/java/msp/simulator/satellite/sensors/package-info.java rename to simulator/src/main/java/msp/simulator/satellite/ADACS/sensors/package-info.java index 7ba0f065..25d4eb29 100644 --- a/simulator/src/main/java/msp/simulator/satellite/sensors/package-info.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADACS/sensors/package-info.java @@ -19,4 +19,4 @@ * @author Florian CHAUBEYRE * @author Braeden BORG */ -package msp.simulator.satellite.sensors; +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..0bc778b3 --- /dev/null +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/ADCS.java @@ -0,0 +1,63 @@ +/* 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.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.ADCSPhysics.ADCSPhysics; +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; + 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.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(); + + 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 new file mode 100644 index 00000000..f872ce8c --- /dev/null +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/ADCSPhysics/ADCSPhysics.java @@ -0,0 +1,52 @@ +/* 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 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 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 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/actuators/Actuators.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/Actuators.java similarity index 76% rename from simulator/src/main/java/msp/simulator/satellite/actuators/Actuators.java rename to simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/Actuators.java index 0bc5ae3c..5a148b84 100644 --- a/simulator/src/main/java/msp/simulator/satellite/actuators/Actuators.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/Actuators.java @@ -12,7 +12,7 @@ * limitations under the License. */ -package msp.simulator.satellite.actuators; +package msp.simulator.satellite.ADCS.Actuators; import org.slf4j.Logger; import org.slf4j.LoggerFactory; @@ -27,13 +27,20 @@ public class Actuators { /** Logger of the class */ private static final Logger logger = LoggerFactory.getLogger(Actuators.class); - + private MagnetoTorquers magnetorquer; /** * */ public Actuators() { - logger.info(CustomLoggingTools.indentMsg(logger, + this.magnetorquer = new MagnetoTorquers(); + Actuators.logger.info(CustomLoggingTools.indentMsg(Actuators.logger, "Building the Actuators...")); } + /** + * @return + */ + public MagnetoTorquers getMagnetorquers() { + 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 new file mode 100644 index 00000000..2302cfd4 --- /dev/null +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/MagnetoTorquers.java @@ -0,0 +1,74 @@ +/* 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.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() { + 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) { + 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; + 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; + } + 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; + } + result = new Vector3D(x,y,z); + return result; + } + +} diff --git a/simulator/src/main/java/msp/simulator/satellite/actuators/package-info.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/package-info.java similarity index 93% rename from simulator/src/main/java/msp/simulator/satellite/actuators/package-info.java rename to simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/package-info.java index 9303c151..0ae2ff98 100644 --- a/simulator/src/main/java/msp/simulator/satellite/actuators/package-info.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/package-info.java @@ -17,4 +17,4 @@ * * @author Florian CHAUBEYRE */ -package msp.simulator.satellite.actuators; \ No newline at end of file +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..9337b7cc --- /dev/null +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/B_Dot.java @@ -0,0 +1,43 @@ +/* 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.Actuators.Actuators; +import msp.simulator.satellite.ADCS.Estimators.BdotEstimator.BdotEstimator; + +/** + * + * @author Jack McRobbie> + */ +public class B_Dot { + private static final Vector3D bdotGains = new Vector3D(-100000,-100000,-100000); + private Actuators actuators; + private BdotEstimator est; + + public B_Dot(Satellite sat) { + this.est = new BdotEstimator(sat); + this.actuators = new Actuators(); + } + public Vector3D computeDipole() { + 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/Controller/Controller.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/Controller.java new file mode 100644 index 00000000..694dd7dc --- /dev/null +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/Controller.java @@ -0,0 +1,48 @@ +/* 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 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); + Controller.logger.info(CustomLoggingTools.indentMsg(this.logger, + " -> Building the ADCS Controller: Success.")); + + } + public Vector3D getDipole() { + 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 new file mode 100644 index 00000000..e7ec5215 --- /dev/null +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/BdotEstimator.java @@ -0,0 +1,65 @@ +/* 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 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 + */ +public class BdotEstimator { + private LowPassFilter lowpassfilter; + private Magnetometer mag; + 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); + lowpassfilter = new LowPassFilter(1.0,0.1,initalState); + 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); + return bdot; + } + private Vector3D getFirstOrderDiff() { + this.mag = this.satellite.getADCS().getSensors().getMagnetometer(); + Vector3D magreading = mag.retrieveNoisyField().getFieldVector().scalarMultiply(0.000000001); + if(this.lastMagFieldReading == Vector3D.NEGATIVE_INFINITY) { + this.lastMagFieldReading = magreading; + return Vector3D.ZERO; + } + 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,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 new file mode 100644 index 00000000..baed4393 --- /dev/null +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/LowPassFilter.java @@ -0,0 +1,66 @@ +/* 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 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; + } + + public double getTimeConstant() { + return this.timeConstant; + } +} diff --git a/simulator/src/main/java/msp/simulator/satellite/actuators/MagnetoTorquers.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/Estimators.java similarity index 56% rename from simulator/src/main/java/msp/simulator/satellite/actuators/MagnetoTorquers.java rename to simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/Estimators.java index ea6a3e8a..671a382c 100644 --- a/simulator/src/main/java/msp/simulator/satellite/actuators/MagnetoTorquers.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/Estimators.java @@ -11,30 +11,21 @@ * See the License for the specific language governing permissions and * limitations under the License. */ +package msp.simulator.satellite.ADCS.Estimators; -package msp.simulator.satellite.actuators; - -import org.slf4j.Logger; -import org.slf4j.LoggerFactory; - -import msp.simulator.utils.logs.CustomLoggingTools; +import msp.simulator.satellite.Satellite; /** * - * @author Florian CHAUBEYRE + * @author Jack McRobbie */ -public class MagnetoTorquers { - - - /** Logger of the class */ - private static final Logger logger = LoggerFactory.getLogger(MagnetoTorquers.class); +public class Estimators { /** - * + * @param sat */ - public MagnetoTorquers() { - logger.info(CustomLoggingTools.indentMsg(logger, - "Building the MagnetoTorquers...")); + public Estimators(Satellite sat) { + // TODO Auto-generated constructor stub } } diff --git a/simulator/src/main/java/msp/simulator/satellite/Satellite.java b/simulator/src/main/java/msp/simulator/satellite/Satellite.java index 0c81e918..532bcb37 100644 --- a/simulator/src/main/java/msp/simulator/satellite/Satellite.java +++ b/simulator/src/main/java/msp/simulator/satellite/Satellite.java @@ -15,15 +15,16 @@ package msp.simulator.satellite; import org.hipparchus.geometry.euclidean.threed.Vector3D; +import org.orekit.time.AbsoluteDate; import org.slf4j.Logger; 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; /** @@ -38,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 */ @@ -56,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(); @@ -73,9 +72,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()) { @@ -83,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 @@ -104,9 +101,9 @@ public void executeStepMission() { "Simulation_Magnetometer_Z", 0, rawMag_z ); - + /* Gyrometer Sensor Measurement */ - Vector3D gyroMeasure = this.getSensors().getGyrometer().getData_rotAcc(); + 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()); @@ -123,13 +120,13 @@ 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() + double posXIR = this.adcsModule.getSensors().getPosXIRSensor() .calculateInfraredReading(nadir_body); byte[] rawIR_xPos = MemcachedRawTranscoder.toRawByteArray(posXIR); this.io.getMemcached().set( @@ -137,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( @@ -145,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( @@ -153,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( @@ -161,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( @@ -169,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( @@ -201,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. @@ -217,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/Assembly.java b/simulator/src/main/java/msp/simulator/satellite/assembly/Assembly.java index d1d58428..be0e1667 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 */ @@ -34,6 +38,9 @@ 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; @@ -52,6 +59,7 @@ public Assembly(Environment environment) { this.satelliteBody = new SatelliteBody(environment); this.satelliteStates = new SatelliteStates(environment, satelliteBody); + this.earth = environment.getSolarSystem().getEarth(); } /** @@ -93,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]); @@ -109,5 +117,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.getSatelliteFrame(), + date + ); + } catch (OrekitException e) { + e.printStackTrace(); + } + + return itrf2body; + } + -} +} \ No newline at end of file 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..2818fb72 100644 --- a/simulator/src/main/java/msp/simulator/satellite/assembly/SatelliteBody.java +++ b/simulator/src/main/java/msp/simulator/satellite/assembly/SatelliteBody.java @@ -40,12 +40,11 @@ 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 */ { - {1191.648 * 1.3e-6, 0 , 0 }, - { 0 , 1169.506 * 1.3e-6, 0 }, - { 0 , 0 , 1203.969 * 1.3e-6 }, + {0.001, 0 , 0 }, + { 0 , 0.001, 0 }, + { 0 , 0 , 0.001 }, }; /** Simple balance inertia matrix (Unit matrix). */ diff --git a/simulator/src/main/java/msp/simulator/user/Dashboard.java b/simulator/src/main/java/msp/simulator/user/Dashboard.java index e6f5db6e..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,13 +114,14 @@ 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); /* **** 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); @@ -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/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java b/simulator/src/main/java/msp/simulator/utils/logs/ephemeris/EphemerisGenerator.java index dd440d68..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 @@ -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; @@ -94,9 +95,36 @@ public class EphemerisGenerator { /** Attitude AEM ephemeris. */ private File fileAEM; - + + /** 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; + + /** vector for the change in the magnetic field vector */ + + private File fileAEM_bDot; + /** Attitude AEM File Writer. */ + private FileWriter writerAEM_mag; + private FileWriter writerAEM; + + private FileWriter writerAEM_angMomentum; + private FileWriter writerAEM_angVel; + private FileWriter writerAEM_torque; + private FileWriter writerAEM_bDot; /** OrbitWrapper OEM File Writer */ private FileWriter writerOEM; @@ -143,6 +171,11 @@ 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_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(); @@ -150,20 +183,57 @@ public void start() { if (!fileAEM.getParentFile().exists()) { fileAEM.getParentFile().mkdir(); } + 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(); + } + if (!fileAEM_bDot.getParentFile().exists()) { + fileAEM_bDot.getParentFile().mkdir(); + } + try { + /* Creating the files. */ fileOEM.createNewFile(); fileAEM.createNewFile(); - + fileAEM_mag.createNewFile(); + fileAEM_angMomentum.createNewFile(); + fileAEM_angVelocity.createNewFile(); + fileAEM_torque.createNewFile(); + fileAEM_bDot.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); + 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)); - + 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.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(); } @@ -176,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(); } @@ -189,7 +264,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(); @@ -201,7 +276,138 @@ 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 */ + + /** 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]; + Vector3D torque = new Vector3D(torqueX,torqueY,torqueZ); + if (torque.getNorm() != 0) { + torque = torque.normalize(); + } + buff + .append(days) + .append(" ") /* Column Separator */ + .append(seconds) + .append(" ") + .append(torque.getX()) + .append(" ") + .append(torque.getY()) + .append(" ") + .append(torque.getZ()) + .append(" ") + .append(0) + ; + this.writerAEM_torque.append(buff+LS); + this.writerAEM_torque.flush(); + + buff = new StringBuffer(); + + /** Writing to the angular velocity file */ + Vector3D vel = satellite.getStates().getCurrentState().getAttitude().getSpin(); + if (vel.getNorm() != 0) { + vel = vel.normalize(); + } + buff + .append(days) + .append(" ") /* Column Separator */ + .append(seconds) + .append(" ") + .append(vel.getX()) + .append(" ") + .append(vel.getY()) + .append(" ") + .append(vel.getZ()) + .append(" ") + .append(0) + ; + this.writerAEM_angVel.append(buff+LS); + this.writerAEM_angVel.flush(); + + buff = new StringBuffer(); + + /** Writing to the angular momentum file */ + Vector3D mom = satellite.getAssembly().getAngularMomentum(); + if(mom.getNorm()!= 0) { + mom = mom.normalize(); + } + buff + .append(days) + .append(" ") /* Column Separator */ + .append(seconds) + .append(" ") + .append(mom.getX()) + .append(" ") + .append(mom.getY()) + .append(" ") + .append(mom.getZ()) + .append(" ") + .append(0) + ; + this.writerAEM_angMomentum.append(buff+LS); + this.writerAEM_angMomentum.flush(); + + buff = new StringBuffer(); + + /* Writing the current mag field to the log file. */ + Vector3D mag_field = satellite.getADCS().getSensors().getMagnetometer().retrievePerfectField().getFieldVector(); + Vector3D mag_unit; + mag_unit = mag_field.normalize(); + buff + .append(days) + .append(" ") /* Column Separator */ + .append(seconds) + .append(" ") + .append(mag_unit.getX()) + .append(" ") + .append(mag_unit.getY()) + .append(" ") + .append(mag_unit.getZ()) + .append(" ") + .append(0) + ; + 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 .getPVCoordinates(FramesFactory.getEME2000()) @@ -219,6 +425,7 @@ public void writeStep(Satellite satellite) { .append(position.getZ() * 1e-3) ; + this.writerOEM.append(buff.toString() + LS); this.writerOEM.flush(); @@ -254,22 +461,65 @@ 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) { 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 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(