Science/profile_station.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 profile_station {
  """
  This mission yoyos in a circle around a specified location.
  """

  arguments {
    # Mission-wide settings

    MissionTimeout = 4 hour
      """
      Maximum duration of mission
      """

    NeedCommsTime = 60 minute
      """
      Elapsed time after previous surface communications when vehicle will
      begin to ascend for additional surface communications
      """

    # You probably need to change these.

    Lat = 36.806966 degree
      """
      The latitude of the center of the circle.
      """

    Lon = -121.824326 degree
      """
      The longitude of the center of the circle.
      """

    Radius = 300 meter
      """
      Radius to circle at
      """

    # You probably do not need to change these.

    YoYoMinDepth = 2 meter
      """
      Minimum depth while performing the YoYo behavior.
      """

    YoYoMaxDepth = 200 meter
      """
      Maximum depth while performing the YoYo behavior.
      """

    YoYoMinAltitude = 7 meter
      """
      Minimum altitude while performing the YoYo behavior (for
      bottom-terminated YoYos).
      """

    YoYoPitch = 20 degree
      """
      Pitch (plus and minus) for yo-yo behavior.
      """

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

    SurfaceDiveSpeed = NaN meter_per_second
      """
      Vehicle speed to get off of surface, same as speed if skipped
      """

    SurfaceThreshold = Control:VerticalControl.surfaceThreshold
      """
      Depth at which to return to normal speed, skipped if SurfaceDiveSpeed NaN
      """

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

    # You are even less likely to need to change these.

    CircleMaxError = 100 meter
      """
      If this distance away from the circle, drive straight towards (or away
      from the center). Otherwise, try to reduce distance from the ideal
      circle.
      """

    CircleTurnToPort = false
      """
      If true, vehicle turns to the left around the center point. If false,
      vehicle turns to the right.
      """

    KwpHeading = 0.010 radian_per_meter
      """
      Used to relax waypoint cross-track error constant that is adjusted for
      docking. (You can override this setting by passing an argument.)
      """

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

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

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

  output {
    DiveMode = 0 count
      """
      Mission variable (don't change). The mission sets this variable to
      switch between dive modes. Initialized to DoingComms.
      """

    DoingComms = 0 count
      """
      Mission variable (don't change). The mission will run in this mode
      when doing surface comms.
      """

    SurfaceSink = 1 count
      """
      Mission variable (don't change). The mission will run in this mode
      when sinking from the surface using only buoyancy.
      """

    YoYo = 2 count
      """
      Mission variable (don't change). The mission will run in this mode
      when performing the profile.
      """
  }

  # Missions should almost always start with a timeout

  timeout duration=MissionTimeout

  insert 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 = true
  }

  # Most missions should use the NeedComms aggregate.

  insert id="NeedComms" Insert/NeedComms.tl

  assign in sequence NeedComms:DiveInterval = NeedCommsTime

  # Missions should almost always start with standard safety envelopes; most missions should not expose the parameters of these envelopes.

  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

  # Check for additional instructions first.

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

    assign in sequence DiveMode = DoingComms

    call priorityHere=false refId="NeedComms"

    aggregate setInitial {
      run in sequence

      break if ( SinkOffSurface )

      assign in sequence DiveMode = YoYo
    }

    aggregate setSink {
      run in sequence

      break if ( not SinkOffSurface )

      assign in sequence DiveMode = SurfaceSink
    }
  }

  call id="StartingMission" priorityHere=false refId="SurfaceComms"

  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
        }

        assign in sequence DiveMode = YoYo
      }
    }

    aggregate YoYoDive {
      run while ( DiveMode == YoYo )

      aggregate dive {
        run in sequence

        # Many missions will keep mass position and buoyancy volume fixed at defaults.

        behavior Guidance:Pitch {
          run in parallel

          set massPosition = Control:VerticalControl.massDefault
        }

        behavior Guidance:Buoyancy {
          run in parallel

          set position = Control:VerticalControl.buoyancyNeutral
        }

        aggregate SetSpeed {
          run in parallel

          aggregate SurfaceDive {
            run while (
              not isNaN ( SurfaceDiveSpeed )
              and Universal:depth <= SurfaceThreshold
            )

            aggregate SurfaceSpeed {
              run in parallel

              behavior Guidance:SetSpeed {
                run in parallel

                set speed = SurfaceDiveSpeed
              }
            }
          }

          aggregate ProfileSpeed {
            run while (
              isNaN ( SurfaceDiveSpeed )
              or Universal:depth > Control:VerticalControl.surfaceThreshold
            )

            behavior Guidance:SetSpeed {
              run in parallel

              set speed = Speed
            }
          }
        }

        behavior Guidance:DepthEnvelope {
          """
          Another depth envelope for the YoYo behavior. This envelope should
          fall within the limits of the standard safety envelopes in
          Insert/StandardEnvelopes.tl in order to avoid commanding high pitch
          angles for depth-terminated YoYos.
          """

          run in parallel

          set minDepth = YoYoMinDepth
          set maxDepth = YoYoMaxDepth
          set pitch = YoYoPitch
        }

        behavior Guidance:AltitudeEnvelope {
          """
          Another altitude envelope for the YoYo behavior. This envelope
          should fall within the limits of the standard safety envelopes in
          Insert/StandardEnvelopes.tl in order to avoid commanding high pitch
          angles for bottom-terminated YoYos.
          """

          run in parallel

          set minAltitude = YoYoMinAltitude
          set pitch = YoYoPitch
        }

        behavior Guidance:YoYo {
          run in parallel

          set pitch = YoYoPitch
        }
      }
    }
  }

  aggregate CircleWrapper {
    run in sequence repeat=4096

    assign in parallel Control:HorizontalControl.kwpHeading = KwpHeading

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

      call priorityHere=false refId="DiveCmd"
    }

    behavior Guidance:Circle {
      run in sequence

      set latitude = Lat
      set longitude = Lon
      set radius = Radius
      set maxError = CircleMaxError
      set turnToPort = CircleTurnToPort
    }
  }
}