Transport/keepstation.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 keepstation {
  """
  Vehicle transits to desired waypoint (or stays put if none is commanded)
  and stays within the specified radius.
  """

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

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

    Latitude = NaN degree
      """
      Latitude of waypoint to hold. If NaN, hold the latitude at start of
      mission.
      """

    Longitude = NaN degree
      """
      Longitude of waypoint to hold. If NaN, hold the longitude at start of
      mission.
      """

    Depth = 30 meter
      """
      Depth held during drift mode
      """

    ApproachDepth = 15 meter
      """
      Depth for initial approach to station.
      """

    DepthDeadband = 4 meter
      """
      How much vertical drift from the specified depth is allowed in drift
      mode
      """

    Speed = 0.75 meter_per_second
      """
      Vehicle transit speed.
      """

    Radius = 200 meter
      """
      Radius of circle around waypoint to hold.
      """

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

    MaxDepth = 45 meter
      """
      Maximum allowable depth during the mission.
      """

    MinOffshore = 2 kilometer
      """
      Minimum distance from the shoreline to maintain
      """
  }

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

    TransitToStation = 2 count
      """
      Mission variable (don't change). The mission will run in this mode
      when performing the transit to station
      """

    StationKeep = 3 count
      """
      Mission variable (don't change). The mission will run in this mode
      when keeping station
      """

    OnStation = false
      """
      Mission variable (don't change). Determines last mode (Transit or StationKeep)
      to resume after needcomms
      """
  }

  timeout duration=MissionTimeout

  insert id="NeedComms" Insert/NeedComms.tl

  assign in sequence NeedComms:DiveInterval = NeedCommsTime

  insert Insert/StandardEnvelopes.tl

  assign in sequence StandardEnvelopes:MaxDepth = MaxDepth

  assign in sequence StandardEnvelopes:MinOffshore = MinOffshore

  insert Insert/BackseatDriver.tl

  insert Insert/PowerOnly.tl

  behavior Guidance:Pitch {
    run in parallel

    set massPosition = Control:VerticalControl.massDefault
  }

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

    assign in sequence DiveMode = DoingComms

    call priorityHere=false refId="NeedComms"

    aggregate setTransit {
      run in sequence

      break if (
        SinkOffSurface
        or OnStation
      )

      assign in sequence DiveMode = TransitToStation
    }

    aggregate setStation {
      run in sequence

      break if (
        SinkOffSurface
        or not OnStation
      )

      assign in sequence DiveMode = StationKeep
    }

    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
        }

        # Once we've sunk, either resume transit or station keeping depths and speeds
        aggregate sinkToTransit {
          run in sequence

          break if ( OnStation )

          assign in sequence DiveMode = TransitToStation
        }

        aggregate sinkToStation {
          run in sequence

          break if ( not OnStation )

          assign in sequence DiveMode = StationKeep
        }
      }
    }

    aggregate TransitToStation {
      run while ( DiveMode == TransitToStation )

      aggregate dive {
        run in sequence

        behavior Guidance:Buoyancy {
          run in parallel

          set position = Control:VerticalControl.buoyancyNeutral
        }

        behavior Guidance:Pitch {
          run in parallel

          set depth = ApproachDepth
        }

        behavior Guidance:SetSpeed {
          run in parallel

          set speed = Speed
        }
      }
    }

    aggregate StationKeep {
      run while ( DiveMode == StationKeep )

      assign in parallel Control:VerticalControl.depthDeadband = DepthDeadband

      behavior Guidance:Pitch {
        run in parallel

        set depth = Depth
      }

      behavior Guidance:KeepStation {
        run in parallel

        set latitude = Latitude
        set longitude = Longitude
        set radius = Radius
        set speed = Speed
      }
    }
  }

  aggregate TransitToStation {
    """
    Need a separate aggregate for transit if we want to specify a
    different depth for the approach.
    """

    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
    }

    assign in sequence DiveMode = StationKeep
    assign in sequence OnStation = true
  }

  call id="OnStation" priorityHere=false refId="NeedComms"

  aggregate KeepStation {
    run in sequence

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

      call priorityHere=false refId="DiveCmd"
    }

    behavior Guidance:Wait {
      """
      Due to the way the KeepStation behavior is currently
      implemented, we must run it in parallel and use a Wait to keep
      from completing the mission before the timeout expires. However,
      this new version of the mission uses a top-level timeout so that
      the entire mission will not run for longer then MissionTimeout.
      """

      run in sequence

      set duration = MissionTimeout
    }
  }
}