Engineering/tail_acoustic_contact.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 follow_that_car {
  """
  Home to target when outside TailDistance, otherwise drift.
  Stay close... but not too close. U+1F575
  """

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

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

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

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

    TailDistance = 10 meter
      """
      How close to get before stopping. DAT.ignoreElevationAngle will
      be set by the mission, so this distance is horizontal range, not slant
      range.
      """

    TailDeadband = 10 meter
      """
      How much additional distance to allow from contact before resuming chase
      after waiting
      """

    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 during driving intervals
      """

    Depth = 50 meter
      """
      Vehicle depth
      """

    WaitDepthDeadband = 10 meter
      """
      Depth deviation to allow before using buoyancy to correct
      during wait
      """

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

    MaxDepth = 205 meter
      """
      Maximum depth of the enter mission
      """

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

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

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

  output {
    # Internal variables, do not change
    LocationUpdated = false
      """
      Mission variable (don't change). Flag to indicate contact lat/lon was updated
      """

    Chasing = false
      """
      Mission variable (don't change). Flag to indicate whether in chase or wait mode
      """

    DistanceAllowed = TailDistance
      """
      Mission variable (don't change). Allowable distance from contact before chasing
      """
  }

  timeout duration=MissionTimeout

  insert Insert/Science.tl

  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

  insert Insert/BackseatDriver.tl

  insert Insert/PowerOnly.tl

  # 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
  }

  # Use horizontal range, not slant range
  assign in parallel Sensor:DAT.ignoreElevationAngle = true

  aggregate UpdateLocation {
    run when (
      elapsed ( Estimation:TrackAcousticContact.range_to_contact ) < ( elapsed ( LocationUpdated ) )
    )

    break if (
      isNaN ( Estimation:TrackAcousticContact.contact_latitude )
      or isNaN ( Estimation:TrackAcousticContact.contact_longitude )
    )

    assign in sequence ContactLatitude = Estimation:TrackAcousticContact.contact_latitude

    assign in sequence ContactLongitude = Estimation:TrackAcousticContact.contact_longitude

    assign in sequence LocationUpdated = true

    syslog debug "Updated target location to " + ContactLatitude~degree + ", "
         + ContactLongitude~degree
  }

  call id="StartingMission" refId="NeedComms"

  aggregate Tail {
    run in sequence

    aggregate Chase {
      """
      Head towards the target
      """

      run while (
        # Universal range, directly from the DAT, will always be slant range
        # If we're ignoring elevation, TrackAcousticContact range will be horizontal range
        # which is what we want.
        # Yeah, ignoreElevationAngle is a DAT setting, shhhhhhhh.
        Estimation:TrackAcousticContact.range_to_contact > DistanceAllowed
        or isNaN ( Estimation:TrackAcousticContact.range_to_contact )
      )

      aggregate SetChase {
        run in sequence

        break if ( Chasing )

        assign in sequence DistanceAllowed = TailDistance

        syslog important "Target distance " + Estimation:TrackAcousticContact.range_to_contact~m
             + " exceeded max " + DistanceAllowed~m + ", giving chase."
        assign in sequence Chasing = true
      }

      behavior Guidance:Mass {
        run in parallel

        set position = 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 = Depth
      }

      aggregate Wpt {
        run in sequence

        behavior Guidance:Waypoint {
          run in sequence

          set latitude = ContactLatitude
          set longitude = ContactLongitude
        }
      }
    }

    aggregate Wait {
      """
      Drift until target is out of range
      """

      run while (
        # Horizontal range (if set to compute it) here too
        Estimation:TrackAcousticContact.range_to_contact <= DistanceAllowed
      )

      aggregate SetWait {
        run in sequence

        break if ( not Chasing )

        assign in sequence DistanceAllowed = TailDistance + TailDeadband

        syslog important "Caught up to target at distance "
             + Estimation:TrackAcousticContact.range_to_contact~m + " waiting until target is "
             + DistanceAllowed~m + "away to resume pursuit."
        assign in sequence Chasing = false
      }

      assign in parallel Control:VerticalControl.depthDeadband = WaitDepthDeadband

      behavior Guidance:SetSpeed {
        run in parallel

        set speed = 0 meter_per_second
      }

      behavior Guidance:Pitch {
        run in parallel

        set depth = Depth
        set pitch = 0 degree
        set elevatorAngle = 0 degree
      }

      behavior Guidance:Point {
        run in parallel

        set rudderAngle = 0 degree
      }

      behavior Guidance:Wait {
        run in sequence

        set duration = MissionTimeout
      }
    }
  }
}