From 68383b5a27e724abda7ed9156299c3576f47c394 Mon Sep 17 00:00:00 2001 From: jmcrobbie Date: Tue, 5 Feb 2019 17:31:00 +1030 Subject: [PATCH 01/14] 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 02/14] 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 03/14] 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 04/14] 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 05/14] 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 06/14] 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 07/14] 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 08/14] 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 8661fc297872ff6d9907b67af4f8034b88dbb8f0 Mon Sep 17 00:00:00 2001 From: jmcrobbie Date: Thu, 14 Feb 2019 20:07:31 +1030 Subject: [PATCH 09/14] 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 10/14] 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 11/14] 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 12/14] 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 13/14] 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 14/14] 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