Science/trackPatch.tl

#   Copyright (c) 2024 MBARI
#   MBARI Proprietary Information. Confidential. All Rights Reserved
#   Unauthorized copying or distribution of this file via any medium is strictly
#   prohibited.
#
#   WARNING - This file contains information whose export is restricted by the
#   Export Administration Act of 1979 (Title 50, U.S.C., App. 2401 et seq.), as
#   amended. Violations of these export laws are subject to severe civil and/or
#   criminal penalties.

mission trackPatch {
  """
  Vehicle runs yo-yos, with patch detect enabled for cholorphyll
  concentration.
  """

  arguments {
    # Vehicle safety parameters

    MinAltitude = 5 meter
      """
      Minimum altitude for the entire mission.
      """

    MaxDepth = 65 meter
      """
      Maximum depth for the entire mission.
      """

    MinWaterDepth = 10 meter
      """
      Minimum water depth for the entire mission.
      """

    MinOffshore = 2000 meter
      """
      Minimum offshore distance for the entire mission.
      """

    NeedCommsTime = 180 minute
      """
      Maximum time between surfacings for communications
      """

    MissionTimeout = 56 hour
      """
      Timeout for the entire mission
      """

    MassDefault = Control:VerticalControl.massDefault
      """
      Static setting for mass during the mission. Defaults to massDefault
      setting in the Config/Control.cfg file.
      """

    BuoyancyNeutral = Control:VerticalControl.buoyancyNeutral
      """
      Buoyancy bladder position while performing the YoYo behavior. Defaults
      to buoyancyNeutral setting in the Config/Control.cfg file.
      """

    # Basic guidance settings

    YoYoMinDepth = 5.0 meter
      """
      Minimum depth during yo-yo's.
      """

    YoYoMaxDepth = 25.0 meter
      """
      Maximum depth during yo-yo's.
      """

    YoYoUpPitch = 20.0 degree
      """
      Pitch when yo-yo's are ascending.
      """

    YoYoDownPitch = -20.0 degree
      """
      Pitch when yo-yo's are descending.
      """

    Speed = 1 meter_per_second
      """
      Vehicle speed.
      """

    WpMaxDistance = 1000 meter
      """
      Maximum length of each tracking leg
      """

    WpTimeout = 8000 second
      """
      Maximum duration of each tracking leg
      """

    WaterFrame = true
      """
      Set WaterFrame true to drive towards waypoints in the water frame of
      reference
      """

    Repeat = 20 count
      """
      Maximum number of tracking legs
      """

    # Constant settings for each tracking leg

    Turn = 90 degree
      """
      Amount to turn at each leg
      """

    Reverse = 180 degree
      """
      Change in bearing which reverses vehicle direction
      """

    TwoPi = 360 degree
      """
      Applied to stop run-up of angles
      """

    TimeWindowPeakRpt = NaN minute
      """
      If greater than zero, report a peak every window. If NaN or zero, this
      variable is ignored.
      """

    # debug
    #       </Description><Units:minute/><Value>1</Value></DefineArg>

    LowPassWindowLen = 20 count
      """
      Low-pass window length (based on depth sensor sampling interval 0.4
      second) for low-pass filtering.
      """

    PeakChlShallowBnd = 10 meter
      """
      Shallow depth bound for detecting chl peak on each descent or ascent
      profile.
      """

    # debug
    #       </Description><Units:meter/><Value>3</Value></DefineArg>

    PeakChlDeepBnd = 20 meter
      """
      Deep depth bound for detecting chl peak on each descent or ascent
      profile.
      """

    # debug
    #       </Description><Units:meter/><Value>8</Value></DefineArg>

    NumOfProfilesSlidingwindow = 10 count
      """
      Length of horizontal sliding window. The highest yoyo-wise chl peak
      (after low-pass filtering by a filter of length FilterWidthHorizontal)
      within this window.
      """

    FilterWidth = 4 count
      """
      Width of boxcar filter used in horizontal patch detection
      """

    BeginThreshold = NaN microgram_per_liter
      """
      If non-NaN, sets threshold for starting horizontal patch detection
      """

    OffPeakFraction = 80 percent
      """
      When filtered horizontal value is this fraction of the peak, consider it
      outside the patch.
      """

    PatchMode = false
      """
      If true, use center of "in peak" range as the turn-around point. If
      false use maximum horizontal peak location as the turn-around point.
      """

    PatchTrackingHorizontal = true
      """
      Set true to turn on horizontal patch tracking
      """

    # Variable Settings for each tracking leg

    WpBearing = -90 degree
      """
      Bearing being followed in the current leg. Set this to initialize start
      direction of patch tracking.
      """
  }

  output {
    PatchDetectOn = false
      """
      Mission sets this to true when the patch has been crossed.
      """

    WpDone = false
      """
      Mission sets this to true when the cross-patch waypoint has been
      reached.
      """

    PatchChl = NaN microgram_per_liter

    PatchChlDepth = NaN meter

    PatchChlLatitude = NaN degree

    PatchChlLongitude = NaN degree

    PatchChlDistance = 0 meter
  }

  # The timeout

  timeout duration=MissionTimeout

  # And now the mission

  behavior Guidance:AltitudeEnvelope {
    run in parallel

    set minAltitude = MinAltitude
  }

  behavior Guidance:DepthEnvelope {
    run in parallel

    set maxDepth = MaxDepth
  }

  insert id="SurfaceCommsSplit" Insert/Surface.tl {
    redefineArg SurfacePitch = 20 degree
    redefineArg SurfaceSpeed = 1 meter_per_second
    redefineArg UnpackSplitAtSurface = true
  }

  aggregate SurfaceComms {
    run when ( called )

    insert Insert/Surface.tl {
      redefineArg SurfacePitch = 20 degree
      redefineArg SurfaceSpeed = 1 meter_per_second
    }
  }

  # GPS Update.  Don't go too long w/o a GPS fix.

  aggregate NeedComms {
    run when (
      Universal:platform_pitch_angle > 0 degree
      and ( elapsed ( Universal:time_fix ) > NeedCommsTime )
      or ( elapsed ( Universal:time_fix ) > ( NeedCommsTime + 10 minute ) )
    )

    syslog important "NeedComms Timeout Surfacing"

    call refId="SurfaceComms"
  }

  behavior Guidance:OffshoreEnvelope {
    run in parallel

    set minOffshore = MinOffshore
  }

  behavior Guidance:WaterDepthEnvelope {
    run in parallel

    set minWaterDepth = MinWaterDepth
  }

  behavior Guidance:Pitch {
    run in parallel

    set massPosition = MassDefault
  }

  behavior Guidance:Buoyancy {
    run in parallel

    set position = BuoyancyNeutral
  }

  # If we're executing here or below, lets' get science data!

  insert Insert/Science.tl {
    redefineArg PeakDetectChlActive = true
    redefineArg PatchTracking = true
    redefineArg TimeWindowPeakReport = TimeWindowPeakRpt
    redefineArg LowPassWindowLength = LowPassWindowLen
    redefineArg PeakShallowBound = PeakChlShallowBnd
    redefineArg PeakDeepBound = PeakChlDeepBnd
    redefineArg OffPeakFractionHorizontal = OffPeakFraction
  }

  behavior Guidance:SetSpeed {
    run in parallel

    set speed = Speed
  }

  behavior Guidance:DepthEnvelope {
    run in parallel

    set minDepth = YoYoMinDepth
    set maxDepth = YoYoMaxDepth
    set downPitch = YoYoDownPitch
    set upPitch = YoYoUpPitch
  }

  behavior Guidance:YoYo {
    run in parallel

    set downPitch = YoYoDownPitch
    set upPitch = YoYoUpPitch
  }

  aggregate PatchDetectHorizontal {
    run in sequence repeat=Repeat

    aggregate PatchDetection {
      run when ( PatchDetectOn )

      behavior Trigger:PeakDetectHorizontal id="PeakDetectHorizontalTrigger" {
        run in sequence

        break if ( WpDone )

        set patchTracking = PatchTrackingHorizontal
        set detectFrom = Science:PeakChl
        set depthFrom = Science:PeakChlDepth
        set latitudeFrom = Science:PeakChlLatitude
        set longitudeFrom = Science:PeakChlLongitude
        set beginThreshold = BeginThreshold
        set offPeakFraction = OffPeakFraction
        set filterWidth = FilterWidth
        set numProfilesSlidingwindow = NumOfProfilesSlidingwindow
        set centerPeak = PatchMode
        outputArg PatchChl = peakDetect
        outputArg PatchChlDepth = peakDepth
        outputArg PatchChlLatitude = peakLatitude
        outputArg PatchChlLongitude = peakLongitude
        outputArg PatchChlDistance = peakDistance
      }

      syslog important "Fell below OffPeakFraction. Patch value is " + PatchChl~microgram_per_liter
           + " at lat = " + PatchChlLatitude~degree + ", lon = " + PatchChlLongitude~degree + ", dep = "
           + PatchChlDepth~meter + ", distance = " + PatchChlDistance~meter

      sendData service=express (
        PatchChl,
        PatchChlDepth,
        PatchChlLatitude,
        PatchChlLongitude
      )

      assign id="AssignPatchDetectOff" in sequence PatchDetectOn = false
    }

    assign id="AssignPatchDetectOn" in sequence PatchDetectOn = true

    syslog important "Next: bearing for CrossLeg: " + WpBearing~degree + ". WpMaxDistance = "
         + WpMaxDistance~meter + ". Current lat, lon = " + Universal:latitude~degree + ", "
         + Universal:longitude~degree + "."

    aggregate CrossLeg {
      """
      This is a long leg across the width of the patch. Also the first
      leg in a mission run. Two waypoints run in parallel: a short run
      to the surfacing point, and a longer run to the edge of the
      patch.
      """

      run in sequence

      break if (
        WpDone
        or ( PatchDetectOn == false )
      )

      behavior Guidance:Waypoint id="CrossSurfaceWp" {
        run in sequence

        timeout duration=WpTimeout

        set distanceDelta = PatchChlDistance
        set distanceDeltaBearing = WpBearing
        set waterFrame = WaterFrame
      }

      call refId="SurfaceComms"

      behavior Guidance:Waypoint id="CrossPeakWp" {
        run in sequence

        timeout duration=WpTimeout

        set distanceDelta = WpMaxDistance
        set distanceDeltaBearing = WpBearing
        set waterFrame = WaterFrame
      }

      syslog important "CrossPeakWp done. Lat, lon = " + Universal:latitude~degree + ", "
           + Universal:longitude~degree + ". Patch value is " + PatchChl~microgram_per_liter + " at lat = "
           + PatchChlLatitude~degree + ", lon = " + PatchChlLongitude~degree + ", dep = "
           + PatchChlDepth~meter + ", distance = " + PatchChlDistance~meter

      assign id="AssignPatchDetectOffInCrossLeg" in sequence PatchDetectOn = false

      assign id="AssignWpDoneInCrossLeg" in sequence WpDone = true
    }

    assign id="AssignWpBearingForInLeg" in sequence WpBearing = ( WpBearing + Reverse ) % TwoPi

    syslog important "Next: bearing for InLeg: " + WpBearing~degree + ". Current lat, lon = "
         + Universal:latitude~degree + ", " + Universal:longitude~degree + ". Patch value is "
         + PatchChl~microgram_per_liter + " at lat = " + PatchChlLatitude~degree + ", lon = "
         + PatchChlLongitude~degree + ", dep = " + PatchChlDepth~meter + ", distance = "
         + PatchChlDistance~meter

    behavior Guidance:Waypoint id="InLegWp" {
      """
      Simply drive back towards the patch center
      """

      run in sequence

      timeout duration=WpTimeout

      set distanceDelta = PatchChlDistance
      set distanceDeltaBearing = WpBearing
      set waterFrame = WaterFrame
    }

    assign id="AssignWpBearingForOutLeg" in sequence WpBearing = ( WpBearing - Turn ) % TwoPi

    syslog important "Next: bearing for OutLeg: " + WpBearing~degree + ". WpMaxDistance = "
         + WpMaxDistance~meter + ". Current lat, lon = " + Universal:latitude~degree + ", "
         + Universal:longitude~degree + ". Patch value is " + PatchChl~microgram_per_liter + " at lat = "
         + PatchChlLatitude~degree + ", lon = " + PatchChlLongitude~degree + ", dep = "
         + PatchChlDepth~meter + ", distance = " + PatchChlDistance~meter

    call refId="SurfaceCommsSplit"

    assign id="AssignPatchDetectOnForOutLeg" in sequence PatchDetectOn = true

    aggregate OutLeg {
      """
      Drive outward until edge of patch is detected.
      """

      run in sequence

      break if (
        WpDone
        or ( PatchDetectOn == false )
      )

      behavior Guidance:Waypoint id="OutLegWp" {
        run in sequence

        timeout duration=WpTimeout

        set distanceDelta = WpMaxDistance
        set distanceDeltaBearing = WpBearing
        set waterFrame = WaterFrame
      }

      assign id="AssignPatchDetectOffInOutLeg" in sequence PatchDetectOn = false

      assign id="AssignWpDoneInOutLeg" in sequence WpDone = true
    }

    assign id="AssignWpBearingForCrossLeg" in sequence WpBearing = ( WpBearing + Reverse ) % TwoPi
  }
}