Engineering/tritoncam_expanding_donut.tl

mission expanding_donut {
  """
  Expanding-donut mission for Triton LRAUV.
  """

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

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

    # Absolute search depth band. Owned by the frontseat and never overwritten by the backseat. Published to
    # the backseat as _.bs_min/max_depth_limit; the backseat clamps its circle sub-band inside this. 
    MinDepth = 10 meter
      """
      Minimum depth of the search yoyo envelope (absolute; frontseat-owned).
      """

    MaxDepth = 60 meter
      """
      Maximum depth of the search yoyo envelope (absolute; frontseat-owned).
      """

    # Narrowed sub-band held only while spiraling (_.InYoyoCircle>=1).
    CircleMinDepth = 10 meter
      """
      Minimum depth of the yoyo sub-band while spiraling. Set by backseat via _.CircleMinDepth.
      """

    CircleMaxDepth = 60 meter
      """
      Maximum depth of the yoyo sub-band while spiraling. Set by backseat via _.CircleMaxDepth.
      """

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

    RudderAngleDonut = 15 degree
      """
      Rudder angle applied while spiraling a hotspot. Updated by backseat via
      _.RudderCommand
      """

    DonutCheckInterval = 30 second
      """
      How often, while spiraling, to re-check whether the backseat still wants
      to circle (_.InYoyoCircle). The donut aggregate breaks out as soon as
      the backseat drops back to transit.
      """

    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 = 3 count
      """Backseat strategy id (STRATEGY_IDS): 3=expanding_donut."""

    BsSignalThreshold = 10 count
      """expanding_donut signal_threshold: per-profile peak to mark a profile "high signal"."""

    BsThreshHighSignalCounts = 1 count
      """expanding_donut thresh_high_signal_counts: consecutive high profiles -> start the spiral."""

    BsThreshLowSignalCounts = 1 count
      """expanding_donut thresh_low_signal_counts: consecutive low profiles -> back to transit."""

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

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

    BsRudderStart = 15 degree
      """expanding_donut rudder_start: tight first ring of the spiral."""

    BsRudderEnd = 3 degree
      """expanding_donut rudder_end: widest ring of the spiral."""
  }

  # --- 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 UpdateCircleMin {
    run when (
      elapsed ( customUri "_.CircleMinDepth" ) < ( elapsed ( CircleMinDepth ) )
      and ( elapsed ( CircleMinDepth ) > StateUpdateDelay )
    )
    assign in sequence CircleMinDepth = customUri "_.CircleMinDepth"
    syslog info "Backseat updated CircleMinDepth: " + CircleMinDepth~meter
  }

  aggregate UpdateCircleMax {
    run when (
      elapsed ( customUri "_.CircleMaxDepth" ) < ( elapsed ( CircleMaxDepth ) )
      and ( elapsed ( CircleMaxDepth ) > StateUpdateDelay )
    )
    assign in sequence CircleMaxDepth = customUri "_.CircleMaxDepth"
    syslog info "Backseat updated CircleMaxDepth: " + CircleMaxDepth~meter
  }

  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_signal_threshold"          = BsSignalThreshold
    assign in sequence customUri "_.bs_thresh_high_signal_counts" = BsThreshHighSignalCounts
    assign in sequence customUri "_.bs_thresh_low_signal_counts"  = BsThreshLowSignalCounts
    assign in sequence customUri "_.bs_median_window_samples"     = BsMedianWindowSamples
    assign in sequence customUri "_.bs_ma_window_seconds"         = BsMaWindowSeconds
    assign in sequence customUri "_.bs_rudder_start"              = BsRudderStart
    assign in sequence customUri "_.bs_rudder_end"                = BsRudderEnd
  }

  # The spiral: the backseat decays _.RudderCommand while _.InYoyoCircle is set
  aggregate UpdateRudder {
    run when (
      elapsed ( customUri "_.RudderCommand" ) < ( elapsed ( RudderAngleDonut ) )
      and ( elapsed ( RudderAngleDonut ) > StateUpdateDelay )
    )
    assign in sequence RudderAngleDonut = customUri "_.RudderCommand"
  }

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

  aggregate Search {
    run while ( customUri "_.InYoyoCircle" < 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
    }
  }

  aggregate CircleBand {
    run while ( customUri "_.InYoyoCircle" >= 1 count )

    behavior Guidance:DepthEnvelope {
      run in parallel
      set minDepth = CircleMinDepth
      set maxDepth = CircleMaxDepth
      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; spiral outward when the backseat asks ---

  aggregate Lap {
    run in sequence repeat = Repeat

    macro $i = 1..7 {
      aggregate Wpt$i {
        run in sequence
        break if ( customUri "_.InYoyoCircle" >= 1 count )

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

    aggregate ExpandingDonut {
      run in sequence repeat = 10000
      break if ( customUri "_.InYoyoCircle" < 1 count )

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

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