Engineering/tritoncam_hockey_stop.tl

mission hockey_stop {
  """
  Hockey-stop mission for Triton LRAUV.

  Companion frontseat mission for the adaptive backseat running the
  `hockey_stop` strategy. The frontseat owns the absolute [MinDepth, MaxDepth]
  safety band and yo-yos it while searching (_.InLeg==0). When the backseat
  detects a target it raises _.InLeg and publishes _.LegDepth (the detection
  depth, clamped inside the safety band); the Leg aggregate holds that depth and
  drives _.speed_command (negative during the hockey stop, positive during the
  leg). The absolute band is never overwritten, so search resumes on leg exit.
  """

  arguments {
    MissionTimeout = 2 hour
      """
      Maximum duration of mission
      """

    NeedCommsTime = 1 hour
      """
      How often to surface for communications
      """

    MinDepth = 10 meter
      """
      Minimum depth of the (fixed) yoyo search envelope.
      """

    MaxDepth = 60 meter
      """
      Maximum depth of the (fixed) yoyo search envelope.
      """

    Speed = 1 meter_per_second
      """
      Vehicle speed. Updated by backseat via _.speed_command (negative during the
      hockey stop, positive during the leg).
      """

    YoYoDownPitch = -15 degree
      """
      Vehicle pitch while diving (negative = nose down).
      """

    YoYoUpPitch = 15 degree
      """
      Vehicle pitch while climbing (positive = nose up).
      """

    LegRudder = 15 degree
      """
      Rudder angle applied during the leg (and hockey stop). The turn
      magnitude is decided here on the frontseat, not by the backseat.
      """

    LegDepth = 50 meter
      """
      Depth held during the leg. Updated by the backseat via _.LegDepth (the
      depth at which the target was detected).
      """

    LegCheckInterval = 15 second
      """
      How often, while running the leg, to re-check whether the backseat still
      wants it (_.InLeg). The leg breaks out as soon as the backseat lowers the
      flag.
      """

    StateUpdateDelay = 1 second
      """
      Minimum time delay between state updates to avoid rapid changes.
      """

    MinAltitude = 5 meter
    MaxSafeDepth = 100 meter
    MinOffshore = 1.5 kilometer

    Repeat = 10 count
      """
      Number of times to cycle through the waypoint list.
      """

    Lat[1..7] = NaN degree
      """
      Latitude of waypoint {$}. NaN entries are skipped. The vehicle yo-yos
      while transiting to each waypoint, so at least one is required.
      """

    Lon[1..7] = NaN degree
      """
      Longitude of waypoint {$}. NaN entries are skipped.
      """

    MassHold = true
      """
      Set to True to hold mass at default position.
      """

    # --- Backseat mission-driven config ---
    # published to the slate as _.bs_* for the backseat to read once at startup
    PublishPeriod = 2 second
      """Re-publish period for the _.bs_* config below."""

    Strategy = 2 count
      """Backseat strategy id (STRATEGY_IDS): 2=hockey_stop."""

    BsDetectionThreshold = 1 count
      """hockey_stop detection_threshold: cumulative counts to trigger a hockey-stop."""

    BsDetectionMaxGapSec = 30 second
      """hockey_stop detection_max_gap_sec: max quiet gap before the detection cluster resets."""

    BsPostDetectionPattern = 1 count
      """hockey_stop post_detection_pattern: 0=straight, 1=circle, 2=spiral."""

    BsLegTimeoutNoDetectionSec = 30 second
      """hockey_stop leg_timeout_no_detection_sec: exit the leg after this long with no detection."""

    BsSpiralGrowthMinutes = 5 count
      """hockey_stop spiral_growth_minutes (raw minutes): spiral rudder decay window."""

    BsMaxLegMinutes = 0 count
      """hockey_stop max_leg_minutes (raw minutes): hard cap on the leg; 0 = no cap."""
  }

  # --- Mission Start ---
  timeout duration=MissionTimeout

  insert id="NeedComms" Insert/NeedComms.tl
  assign in sequence NeedComms:DiveInterval = NeedCommsTime

  insert Insert/StandardEnvelopes.tl
  assign in sequence StandardEnvelopes:MinAltitude = MinAltitude
  assign in sequence StandardEnvelopes:MaxDepth = MaxSafeDepth
  assign in sequence StandardEnvelopes:MinOffshore = MinOffshore

  # --- Backseat Parameter Updates ---
  aggregate UpdateLegDepth {
    run when (
      elapsed ( customUri "_.LegDepth" ) < ( elapsed ( LegDepth ) )
      and ( elapsed ( LegDepth ) > StateUpdateDelay )
    )
    assign in sequence LegDepth = customUri "_.LegDepth"
  }

  aggregate UpdateSpeed {
    run when (
      elapsed ( customUri "_.speed_command" ) < ( elapsed ( Speed ) )
      and (elapsed ( Speed ) > StateUpdateDelay)
    )
    assign in sequence Speed = customUri "_.speed_command"
  }

  # --- Publish mission-driven config to the backseat (read once at startup) ---
  # Needs BackseatComponent. fullSlateAccess=1 on the frontseat.
  aggregate PublishBackseatParams {
    run when ( elapsed ( customUri "_.bs_strategy" ) > PublishPeriod )

    assign in sequence customUri "_.bs_strategy"                     = Strategy
    assign in sequence customUri "_.bs_min_depth_limit"              = MinDepth
    assign in sequence customUri "_.bs_max_depth_limit"              = MaxDepth
    assign in sequence customUri "_.bs_detection_threshold"          = BsDetectionThreshold
    assign in sequence customUri "_.bs_detection_max_gap_sec"        = BsDetectionMaxGapSec
    assign in sequence customUri "_.bs_post_detection_pattern"       = BsPostDetectionPattern
    assign in sequence customUri "_.bs_leg_timeout_no_detection_sec" = BsLegTimeoutNoDetectionSec
    assign in sequence customUri "_.bs_spiral_growth_minutes"        = BsSpiralGrowthMinutes
    assign in sequence customUri "_.bs_max_leg_minutes"              = BsMaxLegMinutes
  }

  # --- Science & Backseat Payload ---
  insert id="Science" Insert/Science.tl

  insert Insert/BackseatDriver.tl
  assign in sequence BackseatDriver:EnableBackseat = true

  insert Insert/PowerOnly.tl

  behavior Guidance:BackseatDriver {
    run in parallel
  }

  # --- Check for comms before diving ---
  call id="StartingMission" priorityHere=false refId="NeedComms"

  # --- Vehicle Control (mirrors sci2_peak_layer_yoyo / adaptive_yoyo) ---
  behavior Guidance:Buoyancy {
    run in parallel
    set position = Control:VerticalControl.buoyancyNeutral
  }

  behavior Guidance:Pitch {
    run while ( MassHold )
    set massPosition = Control:VerticalControl.massDefault
  }

  behavior Guidance:SetSpeed {
    run in parallel
    set speed = Speed
  }

  behavior Guidance:AltitudeEnvelope {
    run in parallel
    set minAltitude = MinAltitude
  }

  # Yo-yo the fixed search band while not in a leg (_.InLeg==0). When the backseat
  # raises the flag, this deactivates and the Leg aggregate below holds LegDepth.
  aggregate Search {
    run while ( customUri "_.InLeg" < 1 count )

    behavior Guidance:DepthEnvelope {
      run in parallel
      set minDepth = MinDepth
      set maxDepth = MaxDepth
      set downPitch = YoYoDownPitch
      set upPitch = YoYoUpPitch
    }

    behavior Guidance:YoYo {
      run in parallel
      set downPitch = YoYoDownPitch
      set upPitch = YoYoUpPitch
    }
  }

  # --- Waypoints come from the launch-time Lat[1..7]/Lon[1..7] arguments
  #     (NaN = skipped by Guidance:Waypoint). Set them when loading the mission.
  #     Example test triangle near the deploy site (uncomment to hardcode):
  # assign in sequence Lat[1] = 36.812 degree
  # assign in sequence Lon[1] = -121.808 degree
  # assign in sequence Lat[2] = 36.807 degree
  # assign in sequence Lon[2] = -121.814 degree
  # assign in sequence Lat[3] = 36.802 degree
  # assign in sequence Lon[3] = -121.808 degree

  # --- Transit the waypoint list; run a leg when the backseat raises _.InLeg
  #     (detection -> hockey stop -> leg) ---

  aggregate Lap {
    run in sequence repeat = Repeat

    macro $i = 1..7 {
      aggregate Wpt$i {
        run in sequence
        # Backseat raised the leg flag -> leave waypoint transit for the leg.
        break if ( customUri "_.InLeg" >= 1 count )

        behavior Guidance:Waypoint {
          run in sequence
          set latitude = Lat[$i]
          set longitude = Lon[$i]
        }
      }
    }

    aggregate Leg {
      run in sequence repeat = 10000
      # Backseat lowered the flag (no detection / timeout) -> resume transit.
      break if ( customUri "_.InLeg" < 1 count )

      # Hold the detection depth (Search's yoyo is inactive while InLeg>=1).
      behavior Guidance:Pitch {
        run in parallel
        set depth = LegDepth
      }

      behavior Guidance:Point {
        run in parallel
        set rudderAngle = LegRudder
      }

      behavior Guidance:Wait {
        run in sequence
        set duration = LegCheckInterval
      }
    }
  }
}