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/Main.java b/simulator/src/main/java/msp/simulator/Main.java index 08391e54..c471986c 100644 --- a/simulator/src/main/java/msp/simulator/Main.java +++ b/simulator/src/main/java/msp/simulator/Main.java @@ -47,8 +47,7 @@ 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.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 new file mode 100644 index 00000000..35dc2c83 --- /dev/null +++ b/simulator/src/main/java/msp/simulator/dynamic/torques/ControllerTorqueProvider.java @@ -0,0 +1,42 @@ +/* Copyright 20017-2018 Melbourne Space Program + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package msp.simulator.dynamic.torques; +import org.hipparchus.geometry.euclidean.threed.Vector3D; +import org.orekit.time.AbsoluteDate; + +import msp.simulator.NumericalSimulator; +import msp.simulator.dynamic.torques.TorqueOverTimeScenarioProvider.Step; +import msp.simulator.environment.Environment; +import msp.simulator.satellite.Satellite; +/** + * + * @author Jack McRobbie + * This class represents the satellites own method + * for providing stabilization and control + */ +public class ControllerTorqueProvider implements TorqueProvider{ + private Satellite sat; + private Vector3D steptorque; + public ControllerTorqueProvider(Satellite satellite, AbsoluteDate date, Environment environment) { + this.sat = satellite; + this.steptorque = Vector3D.ZERO; + } + /** {@inheritDoc} */ + @Override + public Vector3D getTorque(AbsoluteDate date) { + this.steptorque = sat.getADCS().ComputeTorque(); + /* Finally returns the torque of the step (updated if needed). */ + return this.steptorque; + } +} diff --git a/simulator/src/main/java/msp/simulator/dynamic/torques/MemCachedTorqueProvider.java b/simulator/src/main/java/msp/simulator/dynamic/torques/MemCachedTorqueProvider.java index 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/TorqueProviderEnum.java b/simulator/src/main/java/msp/simulator/dynamic/torques/TorqueProviderEnum.java index 9b860d12..3068667f 100644 --- a/simulator/src/main/java/msp/simulator/dynamic/torques/TorqueProviderEnum.java +++ b/simulator/src/main/java/msp/simulator/dynamic/torques/TorqueProviderEnum.java @@ -23,7 +23,7 @@ public enum TorqueProviderEnum { /* Command torque provider. */ MEMCACHED(0), SCENARIO(0), - + CONTROLLER(0), /* Disturbances. */ GRAVITY(1), ATMOSPHERIC(2), diff --git a/simulator/src/main/java/msp/simulator/dynamic/torques/Torques.java b/simulator/src/main/java/msp/simulator/dynamic/torques/Torques.java index acad2188..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; @@ -69,7 +69,15 @@ 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 + )); + break; case MEMCACHED: this.torqueProviders.add( TorqueProviderEnum.MEMCACHED.getIndex(), diff --git a/simulator/src/main/java/msp/simulator/satellite/sensors/Gyrometer.java b/simulator/src/main/java/msp/simulator/satellite/ADACS/sensors/Gyrometer.java similarity index 98% rename from simulator/src/main/java/msp/simulator/satellite/sensors/Gyrometer.java rename to simulator/src/main/java/msp/simulator/satellite/ADACS/sensors/Gyrometer.java index 31bee55f..6e3b25e3 100644 --- a/simulator/src/main/java/msp/simulator/satellite/sensors/Gyrometer.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADACS/sensors/Gyrometer.java @@ -11,7 +11,7 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -package msp.simulator.satellite.sensors; +package msp.simulator.satellite.ADACS.sensors; import org.hipparchus.geometry.euclidean.threed.Vector3D; import org.hipparchus.util.FastMath; diff --git a/simulator/src/main/java/msp/simulator/satellite/sensors/InfraredSensor.java b/simulator/src/main/java/msp/simulator/satellite/ADACS/sensors/InfraredSensor.java similarity index 98% rename from simulator/src/main/java/msp/simulator/satellite/sensors/InfraredSensor.java rename to simulator/src/main/java/msp/simulator/satellite/ADACS/sensors/InfraredSensor.java index 7d90dcb3..f1b2b0a2 100644 --- a/simulator/src/main/java/msp/simulator/satellite/sensors/InfraredSensor.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADACS/sensors/InfraredSensor.java @@ -1,4 +1,4 @@ -package msp.simulator.satellite.sensors; +package msp.simulator.satellite.ADACS.sensors; import org.hipparchus.geometry.euclidean.threed.*; import org.hipparchus.util.FastMath; diff --git a/simulator/src/main/java/msp/simulator/satellite/sensors/Magnetometer.java b/simulator/src/main/java/msp/simulator/satellite/ADACS/sensors/Magnetometer.java similarity index 99% rename from simulator/src/main/java/msp/simulator/satellite/sensors/Magnetometer.java rename to simulator/src/main/java/msp/simulator/satellite/ADACS/sensors/Magnetometer.java index b8c36878..ca0b1780 100644 --- a/simulator/src/main/java/msp/simulator/satellite/sensors/Magnetometer.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADACS/sensors/Magnetometer.java @@ -12,7 +12,7 @@ * limitations under the License. */ -package msp.simulator.satellite.sensors; +package msp.simulator.satellite.ADACS.sensors; import org.hipparchus.geometry.euclidean.threed.Vector3D; import org.hipparchus.util.FastMath; diff --git a/simulator/src/main/java/msp/simulator/satellite/sensors/Sensors.java b/simulator/src/main/java/msp/simulator/satellite/ADACS/sensors/Sensors.java similarity index 98% rename from simulator/src/main/java/msp/simulator/satellite/sensors/Sensors.java rename to simulator/src/main/java/msp/simulator/satellite/ADACS/sensors/Sensors.java index c1c4b83b..68f16408 100644 --- a/simulator/src/main/java/msp/simulator/satellite/sensors/Sensors.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADACS/sensors/Sensors.java @@ -12,7 +12,7 @@ * limitations under the License. */ -package msp.simulator.satellite.sensors; +package msp.simulator.satellite.ADACS.sensors; import org.hipparchus.geometry.euclidean.threed.Vector3D; import org.slf4j.Logger; diff --git a/simulator/src/main/java/msp/simulator/satellite/sensors/package-info.java b/simulator/src/main/java/msp/simulator/satellite/ADACS/sensors/package-info.java similarity index 94% rename from simulator/src/main/java/msp/simulator/satellite/sensors/package-info.java rename to simulator/src/main/java/msp/simulator/satellite/ADACS/sensors/package-info.java index 7ba0f065..25d4eb29 100644 --- a/simulator/src/main/java/msp/simulator/satellite/sensors/package-info.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADACS/sensors/package-info.java @@ -19,4 +19,4 @@ * @author Florian CHAUBEYRE * @author Braeden BORG */ -package msp.simulator.satellite.sensors; +package msp.simulator.satellite.ADACS.sensors; diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/ADCS.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/ADCS.java new file mode 100644 index 00000000..0bc778b3 --- /dev/null +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/ADCS.java @@ -0,0 +1,63 @@ +/* Copyright 20017-2018 Melbourne Space Program + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package msp.simulator.satellite.ADCS; + +import java.util.function.Consumer; + +import org.hipparchus.geometry.euclidean.threed.Vector3D; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import msp.simulator.environment.Environment; +import msp.simulator.satellite.Satellite; +import msp.simulator.satellite.ADACS.sensors.Sensors; +import msp.simulator.satellite.ADCS.ADCSPhysics.ADCSPhysics; +import msp.simulator.satellite.ADCS.Actuators.Actuators; +import msp.simulator.satellite.ADCS.Controller.Controller; +import msp.simulator.satellite.ADCS.Estimators.Estimators; +import msp.simulator.utils.logs.CustomLoggingTools; +/** + * + * @author Jack McRobbie + */ +public class ADCS { + private static final Logger logger = LoggerFactory.getLogger(ADCS.class); + private Sensors sensors; + private Estimators estimators; + private Controller controllers; + private Actuators actuators; + private ADCSPhysics physics; + + + public ADCS(Satellite sat,Environment environment) { + this.sensors = new Sensors(environment, sat.getAssembly()); + this.estimators = new Estimators(sat); + this.controllers = new Controller(sat); + this.physics = new ADCSPhysics(sat, environment); + ADCS.logger.info(CustomLoggingTools.indentMsg(ADCS.logger, + "Building the ADCS Module: Success...")); + } + public Vector3D ComputeTorque() { + Vector3D magneticDipole = this.controllers.getDipole(); + + return this.physics.ComputeMagnetorquerTorque(magneticDipole); +// return new Vector3D(0.01,0.01,0.01); + } + /** + * @return + */ + public Sensors getSensors() { + return sensors; + } +} diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/ADCSPhysics/ADCSPhysics.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/ADCSPhysics/ADCSPhysics.java new file mode 100644 index 00000000..f872ce8c --- /dev/null +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/ADCSPhysics/ADCSPhysics.java @@ -0,0 +1,52 @@ +/* Copyright 20017-2018 Melbourne Space Program + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package msp.simulator.satellite.ADCS.ADCSPhysics; + +import org.hipparchus.geometry.euclidean.threed.Vector3D; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import msp.simulator.environment.Environment; +import msp.simulator.satellite.Satellite; +import msp.simulator.satellite.ADACS.sensors.Sensors; +import msp.simulator.satellite.ADCS.Actuators.MagnetoTorquers; +import msp.simulator.satellite.assembly.SatelliteBody; +import msp.simulator.utils.logs.CustomLoggingTools; + +/** + * + * @author Jack McRobbie + */ +public class ADCSPhysics { + + /** Logger of the class. */ + private static final Logger logger = + LoggerFactory.getLogger(ADCSPhysics.class); + + private Satellite satellite; + private Environment environment; + private Sensors sensor; + public ADCSPhysics(Satellite satellite, Environment environemt) { + this.satellite = satellite; + this.environment = environment; + ADCSPhysics.logger.info(CustomLoggingTools.indentMsg(ADCSPhysics.logger, + " -> Building the ADCS Physics engine: Success.")); + } + public Vector3D ComputeMagnetorquerTorque(Vector3D magneticDipole) { + Vector3D magfield = this.satellite.getADCS().getSensors().getMagnetometer() + .retrievePerfectField().getFieldVector().scalarMultiply(0.000000001); + Vector3D result = Vector3D.crossProduct(magneticDipole,magfield ); + return result; + } +} diff --git a/simulator/src/main/java/msp/simulator/satellite/actuators/Actuators.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/Actuators.java similarity index 76% rename from simulator/src/main/java/msp/simulator/satellite/actuators/Actuators.java rename to simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/Actuators.java index 0bc5ae3c..5a148b84 100644 --- a/simulator/src/main/java/msp/simulator/satellite/actuators/Actuators.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/Actuators.java @@ -12,7 +12,7 @@ * limitations under the License. */ -package msp.simulator.satellite.actuators; +package msp.simulator.satellite.ADCS.Actuators; import org.slf4j.Logger; import org.slf4j.LoggerFactory; @@ -27,13 +27,20 @@ public class Actuators { /** Logger of the class */ private static final Logger logger = LoggerFactory.getLogger(Actuators.class); - + private MagnetoTorquers magnetorquer; /** * */ public Actuators() { - logger.info(CustomLoggingTools.indentMsg(logger, + this.magnetorquer = new MagnetoTorquers(); + Actuators.logger.info(CustomLoggingTools.indentMsg(Actuators.logger, "Building the Actuators...")); } + /** + * @return + */ + public MagnetoTorquers getMagnetorquers() { + return this.magnetorquer; + } } diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/MagnetoTorquers.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/MagnetoTorquers.java new file mode 100644 index 00000000..2302cfd4 --- /dev/null +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/MagnetoTorquers.java @@ -0,0 +1,74 @@ +/* Copyright 20017-2018 Melbourne Space Program + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +package msp.simulator.satellite.ADCS.Actuators; + +import org.hipparchus.geometry.euclidean.threed.Vector3D; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import msp.simulator.utils.logs.CustomLoggingTools; + +/** + * + * @author Florian CHAUBEYRE + * @author Jack McRobbie + */ +public class MagnetoTorquers { + private Vector3D orientation; + private Vector3D MaxDipole; + + + /** Logger of the class */ + private static final Logger logger = LoggerFactory.getLogger(MagnetoTorquers.class); + + /** + * + */ + public MagnetoTorquers() { + MagnetoTorquers.logger.info(CustomLoggingTools.indentMsg(logger, + "Building the MagnetoTorquers...")); + this.orientation = new Vector3D(1,1,1); + this.MaxDipole = new Vector3D(0.1,0.1,0.1); //TODO make configurable from simulation initialization + + } + public Vector3D computeDipole(Vector3D dutyCycle) { + return this.constrainDipole(dutyCycle); + } + /** + * @param dutyCycle requested duty cycle for the magnetorquers + */ + private Vector3D constrainDipole(Vector3D dutyCycle) { + double x = dutyCycle.getX(); + double y = dutyCycle.getY(); + double z = dutyCycle.getZ(); + int sign; + Vector3D result = new Vector3D(x,y,z); + if(Math.abs(x)>this.MaxDipole.getX()) { + sign = (0 > dutyCycle.getX())?-1:1; + x = this.MaxDipole.getX() * sign; + } + if(Math.abs(y)>this.MaxDipole.getY()) { + sign = (0 > dutyCycle.getY())?-1:1; + y = this.MaxDipole.getY() * sign; + } + if(Math.abs(z)>this.MaxDipole.getZ()) { + sign = (0 > dutyCycle.getZ())?-1:1; + z = this.MaxDipole.getZ() * sign; + } + result = new Vector3D(x,y,z); + return result; + } + +} diff --git a/simulator/src/main/java/msp/simulator/satellite/actuators/package-info.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/package-info.java similarity index 93% rename from simulator/src/main/java/msp/simulator/satellite/actuators/package-info.java rename to simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/package-info.java index 9303c151..0ae2ff98 100644 --- a/simulator/src/main/java/msp/simulator/satellite/actuators/package-info.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Actuators/package-info.java @@ -17,4 +17,4 @@ * * @author Florian CHAUBEYRE */ -package msp.simulator.satellite.actuators; \ No newline at end of file +package msp.simulator.satellite.ADCS.Actuators; \ No newline at end of file diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/B_Dot.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/B_Dot.java new file mode 100644 index 00000000..9337b7cc --- /dev/null +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/B_Dot.java @@ -0,0 +1,43 @@ +/* Copyright 20017-2018 Melbourne Space Program + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package msp.simulator.satellite.ADCS.Controller; + +import org.hipparchus.geometry.euclidean.threed.Vector3D; + +import msp.simulator.satellite.Satellite; +import msp.simulator.satellite.ADCS.Actuators.Actuators; +import msp.simulator.satellite.ADCS.Estimators.BdotEstimator.BdotEstimator; + +/** + * + * @author Jack McRobbie> + */ +public class B_Dot { + private static final Vector3D bdotGains = new Vector3D(-100000,-100000,-100000); + private Actuators actuators; + private BdotEstimator est; + + public B_Dot(Satellite sat) { + this.est = new BdotEstimator(sat); + this.actuators = new Actuators(); + } + public Vector3D computeDipole() { + Vector3D dutyCycle = new Vector3D(this.bdotGains.getX()*this.est.computeBdot().getX(), + this.bdotGains.getY()*this.est.computeBdot().getY(), + this.bdotGains.getZ()*this.est.computeBdot().getZ() + ); + Vector3D result = this.actuators.getMagnetorquers().computeDipole(dutyCycle); + return result; + } +} diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/Controller.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/Controller.java new file mode 100644 index 00000000..694dd7dc --- /dev/null +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Controller/Controller.java @@ -0,0 +1,48 @@ +/* Copyright 20017-2018 Melbourne Space Program + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package msp.simulator.satellite.ADCS.Controller; + +import org.hipparchus.geometry.euclidean.threed.Vector3D; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import msp.simulator.satellite.Satellite; +import msp.simulator.satellite.ADCS.ADCSPhysics.ADCSPhysics; +import msp.simulator.utils.logs.CustomLoggingTools; + +/** + * + * @author Jack McRobbie + */ +public class Controller { + + private static final Logger logger = + LoggerFactory.getLogger(Controller.class); + + B_Dot bdot; + Satellite sat; + public Controller(Satellite satellite) { + this.sat = satellite; + bdot = new B_Dot(this.sat); + Controller.logger.info(CustomLoggingTools.indentMsg(this.logger, + " -> Building the ADCS Controller: Success.")); + + } + public Vector3D getDipole() { + Vector3D result = this.bdot.computeDipole(); + Controller.logger.info(result.toString()); + return result; + + } +} diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/BdotEstimator.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/BdotEstimator.java new file mode 100644 index 00000000..e7ec5215 --- /dev/null +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/BdotEstimator.java @@ -0,0 +1,65 @@ +/* Copyright 20017-2018 Melbourne Space Program + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package msp.simulator.satellite.ADCS.Estimators.BdotEstimator; + +import org.hipparchus.geometry.euclidean.threed.Vector3D; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import msp.simulator.satellite.Satellite; +import msp.simulator.satellite.ADACS.sensors.Magnetometer; +import msp.simulator.satellite.ADCS.Actuators.Actuators; +import msp.simulator.utils.logs.CustomLoggingTools; +/** + * + * @author Jack McRobbie + */ +public class BdotEstimator { + private LowPassFilter lowpassfilter; + private Magnetometer mag; + private Vector3D lastMagFieldReading; + private final double timestep; + private Satellite satellite; + private static final Logger logger = LoggerFactory.getLogger(BdotEstimator.class); + + public BdotEstimator(Satellite sat) { + Vector3D initalState = new Vector3D(0.0,0.0,0.0); + lowpassfilter = new LowPassFilter(1.0,0.1,initalState); + timestep = 0.1; // TODO make equal to Controller frequency! + satellite = sat; + this.lastMagFieldReading= Vector3D.NEGATIVE_INFINITY; + BdotEstimator.logger.info(CustomLoggingTools.indentMsg(BdotEstimator.logger, + "Building the Bdot Estimator...")); + } + public Vector3D computeBdot() { + Vector3D bDotUnfiltered = this.getFirstOrderDiff(); + Vector3D bdot = lowpassfilter.ProcessSample(bDotUnfiltered); + return bdot; + } + private Vector3D getFirstOrderDiff() { + this.mag = this.satellite.getADCS().getSensors().getMagnetometer(); + Vector3D magreading = mag.retrieveNoisyField().getFieldVector().scalarMultiply(0.000000001); + if(this.lastMagFieldReading == Vector3D.NEGATIVE_INFINITY) { + this.lastMagFieldReading = magreading; + return Vector3D.ZERO; + } + double x = (magreading.getX() - this.lastMagFieldReading.getX())/this.timestep; + double y = (magreading.getY() - this.lastMagFieldReading.getY())/this.timestep; + double z = (magreading.getZ() - this.lastMagFieldReading.getZ())/this.timestep; + this.lastMagFieldReading = magreading; + Vector3D result = new Vector3D(x,y,z); + return result; + } + +} diff --git a/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/LowPassFilter.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/LowPassFilter.java new file mode 100644 index 00000000..baed4393 --- /dev/null +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/BdotEstimator/LowPassFilter.java @@ -0,0 +1,66 @@ +/* Copyright 20017-2018 Melbourne Space Program + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package msp.simulator.satellite.ADCS.Estimators.BdotEstimator; + +import org.hipparchus.geometry.euclidean.threed.Vector3D; + +/** + * + * @author Jack McRobbie + */ +public class LowPassFilter { + private final String Constructor3d = "Vector3D"; + private final String ConstructorDouble = "double"; + + private double timeConstant; + private double samplePeriodMili; + private double state; + private Vector3D state3d; + private double alpha; + private String flag; + public LowPassFilter(double tc, double spms,double initialState) { + this.timeConstant = tc; + this.samplePeriodMili = spms; + this.alpha = Math.exp(-spms/tc); + state = initialState; + this.flag = this.ConstructorDouble; + } + public LowPassFilter(double tc, double spms, Vector3D initState) { + this.timeConstant = tc; + this.samplePeriodMili = spms; + this.alpha = Math.exp(-spms/tc); + + /* Utilize multiplicative class constructor returns same as initState */ + state3d = new Vector3D(1.0,initState); + this.flag = this.Constructor3d; + } + public double ProcessSample(double new_sample) { + this.state = this.alpha * this.state + (1 - this.alpha) * new_sample; + return this.state; + } + public Vector3D ProcessSample(Vector3D new_sample) { + double x = this.alpha * this.state3d.getX() + (1 - this.alpha) * new_sample.getX(); + double y = this.alpha * this.state3d.getY() + (1 - this.alpha) * new_sample.getY(); + double z = this.alpha * this.state3d.getZ() + (1 - this.alpha) * new_sample.getZ(); + this.state3d = new Vector3D(x,y,z); + return this.state3d; + } + public double getSamplePeriod() { + return this.samplePeriodMili/1000.0; + } + + public double getTimeConstant() { + return this.timeConstant; + } +} diff --git a/simulator/src/main/java/msp/simulator/satellite/actuators/MagnetoTorquers.java b/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/Estimators.java similarity index 56% rename from simulator/src/main/java/msp/simulator/satellite/actuators/MagnetoTorquers.java rename to simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/Estimators.java index ea6a3e8a..671a382c 100644 --- a/simulator/src/main/java/msp/simulator/satellite/actuators/MagnetoTorquers.java +++ b/simulator/src/main/java/msp/simulator/satellite/ADCS/Estimators/Estimators.java @@ -11,30 +11,21 @@ * See the License for the specific language governing permissions and * limitations under the License. */ +package msp.simulator.satellite.ADCS.Estimators; -package msp.simulator.satellite.actuators; - -import org.slf4j.Logger; -import org.slf4j.LoggerFactory; - -import msp.simulator.utils.logs.CustomLoggingTools; +import msp.simulator.satellite.Satellite; /** * - * @author Florian CHAUBEYRE + * @author Jack McRobbie */ -public class MagnetoTorquers { - - - /** Logger of the class */ - private static final Logger logger = LoggerFactory.getLogger(MagnetoTorquers.class); +public class Estimators { /** - * + * @param sat */ - public MagnetoTorquers() { - logger.info(CustomLoggingTools.indentMsg(logger, - "Building the MagnetoTorquers...")); + public Estimators(Satellite sat) { + // TODO Auto-generated constructor stub } } diff --git a/simulator/src/main/java/msp/simulator/satellite/Satellite.java b/simulator/src/main/java/msp/simulator/satellite/Satellite.java index 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..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.9002 * 1e-3, 0 , 0 }, - { 0 , 1.9156 * 1e-3, 0 }, - { 0 , 0 , 1.9496 * 1e-3 }, + {0.001, 0 , 0 }, + { 0 , 0.001, 0 }, + { 0 , 0 , 0.001 }, }; /** Simple balance inertia matrix (Unit matrix). */ diff --git a/simulator/src/main/java/msp/simulator/user/Dashboard.java b/simulator/src/main/java/msp/simulator/user/Dashboard.java index 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