Source code for simulator.plant

"""連続時間 物理環境モデル (Plant)

衛星の力学系・センサ応答をシミュレートする。
制御ロジックとは独立し、外部から与えられたサーボパルス指令に応じて状態を更新する。
"""

import math
import random


from .environment import Environment




class SatellitePlant:
    """衛星の物理モデル"""

    # 物理パラメータ
    MOMENT_OF_INERTIA = 0.002  # 衛星本体の慣性モーメント [kg·m²]
    RW_INERTIA = 0.0002  # リアクションホイールの慣性モーメント [kg·m²]
    SERVO_TORQUE_GAIN = 5e-6  # サーボパルス偏差 → RWトルク [N·m/μs]
    FRICTION = 0.008  # 軸受摩擦係数 [N·m·s/rad]（減衰時定数 ≈ 0.25s）
    SERVO_CENTER = 1520  # サーボセンターパルス [μs]

    def __init__(self):
        self.env = Environment()
        self.angular_velocity: float = 0.0  # 衛星の角速度 [rad/s]
        self.rw_speed: float = 0.0  # RW回転速度 [rad/s]

        # 太陽方向のドリフト（軌道上で太陽方向がゆっくり変わる模擬）
        self.sun_drift_rate: float = 0.01  # [rad/s]

    @property
    def sun_angle(self) -> float:
        """太陽方向に対する衛星の角度 [rad] (Environmentから計算)"""
        return self.env.get_sun_angle_in_body()



    def reset(self):
        """状態を初期化する"""
        self.env.set_sat_rotation(0.0)
        self.angular_velocity = 0.0
        self.rw_speed = 0.0




    def step(self, dt: float, servo_pulse_us: int):
        """物理モデルを dt [s] だけ進める

        Args:
            dt: タイムステップ [s]
            servo_pulse_us: サーボパルス指令 [μs]
        """
        pulse_delta = servo_pulse_us - self.SERVO_CENTER

        # RWへのトルク
        rw_torque = pulse_delta * self.SERVO_TORQUE_GAIN

        # RW角加速度
        rw_alpha = rw_torque / self.RW_INERTIA

        # 反作用トルクが衛星本体に作用（角運動量保存）
        sat_torque = -rw_torque - self.FRICTION * self.angular_velocity

        # 衛星角加速度
        sat_alpha = sat_torque / self.MOMENT_OF_INERTIA

        # オイラー積分
        self.angular_velocity += sat_alpha * dt
        self.rw_speed += rw_alpha * dt
        
        # 衛星の絶対姿勢を更新 (ドリフト込み)
        new_rotation = self.env.sat_rotation + (self.angular_velocity + self.sun_drift_rate) * dt
        self.env.set_sat_rotation(new_rotation)

        # 環境の状態を更新 (公転・彗星の移動など)
        self.env.step(dt)




    def get_sensor_readings(self) -> dict:
        """現在の状態からセンサ値を生成（ADCノイズ含む）"""
        angle = self.sun_angle

        # フォトダイオード電圧（4ch、配置角度に依存）
        photo_diode_volt = [
            max(0.0, 1.5 - math.cos(angle + math.radians(135)) + random.gauss(0, 0.02)),
            max(0.0, 1.5 - math.cos(angle + math.radians(45)) + random.gauss(0, 0.02)),
            max(0.0, 1.5 + math.cos(angle + math.radians(135)) + random.gauss(0, 0.02)),
            max(0.0, 1.5 + math.cos(angle + math.radians(45)) + random.gauss(0, 0.02)),
        ]

        # フォトリフレクタ（RW回転検出）
        photo_reflector_volt = 1.0 + 0.5 * math.sin(self.rw_speed * 0.1) + random.gauss(0, 0.05)

        # 電源電圧
        battery_volt = 6.5 + random.gauss(0, 0.05)

        return {
            "photo_diode_volt": photo_diode_volt,
            "photo_reflector_volt": photo_reflector_volt,
            "battery_volt": battery_volt,
            "sun_angle_rad": angle,
            "angular_velocity_rad_per_s": self.angular_velocity,
            "rw_speed_rad_per_s": self.rw_speed,
        }