Science/sci2_slowyo_test.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.

#! TethysL version initially based on automated translation from original Science/sci2.xml
#! and then (syntactically) simplified with use of array/macro extensions in TethysL.

mission sci2_slowyo_test {
  """
  Vehicle yo-yo's to the specified waypoints, with science turned on.
  """

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

    NeedCommsTime = 60 minute
      """
      How often to surface for communications
      """

    NeedCommsSurfaceDepthRate = 0.3 meter_per_second
      """
      How quickly to ascend for comms
      """

    Repeat = 1 count
      """
      Number of times the vehicle should try to cycle through waypoints..
      """

    Lat[1..7] = NaN degree
      """
      Latitude of waypoint {$}. If nan, waypoint is skipped.
      """

    Lon[1..7] = NaN degree
      """
      Longitude of waypoint {$}. If nan, waypoint is skipped.
      """

    ApproachDepthTimeout = 15 minute
      """
      Maximum time duration for the vehicle to reach the target depth
      """

    SlowYoDiveAngle = 15 degree
      """
      Dive Angle while performing SlowYo
      """

    Speed = 1.0 meter_per_second
      """
      Speed while performing the YoYo behavior.
      """

    SlowYoMinDepth = 10 meter
      """
      Min Depth while performing SlowYo
      """

    SlowYoMaxDepth = 40 meter
      """
      Max Depth while performing SlowYo
      """

    SlowYoNumYoYos = 1 count
      """
      Min number of SlowYos for success
      """

    SlowYoTurningWindow = 10 meter
      """
      Distance from min or max depth to start a smooth transition
      """

    SinkOffSurface = false
      """
      If true, use buoyancy only to decend to SinkDepth before driving to first target depth
      """

    SinkDepth = 10 meter
      """
      Initial depth to decend using only buoyancy before driving
      """

    SinkDepthTimeout = 10 minute
      """
      Max wait time for the vehicle to reach the target depth
      """

    CaptureRadius = NaN meter
      """
      Acceptable distance from waypoint.
      """

    PeakDetectChl = false
      """
      Whether to enable peak chl detection on each yo-yo profile
      """

    ShallowBoundChl = NaN meter
      """
      Used only when PeakDetectChl is true. Shallow depth bound for peak chl detection
      """

    DeepBoundChl = NaN meter
      """
      Used only when PeakDetectChl is true. Deep depth bound for peak chl detection
      """

    ChlThreshTrigger = false
      """
      Whether to enable chl threshold triggering
      """

    ChlLowerThreshold = NaN microgram_per_liter
      """
      Used only when ChlThreshTrigger is true. Triggered when peak chl values on NumProfilesThreshold consecutive yo-yo profiles fall in [ChllowerThreshold ChlUpperThreshold]. ChllowerThreshold is ignored if NaN.
      """

    ChlUpperThreshold = NaN microgram_per_liter
      """
      Used only when ChlThreshTrigger is true. Triggered when peak chl values on NumProfilesThreshold consecutive yo-yo profiles fall in [ChllowerThreshold ChlUpperThreshold]. ChlUpperThreshold is ignored if NaN.
      """

    NumProfilesThreshold = 5 count
      """
      Used only when ChlThreshTrigger is true. Triggered when peak chl values on NumProfilesThreshold consecutive yo-yo profiles fall in [ChllowerThreshold ChlUpperThreshold]. ChlUpperThreshold is ignored if NaN.
      """

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

    ElevatorDefault = 0 degree
      """
      Shuts off the elevators.
      """

    MassDefault = Control:VerticalControl.massDefault
      """
      Static setting for mass during the mission. Set to NaN mm for active
      mass position.
      """

    MinAltitude = 7 meter
      """
      Minimum height above the sea floor for the entire mission.
      """

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

    MinOffshore = 2 kilometer
      """
      Minimum offshore distance for the entire mission.
      """
  }

  output {
    DiveMode = 0 count

    DoingComms = 0 count

    SurfaceSink = 1 count

    InitialDive = 2 count

    SlowYo = 3 count
  }

  timeout duration=MissionTimeout

  insert id="Science" Insert/Science.tl {
    """
    Most missions will run the science sensors. If you don't place this
    aggregate above NeedComms, science instruments get turned off on the
    last upcast and while floating on the surface.
    """

    redefineArg PeakDetectChlActive = PeakDetectChl
    redefineArg PeakShallowBound = ShallowBoundChl
    redefineArg PeakDeepBound = DeepBoundChl
    redefineArg ChlThreshTriggerActive = ChlThreshTrigger
    redefineArg ChlLowerThresh = ChlLowerThreshold
    redefineArg ChlUpperThresh = ChlUpperThreshold
    redefineArg NumProfilesThresh = NumProfilesThreshold
  }

  insert id="NeedComms" Insert/NeedComms.tl

  assign in sequence NeedComms:SurfaceDepthRate = NeedCommsSurfaceDepthRate

  insert Insert/StandardEnvelopes.tl

  assign in sequence StandardEnvelopes:MinAltitude = MinAltitude

  assign in sequence StandardEnvelopes:MaxDepth = MaxDepth

  assign in sequence StandardEnvelopes:MinOffshore = MinOffshore

  insert Insert/BackseatDriver.tl

  insert Insert/PowerOnly.tl

  aggregate SurfaceComms {
    run when (
      called
      or ( elapsed ( Universal:time_fix ) > NeedCommsTime )
    )

    assign in sequence DiveMode = DoingComms

    call refId="NeedComms"

    aggregate setInitial {
      run in sequence

      break if ( SinkOffSurface )

      assign in sequence DiveMode = InitialDive
    }

    aggregate setSink {
      run in sequence

      break if ( not SinkOffSurface )

      assign in sequence DiveMode = SurfaceSink
    }
  }

  aggregate DiveCmd {
    run when ( called )

    aggregate SurfaceSink {
      run while ( DiveMode == SurfaceSink )

      aggregate sink {
        run in sequence

        behavior Guidance:Buoyancy id="BuoyancyHold" {
          run in parallel

          set position = NaN cc
        }

        behavior Guidance:SetSpeed {
          run in parallel

          set speed = 0 m/s
        }

        syslog info "Sinking to " + SinkDepth~meter

        behavior Guidance:Pitch {
          run in sequence

          timeout duration=SinkDepthTimeout {
            syslog important "Timed out while trying to reach target sink depth at current depth of "
                 + Universal:depth~meter
          }

          set depth = SinkDepth
        }

        # Skip initial dive if we're already at start-of-slowyo depth
        aggregate sinkToInit {
          run in sequence

          break if ( Universal:depth >= SlowYoMinDepth )

          assign in sequence DiveMode = InitialDive
        }

        aggregate sinkToYo {
          run in sequence

          break if ( Universal:depth < SlowYoMinDepth )

          assign in sequence DiveMode = SlowYo
        }
      }
    }

    aggregate InitialDive {
      """
      Descend from the surface to target depth, limit pitch and
      depth-rate.
      """

      run while ( DiveMode == InitialDive )

      aggregate dive {
        run in sequence

        behavior Guidance:Mass {
          run in parallel

          set position = MassDefault
        }

        behavior Guidance:Buoyancy {
          run in parallel

          set position = BuoyancyNeutral
        }

        syslog info "Waiting for 1 minute while pumping down to neutral."

        behavior Guidance:Wait {
          """
          Take a few moments to ensure the VBS reaches neutral.
          """

          run in sequence

          set duration = 1 minute
        }

        syslog "Moving to " + SlowYoMinDepth~meter

        aggregate ApproachDepth {
          run in sequence

          behavior Guidance:SetSpeed {
            run in parallel

            set speed = Speed
          }

          behavior Guidance:Pitch {
            run in sequence

            timeout duration=ApproachDepthTimeout {
              syslog important "Timed out while trying to reach target depth at current depth of "
                   + Universal:depth~meter

              behavior Guidance:Execute {
                run in sequence

                set command = "stop"
              }
            }

            set depth = SlowYoMinDepth
          }
        }

        assign in sequence DiveMode = SlowYo
      }
    }

    aggregate SlowYoToWaypoint {
      run while ( DiveMode == SlowYo )

      aggregate SlowYoTask {
        run in parallel

        behavior Guidance:Buoyancy {
          run in parallel

          set position = BuoyancyNeutral
        }

        behavior Guidance:Pitch {
          run in parallel

          set elevatorAngle = ElevatorDefault
        }

        behavior Guidance:SetSpeed {
          run in parallel

          set speed = Speed
        }

        aggregate SlowYoLoop {
          run in sequence repeat=Repeat

          behavior Guidance:SlowYo {
            run in sequence

            set diveAngle = SlowYoDiveAngle
            set minDepth = SlowYoMinDepth
            set maxDepth = SlowYoMaxDepth
            set turningWindow = SlowYoTurningWindow
            set numYoYos = SlowYoNumYoYos
          }
        }
      }
    }
  }

  aggregate StopIfTriggered {
    """
    If peak chlorophyll values on NumProfilesThresh consecutive yo-yo profiles fall in [ChlLowerThreshold ChlUpperThreshold], stop mission.
    """

    run when ( Science:TriggeredOnChl )

    behavior Guidance:Execute {
      run in sequence

      set command = "stop"
    }
  }

  call id="StartingMission" refId="SurfaceComms"

  aggregate Lap {
    run in sequence repeat=Repeat

    aggregate Dive {
      run while ( not ( DiveMode == DoingComms ) )

      call refId="DiveCmd"
    }

    aggregate Wpt1 {
      run in sequence

      behavior Guidance:Waypoint {
        run in sequence

        set latitude = Lat[1]
        set longitude = Lon[1]
        set captureRadius = CaptureRadius
      }
    }

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

        behavior Guidance:Waypoint {
          run in sequence

          set latitude = Lat[$i]
          set longitude = Lon[$i]
        }
      }
    }
  }
}