mirror of
https://git.intern.spaceteamaachen.de/ALPAKA/SPATZ.git
synced 2025-06-10 01:55:59 +00:00
Merge pull request 'feature/ambiance' (#6) from feature/ambiance into main
Reviewed-on: https://git.intern.spaceteamaachen.de/ALPAKA/SPATZ/pulls/6
This commit is contained in:
dario
commit
4f100f0bd4
3
.vscode/launch.json
vendored
3
.vscode/launch.json
vendored
@ -9,7 +9,8 @@
|
||||
"type": "debugpy",
|
||||
"request": "launch",
|
||||
"program": "testing.py",
|
||||
"console": "integratedTerminal"
|
||||
"console": "integratedTerminal",
|
||||
"justMyCode": false
|
||||
}
|
||||
]
|
||||
}
|
||||
183
balloon.ipynb
Normal file
183
balloon.ipynb
Normal file
File diff suppressed because one or more lines are too long
290
conversion.ipynb
290
conversion.ipynb
File diff suppressed because one or more lines are too long
1416
data/balloon_data.csv
Normal file
1416
data/balloon_data.csv
Normal file
File diff suppressed because it is too large
Load Diff
File diff suppressed because one or more lines are too long
1728
data/simulations/actual_flight.txt
Normal file
1728
data/simulations/actual_flight.txt
Normal file
File diff suppressed because it is too large
Load Diff
1728
data/simulations/predicted_flight.txt
Normal file
1728
data/simulations/predicted_flight.txt
Normal file
File diff suppressed because it is too large
Load Diff
1724
data/simulations/raw/40km.txt
Normal file
1724
data/simulations/raw/40km.txt
Normal file
File diff suppressed because one or more lines are too long
1729
data/simulations/raw/bending_Sim.txt
Normal file
1729
data/simulations/raw/bending_Sim.txt
Normal file
File diff suppressed because one or more lines are too long
427
demo.ipynb
427
demo.ipynb
File diff suppressed because one or more lines are too long
56663
druckkammer.csv
Normal file
56663
druckkammer.csv
Normal file
File diff suppressed because it is too large
Load Diff
5001
dummy2.csv
Normal file
5001
dummy2.csv
Normal file
File diff suppressed because it is too large
Load Diff
2646
nominal_wind.csv
Normal file
2646
nominal_wind.csv
Normal file
File diff suppressed because it is too large
Load Diff
47
rework_test.py
Normal file
47
rework_test.py
Normal file
@ -0,0 +1,47 @@
|
||||
import cProfile
|
||||
import numpy as np
|
||||
import tqdm
|
||||
import math
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
from spatz.simulation import Simulation
|
||||
from spatz.simulations.rocketpy import RocketPyCSV
|
||||
from spatz.sensors.pressure.ms5611 import MS5611
|
||||
from spatz.sensors.imu.wsen_isds import WSEN_ISDS_ACC, WSEN_ISDS_GYRO
|
||||
from spatz.sensors.imu.h3lis100dl import H3LIS100DL
|
||||
from spatz.sensors.gps.erinome1 import Erinome_I
|
||||
from spatz.observer import Observer
|
||||
|
||||
|
||||
simulation = Simulation().load(RocketPyCSV('nominal_wind.csv'))
|
||||
|
||||
barometer = simulation.add_sensor(MS5611)
|
||||
|
||||
# Orientation matrix of the WSEN-ISDS IMU on the rocket.
|
||||
orientation = np.array([
|
||||
[ 0, 1, 0],
|
||||
[ 0, 0, 1],
|
||||
[-1, 0, 0]
|
||||
])
|
||||
accelerometer = simulation.add_sensor(WSEN_ISDS_ACC, orientation=orientation)
|
||||
gyroscope = simulation.add_sensor(WSEN_ISDS_GYRO)
|
||||
|
||||
orientation = np.array([
|
||||
[0, 1, 0],
|
||||
[0, 0, -1],
|
||||
[1, 0, 0]
|
||||
])
|
||||
high_g = simulation.add_sensor(H3LIS100DL)
|
||||
erinome = simulation.add_sensor(Erinome_I)
|
||||
|
||||
|
||||
for i in tqdm.tqdm(range(5000)):
|
||||
simulation.advance(0.1)
|
||||
pressure = barometer()
|
||||
acc = accelerometer()
|
||||
omegas = gyroscope()
|
||||
g = high_g()
|
||||
gps = erinome()
|
||||
|
||||
df = simulation.get_logger().get_dataframe()
|
||||
df.to_csv('dummy2.csv')
|
||||
91
siemens_parsed.csv
Normal file
91
siemens_parsed.csv
Normal file
@ -0,0 +1,91 @@
|
||||
time,DB5 Objekte_-ST3_Isttemperatur [°C],DB5 Objekte_-V2_Rückmeldung Arbeitsstellung für Trend,DB5 Objekte_-SV1_Istdruck Visu [mBar abs],DB5 Objekte_-SV1_Istdruck Visu Mantisse [mBar abs],DB5 Objekte_-SV1_Istdruck Visu Exponent,DB5 Objekte_-ST1_Isttemperatur [°C],DB5 Objekte_-ST2_Isttemperatur [°C],Pressure
|
||||
578.7037048339844,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
|
||||
1273.2175903320312,24.4,-20.0,902.03241,9.020324,2.0,24.4,25.0,902.0324
|
||||
1967.7777786254883,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
|
||||
2662.1527786254883,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
|
||||
3356.7013778686523,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
|
||||
4051.1342544555664,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
|
||||
4745.648147583008,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
|
||||
5440.127311706543,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
|
||||
6134.5601806640625,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
|
||||
6829.108787536621,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
|
||||
7523.611106872559,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
|
||||
8218.009262084961,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
|
||||
8912.488418579102,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
|
||||
9607.002311706543,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
|
||||
10301.493064880371,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
|
||||
10995.9375,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
|
||||
11690.46297454834,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
|
||||
12384.953704833984,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
|
||||
13079.456024169922,24.4,-20.0,902.03241,9.020324,2.0,24.4,25.0,902.0324
|
||||
13773.888885498047,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
|
||||
14468.298606872559,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
|
||||
15162.789352416992,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
|
||||
15857.291664123535,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
|
||||
16551.71295928955,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
|
||||
17246.30786895752,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
|
||||
17940.706024169922,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
|
||||
18635.150466918945,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
|
||||
19329.733795166016,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
|
||||
20024.201377868652,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
|
||||
20718.622688293457,24.299999,-20.0,923.56897,9.23569,2.0,24.200001,24.9,923.569
|
||||
21413.17130279541,24.299999,-20.0,233.712646,2.337126,2.0,23.6,24.200001,233.7126
|
||||
22107.673614501953,24.299999,-20.0,74.985168,7.498517,1.0,23.6,24.200001,74.98517
|
||||
22802.1759185791,24.299999,-20.0,29.057066,2.905707,1.0,23.700001,24.299999,29.05707
|
||||
23496.527770996094,24.299999,-20.0,12.757988,1.275799,1.0,23.799999,24.4,12.75799
|
||||
24190.94906616211,24.299999,-20.0,269.259064,2.692591,2.0,24.1,24.6,269.2591
|
||||
24885.48610687256,24.4,-20.0,923.56897,9.23569,2.0,24.299999,24.9,923.569
|
||||
25579.90739440918,24.4,-20.0,923.56897,9.23569,2.0,24.4,24.9,923.569
|
||||
26274.456024169922,24.4,-20.0,923.56897,9.23569,2.0,24.299999,24.9,923.569
|
||||
26968.90045928955,24.4,-20.0,923.56897,9.23569,2.0,24.299999,24.9,923.569
|
||||
27663.495376586914,24.4,-20.0,923.56897,9.23569,2.0,24.299999,24.9,923.569
|
||||
28357.98610687256,24.4,-20.0,923.56897,9.23569,2.0,24.299999,24.9,923.569
|
||||
29052.46527862549,24.4,-20.0,923.56897,9.23569,2.0,24.299999,24.9,923.569
|
||||
29746.94443511963,24.4,-20.0,923.56897,9.23569,2.0,24.299999,24.9,923.569
|
||||
30441.354164123535,24.4,-20.0,923.56897,9.23569,2.0,24.299999,24.9,923.569
|
||||
31135.76389312744,24.4,-20.0,923.56897,9.23569,2.0,24.299999,24.9,923.569
|
||||
31830.34722137451,24.4,-20.0,923.56897,9.23569,2.0,24.299999,24.9,923.569
|
||||
32524.872680664062,24.4,-20.0,923.56897,9.23569,2.0,24.299999,24.9,923.569
|
||||
33219.24768066406,24.4,-20.0,923.56897,9.23569,2.0,24.299999,24.9,923.569
|
||||
33913.784729003906,24.4,-20.0,923.56897,9.23569,2.0,24.299999,24.9,923.569
|
||||
34608.22917175293,24.4,-20.0,923.56897,9.23569,2.0,24.299999,24.9,923.569
|
||||
35302.800926208496,24.4,-20.0,923.56897,9.23569,2.0,24.299999,24.9,923.569
|
||||
35997.25694274902,24.299999,-20.0,548.038269,5.480383,2.0,23.799999,24.4,548.0382999999999
|
||||
36691.64351654053,24.299999,-20.0,145.58696,1.45587,2.0,23.5,24.1,145.587
|
||||
37386.18054962158,24.299999,-20.0,53.814953,5.381495,1.0,23.6,24.200001,53.81495
|
||||
38080.67130279541,24.299999,-20.0,21.891624,2.189162,1.0,23.700001,24.299999,21.89162
|
||||
38775.02314758301,24.299999,-20.0,9.773297,9.773297,0.0,23.799999,24.4,9.773297
|
||||
39469.60648345947,24.299999,-20.0,5.539739,5.539739,0.0,23.9,24.4,5.539739
|
||||
40164.10878753662,24.299999,-20.0,3.617636,3.617636,0.0,23.9,24.5,3.617636
|
||||
40858.541664123535,24.299999,-20.0,2.549855,2.549855,0.0,24.0,24.5,2.549855
|
||||
41553.02082824707,24.299999,-20.0,1.91839,1.91839,0.0,24.0,24.6,1.91839
|
||||
42247.534729003906,24.299999,-20.0,1.54061,1.54061,0.0,24.0,24.6,1.54061
|
||||
42942.00231170654,24.299999,-20.0,1.216785,1.216785,0.0,24.0,24.6,1.216785
|
||||
43636.48147583008,24.299999,-20.0,1.007466,1.007466,0.0,24.0,24.6,1.007466
|
||||
44330.92593383789,24.299999,-20.0,0.873253,8.732529,-1.0,24.0,24.6,0.8732529
|
||||
45025.42823791504,24.299999,-20.0,0.756922,7.569218,-1.0,24.0,24.6,0.7569218000000001
|
||||
45719.88426208496,24.299999,-20.0,0.656996,6.569963,-1.0,24.1,24.700001,0.6569963000000001
|
||||
46414.42130279541,24.299999,-20.0,0.597822,5.978216,-1.0,24.1,24.700001,0.5978216
|
||||
47108.93518066406,24.299999,-20.0,0.518181,5.181814,-1.0,24.1,24.700001,0.5181814
|
||||
47803.34490966797,24.299999,-20.0,0.47151,4.715096,-1.0,24.1,24.700001,0.47150960000000003
|
||||
48497.88194274902,24.299999,-20.0,0.429041,4.290406,-1.0,24.1,24.700001,0.4290406
|
||||
49192.36110687256,24.299999,-20.0,0.390398,3.903976,-1.0,24.1,24.700001,0.3903976
|
||||
49886.770835876465,24.299999,-20.0,0.354743,3.547427,-1.0,24.1,24.700001,0.3547427
|
||||
50581.25,24.299999,-20.0,0.322791,3.227909,-1.0,24.1,24.700001,0.3227909
|
||||
51275.76389312744,24.299999,-20.0,0.307912,3.079116,-1.0,24.1,24.700001,0.3079116
|
||||
51970.300926208496,24.299999,-20.0,0.280178,2.80178,-1.0,24.1,24.700001,0.280178
|
||||
52664.733795166016,24.299999,-20.0,0.254589,2.545894,-1.0,24.1,24.700001,0.2545894
|
||||
53359.20137786865,24.299999,-20.0,0.242853,2.428534,-1.0,24.1,24.700001,0.2428534
|
||||
54053.657402038574,24.299999,-20.0,0.22098,2.2098,-1.0,24.1,24.700001,0.22098
|
||||
54748.15971374512,24.299999,-20.0,0.210793,2.107933,-1.0,24.1,24.700001,0.21079330000000002
|
||||
55442.63888549805,24.299999,-20.0,0.193949,1.939492,-1.0,24.1,24.700001,0.19394920000000002
|
||||
56137.04860687256,24.299999,-20.0,0.182712,1.827117,-1.0,24.1,24.700001,0.18271170000000003
|
||||
56831.62036895752,24.299999,-20.0,0.174289,1.742891,-1.0,24.1,24.700001,0.1742891
|
||||
57526.08795928955,24.299999,-20.0,0.166255,1.662551,-1.0,24.1,24.700001,0.1662551
|
||||
58220.52082824707,24.299999,-20.0,0.164419,1.644192,-1.0,24.1,24.700001,0.16441920000000002
|
||||
58914.91898345947,24.299999,-20.0,0.169753,1.69753,-1.0,24.1,24.700001,0.16975300000000001
|
||||
59609.51387786865,24.299999,-20.0,0.174289,1.742891,-1.0,24.1,24.700001,0.1742891
|
||||
60303.912033081055,24.299999,-20.0,0.182712,1.827117,-1.0,24.1,24.700001,0.18271170000000003
|
||||
60998.530097961426,24.299999,-20.0,203.141159,2.031412,2.0,24.200001,24.799999,203.1412
|
||||
61692.90508270264,24.4,-20.0,632.268799,6.322688,2.0,24.299999,24.9,632.2688
|
||||
62387.44211578369,24.4,-20.0,923.56897,9.23569,2.0,24.4,25.0,923.569
|
||||
|
@ -8,4 +8,5 @@ import spatz.transforms as transforms
|
||||
from spatz.transforms import *
|
||||
|
||||
from spatz.dataset import *
|
||||
from spatz.simulation import *
|
||||
from spatz.simulation import *
|
||||
from spatz.simulations import *
|
||||
@ -302,7 +302,7 @@ class Dataset(Advanceable):
|
||||
"""
|
||||
return self.get_mach_number(t) > 1
|
||||
|
||||
def get_velocity(self, t: float | None = None) -> float:
|
||||
def get_total_velocity(self, t: float | None = None) -> float:
|
||||
"""
|
||||
Args:
|
||||
t (float): Allows specification of a different time instead of the current time. None for current time.
|
||||
|
||||
@ -1,51 +1,45 @@
|
||||
import numpy as np
|
||||
import pandas as pd
|
||||
|
||||
from abc import abstractmethod
|
||||
from typing import Any, Tuple, List
|
||||
from numpy.typing import ArrayLike
|
||||
|
||||
from abc import abstractmethod
|
||||
|
||||
|
||||
class Advanceable:
|
||||
def __init__(self) -> None:
|
||||
self.reset()
|
||||
|
||||
def step(self, dt: float):
|
||||
"""Advances the simulation data in time.
|
||||
|
||||
Args:
|
||||
dt (float): The step in time to make.
|
||||
"""
|
||||
self.__t += dt
|
||||
self._on_step(dt)
|
||||
|
||||
def reset(self):
|
||||
"""
|
||||
Reset the Avanceable object to its initial state.
|
||||
"""
|
||||
self.__t = 0
|
||||
self._on_reset()
|
||||
|
||||
@abstractmethod
|
||||
def _on_step(self, dt: float):
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def _on_reset(self):
|
||||
pass
|
||||
|
||||
def get_time(self) -> float:
|
||||
"""
|
||||
Returns:
|
||||
float: Returns the current time of the Advanceable.
|
||||
"""
|
||||
return self.__t
|
||||
from spatz.simulations.advanceable import Advanceable
|
||||
|
||||
|
||||
class Logger(Advanceable):
|
||||
def __init__(self) -> None:
|
||||
super().__init__()
|
||||
|
||||
@abstractmethod
|
||||
def write(self, attrib: str | List[str], value: Any | List[Any] | List[ArrayLike], domain: str = 'all'):
|
||||
pass
|
||||
|
||||
|
||||
class EmptyLogger(Logger):
|
||||
def __init__(self) -> None:
|
||||
super().__init__()
|
||||
|
||||
self.__idx = -1
|
||||
|
||||
def _on_step(self, _: float):
|
||||
pass
|
||||
|
||||
def _on_reset(self):
|
||||
pass
|
||||
|
||||
def write(self, attrib: str | List[str], value: Any | List[Any] | List[ArrayLike], domain: str = 'all'):
|
||||
pass
|
||||
|
||||
def get_dataframe(self) -> pd.DataFrame:
|
||||
pass
|
||||
|
||||
|
||||
class CSVLogger(Logger):
|
||||
def __init__(self) -> None:
|
||||
super().__init__()
|
||||
|
||||
self.__idx = -1
|
||||
|
||||
def _on_step(self, _: float):
|
||||
|
||||
@ -2,6 +2,7 @@ from typing import Any
|
||||
import numpy as np
|
||||
|
||||
from enum import Enum
|
||||
from ambiance import Atmosphere
|
||||
|
||||
|
||||
class PressUnit:
|
||||
@ -42,3 +43,11 @@ class AltitudeModel:
|
||||
press = to_hpa[unit]
|
||||
|
||||
return 44330 * (1 - (press / self.__press_0)**(1 / 5.255))
|
||||
|
||||
|
||||
class StandardAtmosphere:
|
||||
def __init__(self) -> None:
|
||||
pass
|
||||
|
||||
def predict(self, press: float, unit: PressUnit = PressUnit.Pa) -> float:
|
||||
return Atmosphere.from_pressure(press).h[0]
|
||||
|
||||
@ -1,5 +1,6 @@
|
||||
from spatz.sensors.sensor import Sensor
|
||||
from spatz.sensors.gps import GPS
|
||||
from spatz.sensors.imu import Accelerometer, Gyroscope, IMU
|
||||
from spatz.sensors.imu import Accelerometer, Gyroscope, IMU, CoordSystem
|
||||
from spatz.sensors.pressure import PressureSensor
|
||||
from spatz.sensors.temperature import TemperatureSensor
|
||||
from spatz.sensors.compound import CompoundSensor
|
||||
@ -1,16 +1,20 @@
|
||||
from typing import List
|
||||
|
||||
from numpy.typing import ArrayLike
|
||||
from sensors.gps import GPS
|
||||
from spatz.sensors.gps import GPS
|
||||
from spatz.dataset import ArrayLike, Dataset
|
||||
from spatz.simulations.data_source import DataSource
|
||||
from spatz.logger import ArrayLike, Logger
|
||||
from spatz.transforms import Transform
|
||||
|
||||
|
||||
class Erinome_I(GPS):
|
||||
def __init__(self, dataset: Dataset, logger: Logger, transforms: List[Transform] = []):
|
||||
def __init__(self, dataset: DataSource, logger: Logger, transforms: List[Transform] = []):
|
||||
super().__init__(dataset, logger, transforms)
|
||||
|
||||
def _get_name(self):
|
||||
return "Erinome-I"
|
||||
|
||||
def _sensor_specific_effects(self, x: ArrayLike) -> ArrayLike:
|
||||
# TODO: What's the GPS module's behavior?
|
||||
|
||||
|
||||
@ -1,3 +1,6 @@
|
||||
import math
|
||||
import numpy as np
|
||||
|
||||
from typing import Any, List
|
||||
from numpy.linalg import norm
|
||||
|
||||
@ -5,6 +8,7 @@ from numpy.typing import ArrayLike
|
||||
from pandas import NA
|
||||
|
||||
from spatz.dataset import ArrayLike, Dataset
|
||||
from spatz.simulations.data_source import DataSource
|
||||
from spatz.logger import ArrayLike, Logger
|
||||
from spatz.sensors import Sensor
|
||||
from spatz.transforms import Transform
|
||||
@ -14,22 +18,24 @@ from spatz.transforms import Transform
|
||||
|
||||
|
||||
class GPS(Sensor):
|
||||
def __init__(self, dataset: Dataset, logger: Logger, transforms: List[Transform] = []):
|
||||
def __init__(self, dataset: DataSource, logger: Logger, transforms: List[Transform] = []):
|
||||
"""GPS Module which provides the following information:
|
||||
- Longitude (in °)
|
||||
- Latitiude (in °)
|
||||
- Altitude (in m)
|
||||
- Lock (1 or 0)
|
||||
"""
|
||||
super().__init__(dataset, logger, transforms)
|
||||
|
||||
def _get_data(self) -> ArrayLike:
|
||||
vel = norm(self._dataset.get_velocity())
|
||||
long = self._dataset.get_longitude()
|
||||
lat = self._dataset.get_latitude()
|
||||
alt = self._dataset.get_altitude()
|
||||
lock = 1
|
||||
|
||||
# TODO: At which speed do we assume that GPS becomes unreliable?
|
||||
if vel / self._dataset.get_mach_number() > 1:
|
||||
return NA
|
||||
|
||||
x = self._dataset.fetch_values(['latitude', 'longitude', 'altitude'])
|
||||
x = self._sensor_specific_effects(x)
|
||||
self._log('longitude', long)
|
||||
self._log('latitude', lat)
|
||||
self._log('altitude', alt)
|
||||
self._log('lock', lock)
|
||||
|
||||
return x
|
||||
return np.array([long, lat, alt, lock])
|
||||
|
||||
@ -1,3 +1,3 @@
|
||||
from spatz.sensors.imu.accelerometer import Accelerometer
|
||||
from spatz.sensors.imu.accelerometer import Accelerometer, CoordSystem
|
||||
from spatz.sensors.imu.gyroscope import Gyroscope
|
||||
from spatz.sensors.imu.imu import IMU
|
||||
@ -1,11 +1,13 @@
|
||||
import numpy as np
|
||||
|
||||
from typing import List
|
||||
from numpy.typing import ArrayLike
|
||||
from enum import Enum
|
||||
from numpy.typing import ArrayLike, NDArray
|
||||
|
||||
from spatz.sensors import Sensor
|
||||
from spatz.transforms import Transform
|
||||
from spatz.dataset import Dataset
|
||||
from spatz.simulations.data_source import DataSource
|
||||
from spatz.logger import Logger
|
||||
|
||||
|
||||
@ -18,32 +20,55 @@ __all__=[
|
||||
g = 9.81
|
||||
|
||||
|
||||
class CoordSystem:
|
||||
LEFT_HANDED = 0,
|
||||
RIGHT_HANDED = 0,
|
||||
|
||||
|
||||
class Accelerometer(Sensor):
|
||||
def __init__(self, dataset: Dataset, logger: Logger, offset: float = 0, transforms: List[Transform] = []):
|
||||
def __init__(
|
||||
self, dataset: DataSource,
|
||||
logger: Logger,
|
||||
orientation: NDArray = np.identity(3),
|
||||
offset: float = 0,
|
||||
transforms: List[Transform] = []):
|
||||
"""_summary_
|
||||
|
||||
Args:
|
||||
dataset (Dataset): A dataset object to fetch data from.
|
||||
logger (Logger): A logger object to write data to.
|
||||
coord_system (CoordSystem, optional): The type of coordinate system to use. Defaults to CoordSystem.RIGHT_HANDED.
|
||||
orientation (NDArray, optional): The orientation of the sensor inside the spacecraft. Defaults to np.identity(3).
|
||||
offset (float, optional): The offset of the sensor from the origin around which the object rotates. Defaults to 0.
|
||||
transforms (List[Transform], optional): A list of transformations applied to the sensor measurements. Defaults to [].
|
||||
"""
|
||||
super().__init__(dataset, logger, transforms)
|
||||
|
||||
assert orientation.shape == (3, 3), 'Orientation has to be a 3x3 matrix.'
|
||||
|
||||
self._offset = np.array([offset, 0, 0])
|
||||
self._orientation = orientation
|
||||
|
||||
def _get_data(self) -> ArrayLike | float:
|
||||
acc = self._dataset.get_acceleration(frame='FL')
|
||||
acc += np.array([0, 0, g])
|
||||
acc = self._dataset.get_acceleration('global')
|
||||
|
||||
self._logger.write('FL_x', acc[0], self._get_name())
|
||||
self._logger.write('FL_y', acc[1], self._get_name())
|
||||
self._logger.write('FL_z', acc[2], self._get_name())
|
||||
self._logger.write('global_ax', acc[0], self._get_name())
|
||||
self._logger.write('global_ay', acc[1], self._get_name())
|
||||
self._logger.write('global_az', acc[2], self._get_name())
|
||||
|
||||
# Convert FL to body
|
||||
acc = self._dataset.launch_rail_to_body() @ acc
|
||||
acc = self._dataset.global_to_local() @ acc
|
||||
acc += self._dataset.global_to_local() @ np.array([0, 0, g])
|
||||
|
||||
self._logger.write('B_x', acc[0], self._get_name())
|
||||
self._logger.write('B_y', acc[1], self._get_name())
|
||||
self._logger.write('B_z', acc[2], self._get_name())
|
||||
self._logger.write('local_x', acc[0], self._get_name())
|
||||
self._logger.write('local_y', acc[1], self._get_name())
|
||||
self._logger.write('local_z', acc[2], self._get_name())
|
||||
|
||||
# Flip axes to sensor's perspective.
|
||||
acc *= -1
|
||||
# Rotate the acceleration vector to accomodate the accelerometer's orientation on the rocket.
|
||||
acc = self._orientation @ acc
|
||||
|
||||
# Add the effects of the imu's offset.
|
||||
omega = self._dataset.get_angular_velocities()
|
||||
omega = self._dataset.get_angular_velocity()
|
||||
acc += (np.cross(omega, self._offset) + np.cross(omega, np.cross(omega, self._offset)))
|
||||
|
||||
return acc
|
||||
|
||||
41
spatz/sensors/imu/bmi088.py
Normal file
41
spatz/sensors/imu/bmi088.py
Normal file
@ -0,0 +1,41 @@
|
||||
import numpy as np
|
||||
|
||||
from typing import List, AnyStr
|
||||
|
||||
from numpy.typing import ArrayLike
|
||||
from spatz.dataset import ArrayLike, Dataset
|
||||
from spatz.logger import ArrayLike, Logger
|
||||
from spatz.sensors import IMU, Accelerometer, Gyroscope, CoordSystem
|
||||
from spatz.transforms import Transform, GaussianNoise
|
||||
|
||||
|
||||
class BMI088(IMU):
|
||||
def __init__(self, dataset: Dataset, logger: Logger, orientation = np.identity(3), offset=0, transforms: List[Transform] = []):
|
||||
acc = BMI088Acc(dataset, logger, orientation, offset, transforms)
|
||||
gyro = BMI088Gyro(dataset, logger, offset, transforms)
|
||||
|
||||
super().__init__(dataset, logger, acc, gyro, transforms)
|
||||
|
||||
|
||||
class BMI088Gyro(Gyroscope):
|
||||
def __init__(self, dataset: Dataset, logger: Logger, orientation=np.identity(3), transforms: List[Transform] = []):
|
||||
super().__init__(dataset, logger, orientation, transforms)
|
||||
|
||||
def _get_name(self) -> AnyStr:
|
||||
return 'BMI088'
|
||||
|
||||
def _sensor_specific_effects(self, x: ArrayLike) -> ArrayLike:
|
||||
return x
|
||||
|
||||
|
||||
class BMI088Acc(Accelerometer):
|
||||
def __init__(self, dataset: Dataset, logger: Logger, orientation = np.identity(3), offset: float = 0, transforms: List[Transform] = []):
|
||||
super().__init__(dataset, logger, CoordSystem.RIGHT_HANDED, orientation, offset, transforms)
|
||||
|
||||
self.__noise = GaussianNoise(0, 0.05)
|
||||
|
||||
def _get_name(self) -> AnyStr:
|
||||
return 'BMI088'
|
||||
|
||||
def _sensor_specific_effects(self, x: ArrayLike) -> ArrayLike:
|
||||
return self.__noise(0, x)
|
||||
@ -1,20 +1,30 @@
|
||||
import numpy as np
|
||||
|
||||
from numpy.typing import ArrayLike
|
||||
from typing import List
|
||||
|
||||
from spatz.sensors import Sensor
|
||||
from spatz.transforms import Transform
|
||||
from spatz.dataset import Dataset
|
||||
from spatz.simulations.data_source import DataSource
|
||||
from spatz.logger import Logger
|
||||
|
||||
|
||||
class Gyroscope(Sensor):
|
||||
def __init__(self, dataset: Dataset, logger: Logger, offset: float = 0, transforms: List[Transform] = []):
|
||||
def __init__(self, dataset: DataSource, logger: Logger, orientation=np.identity(3), transforms: List[Transform] = []):
|
||||
super().__init__(dataset, logger, transforms)
|
||||
|
||||
self._offset = offset
|
||||
self._orientation = orientation
|
||||
|
||||
def calibrate(self, n_samples):
|
||||
return np.sum(self() for i in range(n_samples)) / n_samples
|
||||
|
||||
def _get_data(self) -> ArrayLike | float:
|
||||
# Rotation in rad/sec
|
||||
x = self._dataset.get_rotation_rates()
|
||||
omegas = self._dataset.get_angular_velocity()
|
||||
omegas = self._orientation @ omegas
|
||||
|
||||
return x
|
||||
self._log('ox', omegas[0])
|
||||
self._log('oy', omegas[1])
|
||||
self._log('oz', omegas[2])
|
||||
|
||||
return omegas
|
||||
|
||||
38
spatz/sensors/imu/h3lis100dl.py
Normal file
38
spatz/sensors/imu/h3lis100dl.py
Normal file
@ -0,0 +1,38 @@
|
||||
import numpy as np
|
||||
|
||||
from typing import List, AnyStr
|
||||
from numpy.core.numeric import identity as identity
|
||||
from numpy.typing import ArrayLike, NDArray
|
||||
|
||||
from spatz.dataset import Dataset
|
||||
from spatz.simulations.data_source import DataSource
|
||||
from spatz.logger import Logger
|
||||
from spatz.sensors import Accelerometer, CoordSystem
|
||||
from spatz.transforms import Transform
|
||||
|
||||
|
||||
class H3LIS100DL(Accelerometer):
|
||||
def __init__(self, dataset: DataSource, logger: Logger, orientation=np.identity(3), offset: float = 0, transforms: List[Transform] = []):
|
||||
super().__init__(dataset, logger, orientation, offset, transforms)
|
||||
|
||||
def _get_name(self) -> AnyStr:
|
||||
return "H3LIS100DL"
|
||||
|
||||
def _sensor_specific_effects(self, x: ArrayLike) -> ArrayLike:
|
||||
# The sensor only measures values between -100g and +100g and in g/LSB.
|
||||
g = 9.81
|
||||
x = np.floor(np.clip(x, -100*g, +100*g) / g)
|
||||
|
||||
for i in range(3):
|
||||
value = np.random.random()
|
||||
if (value < 0.1):
|
||||
x[i] += 1
|
||||
|
||||
if (value > 0.9):
|
||||
x[i] -= 1
|
||||
|
||||
return x
|
||||
|
||||
|
||||
|
||||
|
||||
@ -12,7 +12,7 @@ class IMU(CompoundSensor):
|
||||
dataset: Dataset,
|
||||
logger: Logger,
|
||||
acc: Accelerometer,
|
||||
gyro: Gyroscope,
|
||||
gyro: Gyroscope,
|
||||
transforms: List[Transform] = []):
|
||||
"""_summary_
|
||||
|
||||
|
||||
@ -3,8 +3,9 @@ import numpy as np
|
||||
from typing import AnyStr, List
|
||||
from numpy.typing import ArrayLike
|
||||
|
||||
from spatz.sensors import Accelerometer, Gyroscope, IMU
|
||||
from spatz.transforms import Transform, GaussianNoise
|
||||
from spatz.sensors import Accelerometer, Gyroscope, IMU, CoordSystem
|
||||
from spatz.transforms import Transform, GaussianNoise, DriftingBias
|
||||
from spatz.simulations.data_source import DataSource
|
||||
from spatz.dataset import Dataset
|
||||
from spatz.logger import Logger
|
||||
|
||||
@ -13,21 +14,37 @@ class WSEN_ISDS(IMU):
|
||||
pass
|
||||
|
||||
|
||||
class WSEN_ISDS_ACC(Accelerometer):
|
||||
def __init__(self, dataset: Dataset, logger: Logger, offset: float, transforms: List[Transform] = []):
|
||||
super().__init__(dataset, logger, offset, transforms)
|
||||
g = 9.81
|
||||
|
||||
self.__variance = 0.05
|
||||
self.__noise = GaussianNoise(np.zeros(3), np.identity(3) * self.__variance)
|
||||
|
||||
class WSEN_ISDS_ACC(Accelerometer):
|
||||
def __init__(self, dataset: DataSource, logger: Logger, orientation=np.identity(3), offset=0, transforms: List[Transform] = []):
|
||||
super().__init__(dataset, logger, orientation, offset, transforms)
|
||||
|
||||
self.__bias = DriftingBias(np.zeros(3), np.array([
|
||||
0.00113044 / g * 1000,
|
||||
0.00108539 / g * 1000,
|
||||
0.00127884 / g * 1000
|
||||
]), 400)
|
||||
|
||||
self.__constant_bias = np.random.normal(0, 0.81423, 3)
|
||||
|
||||
self.__normal = GaussianNoise(0, np.array([
|
||||
0.0003330315865455515 / g * 1000,
|
||||
0.00016874534484267122 / g * 1000,
|
||||
0.0003885568325537318 / g * 100
|
||||
]))
|
||||
|
||||
def _get_name(self) -> AnyStr:
|
||||
return 'WSEN_ISDS_ACC'
|
||||
|
||||
def _sensor_specific_effects(self, x: ArrayLike) -> ArrayLike:
|
||||
t = self._dataset.get_time()
|
||||
# Convert to milli-g.
|
||||
x = x / g * 1000
|
||||
|
||||
# Apply noise to the true values.
|
||||
y = self.__noise(t, x)
|
||||
y = self.__constant_bias + self.__normal(t, self.__bias(t, x))
|
||||
noise = y - x
|
||||
|
||||
# Log the chosen noise values.
|
||||
@ -35,20 +52,34 @@ class WSEN_ISDS_ACC(Accelerometer):
|
||||
self._logger.write('acc_y_noise', noise[1], self._get_name())
|
||||
self._logger.write('acc_z_noise', noise[2], self._get_name())
|
||||
|
||||
# The WSEN-ISDS accelerometer only measures acceleration between -16g and 16g.
|
||||
y = np.clip(y, -16000, +16000)
|
||||
|
||||
return y
|
||||
|
||||
|
||||
class WSEN_ISDS_GYRO(Gyroscope):
|
||||
def __init__(self, dataset: Dataset, logger: Logger, offset: float, transforms: List[Transform] = []):
|
||||
super().__init__(dataset, logger, offset, transforms)
|
||||
def __init__(self, dataset: Dataset, logger: Logger, orientation = np.identity(3), transforms: List[Transform] = []):
|
||||
super().__init__(dataset, logger, orientation, transforms)
|
||||
|
||||
self.__bias = DriftingBias(np.zeros(3), np.array([0.00218 * 1000, 0.00105 * 1000, 0.00203 * 1000]), 400)
|
||||
self.__constant_bias = np.random.normal(0, 2*2000, 3)
|
||||
self.__normal = GaussianNoise(0, np.array([0.0049272 * 1000, 0.00557833 * 1000, 0.00407826 * 1000]))
|
||||
|
||||
def _get_name(self) -> AnyStr:
|
||||
return 'WSEN_ISDS_GYRO'
|
||||
|
||||
def _sensor_specific_effects(self, x: ArrayLike) -> ArrayLike:
|
||||
# Convert to degrees per second.
|
||||
x = (x / np.pi) * 180
|
||||
# Convert to milli-degrees per second.
|
||||
x = (x / np.pi) * 180 * 1000
|
||||
|
||||
t = self._dataset.get_time()
|
||||
x = self.__constant_bias + self.__normal(t, self.__bias(t, x))
|
||||
|
||||
# TODO: Noise model.
|
||||
|
||||
self._log('ox', x[0])
|
||||
self._log('oy', x[1])
|
||||
self._log('oz', x[2])
|
||||
|
||||
return x
|
||||
|
||||
31
spatz/sensors/pressure/ms5611.py
Normal file
31
spatz/sensors/pressure/ms5611.py
Normal file
@ -0,0 +1,31 @@
|
||||
from typing import List, AnyStr
|
||||
from numpy.typing import ArrayLike
|
||||
|
||||
from spatz.sensors import PressureSensor
|
||||
from spatz.dataset import Dataset, Phase
|
||||
from spatz.simulations.data_source import DataSource
|
||||
from spatz.logger import Logger
|
||||
from spatz.transforms import GaussianNoise, Transform, ProportionalGaussian
|
||||
|
||||
|
||||
class MS5611(PressureSensor):
|
||||
def __init__(self, dataset: DataSource, logger: Logger, transforms: List[Transform] = [], ts_effects=True):
|
||||
super().__init__(dataset, logger, transforms, ts_effects)
|
||||
|
||||
# Noise model obtained by a test flight using this sensor.
|
||||
# self.__pad_noise = GaussianNoise(0, 0.03)
|
||||
# self.__flight_noise = GaussianNoise(0, 1.5)
|
||||
self.__noise = GaussianNoise(0, 0.00043300242654881085)
|
||||
|
||||
def _get_name(self) -> AnyStr:
|
||||
return 'MS5611'
|
||||
|
||||
def _sensor_specific_effects(self, x: ArrayLike | float) -> ArrayLike | float:
|
||||
t = self._dataset.get_time()
|
||||
|
||||
noisy = self.__noise(t, x) # self.__pad_noise(t, x) if self._dataset.get_phase() == Phase.ONPAD else self.__flight_noise(t, x)
|
||||
|
||||
# Log the noise added to the pressure measurements.
|
||||
self._logger.write('noise', noisy - x, domain=self._get_name())
|
||||
|
||||
return noisy
|
||||
@ -1,32 +0,0 @@
|
||||
from typing import List, AnyStr
|
||||
from numpy.typing import ArrayLike
|
||||
|
||||
from spatz.sensors import PressureSensor
|
||||
from spatz.dataset import Dataset, Phase
|
||||
from spatz.logger import Logger
|
||||
from spatz.transforms import GaussianNoise, Transform
|
||||
|
||||
|
||||
class MS5611_01BA03(PressureSensor):
|
||||
def __init__(self, dataset: Dataset, logger: Logger, transforms: List[Transform] = [], ts_effects=True):
|
||||
super().__init__(dataset, logger, transforms, ts_effects)
|
||||
|
||||
# Noise model obtained by a test flight using this sensor.
|
||||
self.__pad_noise = GaussianNoise(0, 0.03)
|
||||
self.__flight_noise = GaussianNoise(0, 1.5)
|
||||
|
||||
def _get_name(self) -> AnyStr:
|
||||
return 'MS5611_01BA03'
|
||||
|
||||
def _sensor_specific_effects(self, x: ArrayLike | float) -> ArrayLike | float:
|
||||
t = self._dataset.get_time()
|
||||
|
||||
# Transform from Pa to hPa
|
||||
x /= 1e2
|
||||
|
||||
noisy = self.__pad_noise(t, x) if self._dataset.get_phase() == Phase.ONPAD else self.__flight_noise(t, x)
|
||||
|
||||
# Log the noise added to the pressure measurements.
|
||||
self._logger.write('noise', noisy - x, domain=self._get_name())
|
||||
|
||||
return noisy
|
||||
@ -6,11 +6,12 @@ from typing import List
|
||||
from spatz.sensors import Sensor
|
||||
from spatz.logger import Logger
|
||||
from spatz.dataset import Dataset
|
||||
from spatz.simulations.data_source import DataSource
|
||||
from spatz.transforms import Transform
|
||||
|
||||
|
||||
class PressureSensor(Sensor):
|
||||
def __init__(self, dataset: Dataset, logger: Logger, transforms: List[Transform] = [], ts_effects=True, delay=0.0):
|
||||
def __init__(self, dataset: DataSource, logger: Logger, transforms: List[Transform] = [], ts_effects=True, delay=0.0):
|
||||
"""_summary_
|
||||
|
||||
Args:
|
||||
@ -28,7 +29,7 @@ class PressureSensor(Sensor):
|
||||
self._ts_effects = active
|
||||
|
||||
def _get_data(self) -> float:
|
||||
x = self._dataset.get_pressure()
|
||||
x = self._dataset.get_static_pressure()
|
||||
|
||||
if self._ts_effects:
|
||||
# Pre-defined constants.
|
||||
@ -36,16 +37,16 @@ class PressureSensor(Sensor):
|
||||
sigma = 40
|
||||
|
||||
# How far away from transsonic speed (mach 1) are we?
|
||||
vvec = self._dataset.get_velocity()
|
||||
vvec = self._dataset.get_velocity('global')
|
||||
dv = np.abs(np.linalg.norm(vvec) - self._dataset.get_speed_of_sound())
|
||||
|
||||
# Model transsonic effects by a peak at mach 1 which decays the further we are away from it.
|
||||
ts_eff = _p * math.exp(-0.5* (dv / sigma)**2 ) / (sigma * math.sqrt(2*math.pi))
|
||||
|
||||
# Log the values for the transsonic effect.
|
||||
self._logger.write('ts_effects', ts_eff, domain=self._get_name())
|
||||
self._logger.write('mach_no', self._dataset.get_mach_number(), domain='mach')
|
||||
self._logger.write('speedofsound', self._dataset.get_speed_of_sound(), domain='mach')
|
||||
self._log('ts_effects', ts_eff)
|
||||
self._log('mach_no', self._dataset.get_mach_number())
|
||||
self._log('speedofsound', self._dataset.get_speed_of_sound())
|
||||
|
||||
x = x + ts_eff
|
||||
|
||||
|
||||
@ -4,21 +4,29 @@ import numpy as np
|
||||
from abc import abstractmethod
|
||||
from typing import List, AnyStr
|
||||
from numpy.typing import ArrayLike
|
||||
from pandas import NA
|
||||
|
||||
from spatz.transforms import *
|
||||
from spatz.logger import *
|
||||
from spatz.dataset import *
|
||||
from spatz.simulations.data_source import DataSource
|
||||
|
||||
|
||||
class Sensor:
|
||||
def __init__(self, dataset: Dataset, logger: Logger, transforms: List[Transform] = [], min_value=-np.inf, max_value=np.inf):
|
||||
def __init__(
|
||||
self,
|
||||
dataset: DataSource,
|
||||
logger: Logger,
|
||||
transforms: List[Transform] = [],
|
||||
min_value=-np.inf,
|
||||
max_value=np.inf):
|
||||
self._dataset = dataset
|
||||
self._logger = logger
|
||||
self._transforms = transforms
|
||||
self._min_value = min_value
|
||||
self._max_value = max_value
|
||||
|
||||
def set_dataset(self, dataset: Dataset):
|
||||
def set_dataset(self, dataset: DataSource):
|
||||
self._dataset = dataset
|
||||
|
||||
def set_logger(self, logger: Logger):
|
||||
@ -37,12 +45,12 @@ class Sensor:
|
||||
@abstractmethod
|
||||
def _sensor_specific_effects(self, x: ArrayLike | float) -> ArrayLike | float:
|
||||
raise NotImplementedError()
|
||||
|
||||
|
||||
@abstractmethod
|
||||
def _get_data(self) -> ArrayLike | float:
|
||||
raise NotImplementedError()
|
||||
|
||||
def get_init_data() -> ArrayLike:
|
||||
|
||||
def get_init_data(self) -> ArrayLike:
|
||||
pass
|
||||
|
||||
def __call__(self) -> ArrayLike | float:
|
||||
|
||||
@ -1 +1 @@
|
||||
from temperature import TemperatureSensor
|
||||
from spatz.sensors.temperature.temperature import TemperatureSensor
|
||||
26
spatz/sensors/temperature/ms5611.py
Normal file
26
spatz/sensors/temperature/ms5611.py
Normal file
@ -0,0 +1,26 @@
|
||||
from numpy.typing import ArrayLike
|
||||
from typing import List, AnyStr
|
||||
|
||||
from spatz.transforms.noise import GaussianNoise
|
||||
from spatz.dataset import Dataset
|
||||
from spatz.simulations.data_source import DataSource
|
||||
from spatz.sensors import TemperatureSensor
|
||||
from spatz.transforms import Transform
|
||||
|
||||
|
||||
class MS5611Temperature(TemperatureSensor):
|
||||
def __init__(self, dataset: DataSource, transforms: List[Transform] = []):
|
||||
super().__init__(dataset, transforms)
|
||||
|
||||
self.__noise = GaussianNoise(0, 0.5)
|
||||
|
||||
def _get_name(self) -> AnyStr:
|
||||
return 'MS5611'
|
||||
|
||||
def _sensor_specific_effects(self, x: ArrayLike | float) -> ArrayLike | float:
|
||||
t = self._dataset.get_time()
|
||||
noisy = self.__noise(t, x)
|
||||
|
||||
self._log('temperature', noisy)
|
||||
|
||||
return noisy
|
||||
@ -2,11 +2,13 @@ from typing import List
|
||||
from numpy.random import normal
|
||||
from tqdm import tqdm
|
||||
|
||||
from spatz.simulations.advanceable import Advanceable
|
||||
from spatz.simulations.data_source import DataSource
|
||||
from spatz.dataset import Dataset
|
||||
from spatz.logger import Logger
|
||||
from spatz.sensors import Sensor
|
||||
from spatz.dataset import Dataset, Phase
|
||||
from spatz.logger import Logger
|
||||
from spatz.logger import Logger, EmptyLogger
|
||||
from spatz.sensors import Sensor
|
||||
from spatz.observer import Observer
|
||||
|
||||
@ -35,9 +37,11 @@ class UniformTimeSteps:
|
||||
return self.__dt + noise
|
||||
|
||||
|
||||
class Simulation:
|
||||
class Simulation(Advanceable):
|
||||
def __init__(self, time_steps=UniformTimeSteps(0.01)):
|
||||
self.__dataset = None
|
||||
super().__init__()
|
||||
|
||||
self.__data_source = None
|
||||
self.__logger = None
|
||||
self.__sensors: List[Sensor] = []
|
||||
self.__time_steps = time_steps
|
||||
@ -46,26 +50,25 @@ class Simulation:
|
||||
idx = 0
|
||||
|
||||
# Clear all logs and reset the dataset to the first time step.
|
||||
self.__dataset.reset()
|
||||
self.__data_source.reset()
|
||||
self.__logger.reset()
|
||||
|
||||
if verbose:
|
||||
pbar = tqdm(total=self.__dataset.get_length())
|
||||
pbar = tqdm(total=self.__data_source.get_length())
|
||||
|
||||
while True:
|
||||
t = self.__dataset.get_time()
|
||||
t = self.get_time()
|
||||
dt = self.__time_steps(t)
|
||||
t_ = t + dt
|
||||
idx += 1
|
||||
|
||||
if t_ > self.__dataset.get_length():
|
||||
if t_ > self.__data_source.get_length():
|
||||
break
|
||||
|
||||
if until is not None and self.__dataset.get_phase() == until:
|
||||
if until is not None and self.__data_source.get_phase() == until:
|
||||
break
|
||||
|
||||
self.__dataset.step(dt)
|
||||
self.__logger.step(dt)
|
||||
self.advance(dt)
|
||||
|
||||
if verbose:
|
||||
pbar.update(dt)
|
||||
@ -75,22 +78,26 @@ class Simulation:
|
||||
if verbose:
|
||||
pbar.close()
|
||||
|
||||
def get_dataset(self) -> Dataset:
|
||||
return self.__dataset
|
||||
|
||||
def get_logger(self) -> Logger:
|
||||
return self.__logger
|
||||
def _on_step(self, dt: float):
|
||||
self.__data_source.advance(dt)
|
||||
self.__logger.advance(dt)
|
||||
|
||||
def load(self, path: str):
|
||||
self.__dataset = Dataset(path)
|
||||
self.__logger = Logger()
|
||||
def load(self, source: DataSource):
|
||||
self.__data_source = source
|
||||
self.__logger = EmptyLogger()
|
||||
|
||||
for sensor in self.__sensors:
|
||||
sensor.set_dataset(self.__dataset)
|
||||
sensor.set_dataset(self.__data_source)
|
||||
sensor.set_logger(self.__logger)
|
||||
|
||||
return self
|
||||
|
||||
def get_data_source(self) -> DataSource:
|
||||
return self.__data_source
|
||||
|
||||
def get_logger(self) -> Logger:
|
||||
return self.__logger
|
||||
|
||||
def add_sensor(self, sensor, *args, **kwargs) -> Sensor:
|
||||
"""Register a new sensor for this simulation. A registered sensor can be called like a function and returns
|
||||
the current measurements. The class' constructor arguments have to be given aswell.
|
||||
@ -103,7 +110,7 @@ class Simulation:
|
||||
"""
|
||||
assert issubclass(sensor, Sensor), "Expected a subclass of Sensor."
|
||||
|
||||
self.__sensors.append(sensor(self.__dataset, self.__logger, *args, **kwargs))
|
||||
self.__sensors.append(sensor(self.__data_source, self.__logger, *args, **kwargs))
|
||||
|
||||
return self.__sensors[-1]
|
||||
|
||||
@ -123,10 +130,10 @@ class Simulation:
|
||||
attributes = observer_or_attributes
|
||||
assert len(attributes) != 0, "Observed attributes list must be nonempty."
|
||||
|
||||
self.__sensors.append(Observer(self.__dataset, self.__logger, attributes))
|
||||
self.__sensors.append(Observer(self.__data_source, self.__logger, attributes))
|
||||
else:
|
||||
observer = observer_or_attributes
|
||||
self.__sensors.append(observer(self.__dataset, self.__logger))
|
||||
self.__sensors.append(observer(self.__data_source, self.__logger))
|
||||
|
||||
return self.__sensors[-1]
|
||||
|
||||
|
||||
@ -0,0 +1 @@
|
||||
from spatz.simulations.rocketpy import RocketPyCSV
|
||||
47
spatz/simulations/advanceable.py
Normal file
47
spatz/simulations/advanceable.py
Normal file
@ -0,0 +1,47 @@
|
||||
from abc import abstractmethod
|
||||
|
||||
|
||||
class Advanceable:
|
||||
def __init__(self) -> None:
|
||||
self.reset()
|
||||
|
||||
def advance(self, dt: float):
|
||||
"""Advances the simulation data in time.
|
||||
|
||||
Args:
|
||||
dt (float): The step in time to make.
|
||||
"""
|
||||
self.__t += dt
|
||||
self._on_step(dt)
|
||||
|
||||
def advance_to(self, t: float):
|
||||
"""Advances the simulation data to a new point in time.
|
||||
|
||||
Args:
|
||||
t (float): The target point in time.
|
||||
"""
|
||||
assert t > self.__t, 'Advanceable can only move forward in time.'
|
||||
|
||||
self.advance(t - self.__t)
|
||||
|
||||
def reset(self):
|
||||
"""
|
||||
Reset the Avanceable object to its initial state.
|
||||
"""
|
||||
self.__t = 0
|
||||
self._on_reset()
|
||||
|
||||
@abstractmethod
|
||||
def _on_step(self, dt: float):
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def _on_reset(self):
|
||||
pass
|
||||
|
||||
def get_time(self) -> float:
|
||||
"""
|
||||
Returns:
|
||||
float: Returns the current time of the Advanceable.
|
||||
"""
|
||||
return self.__t
|
||||
53
spatz/simulations/astos_source.py
Normal file
53
spatz/simulations/astos_source.py
Normal file
@ -0,0 +1,53 @@
|
||||
import numpy as np
|
||||
|
||||
from typing import Literal
|
||||
from numpy.typing import NDArray
|
||||
|
||||
from spatz.simulations.csv_source import CSVSource
|
||||
|
||||
|
||||
class ASTOSSource(CSVSource):
|
||||
def __init__(self, path: str, interpolation: Literal['linear'] = 'linear') -> None:
|
||||
super().__init__(path, 'time', interpolation)
|
||||
|
||||
def get_length(self) -> float:
|
||||
pass
|
||||
|
||||
def get_position(self) -> NDArray:
|
||||
return self.fetch_values(['x', 'y', 'z'])
|
||||
|
||||
def get_velocity(self, frame: Literal['global', 'local']) -> NDArray:
|
||||
if frame == 'local':
|
||||
pass
|
||||
|
||||
return self.fetch_values(['vx', 'vy', 'vz'])
|
||||
|
||||
def get_acceleration(self, frame: Literal['global', 'local']) -> NDArray:
|
||||
if frame == 'local':
|
||||
pass
|
||||
|
||||
return self.fetch_values(['ax', 'ay', 'az'])
|
||||
|
||||
def get_attitude(self) -> NDArray:
|
||||
pass
|
||||
|
||||
def local_to_global(self) -> NDArray:
|
||||
pass
|
||||
|
||||
def global_to_local(self) -> NDArray:
|
||||
pass
|
||||
|
||||
def get_angular_velocity(self) -> NDArray:
|
||||
pass
|
||||
|
||||
def get_static_pressure(self) -> float:
|
||||
pass
|
||||
|
||||
def get_longitude(self) -> float:
|
||||
pass
|
||||
|
||||
def get_latitude(self) -> float:
|
||||
pass
|
||||
|
||||
def get_altitude(self) -> float:
|
||||
pass
|
||||
91
spatz/simulations/csv_source.py
Normal file
91
spatz/simulations/csv_source.py
Normal file
@ -0,0 +1,91 @@
|
||||
import pandas as pd
|
||||
import numpy as np
|
||||
|
||||
from typing import Literal, List
|
||||
from numpy.typing import NDArray
|
||||
|
||||
from spatz.simulations.data_source import DataSource
|
||||
|
||||
|
||||
|
||||
class CSVSource(DataSource):
|
||||
def __init__(self, path: str, time_col: str, interpolation: Literal['linear']='linear') -> None:
|
||||
"""A data source that extracts all its data from a csv file.
|
||||
|
||||
Args:
|
||||
time_col (str): The name of the column that contains time data.
|
||||
"""
|
||||
super().__init__()
|
||||
|
||||
self._df = pd.read_csv(path)
|
||||
self._time_col = time_col
|
||||
self._idx = 0
|
||||
self._interpolation = interpolation
|
||||
|
||||
def get_length(self) -> float:
|
||||
return max(self._df[self._time_col])
|
||||
|
||||
def _on_reset(self):
|
||||
pass
|
||||
|
||||
def _get_closest_idx(self, t: float) -> int:
|
||||
"""Gets an index _idx_ for the dataframe _df_ such that the values at the given time _t_ are somewhere between
|
||||
_idx_ and _idx+1_.
|
||||
|
||||
Args:
|
||||
t (float): The requested time.
|
||||
|
||||
Returns:
|
||||
int: The computed index.
|
||||
"""
|
||||
idx = (self._df[self._time_col] - t).abs().idxmin()
|
||||
idx = idx if self._df[self._time_col].loc[idx] <= t else idx - 1
|
||||
|
||||
return idx
|
||||
|
||||
def _on_step(self, _: float):
|
||||
self._idx = self._get_closest_idx(self.get_time())
|
||||
|
||||
def fetch_value(self, name: str, t: float | None = None, custom_interpolation=None) -> float:
|
||||
"""Get a specific value from the dataframe.
|
||||
|
||||
Args:
|
||||
name (str): The name of the value to fetch.
|
||||
t (float): Allows specification of a different time instead of the current time. None for current time.
|
||||
|
||||
Returns:
|
||||
float: Returns the requested value.
|
||||
"""
|
||||
idx = self._idx if t is None else self._get_closest_idx(t)
|
||||
|
||||
if self._interpolation == 'linear':
|
||||
t_min = self._df.at[idx, self._time_col]
|
||||
t_max = self._df.at[idx + 1, self._time_col]
|
||||
|
||||
# Sometimes no time passes in-between two samples.
|
||||
if t_max == t_min:
|
||||
return self._df.at[name, idx]
|
||||
|
||||
# Compute the weight for interpolation.
|
||||
alpha = (self.get_time() - t_min) / (t_max - t_min)
|
||||
|
||||
if custom_interpolation is not None:
|
||||
a = self._df.at[idx, name]
|
||||
b = self._df.at[idx + 1, name]
|
||||
|
||||
return custom_interpolation(a, b, alpha)
|
||||
|
||||
# Interpolate linearly between the two data points.
|
||||
return (1 - alpha) * self._df.at[idx, name] + alpha * self._df.at[idx + 1, name]
|
||||
|
||||
def fetch_values(self, names: List[str], t: float | None = None, custom_interpolation=None) -> NDArray:
|
||||
"""Get specific values from the dataframe.
|
||||
|
||||
Args:
|
||||
names (List[str]): Names of the values to get.
|
||||
t (float): Allows specification of a different time instead of the current time. None for current time.
|
||||
|
||||
Returns:
|
||||
np.array: Returns a numpy array containing the requested values in the same order as in the input list.
|
||||
"""
|
||||
return np.asarray([self.fetch_value(name, t, custom_interpolation) for name in names])
|
||||
73
spatz/simulations/data_source.py
Normal file
73
spatz/simulations/data_source.py
Normal file
@ -0,0 +1,73 @@
|
||||
import numpy as np
|
||||
|
||||
from spatz.simulations.advanceable import Advanceable
|
||||
|
||||
from numpy.typing import NDArray
|
||||
from abc import abstractmethod
|
||||
from typing import Literal
|
||||
|
||||
from ambiance import Atmosphere
|
||||
|
||||
|
||||
class DataSource(Advanceable):
|
||||
def __init__(self) -> None:
|
||||
super().__init__()
|
||||
|
||||
def get_speed_of_sound(self) -> float:
|
||||
return Atmosphere(self.get_altitude()).speed_of_sound
|
||||
|
||||
def get_mach_number(self) -> float:
|
||||
speed = np.linalg.norm(self.get_velocity('global'))
|
||||
|
||||
return speed / self.get_speed_of_sound()
|
||||
|
||||
def get_temperature(self) -> float:
|
||||
return Atmosphere(self.get_altitude()).temperature
|
||||
|
||||
@abstractmethod
|
||||
def get_length(self) -> float:
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def get_position(self) -> NDArray:
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def get_velocity(self, frame: Literal['global', 'local']) -> NDArray:
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def get_acceleration(self, frame: Literal['global', 'local']) -> NDArray:
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def get_attitude(self) -> NDArray:
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def local_to_global(self) -> NDArray:
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def global_to_local(self) -> NDArray:
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def get_angular_velocity(self) -> NDArray:
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def get_static_pressure(self) -> float:
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def get_longitude(self) -> float:
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def get_latitude(self) -> float:
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def get_altitude(self) -> float:
|
||||
pass
|
||||
84
spatz/simulations/rocketpy.py
Normal file
84
spatz/simulations/rocketpy.py
Normal file
@ -0,0 +1,84 @@
|
||||
import numpy as np
|
||||
|
||||
from typing import Literal
|
||||
from numpy.typing import NDArray
|
||||
|
||||
from spatz.simulations.csv_source import CSVSource
|
||||
|
||||
|
||||
class RocketPyCSV(CSVSource):
|
||||
def __init__(self, path: str, interpolation: Literal['linear'] = 'linear') -> None:
|
||||
super().__init__(path, ' Time (s)', interpolation)
|
||||
|
||||
def get_position(self) -> NDArray:
|
||||
return self.fetch_values([' X (m)', ' Y (m)', ' Z (m)'])
|
||||
|
||||
def get_velocity(self, frame: Literal['global', 'local']) -> NDArray:
|
||||
vel_global = self.fetch_values([' Vx (m/s)', ' Vy (m/s)', ' Vz (m/s)'])
|
||||
|
||||
if frame == 'global':
|
||||
return vel_global
|
||||
|
||||
return self.global_to_local() @ vel_global
|
||||
|
||||
def get_acceleration(self, frame: Literal['global', 'local']) -> NDArray:
|
||||
acc_global = self.fetch_values([' Ax (m/s²)', ' Ay (m/s²)', ' Az (m/s²)'])
|
||||
|
||||
if frame == 'global':
|
||||
return acc_global
|
||||
|
||||
return self.global_to_local() @ acc_global
|
||||
|
||||
def get_attitude(self) -> NDArray:
|
||||
t_min = self._df.at[self._idx, self._time_col]
|
||||
t_max = self._df.at[self._idx + 1, self._time_col]
|
||||
|
||||
def slerp(a, b, alpha):
|
||||
theta = np.arccos(np.clip(np.dot(a, b), -1, 1))
|
||||
|
||||
if np.isclose(theta, 0):
|
||||
return a
|
||||
|
||||
return (a * np.sin((1-alpha) * theta) + b * np.sin(alpha * theta)) / np.sin(theta)
|
||||
|
||||
qa = np.array([self._df.at[self._idx, ' e0'], self._df.at[self._idx, ' e1'], self._df.at[self._idx, ' e2'], self._df.at[self._idx, ' e3']])
|
||||
qb = np.array([self._df.at[self._idx+1, ' e0'], self._df.at[self._idx+1, ' e1'], self._df.at[self._idx+1, ' e2'], self._df.at[self._idx+1, ' e3']])
|
||||
|
||||
alpha = (self.get_time() - t_min) / (t_max - t_min)
|
||||
|
||||
return slerp(qa, qb, alpha)
|
||||
|
||||
def global_to_local(self) -> NDArray:
|
||||
quat = self.get_attitude()
|
||||
|
||||
e0, e1, e2, e3 = quat[0], quat[1], quat[2], quat[3]
|
||||
|
||||
# Taken from:
|
||||
# https://docs.rocketpy.org/en/latest/technical/equations_of_motion.html
|
||||
mat = np.array([
|
||||
[e0**2 + e1**2 - e2**2 - e3**2, 2*(e1*e2+e0*e3), 2*(e1*e3 - e0*e2)],
|
||||
[2*(e1*e2 - e0*e3), e0**2 - e1**2 + e2**2 - e3**2, 2*(e2*e3 + e0*e1)],
|
||||
[2*(e1*e3 + e0*e2), 2*(e2*e3 - e0*e1), e0**2 - e1**2 - e2**2 + e3**2]
|
||||
])
|
||||
|
||||
return mat
|
||||
|
||||
def local_to_global(self) -> NDArray:
|
||||
return self.global_to_local().T
|
||||
|
||||
def get_angular_velocity(self) -> NDArray:
|
||||
return self.fetch_values([' ω1 (rad/s)',' ω2 (rad/s)',' ω3 (rad/s)'])
|
||||
|
||||
def get_static_pressure(self) -> float:
|
||||
return self.fetch_value(' Pressure (Pa)')
|
||||
|
||||
def get_longitude(self) -> float:
|
||||
return self.fetch_value(' Longitude (°)')
|
||||
|
||||
def get_latitude(self) -> float:
|
||||
return self.fetch_value(' Latitude (°)')
|
||||
|
||||
def get_altitude(self) -> float:
|
||||
return self.fetch_value(' Z (m)')
|
||||
|
||||
|
||||
@ -1,3 +1,3 @@
|
||||
from spatz.transforms.transform import Transform
|
||||
from spatz.transforms.noise import GaussianNoise
|
||||
from spatz.transforms.noise import GaussianNoise, ProportionalGaussian, DriftingBias
|
||||
from spatz.transforms.failures import Downtime
|
||||
@ -7,7 +7,7 @@ from spatz.transforms import Transform
|
||||
|
||||
|
||||
class GaussianNoise(Transform):
|
||||
def __init__(self, mu: ArrayLike, sigma: ArrayLike) -> None:
|
||||
def __init__(self, mu: ArrayLike = None, sigma: ArrayLike = None) -> None:
|
||||
super().__init__()
|
||||
|
||||
self.__mu = mu
|
||||
@ -15,7 +15,8 @@ class GaussianNoise(Transform):
|
||||
|
||||
def __call__(self, _: float, x: ArrayLike) -> ArrayLike:
|
||||
if np.isscalar(x):
|
||||
noise = np.random.normal(0, 1)
|
||||
noise = np.random.normal(0, 1)
|
||||
|
||||
x += self.__sigma * noise + self.__mu
|
||||
else:
|
||||
dim = len(x)
|
||||
@ -26,7 +27,7 @@ class GaussianNoise(Transform):
|
||||
sigma = self.__sigma
|
||||
|
||||
if np.isscalar(self.__mu):
|
||||
mu = np.ones(dim)
|
||||
mu = np.ones(dim) * self.__mu
|
||||
else:
|
||||
mu = self.__mu
|
||||
|
||||
@ -36,6 +37,50 @@ class GaussianNoise(Transform):
|
||||
return x
|
||||
|
||||
|
||||
class DriftingBias(Transform):
|
||||
def __init__(self, init: ArrayLike, covariance: ArrayLike, Tc: float) -> None:
|
||||
"""First order Gauss-Markov (GM) model used to model drift.
|
||||
|
||||
Args:
|
||||
init (ArrayLike): The initial bias.
|
||||
covariance (ArrayLike): Covariance matrix of the process.
|
||||
Tc (float): Correlation time of the process.
|
||||
"""
|
||||
super().__init__()
|
||||
|
||||
self.__t = 0
|
||||
self.__beta = 1 / Tc
|
||||
self.__covariance = covariance
|
||||
self.__Tc = Tc
|
||||
self.__x_old = np.copy(init)
|
||||
|
||||
def __call__(self, t: float, x: ArrayLike) -> ArrayLike:
|
||||
dt = t - self.__t
|
||||
self.__t = t
|
||||
|
||||
w = np.random.normal(np.zeros_like(x), self.__covariance*(1-np.exp(-2 * dt / self.__Tc)))
|
||||
drift = (1 - self.__beta * dt) * self.__x_old + w
|
||||
|
||||
self.__x_old = drift
|
||||
|
||||
return x + drift
|
||||
|
||||
|
||||
class ProportionalGaussian(Transform):
|
||||
def __init__(self, mu, sigma) -> None:
|
||||
super().__init__()
|
||||
|
||||
self.__mu = mu
|
||||
self.__sigma = sigma
|
||||
|
||||
def __call__(self, _: float, x: ArrayLike) -> ArrayLike:
|
||||
noise = np.random.normal(0, 1)
|
||||
|
||||
x += (self.__sigma * x) * noise + (self.__mu * x)
|
||||
|
||||
return x
|
||||
|
||||
|
||||
class PinkNoise(Transform):
|
||||
def __init__(self) -> None:
|
||||
super().__init__()
|
||||
|
||||
141
testing.ipynb
Normal file
141
testing.ipynb
Normal file
File diff suppressed because one or more lines are too long
154
testing.py
154
testing.py
@ -1,10 +1,148 @@
|
||||
from spatz.sensors.antenna.tx_gain import GainPattern
|
||||
import math
|
||||
import pygame
|
||||
import serial
|
||||
import re
|
||||
import numpy as np
|
||||
import time
|
||||
|
||||
pattern = GainPattern("data/gain_pattern/farfield_all.txt")
|
||||
from numpy import array
|
||||
from pyquaternion import Quaternion
|
||||
from math import cos, sin, pi
|
||||
|
||||
# pattern.get_gain(41,66)
|
||||
# pattern.get_gain(40,100)
|
||||
# pattern.get_gain(10,180)
|
||||
# pattern.get_gain(0,95)
|
||||
# pattern.get_gain(21,100)
|
||||
from spatz.sensors.imu.wsen_isds import WSEN_ISDS_GYRO
|
||||
from spatz.simulation import Simulation
|
||||
from spatz.simulations.rocketpy import RocketPyCSV
|
||||
|
||||
# Blatantly stolen from: https://stackoverflow.com/questions/21019471/how-can-i-draw-a-3d-shape-using-pygame-no-other-modules
|
||||
|
||||
X, Y, Z = 0, 1, 2
|
||||
|
||||
|
||||
def rotation_matrix(a, b, by):
|
||||
"""
|
||||
rotation matrix of a, b, by radians around x, y, z axes (respectively)
|
||||
"""
|
||||
sa, ca = sin(a), cos(a)
|
||||
sb, cb = sin(b), cos(b)
|
||||
sby, cby = sin(by), cos(by)
|
||||
return (
|
||||
(cb*cby, -cb*sby, sb),
|
||||
(ca*sby + sa*sb*cby, ca*cby - sby*sa*sb, -cb*sa),
|
||||
(sby*sa - ca*sb*cby, ca*sby*sb + sa*cby, ca*cb)
|
||||
)
|
||||
|
||||
|
||||
class Physical:
|
||||
def __init__(self, vertices, edges, colors):
|
||||
"""
|
||||
a 3D object that can rotate around the three axes
|
||||
:param vertices: a tuple of points (each has 3 coordinates)
|
||||
:param edges: a tuple of pairs (each pair is a set containing 2 vertices' indexes)
|
||||
"""
|
||||
self.__vertices = array(vertices)
|
||||
self.__edges = tuple(edges)
|
||||
self.__colors = tuple(colors)
|
||||
self.__rotation = Quaternion(axis=(1, 0, 0), angle=0) # radians around each axis
|
||||
|
||||
def rotate(self, quaternion):
|
||||
self.__rotation = quaternion
|
||||
|
||||
@property
|
||||
def lines(self):
|
||||
location = array([self.__rotation.rotate(vertex) for vertex in self.__vertices]) # an index->location mapping
|
||||
return (((location[v1], location[v2]), color) for (v1, v2), color in zip(self.__edges, self.__colors))
|
||||
|
||||
|
||||
BLACK, RED, GREEN, BLUE = (0, 0, 0), (255, 128, 128), (128, 255, 128), (128, 128, 255)
|
||||
|
||||
LIGHTRED, LIGHTGREEN, LIGHTBLUE = (128, 64, 64), (64, 128, 64), (64, 64, 128)
|
||||
|
||||
|
||||
class Paint:
|
||||
def __init__(self, shape, shape2):
|
||||
self.__shape = shape
|
||||
self.__shape2 = shape2
|
||||
self.__size = 900, 450
|
||||
self.__clock = pygame.time.Clock()
|
||||
self.__screen = pygame.display.set_mode(self.__size)
|
||||
|
||||
def __fit(self, vec):
|
||||
"""
|
||||
ignore the z-element (creating a very cheap projection), and scale x, y to the coordinates of the screen
|
||||
"""
|
||||
# notice that len(self.__size) is 2, hence zip(vec, self.__size) ignores the vector's last coordinate
|
||||
return [round(70 * coordinate + frame / 2) for coordinate, frame in zip(vec, self.__size)]
|
||||
|
||||
def __draw_shape(self, thickness=4):
|
||||
for (start, end), color in self.__shape.lines:
|
||||
pygame.draw.line(self.__screen, color, self.__fit((start[0]-2, start[1], start[2])), self.__fit((end[0]-2, end[1], end[2])), thickness)
|
||||
|
||||
for (start, end), color in self.__shape2.lines:
|
||||
pygame.draw.line(self.__screen, color, self.__fit((start[0]+2, start[1], start[2])), self.__fit((end[0]+2, end[1], end[2])), thickness)
|
||||
|
||||
def draw(self):
|
||||
self.__screen.fill(BLACK)
|
||||
self.__draw_shape()
|
||||
pygame.display.flip()
|
||||
self.__clock.tick(40)
|
||||
|
||||
|
||||
def main():
|
||||
from pygame import K_q, K_w, K_a, K_s, K_z, K_x
|
||||
|
||||
rotation = Quaternion(axis=[1, 0, 0], angle=pi/2)
|
||||
|
||||
axes = Physical(
|
||||
vertices=((0, 0, 0), (0, 0, 2), (0, 2, 0), (2, 0, 0)),
|
||||
edges=({0, 1}, {0, 2}, {0, 3}),
|
||||
colors=(BLUE, GREEN, RED)
|
||||
)
|
||||
|
||||
truth = Physical(
|
||||
vertices=((0, 0, 0), (0, 0, 2), (0, 2, 0), (2, 0, 0)),
|
||||
edges=({0, 1}, {0, 2}, {0, 3}),
|
||||
colors=(LIGHTBLUE, LIGHTGREEN, LIGHTRED)
|
||||
)
|
||||
|
||||
pygame.init()
|
||||
pygame.display.set_caption("Simulation")
|
||||
renderer = Paint(axes, truth)
|
||||
|
||||
simulation = Simulation().load(RocketPyCSV('nominal_wind.csv'))
|
||||
|
||||
gyro = simulation.add_sensor(WSEN_ISDS_GYRO)
|
||||
offset = gyro.calibrate(100)
|
||||
att = simulation.add_observer([' e0', ' e1', ' e2', ' e3'])
|
||||
dt = 0.01
|
||||
|
||||
quat = np.array([-0.706864,0.018510,0.018510,-0.706864])
|
||||
|
||||
while True:
|
||||
time.sleep(0.01)
|
||||
simulation.advance(dt)
|
||||
omegas = gyro() - offset
|
||||
true = att()
|
||||
|
||||
omegas = (omegas / 1000) * np.pi / 180
|
||||
|
||||
matrix = np.array([
|
||||
[1, -dt/2*omegas[0], -dt/2*omegas[1], -dt/2*omegas[2]],
|
||||
[dt/2*omegas[0], 1, dt/2*omegas[2], -dt/2*omegas[1]],
|
||||
[dt/2*omegas[1], -dt/2*omegas[2], 1, dt/2*omegas[0]],
|
||||
[dt/2*omegas[2], dt/2*omegas[1], -dt/2*omegas[0], 1]
|
||||
])
|
||||
|
||||
quat = matrix @ quat
|
||||
quat /= np.linalg.norm(quat)
|
||||
|
||||
|
||||
for event in pygame.event.get():
|
||||
if event.type == pygame.QUIT:
|
||||
exit()
|
||||
|
||||
axes.rotate(Quaternion(x=quat[0], y=quat[1], z=quat[2], w=quat[3]))
|
||||
truth.rotate(Quaternion(x=true[0], y=true[1], z=true[2], w=true[3]))
|
||||
|
||||
renderer.draw()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
Loading…
x
Reference in New Issue
Block a user