Science/circle_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 {
  """
  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 = 60 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 depth of the asset to be tracked, if known.
      """

    ListenToTerminateMissionMsg = false
      """
      Whether to listen to the _.terminateMission acoustic message.
      """

    SendNearSignal = false
      """
      Whether to send the _.near acoustic message to the contact.
      """

    ModemIDForNearSignal = 0 enum
      """
      ID of modem to send _.near acoustic message. Defaults to the same as ContactLabel
      but doesn't have to be the same modem as that being 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.
      """

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

    InnerCircleRadius = 50 meter
      """
      Radius of circle around acoustic contact during wait time
      """

    ThreshRangeContact = 100 meter
      """
      Send _.near acoustic message to the contact when the range is closer than this threshold.
      """

    # You probably do not need to change these.

    TrackingUpdatePeriod = 60 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)
      """

    InnerCircleTime = 0 minute
      """
      Time before each normal communication cycle to move to the inner circle.
      Default value of 0 means vehicle will stay at OuterCircleRadius the
      whole time.
      """

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

    FixedDepth = NaN meter
      """
      If non-nan, vehicle flies a flat-and-level circle at the specified depth
      instead of yo-yos. See noyo settings below.
      """

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

    NoYoApproachSpeed = 0.7 meter_per_second
      """
      Speed while descending to target depth for flat-and-level circle
      Not used for yoyos
      """

    NoYoApproachDepthRate = 0.05 meter_per_second
      """
      Descent depth rate (Positive depth rate means going down) for
      flat-and-level circle
      Not used for yoyos
      """

    NoYoApproachPitchLimit = 20 degree
      """
      Max vehicle pitch (+/-) when decending from the surface for
      flat-and-level circle
      Not used for yoyos
      """

    NoYoApproachDepthTimeout = 15 minute
      """
      Maximum time duration for the vehicle to reach the target depth for
      flat-and-level circle
      Not used for yoyos
      """

    NoYoApproachSettleTimePreDive = 1 minute
      """
      Time duration for the vehicle to pump down the VBS *BEFORE* leaving the
      surface for flat-and-level circle
      Not used for yoyos
      """

    NoYokpDepth = 0.07 radian_per_meter
      """
      Dive loop proportional gain (applied only while the vehicle is
      descending to target depth).
      """
  }

  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 to mode
      when doing surface comms.
      """

    InitialDive = 1 count
      """
      Mission variable (don't change). The mission will run in this mode after
      surface comms are done to dive the vehicle to the approach depth.
      """

    MaintainDepth = 2 count
      """
      Mission variable (don't change). The mission will run in this mode to
      maintain the vehicle at the target depth.
      """

    TrueVar = true

    NearSignalSent = false
      """
      Have sent the _.near message to the contact.
      """
  }

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

  # <Estimation:Tracking>
  #     <Parallel/>
  #     <Setting><Estimation:Tracking.contactDepthSetting/><Arg Name="ContactDepth"/></Setting>
  #     <Setting><Estimation:Tracking.contactLabelSetting/><Arg Name="ContactLabel"/></Setting>
  #     <Setting><Estimation:Tracking.numberOfSamplesSetting/><Arg Name="NumberOfPings"/></Setting>
  #     <Setting><Estimation:Tracking.updatePeriodSetting/><Arg Name="TrackingUpdatePeriod"/></Setting>
  # </Estimation:Tracking>

  behavior Estimation:TrackAcousticContact {
    run in parallel

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

  aggregate UpdateCenter {
    run when (
      elapsed ( Estimation:TrackAcousticContact.range_to_contact ) < AcousticContactTimeout
      and (
        CenterLatitude != Estimation:TrackAcousticContact.contact_latitude
        or CenterLongitude != Estimation:TrackAcousticContact.contact_longitude
      )
    )

    assign in sequence CenterLatitude = Estimation:TrackAcousticContact.contact_latitude

    assign in sequence CenterLongitude = Estimation:TrackAcousticContact.contact_longitude
  }

  # Most missions have the NeedComms aggregate at high priority.

  insert id="NeedComms" Insert/NeedComms.tl

  assign in sequence NeedComms:DiveInterval = MissionTimeout

  assign in sequence NeedComms:WaitForPitchUp = 0 minute

  # 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

  insert Insert/BackseatDriver.tl

  insert Insert/PowerOnly.tl

  insert Insert/TerminateMissionByMsg.tl {
    redefineArg ListenToMsgTerminateMission = ListenToTerminateMissionMsg
  }

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

    redefineArg PeakDetectChlActive = true
  }

  aggregate SendNearSignal {
    run when (
      ( ( SendNearSignal )
        and ( Universal:acoustic_contact_range < ThreshRangeContact )
        and not ( NearSignalSent )
      )
    )

    aggregate NearSignal {
      run in sequence

      break if ( NearSignalSent )

      behavior Sensor:SendDirect {
        run in sequence

        set destType = "modem"
        set destId = ModemIDForNearSignal
        set destName = "_.near"
        set value = TrueVar
        set unit = "bool"
      }

      syslog important "Set _.near to " + TrueVar~bool
      assign in sequence NearSignalSent = true
    }
  }

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

    assign in sequence DiveMode = DoingComms

    call refId="NeedComms"

    assign in sequence DiveMode = InitialDive
  }

  aggregate DiveCmd {
    """
    Called when the vehicle should be underwater. Arbitrates between 3
    dive modes: 1) descend from the surface to initial depth. 2) settle
    at target depth using the VB. 3) cruise at target depth.
    """

    run when ( called )

    aggregate NoYoInitialDive {
      """
      Descend from the surface to target depth, limit pitch and
      depth-rate.
      """

      run while (
        DiveMode == InitialDive
        and not isNaN ( FixedDepth )
      )

      aggregate dive {
        run in sequence

        assign in parallel Control:VerticalControl.pitchLimit = NoYoApproachPitchLimit

        assign in parallel Control:VerticalControl.kpDepth = NoYokpDepth

        behavior Guidance:Mass {
          run in parallel

          set position = Control:VerticalControl.massDefault
        }

        behavior Guidance:Buoyancy {
          run in parallel

          set position = Control:VerticalControl.buoyancyNeutral
        }

        syslog info "Waiting for " + NoYoApproachSettleTimePreDive~minute
             + "while pumping down to neutral."

        behavior Guidance:Wait {
          """
          Take a few moments to ensure the VBS reaches neutral.
          """

          run in sequence

          set duration = NoYoApproachSettleTimePreDive
        }

        syslog "Moving to " + FixedDepth~meter

        aggregate ApproachDepth {
          run in sequence

          behavior Guidance:SetSpeed {
            run in parallel

            set speed = NoYoApproachSpeed
          }

          behavior Guidance:Pitch {
            run in sequence

            timeout duration=NoYoApproachDepthTimeout {
              syslog important "Timed out while trying to reach target depth at current depth of "
                   + Universal:depth~meter

              behavior Guidance:Execute {
                run in sequence

                set command = "stop"
              }
            }

            set depth = FixedDepth
            set depthRate = NoYoApproachDepthRate
          }
        }

        assign in sequence DiveMode = MaintainDepth
      }
    }

    aggregate NoYoMaintainDepth {
      """
      Cruise at the target depth.
      """

      run while (
        DiveMode == MaintainDepth
        and not isNaN ( FixedDepth )
      )

      aggregate dive {
        run in parallel

        aggregate ThrusterDive {
          run while ( Speed > 0.0 meter_per_second )

          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 = FixedDepth
        }
      }
    }

    aggregate YoYo {
      run while (
        DiveMode != DoingComms
        and isNaN ( FixedDepth )
      )

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

  # Start mission sequence

  call id="StartingMission" refId="SurfaceComms"

  aggregate DiveAndCircle {
    run in sequence

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

      call refId="DiveCmd"
    }

    aggregate CircleWrapper {
      run in sequence

      aggregate CircleContact {
        """
        Circle the contact while updating its position using the
        tracking behavior.
        """

        run when (
          elapsed ( Universal:time_fix ) < ( NeedCommsTime - InnerCircleTime )
        )

        assign in parallel Control:HorizontalControl.kwpHeading = KwpHeading

        behavior Guidance:Circle {
          run in sequence

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

      aggregate CircleContactClose {
        """
        Circle the contact closely while updating its position using the
        tracking behavior.
        """

        run when (
          elapsed ( Universal:time_fix ) >= ( NeedCommsTime - InnerCircleTime )
        )

        assign in parallel Control:HorizontalControl.kwpHeading = KwpHeading

        behavior Guidance:Circle {
          run in sequence

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