Science/cork_and_screw_2.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 CorkAndScrew {
  arguments {
    # Almost every mission should start with an overall timeout and a NeedCommsTime. Drift Missions will have an acoustic timeout instead of a NeedCommsTime.

    # You probably need to change these.

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

    NumRoundsWholeSet = 1 count
      """
      Number of rounds of whole set of outer cylinders and inner screws.
      """

    NumScrewsPerCylinder = 1 count
      """
      Number of inner screws per outer cylinder.
      """

    NumDepths = 10 count
      """
      Total number of inner screw depths.
      """

    # debug
    #     </Description><Units:count/><Value>3</Value></DefineArg>

    NeedCommsTimeOnCorkScrew = 60 minute
      """
      On cork and screw, how often to surface for commumications
      """

    # debug
    #     </Description><Units:minute/><Value>20</Value></DefineArg>

    AcousticTrackingTimeout = 21 hour
      """
      If the vehicle does not receive an acoustic signal for more than this
      length of time, it will surface for communications. Set longer than
      MissionTimeout to effectively disable.
      """

    AcousticContactTimeout = 15 minute
      """
      Only use a sufficiently fresh handshake to update the tracking center.
      """

    MaxWaitNotAchievingDepth = 2 hour
      """
      Maximum wait time the vehicle cannot reach the targeted depth.
      """

    OuterCylinder = true
      """
      Whether to do an outer cylinder yoyo. Initialized to True.
      """

    # debug
    #     </Description><False/></DefineArg>

    InnerScrew = false
      """
      Whether to do an inner screw. Initialized to True.
      """

    # debug
    #     </Description><True/></DefineArg>

    CylinderTimeout = 240 minute
      """
      Maximum duration of cylinder yoyo.
      """

    ScrewEachDepthTimeout = 120 minute
      """
      Maximum duration at each depth on inner screw.
      """

    SpeedCylinder = 1.0 meter_per_second
      """
      Vehicle speed on cylinder.
      """

    SpeedScrew = 1.0 meter_per_second
      """
      Vehicle speed on screw.
      """

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

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

    AcousticTargetDepth = NaN meter
      """
      Depth of acoustic target if known and fixed (or nearly fixed). For
      example, if the acoustic target is a Wave Glider, set it to zero. This
      will improve 2D projected position estimates in the Earth reference
      frame. Defaults to NaN.
      """

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

    # debug
    #     </Description><Units:degree/><Value>36.80</Value></DefineArg>

    CenterLongitude = NaN 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.
      """

    # debug
    #     </Description><Units:degree/><Value>-121.82</Value></DefineArg>

    CylinderYoYoMinDep = 1.5 meter
      """
      Minimum depth while performing the YoYo behavior on outer cylinder.
      """

    CylinderYoYoMaxDep = 30 meter
      """
      Maximum depth while performing the YoYo behavior on outer cylinder.
      """

    CylinderRadius = 50 meter
      """
      Radius of outer cylinder.
      """

    MaxCirclesOnCylinder = 1 count
      """
      Maximum number of circles on cylinder.
      """

    ScrewRadius = 100 meter
      """
      Radius of inner screw.
      """

    MaxCirclesEachDepthOnScrew = 1 count
      """
      Maximum number of circles on cylinder.
      """

    Depth[1..20] = NaN meter
      """
      Depth {$}.
      """

    # Use caution when changing these.

    YoYoMinAltitude = 9 meter
      """
      Minimum altitude while performing the YoYo behavior (for
      bottom-terminated YoYos).
      """

    MinAltitude = 7 meter
      """
      Minimum altitude for the entire mission.
      """

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

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

    NumberOfFixesLowPass = 4 count
      """
      Number of fixes to average to produce smoothed lat/lon/dep output
      (initialized to 4).
      """

    NumberOfStartingFixesToIgnore = 2 count
      """
      Number of fixes to ignore at the start of mission (as the vehicle just
      leaves surface, contact's location estimate especailly bearing can be
      erroneous. Initialized to 8, corresponding to 2 minutes if query
      interval TrackingUpdatePeriod = 15 s.
      """

    # debug
    #     </Description><Units:count/><Value>0</Value></DefineArg>

    VerticalTolerance = 0.1 meter
      """
      Depth error allowed for being considered at/near the target depth.
      """

    CylinderCircleMaxError = 50 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.
      """

    CylinderCircleTurnToPort = true
      """
      If true, vehicle turns to the left around the center point. If false,
      vehicle turns to the right.
      """

    ScrewCircleMaxError = 50 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.
      """

    ScrewCircleTurnToPort = true
      """
      If true, vehicle turns to the left around the center point. If false,
      vehicle turns to the right.
      """

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

    YoYoUpPitch = 20 degree
      """
      Vehicle up pitch while performing the YoYo behavior.
      """

    YoYoDownPitch = -20 degree
      """
      Vehicle down pitch while performing the YoYo behavior.
      """

    BuoyancyNeutral = Control:VerticalControl.buoyancyNeutral
      """
      Buoyancy bladder position while performing the YoYo behavior. Defaults
      to buoyancyNeutral setting in the Config/Control.cfg file. Set to NaN cc
      for active buoyancy
      """

    MassDefault = Control:VerticalControl.massDefault
      """
      Static setting for mass during the mission. Set to NaN mm for active
      mass position
      """

    # Internal variables that you should not change

    YoYoMinDepth = 4.5 meter
      """
      Minimum depth while performing the YoYo behavior. Program will set to
      CylinderYoYoMinDep.
      """

    YoYoMaxDepth = 27 meter
      """
      Maximum depth while performing the YoYo behavior. Program will set to
      CylinderYoYoMaxDep.
      """

    MissionStartCommsCompleted = false
      """
      MissionStartComms is completed (initialized to false).
      """

    StartScrew = false
      """
      Start screw (initialized to false).
      """

    DepthSet = 10.0 meter
      """
      Depth held for inner screw. Initialized to 10 meters.
      """

    DepthOptionsSet = false
      """
      Depth options for inner screw already set. Initialized to false.
      """

    DepthCompleted = false
      """
      Depth holding on inner screw completed. Initialized to false.
      """

    FromLastTimeFixToCenterUpdate = 0 minute
      """
      Elapsed time from last time-fix to center update time (initialized to
      zero).
      """

    ElapsedTimeFix = 0 minute
      """
      Elapsed time from last time-fix since last fix (initialized to zero).
      """

    ContactLatitudeLowpass = NaN degree
      """
      Low-pass filtered latitude of contact (initialized to NaN).
      """

    ContactLongitudeLowpass = NaN degree
      """
      Low-pass filtered longitude of contact (initialized to NaN).
      """

    CntScrews = 1 count
      """
      Count of inner screws (initilized to 1).
      """

    CntDepths = 1 count
      """
      Count of inner screw depths (initilized to 1).
      """
  }

  # Missions should almost always start with a timeout and a NeedComms aggregate. Drift missions will not use NeedComms.

  timeout duration=MissionTimeout

  insert id="NeedComms" Insert/NeedComms.tl

  assign in sequence NeedComms:DiveInterval = NeedCommsTimeOnCorkScrew

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

  behavior Estimation:Tracking {
    run in parallel

    set contactLabelSetting = ContactLabel
    set numberOfSamplesSetting = NumberOfPings
    set updatePeriodSetting = TrackingUpdatePeriod
    set numFixesLowPassSetting = NumberOfFixesLowPass
    set numStartingFixesToIgnoreSetting = NumberOfStartingFixesToIgnore
    set contactDepthSetting = AcousticTargetDepth
  }

  assign in parallel Control:VerticalControl.depthDeadband = VerticalTolerance

  aggregate UpdateCenter {
    run when (
      elapsed ( Estimation:Tracking.range_to_contact ) < AcousticContactTimeout
    )

    assign in sequence ContactLatitudeLowpass = Estimation:Tracking.contact_latitude_lowpass

    assign in sequence ContactLongitudeLowpass = Estimation:Tracking.contact_longitude_lowpass

    assign in sequence ElapsedTimeFix = elapsed ( Universal:time_fix )

    # debug
    #   <Syslog Severity="Important">Overwrite or update. elapsed_time_fix, FromLastTimeFixToCenterUpdate, latitude_fix, longitude_fix = <Arg Name="ElapsedTimeFix"/><Units:second/>, <Arg Name="FromLastTimeFixToCenterUpdate"/><Units:second/>, <Universal:latitude_fix/><Units:degree/>, <Universal:longitude_fix/><Units:degree/></Syslog>
    #   <Syslog Severity="Important">Overwrite or update. CenterLatitude, CenterLongitude, contact_latitude, contact_longitude, contact_latitude_lowpass, contact_longitude_lowpass = <Arg Name="CenterLatitude"/><Units:degree/>, <Arg Name="CenterLongitude"/><Units:degree/>, <Estimation:Tracking.contact_latitude/><Units:degree/>, <Estimation:Tracking.contact_longitude/><Units:degree/>, <Estimation:Tracking.contact_latitude_lowpass/><Units:degree/>, <Estimation:Tracking.contact_longitude_lowpass/><Units:degree/></Syslog>

    aggregate UpdateWithContactLowpassLatLon {
      run when (
        not ( isNaN ( ContactLatitudeLowpass ) )
        and ( not ( isNaN ( ContactLongitudeLowpass ) ) )
        and ( elapsed ( Universal:time_fix ) >= ( FromLastTimeFixToCenterUpdate + TrackingUpdatePeriod ) )
      )

      assign in sequence CenterLatitude = Estimation:Tracking.contact_latitude_lowpass

      assign in sequence CenterLongitude = Estimation:Tracking.contact_longitude_lowpass

      # debug
      #         <Syslog Severity="Important">Updating. FromLastTimeFixToCenterUpdate, CenterLatitude, CenterLongitude = <Arg Name="FromLastTimeFixToCenterUpdate"/><Units:second/>, <Arg Name="CenterLatitude"/><Units:degree/>, <Arg Name="CenterLongitude"/><Units:degree/></Syslog>

      assign in sequence FromLastTimeFixToCenterUpdate = elapsed ( Universal:time_fix )
    }

    aggregate OverwriteWithGPSLatLon {
      run when (
        elapsed ( Universal:time_fix ) < FromLastTimeFixToCenterUpdate
      )

      # debug
      #         <Assign><Sequence/><Arg Name="ElapsedTimeFix"/><Elapsed><Universal:time_fix/></Elapsed></Assign>
      #         <Syslog Severity="Important">To overwriteWithGPSLatLon. latitude_fix, longitude_fix, elapsed_time_fix, FromLastTimeFixToCenterUpdate = <Universal:latitude_fix/><Units:degree/>, <Universal:longitude_fix/><Units:degree/>, <Arg Name="ElapsedTimeFix"/><Units:second/>, <Arg Name="FromLastTimeFixToCenterUpdate"/><Units:second/></Syslog>

      assign in sequence CenterLatitude = Universal:latitude_fix

      assign in sequence CenterLongitude = Universal:longitude_fix

      assign in sequence FromLastTimeFixToCenterUpdate = elapsed ( Universal:time_fix )
    }
    # debug
    #       <Syslog Severity="Important">Overwriting. FromLastTimeFixToCenterUpdate, CenterLatitude, CenterLongitude = <Arg Name="FromLastTimeFixToCenterUpdate"/><Units:second/>, <Arg Name="CenterLatitude"/><Units:degree/>, <Arg Name="CenterLongitude"/><Units:degree/></Syslog>

  }

  # Missions should almost always start with standard safety 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/AbortDrift.tl {
    redefineArg AcousticTimeout = AcousticTrackingTimeout
  }

  # Many missions will keep mass position fixed at the default. This mission allows the buoyancy volume to change to track the chlorophyll peak while drifting.

  behavior Guidance:Pitch {
    run in parallel

    set massPosition = MassDefault
  }

  insert Insert/Science.tl

  behavior Guidance:AltitudeEnvelope {
    """
    Another altitude envelope for the YoYo behavior. This envelope
    should fall within the limits of the standard safety envelopes in
    Insert/StandardEnvelopes.tl in order to avoid commanding high pitch
    angles for bottom-terminated YoYos.
    """

    run in parallel

    set minAltitude = YoYoMinAltitude
  }

  aggregate MissionStart {
    run in sequence

    insert id="NeedComms" Insert/NeedComms.tl

    call id="MissionStartComms" refId="NeedComms"

    assign in sequence MissionStartCommsCompleted = true
  }

  aggregate CircleAtDesignatedDepth {
    run when ( called )

    syslog important "Circle at depth " + DepthSet~meter + " around location "
         + CenterLatitude~degree + "," + CenterLongitude~degree + ", at radius = " + ScrewRadius~meter

    aggregate CircleWrapper {
      run in sequence repeat=MaxCirclesEachDepthOnScrew

      timeout duration=ScrewEachDepthTimeout

      assign in parallel Control:HorizontalControl.kwpHeading = KwpHeading

      behavior Guidance:Circle {
        run in sequence

        set latitude = CenterLatitude
        set longitude = CenterLongitude
        set radius = ScrewRadius
        set maxError = ScrewCircleMaxError
        set turnToPort = ScrewCircleTurnToPort
      }
    }
  }

  aggregate OuterCylinderAndInnerScrew {
    run in sequence repeat=NumRoundsWholeSet

    aggregate OuterCylinder {
      run in sequence

      behavior Guidance:SetSpeed {
        run in parallel

        set speed = SpeedCylinder
      }

      behavior Guidance:DepthEnvelope {
        run in parallel

        set minDepth = YoYoMinDepth
        set maxDepth = YoYoMaxDepth
        set downPitch = YoYoDownPitch
        set upPitch = YoYoUpPitch
      }

      behavior Guidance:YoYo {
        run in parallel

        set downPitch = YoYoDownPitch
        set upPitch = YoYoUpPitch
      }

      behavior Guidance:Buoyancy {
        run in parallel

        set position = BuoyancyNeutral
      }

      aggregate OuterCylinderYoYo {
        run in sequence

        aggregate CylinderIfNonNaNLatLon {
          run in sequence

          break if (
            not ( OuterCylinder )
            or ( isNaN ( CenterLatitude ) )
            or ( isNaN ( CenterLongitude ) )
          )

          timeout duration=CylinderTimeout

          syslog important "Yoyo profile on a cylinder around location " + CenterLatitude~degree
               + "," + CenterLongitude~degree + ", at radius = " + CylinderRadius~meter

          assign in sequence YoYoMinDepth = CylinderYoYoMinDep

          assign in sequence YoYoMaxDepth = CylinderYoYoMaxDep

          aggregate CylinderWrapper {
            run in sequence repeat=MaxCirclesOnCylinder

            assign in parallel Control:HorizontalControl.kwpHeading = KwpHeading

            behavior Guidance:Circle {
              run in sequence

              set latitude = CenterLatitude
              set longitude = CenterLongitude
              set radius = CylinderRadius
              set maxError = CylinderCircleMaxError
              set turnToPort = CylinderCircleTurnToPort
            }
          }
        }

        syslog "Outer cylinder completed."

        assign in sequence StartScrew = true

        assign in sequence DepthCompleted = false

        assign in sequence CntScrews = 1 count

        assign in sequence CntDepths = 1 count
      }
    }

    aggregate InnerScrew {
      run in sequence

      break if (
        not ( InnerScrew )
        or ( isNaN ( CenterLatitude ) )
        or ( isNaN ( CenterLongitude ) )
        or ( CntScrews == ( NumScrewsPerCylinder + 1 count ) )
      )

      aggregate DepthHolding {
        run while (
          MissionStartCommsCompleted
          and StartScrew
        )

        behavior Guidance:Pitch {
          run in parallel

          set depth = DepthSet
        }

        behavior Guidance:SetSpeed {
          run in parallel

          set speed = SpeedScrew
        }

        behavior Guidance:Buoyancy {
          run in parallel

          set position = BuoyancyNeutral
        }
      }

      aggregate ScrewThroughDepths {
        run in parallel

        macro $k = 1..20 {
          aggregate SetOptionsOfDepth$k {
            run when (
              not ( DepthOptionsSet )
              and ( CntDepths == $k count )
              and ( CntDepths <= NumDepths )
            )

            assign in sequence DepthSet = Depth[$k]

            assign in sequence DepthOptionsSet = true
          }
        }

        aggregate CircleAtDepth {
          run when (
            MissionStartCommsCompleted
            and StartScrew
            and DepthOptionsSet
            and ( not ( DepthCompleted ) )
          )

          syslog important "Go to depth " + DepthSet~meter + "."

          behavior Guidance:Circle {
            run in progression

            set latitude = CenterLatitude
            set longitude = CenterLongitude
            set radius = ScrewRadius
            set maxError = ScrewCircleMaxError
            set turnToPort = ScrewCircleTurnToPort
          }

          behavior Guidance:Pitch {
            run in progression

            timeout duration=MaxWaitNotAchievingDepth {
              syslog important "Timed out trying to reach the targeted depth. Stopping mission."

              behavior Guidance:Execute {
                run in sequence

                set command = "stop"
              }
            }

            set depth = DepthSet
          }

          call id="Hold" refId="CircleAtDesignatedDepth"

          assign in sequence DepthCompleted = true

          assign in sequence DepthOptionsSet = false
        }

        aggregate IncrementDepthCounter {
          run when ( DepthCompleted )

          assign in sequence CntDepths = CntDepths + 1 count

          syslog important "CntDepths = " + CntDepths~count + "."

          assign in sequence DepthCompleted = false
        }

        aggregate IncrementScrewCounter {
          run when (
            DepthCompleted
            and ( CntDepths == ( NumDepths + 1 count ) )
          )

          assign in sequence CntDepths = 1 count

          assign in sequence CntScrews = CntScrews + 1 count

          syslog important "CntScrews = " + CntScrews~count + "."
        }
      }
    }
  }

  aggregate MissionCompleted {
    run in sequence

    syslog important "Completed outer cylinders and inner screws. Stopping mission."

    behavior Guidance:Execute {
      run in sequence

      set command = "stop"
    }
  }
}