Engineering/hotBunk.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 hotBunk {
  """
  Home to target and, when within ContactDistance, perform a hockey stop (rudder hard over, speed 0) and
  wait for settle time to expire at ContactDepth. Ideally this mission would be used in conjunction with
  the scheduler in order to kick off another mission underwater once the settle time has expired.
  """

  arguments {
    # Define mission arguments

    MissionTimeout = 2 hour
      """
      Maximum length of mission
      """

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

    ContactLabel = 9 count
      """
      The acoustic address of the asset to be tracked
      """

    ContactDepth = 10 meter
      """
      The depth of the asset to be tracked
      """

    ContactDistance = 50 meter
      """
      How close to get before stopping
      """

    TrackingUpdatePeriod = 5 second
      """
      How long to wait between acoustic queries.
      """

    NumberOfPings = 1 count
      """
      Number of pings requested each time.
      """

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

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

    SettleTime = 10 minute
      """
      Wait this long to settle after stopping the prop
      """

    CenterLatitude = 36.797 degree
      """
      The latitude of the center of the circle. Fill this in to specify a
      start position, it will be overwritten once the vehicle receives an
      acoustic signal from the contact.
      """

    CenterLongitude = -121.847 degree
      """
      The longitude of the center of the circle. Fill this in to specify a
      start position, it will be overwritten once the vehicle receives an
      acoustic signal from the contact.
      """

    CircleRadius = 25 meter
      """
      Radius of circle around acoustic contact during wait time
      """

    CircleMaxError = 10 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.
      """
  }

  output {
    # Internal variables that you should not change

    CenterTouched = false
      """
      Flag to indicate center lon/lat was updated.
      """
  }

  # Mission 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.
    """
  }

  # Safety nets and surfacing behaviors

  insert Insert/StandardEnvelopes.tl

  assign in sequence StandardEnvelopes:MinAltitude = MinAltitude

  assign in sequence StandardEnvelopes:MaxDepth = MaxDepth

  assign in sequence StandardEnvelopes:MinOffshore = MinOffshore

  insert id="NeedComms" Insert/NeedComms.tl

  assign in sequence NeedComms:DiveInterval = NeedCommsTime

  assign in sequence NeedComms:WaitForPitchUp = 0 minute

  # Add track acoustic contact directive high in the stack.
  behavior Estimation:TrackAcousticContact {
    run in parallel

    set contactLabelSetting = ContactLabel
    set contactDepthSetting = ContactDepth
    set numberOfSamplesSetting = NumberOfPings
    set updatePeriodSetting = TrackingUpdatePeriod
  }

  aggregate UpdateCenter {
    run when (
      elapsed ( Estimation:TrackAcousticContact.range_to_contact ) < ( elapsed ( CenterTouched ) )
    )

    assign in sequence CenterLatitude = Estimation:TrackAcousticContact.contact_latitude

    assign in sequence CenterLongitude = Estimation:TrackAcousticContact.contact_longitude

    assign in sequence CenterTouched = true
  }

  # Start mission sequence.

  aggregate Home {
    """
    Head towards the target.
    """

    run in sequence

    break if (
      Universal:acoustic_contact_range < ContactDistance
    )

    behavior Guidance:Pitch {
      run in parallel

      set massPosition = Control:VerticalControl.massDefault
    }

    behavior Guidance:Buoyancy {
      run in parallel

      set position = Control:VerticalControl.buoyancyNeutral
    }

    behavior Guidance:SetSpeed {
      run in parallel

      set speed = Speed
    }

    behavior Guidance:Pitch {
      run in parallel

      set depth = ContactDepth
    }

    behavior Guidance:Circle {
      run in parallel

      set latitude = CenterLatitude
      set longitude = CenterLongitude
      set radius = CircleRadius
      set maxError = CircleMaxError
      set turnToPort = CircleTurnToPort
    }
  }

  aggregate Hockey_Stop {
    run in sequence

    syslog important "Hockey stop! Range: " + Universal:acoustic_contact_range~meter

    behavior Guidance:SetSpeed {
      run in parallel

      set speed = 0 meter_per_second
    }

    behavior Guidance:Point {
      run in parallel

      set rudderAngle = 13 degree
    }

    behavior Guidance:Pitch {
      run in parallel

      set depth = ContactDepth
    }

    behavior Guidance:Wait {
      run in sequence

      set duration = SettleTime
    }
  }
}