Transport/transit_sink.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 transit_sink {
  """
  Vehicle dives immediately with no comms/GPS and transits to desired waypoint and can be commanded to use/hold
  mass or buoyancy. When SinkOffSurface is true, the vehicle descends to SinkDepth using buoyancy only before
  driving, and after each NeedComms interval.
  """

  arguments {
    MissionTimeout = 60 minute
      """
      Maximum duration of mission
      """

    NeedCommsTime = 30 minute
      """
      How often to surface for commumications
      """

    Latitude = NaN degree
      """
      Latitude of waypoint to seek. If set to NaN, uses latitude at mission
      initialization.
      """

    Longitude = NaN degree
      """
      Longitude of waypoint to seek. If set to NaN, uses longitude at mission
      initialization.
      """

    Northings = 0 meter
      """
      Northward distance of the waypoint to seek. If combined with Latitude
      and/or Longitude, this is an offset from the specified Latitude and/or
      Longitude.
      """

    Eastings = 0 meter
      """
      Eastward distance of the waypoint to seek. If combined with Latitude
      and/or Longitude, this is an offset from the specified Latitude and/or
      Longitude.
      """

    Depth = 10 meter
      """
      Depth of flat and level flight during the mission.
      """

    Speed = 1 meter_per_second
      """
      Speed of vehicle (relative to water) during the mission.
      """

    SinkOffSurface = false
      """
      If true, use buoyancy only to decend to SinkDepth before driving to the
      commanded Depth. Also applies after each NeedComms interval.
      """

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

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

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

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

    MassHold = true
      """
      Set to True in order to hold mass at default position, False to allow
      mass to run on its own.
      """

    BuoyancyHold = true
      """
      Set to True in order to hold buoyancy at neutral position, False to
      allow buoyancy to run on its own.
      """
  }

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

    Cruise = 2 count
      """
      Mission variable (don't change). The mission will run in this mode
      when driving to the waypoint at commanded Depth.
      """
  }

  timeout duration=MissionTimeout

  insert id="NeedComms" Insert/NeedComms.tl

  assign in sequence NeedComms:DiveInterval = NeedCommsTime

  assign in sequence NeedComms:WaitForPitchUp = 1 second

  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

  # Wraps NeedComms to keep DiveMode in sync: comms goes to DoingComms, then
  # SurfaceSink or Cruise afterward depending on SinkOffSurface.

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

    assign in sequence DiveMode = DoingComms

    call priorityHere=false refId="NeedComms"

    aggregate setCruise {
      run in sequence

      break if ( SinkOffSurface )

      assign in sequence DiveMode = Cruise
    }

    aggregate setSink {
      run in sequence

      break if ( not SinkOffSurface )

      assign in sequence DiveMode = SurfaceSink
    }
  }

  # Bootstrap DiveMode at mission start. Transit skips initial comms (unlike
  # keepstation), so we can't rely on SurfaceComms to seed the mode; set it
  # explicitly based on SinkOffSurface.

  aggregate initCruise {
    run in sequence

    break if ( SinkOffSurface )

    assign in sequence DiveMode = Cruise
  }

  aggregate initSink {
    run in sequence

    break if ( not SinkOffSurface )

    assign in sequence DiveMode = SurfaceSink
  }

  # Persistent buoyancy/mass holds, disabled during SurfaceSink so the sink
  # phase can command buoyancy freely.

  behavior Guidance:Buoyancy id="BuoyancyHold" {
    run while (
      BuoyancyHold
      and not ( DiveMode == SurfaceSink )
    )

    set position = Control:VerticalControl.buoyancyNeutral
  }

  behavior Guidance:Pitch id="MassHold" {
    run while (
      MassHold
      and not ( DiveMode == SurfaceSink )
    )

    set massPosition = Control:VerticalControl.massDefault
  }

  # Phase-specific vertical control. SurfaceSink drops using buoyancy only,
  # then hands off to Cruise which sets the transit depth and speed.

  aggregate DiveCmd {
    run when ( called )

    aggregate SurfaceSink {
      run while ( DiveMode == SurfaceSink )

      aggregate sink {
        run in sequence

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

          set position = NaN cc
        }

        behavior Guidance:SetSpeed id="SinkSpeed" {
          run in parallel

          set speed = 0 m/s
        }

        syslog info "Sinking to " + SinkDepth~meter

        behavior Guidance:Pitch id="SinkPitch" {
          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 = Cruise
      }
    }

    aggregate Cruise {
      run while ( DiveMode == Cruise )

      aggregate drive {
        run in parallel

        behavior Guidance:Pitch {
          run in parallel

          set depth = Depth
        }

        behavior Guidance:SetSpeed {
          run in parallel

          set speed = Speed
        }
      }
    }
  }

  aggregate Transit {
    run in sequence

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

      call priorityHere=false refId="DiveCmd"
    }

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

      set latitude = Latitude
      set longitude = Longitude
      set northingsDelta = Northings
      set eastingsDelta = Eastings
    }

    call id="PhoneHome" priorityHere=false refId="NeedComms"
  }
}