Engineering/tritoncam_adaptive_yoyo.tl

mission adaptive_yoyo {
  """
  Adaptive YoYo mission for Triton LRAUV.
  """

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

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

    MinDepth = 10 meter
      """
      Minimum depth of the yoyo envelope. 
      """

    MaxDepth = 60 meter
      """
      Maximum depth of the yoyo envelope. 
      """

    Speed = 1 meter_per_second
      """
      Vehicle speed. Updated by backseat via _.speed_command.
      """

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

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

    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.
      """

    BuoyancyHold = true
      """
      Set to True to hold buoyancy at neutral position.
      """

    # --- Backseat mission-driven config ---
    # Published to the slate as _.bs_* for the backseat to read at startup.
    PublishPeriod = 2 second
      """
      Re-publish period for the _.bs_* config below, so a backseat that starts
      after the mission still reads it.
      """

    Strategy = 0 count
      """
      Backseat strategy id (STRATEGY_IDS): 0=adaptive_yoyo.
      """

    BsMinPeakThreshold = 1 count
      """
      adaptive_yoyo min_peak_threshold: noise floor the peak must exceed before flip engages
      """

    BsPeakFraction = 0.5 count
      """
      adaptive_yoyo peak_fraction: flip when the smoothed signal drops below
      this x the tracked peak
      """

    BsMedianWindowSamples = 1 count
      """
      adaptive_yoyo median_window_samples: median-filter length.
      """

    BsMaWindowSeconds = 2 second
      """
      adaptive_yoyo ma_window_seconds: moving-average window.
      """
  }

  # --- 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 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 ---
  # Requires 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_min_peak_threshold"    = BsMinPeakThreshold
    assign in sequence customUri "_.bs_peak_fraction"         = BsPeakFraction
    assign in sequence customUri "_.bs_median_window_samples" = BsMedianWindowSamples
    assign in sequence customUri "_.bs_ma_window_seconds"     = BsMaWindowSeconds
  }

  # --- Science & Backseat Payload ---
  # Science is for logging only — keep frontseat edge-detection OFF (it defaults off); the backseat owns the adaptive flip via _.GoDown.
  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 ---
  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
  }

  # Top-level yoyo pitch + altitude limits
  behavior Guidance:DepthEnvelope {
    run in parallel
    set downPitch = YoYoDownPitch
    set upPitch = YoYoUpPitch
  }

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

  # --- 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

  # --- YoYo while transiting waypoints ---
  aggregate Lap {
    run in sequence repeat = Repeat

    macro $i = 1..7 {
      aggregate Wpt$i {
        run in sequence

        behavior Guidance:DepthEnvelope {
          run in parallel
          set minDepth = MinDepth
          set maxDepth = MaxDepth
        }

        behavior Guidance:Pitch {
          run while ( customUri "_.GoDown" >= 1 count )
          set pitch = YoYoDownPitch
        }

        behavior Guidance:Pitch {
          run while ( customUri "_.GoDown" < 1 count )
          set pitch = YoYoUpPitch
        }

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