Engineering/circle_acoustic_backseat.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 circle_acoustic_backseat {
  """
  This mission runs a circle around a specified acoustic contact. The
  tracking behavior periodically queries the DAT for range and direction
  to the contact, and the DAT acoustically interrogates the contact. The
  tracking component then calculates the position of the contact to update
  the center of the circle. When the vehicle surfaces and updates its own
  position with GPS, it queries the contact again, then sends the contact
  position to shore.
  """

  arguments {
    # Mission-wide settings

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

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

    AcousticContactTimeout = 15 minute
      """
      If the vehicle does not receive an acoustic signal for more than this
      length of time, it will surface for communications with shore.
      """

    # You probably need to change these.

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

    ContactDepth = NaN meter
      """
      The acoustic address of the asset to be tracked
      """

    CenterLatitude = 36.81544 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.82506 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.
      """

    CenterNorthingsDelta = 0 meter
      """
      Desired change in latitude to achieve, expressed as a distance.
      """

    CenterEastingsDelta = 0 meter
      """
      Desired change in longitude to achieve, expressed as a distance.
      """

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

    # You probably do not need to change these.

    TrackingUpdatePeriod = 15 second
      """
      How long to wait between acoustic queries
      """

    NumberOfPings = 1 count
      """
      Number of return pings to request with each acoustic query (more than 1
      will activate oneway mode)
      """

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

    YoYoMinDepth = 3 meter
      """
      Minimum depth during yo-yo's.
      """

    YoYoMaxDepth = 33 meter
      """
      Maximum depth during yo-yo's.
      """

    YoYoMinAltitude = 10 meter
      """
      Min altitude while in yo-yo mode.
      """

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

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

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

    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 = 7 meter
      """
      Minimum height above the sea floor for the entire mission.
      """

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

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

  # Missions should always start with a timeout.

  timeout duration=MissionTimeout

  # Add tracking directive high in the stack (above NeedComms to enable tracking from surface).

  insert Insert/AbortDrift.tl {
    redefineArg AcousticTimeout = AcousticContactTimeout
  }

  behavior Estimation:Tracking {
    run in parallel

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

  aggregate UpdateCenter {
    run when (
      elapsed ( customUri "_.northingsDelta" ) < ( elapsed ( CenterNorthingsDelta ) )
      or ( elapsed ( customUri "_.eastingsDelta" ) < ( elapsed ( CenterEastingsDelta ) ) )
    )

    assign in sequence CenterNorthingsDelta = customUri "_.northingsDelta"

    assign in sequence CenterEastingsDelta = customUri "_.eastingsDelta"

    assign in sequence CenterLatitude = Universal:latitude

    assign in sequence CenterLongitude = Universal:longitude

    syslog info "Upadating circle center " + CenterLatitude~arcdeg + "+"
         + CenterNorthingsDelta~meter + "/" + CenterLongitude~arcdeg + "+" + CenterEastingsDelta~meter
  }

  aggregate UpdateYoYoMaxDepth {
    run when (
      elapsed ( customUri "_.YoYoMaxDepth" ) < ( elapsed ( YoYoMaxDepth ) )
      and ( customUri "_.YoYoMaxDepth" < MaxDepth )
    )

    assign in sequence YoYoMaxDepth = customUri "_.YoYoMaxDepth"

    syslog info "Upadating YoYoMaxDepth " + customUri "_.YoYoMaxDepth"~meter
  }

  aggregate UpdateYoYoMinDepth {
    run when (
      elapsed ( customUri "_.YoYoMinDepth" ) < ( elapsed ( YoYoMinDepth ) )
      and ( customUri "_.YoYoMinDepth" < MaxDepth )
    )

    assign in sequence YoYoMinDepth = customUri "_.YoYoMinDepth"

    syslog info "Upadating YoYoMinDepth " + customUri "_.YoYoMinDepth"~meter
  }

  aggregate UpdateCircleRadius {
    run when (
      elapsed ( customUri "_.CircleRadius" ) < ( elapsed ( CircleRadius ) )
    )

    assign in sequence CircleRadius = abs ( customUri "_.CircleRadius" )

    syslog info "Upadating CircleRadius " + customUri "_.CircleRadius"~meter
  }

  # Most missions have the NeedComms aggregate at high priority.

  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

  # Power the science payload.

  insert id="Science" Insert/Science.tl {
    """
    Get science data, including PeakDetectChl
    """

    redefineArg PeakDetectChlActive = true
  }

  # Power the Backseat payload.

  insert Insert/BackseatDriver.tl

  assign in sequence BackseatDriver:EnableBackseat = true

  insert Insert/PowerOnly.tl

  # Check for additional instructions first.

  call id="StartingMission" refId="NeedComms"

  # Start mission sequence.

  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
  }

  aggregate YoYos {
    run in parallel

    behavior Guidance:DepthEnvelope {
      run in parallel

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

    behavior Guidance:AltitudeEnvelope {
      run in parallel

      set minAltitude = YoYoMinAltitude
      set pitch = YoYoPitch
    }

    behavior Guidance:YoYo {
      run in parallel

      set pitch = YoYoPitch
    }
  }

  aggregate CircleWrapper {
    """
    Circle the contact while updating its position using the backseat
    commands.
    """

    run in sequence

    assign in parallel Control:HorizontalControl.kwpHeading = KwpHeading

    behavior Guidance:Circle {
      run in parallel

      set latitude = CenterLatitude
      set longitude = CenterLongitude
      set northingsDelta = CenterNorthingsDelta
      set eastingsDelta = CenterEastingsDelta
      set radius = CircleRadius
      set maxError = CircleMaxError
      set turnToPort = CircleTurnToPort
    }

    behavior Guidance:Wait {
      run in sequence

      set duration = MissionTimeout
    }
  }
}