Source code for sils.controller

"""離散時間 制御ロジック (Controller)

satelite/control.cpp の Python 移植。
衛星のメインループ（~200Hz）ごとに呼ばれ、サーボパルス指令を返す。
"""

import math




class ControlState:
    UNLOAD_TIME = 0
    UNLOAD_ROTATE = 1
    STABLE = 2
    LOTATE = 3



def _angle_diff_rad(input_rad: float, base_rad: float) -> float:
    """角度差を -π〜π に正規化"""
    diff = input_rad - base_rad
    if diff > math.pi:
        diff -= 2 * math.pi
    elif diff < -math.pi:
        diff += 2 * math.pi
    return diff




class SatelliteController:
    """衛星の姿勢制御ステートマシン (control.cpp の移植)"""

    # 制御パラメータ（control.cpp と一致させる）
    SERVO_CENTER_US = 1520
    PULSE_DELTA_US = 1000
    UNLOAD_MIN_DURATION_US = 300000  # 300 ms
    ROTATE_DURATION_US = 200000  # 200 ms
    ANGLE_TOLERANCE_RAD = 10.0 / 180.0 * math.pi
    ANGULAR_VELOCITY_TOLERANCE_RAD_PER_S = 0.5 / 180.0 * math.pi

    def __init__(self):
        self.state = ControlState.UNLOAD_TIME
        self.state_start_time_us: int = 0
        self.current_servo_pulse_us: int = self.SERVO_CENTER_US
        self.target_angle_rad: float = 0.0
        self.is_target_in_angle: bool = False
        self.control_enabled: bool = True

        # 内部時間カウンタ（μs）
        self._time_us: int = 0



    def reset(self):
        """状態を初期化する"""
        self.state = ControlState.UNLOAD_TIME
        self.state_start_time_us = 0
        self.current_servo_pulse_us = self.SERVO_CENTER_US
        self.target_angle_rad = 0.0
        self.is_target_in_angle = False
        self.control_enabled = True
        self._time_us = 0




    def set_target(self, angle_rad: float):
        """目標角度を設定"""
        self.target_angle_rad = angle_rad


    def _state_transition(self, new_state: int):
        self.state = new_state
        self.state_start_time_us = self._time_us



    def set_control_enabled(self, enabled: bool):
        self.control_enabled = enabled
        if not enabled:
            self._state_transition(ControlState.UNLOAD_TIME)
            self.current_servo_pulse_us = self.SERVO_CENTER_US
            self.is_target_in_angle = False




    def is_control_enabled(self) -> bool:
        return self.control_enabled




    def update(self, dt_us: int, sun_angle_rad: float, angular_velocity_rad_per_s: float) -> int:
        """制御ループ1ステップ（衛星メインループ相当）

        Args:
            dt_us: 経過時間 [μs]
            sun_angle_rad: 太陽センサから推定した角度 [rad]
            angular_velocity_rad_per_s: IMU角速度 [rad/s]

        Returns:
            サーボパルス指令 [μs]
        """
        self._time_us += dt_us
        now = self._time_us

        if not self.control_enabled:
            return self.SERVO_CENTER_US

        # 角度誤差（control.cpp と同じ符号規約）
        error = _angle_diff_rad(-self.target_angle_rad, sun_angle_rad)

        if self.state == ControlState.UNLOAD_TIME:
            self.current_servo_pulse_us = self.SERVO_CENTER_US
            if (now - self.state_start_time_us) > self.UNLOAD_MIN_DURATION_US:
                self._state_transition(ControlState.UNLOAD_ROTATE)

        elif self.state == ControlState.UNLOAD_ROTATE:
            self.current_servo_pulse_us = self.SERVO_CENTER_US
            if abs(angular_velocity_rad_per_s) < self.ANGULAR_VELOCITY_TOLERANCE_RAD_PER_S:
                self._state_transition(ControlState.STABLE)

        elif self.state == ControlState.STABLE:
            self.current_servo_pulse_us = self.SERVO_CENTER_US
            # TODO(姿勢制御): バンバン制御 (PULSE_DELTA_US 固定キック) のため目標近傍で
            #   整定せず限界サイクルに入り、|error| < ANGLE_TOLERANCE_RAD かつ STABLE が
            #   持続しない。結果 is_target_in_angle が安定して立たず、ミッション成功時の
            #   自動撮影が発火しない。修正方針は control.cpp の同 TODO を参照。
            #   実機 (satelite/firmware/control.cpp) は移植関係なので同期して修正すること。
            #   検証: ground/e2e/tests/test_target_capture.py::test_target_triggers_auto_capture
            #   が現在 xfail。修正後に xfail を外す。
            if abs(error) > self.ANGLE_TOLERANCE_RAD:
                if error < 0:
                    self.current_servo_pulse_us = self.SERVO_CENTER_US + self.PULSE_DELTA_US
                else:
                    self.current_servo_pulse_us = self.SERVO_CENTER_US - self.PULSE_DELTA_US
                self._state_transition(ControlState.LOTATE)
                self.is_target_in_angle = False
            else:
                self.is_target_in_angle = True

        elif self.state == ControlState.LOTATE:
            if (now - self.state_start_time_us) >= self.ROTATE_DURATION_US:
                self._state_transition(ControlState.UNLOAD_TIME)

        return self.current_servo_pulse_us




    def get_control_state(self) -> int:
        return self.state




    def is_control_finished(self) -> bool:
        return self.is_target_in_angle