#!/usr/bin/env python3 """Pure safety state for direct TS1P right-stick head-pitch control.""" from __future__ import annotations import math from typing import Any, Mapping def right_stick_vertical(data: Mapping[str, Any]) -> float | None: """Return the TS1P right-stick vertical axis or fail closed.""" try: joysticks = data["joystick"] if not isinstance(joysticks, Mapping): return None right = joysticks["right"] if not isinstance(right, (list, tuple)) or len(right) != 2: return None value = float(right[0]) if not math.isfinite(value) or abs(value) > 1.2: return None return max(-1.0, min(1.0, value)) except (KeyError, TypeError, ValueError): return None def left_z_pressed(data: Mapping[str, Any]) -> bool | None: """Return the physical left Z state, accepting only explicit binary data.""" try: buttons = data["button"] if not isinstance(buttons, Mapping): return None left = buttons["left"] if not isinstance(left, (list, tuple)) or len(left) < 3: return None raw = left[2] if isinstance(raw, bool): return raw if isinstance(raw, int) and not isinstance(raw, bool) and raw in (0, 1): return bool(raw) return None except (KeyError, TypeError): return None def head_pitch_command_axis(data: Mapping[str, Any]) -> float | None: """Gate right-stick pitch behind left Z; malformed input fails closed.""" pressed = left_z_pressed(data) axis = right_stick_vertical(data) if pressed is None or axis is None: return None return axis if pressed else 0.0 class HeadPitchController: """Integrate a joystick velocity request into a bounded pitch target.""" def __init__( self, *, deadzone: float, max_speed_rad_s: float, max_accel_rad_s2: float, min_pitch_rad: float, max_pitch_rad: float, axis_sign: float, max_dt_s: float = 0.1, ) -> None: values = ( deadzone, max_speed_rad_s, max_accel_rad_s2, min_pitch_rad, max_pitch_rad, axis_sign, max_dt_s, ) if not all(math.isfinite(value) for value in values): raise ValueError("head-pitch settings must be finite") if not 0.0 <= deadzone < 1.0: raise ValueError("head-pitch deadzone must be in [0, 1)") if max_speed_rad_s <= 0.0 or max_accel_rad_s2 <= 0.0: raise ValueError("head-pitch speed and acceleration must be positive") if min_pitch_rad >= max_pitch_rad: raise ValueError("head-pitch range is invalid") if axis_sign == 0.0 or max_dt_s <= 0.0: raise ValueError("head-pitch sign and maximum dt must be non-zero") self.deadzone = float(deadzone) self.max_speed_rad_s = float(max_speed_rad_s) self.max_accel_rad_s2 = float(max_accel_rad_s2) self.min_pitch_rad = float(min_pitch_rad) self.max_pitch_rad = float(max_pitch_rad) self.axis_sign = math.copysign(1.0, axis_sign) self.max_dt_s = float(max_dt_s) self.armed = False self.require_neutral = True self.active = False self.target_rad: float | None = None self.velocity_rad_s = 0.0 self.last_update_at: float | None = None self.last_transition = "initialized" def new_session(self) -> None: self.armed = True self.require_neutral = True self.active = False self.target_rad = None self.velocity_rad_s = 0.0 self.last_update_at = None self.last_transition = "new_session" def disarm(self, reason: str = "disarmed") -> None: self.armed = False self.require_neutral = True self.active = False self.target_rad = None self.velocity_rad_s = 0.0 self.last_update_at = None self.last_transition = reason def _shaped_axis(self, axis: float) -> float: magnitude = abs(axis) if magnitude <= self.deadzone: return 0.0 normalized = (magnitude - self.deadzone) / (1.0 - self.deadzone) return math.copysign(normalized, axis) def update( self, now: float, *, armed: bool, input_healthy: bool, feedback_healthy: bool, external_busy: bool, axis: float | None, feedback_position_rad: float | None, ) -> float | None: """Return one absolute pitch target, or ``None`` when output is gated.""" if not math.isfinite(now): raise ValueError("head-pitch clock must be finite") if not armed: if self.armed: self.disarm() return None if not self.armed: self.new_session() feedback_valid = ( feedback_position_rad is not None and math.isfinite(float(feedback_position_rad)) ) if ( not input_healthy or not feedback_healthy or external_busy or axis is None or not feedback_valid ): self.require_neutral = True self.active = False self.target_rad = None self.velocity_rad_s = 0.0 self.last_update_at = now if external_busy: self.last_transition = "external_command_busy" elif not feedback_healthy or not feedback_valid: self.last_transition = "feedback_unhealthy" else: self.last_transition = "input_unhealthy" return None feedback = float(feedback_position_rad) shaped = self._shaped_axis(float(axis)) if self.require_neutral: self.active = False self.target_rad = feedback self.velocity_rad_s = 0.0 self.last_update_at = now if shaped == 0.0: self.require_neutral = False self.last_transition = "neutral_ready" return None if shaped == 0.0: self.active = False self.target_rad = feedback self.velocity_rad_s = 0.0 self.last_update_at = now self.last_transition = "neutral" return None if self.target_rad is None: self.target_rad = feedback dt = 0.0 if self.last_update_at is not None: dt = min(self.max_dt_s, max(0.0, now - self.last_update_at)) self.last_update_at = now desired_velocity = shaped * self.axis_sign * self.max_speed_rad_s max_velocity_step = self.max_accel_rad_s2 * dt self.velocity_rad_s += max( -max_velocity_step, min(max_velocity_step, desired_velocity - self.velocity_rad_s), ) self.target_rad = max( self.min_pitch_rad, min( self.max_pitch_rad, self.target_rad + self.velocity_rad_s * dt, ), ) self.active = True self.last_transition = "active" return self.target_rad __all__ = [ "HeadPitchController", "head_pitch_command_axis", "left_z_pressed", "right_stick_vertical", ]