Engineering/passive_acoustic_monitoring.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 PAM {
  """
  The mission will dive LRAUV to the start/center depth, then hold the
  vehicle at neutral buoyancy/trim with all actuators disengaged. The
  science payload will be powered.
  """

  arguments {
    MissionTimeout = 180 minute
      """
      Maximum duration of mission
      """

    AcousticTrackingTimeout = 240 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.
      """

    SurfacingIntervalDuringListening = 180 minute
      """
      When the listening depth bound is breached and the submergence time has
      surpassed SurfacingIntervalDuringListening, the vehicle will surface.
      """

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

    NeedCommsMaxWait = 180 minute
      """
      If the submergence time has surpassed NeedCommsMaxWait, the vehicle must
      surface, to caution against drifting too close to shore during
      submergence. Can set it to a huge number to effectively disable it.
      """

    NeedCommsTimeInTransit = 60 minute
      """
      How often to surface for commumications during transit.
      """

    SkipStartComms = false
      """
      Skip communications at start of mission.
      """

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

    Depth = 30 meter
      """
      Center depth of listening drift.
      """

    SettleTime = 5 minute
      """
      Time to wait after reaching target depth.
      """

    DeepBound = 32 meter
      """
      Deep depth bound of listening drift. Vehicle will return to center depth
      when depth bound is exceeded.
      """

    ShallowBound = 28 meter
      """
      Shallow depth bound of listening drift. Vehicle will return to center
      depth when depth bound is exceeded.
      """

    DeepBoundFineTune = 40 meter
      """
      Deep depth bound (deeper than DeepBound) used in fine-tune phase.
      """

    ShallowBoundFineTune = 20 meter
      """
      Shallow depth bound (shallower than ShallowBound) used in fine-tune
      phase.
      """

    ThreshQuietTimeFineTune = 60 minute
      """
      When the quiet time exceeds this threshold, the vehicle enters the
      fine-tune phase.
      """

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

    WaitAfterBuoyancyChangeFineTune = 20 minute
      """
      Wait time after buoyancy change in fine-tune phase.
      """

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

    InitialYoyoToListeningLocation = false
      """
      Initial yoyo transit to the listening location.
      """

    InitialLevelRunToListeningLocation = true
      """
      Initial level run to the listening location.
      """

    YoyoBackToListeningLocation = false
      """
      Yoyo back to the listening location.
      """

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

    LevelRunBackToListeningLocation = true
      """
      Level run back to the listening location.
      """

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

    DepthLevelRunToListeningLocation = 30 meter
      """
      Level run depth to the listening location.
      """

    SpeedTransit = 1 meter_per_second
      """
      Vehicle speed when transiting.
      """

    ApproachSpeedFirstTime = 1 meter_per_second
      """
      Vehicle thruster speed used to reach center depth for the 1st time.
      """

    RudderAngleInitialDive = 9 degree
      """
      Rudder angle in initial spiral-down to leave surface. Initialized to 9
      degrees.
      """

    RudderAngleDepthSettlingPropOn = 13 degree
      """
      Rudder angle during depth settling when propeller is on.
      """

    ApproachNotFirstTimePropOn = false
      """
      True: propeller on. False: propeller off.
      """

    MinAltitude = 5 meter

    MaxDepth = 50 meter

    MinOffshore = 2 kilometer

    TransitYoYoMinDepth = 2 meter
      """
      Minimum depth while performing the YoYo behavior.
      """

    TransitYoYoMaxDepth = 30 meter
      """
      Maximum depth while performing the YoYo behavior.
      """

    LatListening = NaN degree
      """
      Nominal latitude of acoustic monitoring.
      """

    # debug
    #                             </Description><Units:degree/><Value>36.815</Value></DefineArg>
    #                             </Description><Units:degree/><Value>36.810</Value></DefineArg>

    LonListening = NaN degree
      """
      Nominal longitude of acoustic monitoring.
      """

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

    CheckDepths = 1 bool

    KpBuoyancyServo = 4000 count
      """
      Proportional gain for buoyancy servo feedback control.
      """

    KiBuoyancyServo = 200 count
      """
      Integral gain for buoyancy servo feedback control.
      """

    KdBuoyancyServo = 300 count
      """
      Derivative gain for buoyancy servo feedback control.
      """

    BuoyancyErrorAllowance = 2 cubic_centimeter

    BuoyancyAdjustmentStepUnitless = 16 none
      """
      Initial value of buoyancy adjustment step size. Adjusted during mission.
      """

    # debug
    #    </Description><Units:none/><Value>64</Value></DefineArg>

    BuoyancyAdjustmentStepMinUnitless = 2 none
      """
      Minimum value of buoyancy adjustment step size.
      """

    # debug
    #                    </Description><Units:none/><Value>32</Value></DefineArg>

    BuoyancyAdjustmentStepFineTuneUnitless = 2 none
      """
      Buoyancy adjustment step size in fine-tune phase.
      """

    # debug
    #             </Description><Units:none/><Value>32</Value></DefineArg>

    MassPositionAdjustmentMMperCC = 0.033 millimeter
      """
      1 cc of buoyancy adjustment corresponds to MassPositionAdjustmentMMperCC mm of mass position adjustment. Mass moving forward: positive; moving backward: negative.
      """

    # You probably do not need to change these.

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

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

  output {
    # Internal variables that you do not need to change

    SettleSpeed = 1 meter_per_second

    SinkingOrFloatingRate = NaN meter_per_second
      """
      Depth rate of sinking or floating from the target depth to the depth
      bound being violated.
      """

    NeutralBuoyancy = Control:VerticalControl.buoyancyNeutral

    NeutralBuoyancyPrevious = NaN cubic_centimeter

    BuoyancyBase = 1 cubic_centimeter

    BuoyancyAdjustmentStep = NaN cubic_centimeter

    BuoyancyAdjustmentStepMin = NaN cubic_centimeter

    BuoyancyAdjustmentStepFineTune = NaN cubic_centimeter

    BuoyancyAdjustmentStepPrevious = NaN cubic_centimeter

    BuoyancyAdjustmentStepNew = NaN cubic_centimeter

    MassPosition = Control:VerticalControl.massDefault

    MassPositionPrevious = NaN millimeter

    MassPositionAdjustmentStep = NaN millimeter

    MassPositionAdjustmentStepFineTune = NaN millimeter

    MassPositionAdjustmentStepPrevious = NaN millimeter

    MassPositionAdjustmentStepNew = NaN millimeter

    DeepBoundRegainDepth = NaN meter

    ShallowBoundRegainDepth = NaN meter

    DeepBoundMinusTargetDepth = NaN meter

    DepthMinusTargetDepth = NaN meter

    TargetDepthMinusShallowBound = NaN meter

    TargetDepthMinusDepth = NaN meter

    WaitAfterBuoyancyChange = 0 second

    TimeTargetDepth = NaN second

    ElapsedSinceReachingTargetDepth = NaN second

    ElapsedSinceLastGPSfix = NaN second

    InitialTransitDone = true

    FineTune = false

    MovingToTargetDepth = false

    BuoyancyAdjustmentDone = false

    AdjustingBuoyancy = false

    Settled = false

    DeltaReduced = false

    Sinking = false

    SurfacingStarted = false

    Cnt = 0 count
  }

  # The timeout

  timeout duration=MissionTimeout

  insert Insert/Science.tl

  insert id="NeedComms" Insert/NeedComms.tl

  assign in sequence NeedComms:DiveInterval = NeedCommsMaxWait

  assign in sequence NeedComms:WaitForPitchUp = 0 minute

  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
  }

  assign in parallel Servo:BuoyancyServo.deviationVolume = BuoyancyErrorAllowance

  assign in parallel Servo:BuoyancyServo.pidW = KpBuoyancyServo

  assign in parallel Servo:BuoyancyServo.pidX = KiBuoyancyServo

  assign in parallel Servo:BuoyancyServo.pidY = KdBuoyancyServo

  assign in sequence BuoyancyAdjustmentStep = BuoyancyBase * BuoyancyAdjustmentStepUnitless

  assign in sequence BuoyancyAdjustmentStepMin = BuoyancyBase * BuoyancyAdjustmentStepMinUnitless

  assign in sequence BuoyancyAdjustmentStepFineTune = BuoyancyBase * BuoyancyAdjustmentStepFineTuneUnitless

  assign in sequence MassPositionAdjustmentStep = MassPositionAdjustmentMMperCC * BuoyancyAdjustmentStepUnitless

  assign in sequence MassPositionAdjustmentStepFineTune = MassPositionAdjustmentMMperCC * BuoyancyAdjustmentStepFineTuneUnitless

  aggregate CommsAndOptionallyTransitBackToListeningLocation {
    run when ( called )

    aggregate CommsOnly {
      run in sequence

      break if (
        ( YoyoBackToListeningLocation
          or LevelRunBackToListeningLocation
        )
        and (
          ( LatListening >= 0 degree
            or ( LatListening < 0 degree )
          )
          and (
            LonListening >= 0 degree
            or ( LonListening < 0 degree )
          )
        )
      )

      call id="SurfacingCommsOnly" refId="NeedComms"

      assign in sequence Cnt = 1 count

      assign in sequence MovingToTargetDepth = false

      assign in sequence SurfacingStarted = false
    }

    aggregate CommsAndYoYo {
      run in sequence

      break if (
        not ( YoyoBackToListeningLocation )
        or (
          not (
            ( LatListening >= 0 degree
              or ( LatListening < 0 degree )
            )
            and (
              LonListening >= 0 degree
              or ( LonListening < 0 degree )
            )
          )
        )
      )

      call id="YoyoStartComms" refId="NeedComms"

      syslog important "Yoyo back to listening location LatListening = " + LatListening~degree
           + ", LonListening = " + LonListening~degree + "."

      assign in sequence NeedComms:DiveInterval = NeedCommsTimeInTransit

      behavior Guidance:SetSpeed {
        run in parallel

        set speed = SpeedTransit
      }

      behavior Guidance:DepthEnvelope {
        run in parallel

        set minDepth = TransitYoYoMinDepth
        set maxDepth = TransitYoYoMaxDepth
        set downPitch = YoYoDownPitch
        set upPitch = YoYoUpPitch
      }

      behavior Guidance:YoYo {
        run in parallel

        set downPitch = YoYoDownPitch
        set upPitch = YoYoUpPitch
      }

      behavior Guidance:Pitch {
        run in parallel

        set massPosition = MassPosition
      }

      behavior Guidance:Buoyancy {
        run in parallel

        set position = NeutralBuoyancy
      }

      behavior Guidance:Waypoint {
        run in sequence

        set latitude = LatListening
        set longitude = LonListening
      }

      assign in sequence NeedComms:DiveInterval = NeedCommsMaxWait

      assign in sequence Cnt = 1 count

      assign in sequence MovingToTargetDepth = false

      assign in sequence SurfacingStarted = false
    }

    aggregate CommsAndLevelRun {
      run in sequence

      break if (
        not ( LevelRunBackToListeningLocation )
        or (
          not (
            ( LatListening >= 0 degree
              or ( LatListening < 0 degree )
            )
            and (
              LonListening >= 0 degree
              or ( LonListening < 0 degree )
            )
          )
        )
      )

      call id="LevelRunStartComms" refId="NeedComms"

      syslog important "Level run back to listening location LatListening = " + LatListening~degree
           + ", LonListening = " + LonListening~degree + "."

      assign in sequence NeedComms:DiveInterval = NeedCommsTimeInTransit

      behavior Guidance:SetSpeed {
        run in parallel

        set speed = SpeedTransit
      }

      behavior Guidance:Mass {
        run in parallel

        set position = MassPosition
      }

      behavior Guidance:Buoyancy {
        run in parallel

        set position = NeutralBuoyancy
      }

      behavior Guidance:Pitch {
        run in parallel

        set depth = DepthLevelRunToListeningLocation
      }

      behavior Guidance:Waypoint {
        run in sequence

        set latitude = LatListening
        set longitude = LonListening
      }

      assign in sequence NeedComms:DiveInterval = NeedCommsMaxWait

      assign in sequence Cnt = 1 count

      assign in sequence MovingToTargetDepth = false

      assign in sequence SurfacingStarted = false
    }
  }

  syslog important "NeutralBuoyancy = " + NeutralBuoyancy~cubic_centimeter + "."

  syslog important "BuoyancyAdjustmentStep = " + BuoyancyAdjustmentStep~cubic_centimeter + "."

  syslog important "BuoyancyAdjustmentStepFineTune = "
       + BuoyancyAdjustmentStepFineTune~cubic_centimeter + "."

  syslog important "MassPosition = " + MassPosition~millimeter + "."

  syslog important "MassPositionAdjustmentStep = " + MassPositionAdjustmentStep~millimeter + "."

  syslog important "MassPositionAdjustmentStepFineTune = "
       + MassPositionAdjustmentStepFineTune~millimeter + "."

  assign in sequence DeepBoundRegainDepth = DeepBound

  assign in sequence ShallowBoundRegainDepth = ShallowBound

  syslog important "ShallowBound = " + ShallowBound~meter + ", ShallowBoundRegainDepth = "
       + ShallowBoundRegainDepth~meter + "."

  syslog important "DeepBound = " + DeepBound~meter + ", DeepBoundRegainDepth = "
       + DeepBoundRegainDepth~meter + "."

  assign in sequence TargetDepthMinusShallowBound = Depth - ShallowBound

  syslog important "TargetDepthMinusShallowBound = " + TargetDepthMinusShallowBound~meter + "."

  assign in sequence DeepBoundMinusTargetDepth = DeepBound - Depth

  syslog important "DeepBoundMinusTargetDepth = " + DeepBoundMinusTargetDepth~meter + "."

  aggregate CheckTargetDepths {
    run in sequence

    break if ( not ( CheckDepths ) )

    aggregate CheckDepth {
      run in sequence

      break if (
        not (
          Depth > DeepBound
          or ( Depth < ShallowBound )
        )
      )

      syslog important "Aborting mission. Target depth is deeper than DeepBound or shallower than ShallowBound."

      behavior Guidance:Execute {
        run in sequence

        set command = "stop"
      }
    }

    assign in sequence CheckDepths = 0 bool
  }

  aggregate StartingMission {
    run in sequence

    break if ( SkipStartComms )

    # DO NOT REMOVE the syslog entry below. The MissionManager seems to ignore the value of SkipComms, does not evaluate the Break statement properly, and always calls NeedComms. Adding the syslog call somehow fixes it.

    syslog info "Checking for additional instructions before submerging."

    call id="StartingMission" refId="NeedComms"
  }

  aggregate InitialTransitToListeningLocation {
    run in sequence

    aggregate InitialYoYo {
      run in sequence

      break if (
        not ( InitialYoyoToListeningLocation )
        or (
          not (
            ( LatListening >= 0 degree
              or ( LatListening < 0 degree )
            )
            and (
              LonListening >= 0 degree
              or ( LonListening < 0 degree )
            )
          )
        )
      )

      assign in sequence InitialTransitDone = false

      syslog important "Initial yoyo transit to listening location LatListening = "
           + LatListening~degree + ", LonListening = " + LonListening~degree + "."

      assign in sequence NeedComms:DiveInterval = NeedCommsTimeInTransit

      behavior Guidance:SetSpeed {
        run in parallel

        set speed = SpeedTransit
      }

      behavior Guidance:DepthEnvelope {
        run in parallel

        set minDepth = TransitYoYoMinDepth
        set maxDepth = TransitYoYoMaxDepth
        set downPitch = YoYoDownPitch
        set upPitch = YoYoUpPitch
      }

      behavior Guidance:YoYo {
        run in parallel

        set downPitch = YoYoDownPitch
        set upPitch = YoYoUpPitch
      }

      behavior Guidance:Pitch {
        run in parallel

        set massPosition = MassPosition
      }

      behavior Guidance:Buoyancy {
        run in parallel

        set position = NeutralBuoyancy
      }

      behavior Guidance:Waypoint {
        run in sequence

        set latitude = LatListening
        set longitude = LonListening
      }

      call id="InitialYoYoReachedWaypoint" refId="NeedComms"

      assign in sequence NeedComms:DiveInterval = NeedCommsMaxWait

      assign in sequence InitialTransitDone = true
    }

    aggregate InitialLevelRun {
      run in sequence

      break if (
        not ( InitialLevelRunToListeningLocation )
        or (
          not (
            ( LatListening >= 0 degree
              or ( LatListening < 0 degree )
            )
            and (
              LonListening >= 0 degree
              or ( LonListening < 0 degree )
            )
          )
        )
      )

      assign in sequence InitialTransitDone = false

      syslog important "Initial level run transit to listening location LatListening = "
           + LatListening~degree + ", LonListening = " + LonListening~degree + "."

      assign in sequence NeedComms:DiveInterval = NeedCommsTimeInTransit

      behavior Guidance:SetSpeed {
        run in parallel

        set speed = SpeedTransit
      }

      behavior Guidance:Mass {
        run in parallel

        set position = MassPosition
      }

      behavior Guidance:Buoyancy {
        run in parallel

        set position = NeutralBuoyancy
      }

      behavior Guidance:Pitch {
        run in parallel

        set depth = DepthLevelRunToListeningLocation
      }

      behavior Guidance:Waypoint {
        run in sequence

        set latitude = LatListening
        set longitude = LonListening
      }

      call id="InitialLevelRunReachedWaypoint" refId="NeedComms"

      assign in sequence NeedComms:DiveInterval = NeedCommsMaxWait

      assign in sequence InitialTransitDone = true
    }
  }

  aggregate PAM {
    run in sequence repeat=999

    aggregate SettingVariables {
      run in sequence

      break if ( SurfacingStarted )

      behavior Guidance:Mass {
        run in parallel

        set position = MassPosition
      }

      behavior Guidance:Buoyancy {
        run in parallel

        set position = NeutralBuoyancy
      }

      assign in sequence Cnt = Cnt + 1 count

      syslog important "Cnt = " + Cnt~count

      syslog important "NeutralBuoyancy = " + NeutralBuoyancy~cubic_centimeter
           + ", BuoyancyAdjustmentStep = " + BuoyancyAdjustmentStep~cubic_centimeter + ", MassPosition = "
           + MassPosition~millimeter + ", MassPositionAdjustmentStep = "
           + MassPositionAdjustmentStep~millimeter

      assign in sequence Settled = false

      assign in sequence BuoyancyAdjustmentDone = false

      assign in sequence DeltaReduced = false
    }

    aggregate Waits {
      run in sequence

      behavior Guidance:Mass {
        run in parallel

        set position = MassPosition
      }

      behavior Guidance:Buoyancy {
        run in parallel

        set position = NeutralBuoyancy
      }

      aggregate WaitForSurfaceCommsCompletion {
        run in sequence

        break if ( not ( SurfacingStarted ) )

        behavior Guidance:Wait {
          run in sequence

          set duration = 1 minute
        }
      }

      aggregate WaitBuoyancyChange {
        run in sequence

        # WaitAfterBuoyancyChange is nonzero only when FineTune is True

        behavior Guidance:Wait {
          run in sequence

          set duration = WaitAfterBuoyancyChange
        }
      }
    }

    aggregate ApproachDepthSettleAtDepth {
      run in sequence

      break if ( SurfacingStarted )

      aggregate ApproachDepth {
        run in sequence

        break if (
          AdjustingBuoyancy
          or (
            not ( MovingToTargetDepth )
            and ( Universal:depth <= DeepBoundRegainDepth )
            and ( Universal:depth >= ShallowBoundRegainDepth )
          )
        )

        aggregate ApproachDepthPropellerOn {
          run in sequence

          break if (
            not (
              ApproachNotFirstTimePropOn
              or ( Cnt == 1 count )
            )
          )

          behavior Guidance:Mass {
            run in parallel

            set position = MassPosition
          }

          behavior Guidance:Buoyancy {
            run in parallel

            set position = NeutralBuoyancy
          }

          assign in sequence MovingToTargetDepth = true

          syslog important "Depth " + Universal:depth~meter
               + " outside of the envelope defined by ShallowBoundRegainDepth = "
               + ShallowBoundRegainDepth~meter + " and DeepBoundRegainDepth = " + DeepBoundRegainDepth~meter
               + ". Moving to " + Depth~meter + ". Propeller on."

          behavior Guidance:Point {
            run in parallel

            set rudderAngle = RudderAngleInitialDive
          }

          behavior Guidance:SetSpeed {
            run in parallel

            set speed = ApproachSpeedFirstTime
          }

          behavior Guidance:Pitch {
            run in sequence

            set depth = Depth
          }
        }

        aggregate ApproachDepthPropellerOff {
          run in sequence

          break if (
            ApproachNotFirstTimePropOn
            or ( Cnt == 1 count )
          )

          behavior Guidance:Mass {
            run in parallel

            set position = MassPosition
          }

          assign in sequence MovingToTargetDepth = true

          syslog important "Depth " + Universal:depth~meter
               + " breaches the envelope defined by ShallowBoundRegainDepth = " + ShallowBoundRegainDepth~meter
               + " and DeepBoundRegainDepth = " + DeepBoundRegainDepth~meter + ". Moving to " + Depth~meter
               + ". Propeller off."

          behavior Guidance:Point {
            run in parallel

            set rudderAngle = 0 degree
          }

          behavior Guidance:SetSpeed {
            run in parallel

            set speed = 0 meter_per_second
          }

          behavior Guidance:Pitch {
            run in sequence

            set depth = Depth
          }
        }
      }

      aggregate Settle {
        run in sequence

        break if ( not ( MovingToTargetDepth ) )

        behavior Guidance:Mass {
          run in parallel

          set position = MassPosition
        }

        behavior Guidance:Buoyancy {
          run in parallel

          set position = NeutralBuoyancy
        }

        aggregate VBSDepthLimit {
          """
          Check if the vehicle can settle using the VBS.
          """

          run in sequence

          aggregate ShallowerThanVBSDepthLimit {
            run in sequence

            break if (
              Universal:depth > Control:VerticalControl.buoyancyPumpDepth
            )

            assign in sequence SettleSpeed = 0 meter_per_second
          }

          aggregate DeeperThanVBSDepthLimit {
            run in sequence

            break if (
              Universal:depth <= Control:VerticalControl.buoyancyPumpDepth
            )

            assign in sequence SettleSpeed = 1 meter_per_second
          }
        }

        aggregate settle_at_depth {
          run in sequence

          syslog important "Reached " + Universal:depth~meter + ", settling for "
               + SettleTime~minute + "at" + SettleSpeed~meter_per_second

          aggregate SettleAtDepthPropellerOn {
            run in sequence

            break if ( not ( SettleSpeed > 0 meter_per_second ) )

            behavior Guidance:Point {
              run in parallel

              set rudderAngle = RudderAngleDepthSettlingPropOn
            }

            behavior Guidance:SetSpeed {
              run in parallel

              set speed = SettleSpeed
            }

            behavior Guidance:Pitch {
              run in parallel

              set depth = Depth
            }

            assign in sequence TimeTargetDepth = 0 second

            behavior Guidance:Wait {
              run in sequence

              set duration = SettleTime
            }

            assign in sequence MovingToTargetDepth = false

            assign in sequence Settled = true
          }

          aggregate SettleAtDepthPropellerOff {
            run in sequence

            break if ( SettleSpeed > 0 meter_per_second )

            behavior Guidance:Point {
              run in parallel

              set rudderAngle = 0 degree
            }

            behavior Guidance:SetSpeed {
              run in parallel

              set speed = SettleSpeed
            }

            behavior Guidance:Pitch {
              run in parallel

              set depth = Depth
            }

            assign in sequence TimeTargetDepth = 0 second

            behavior Guidance:Wait {
              run in sequence

              set duration = SettleTime
            }

            assign in sequence MovingToTargetDepth = false

            assign in sequence Settled = true
          }
        }
      }

      aggregate Hover {
        """
        Hover neutral within depth bound.
        """

        run in sequence

        break if (
          Universal:depth > DeepBound
          or ( Universal:depth < ShallowBound )
        )

        behavior Guidance:Mass {
          run in parallel

          set position = MassPosition
        }

        behavior Guidance:Buoyancy {
          run in parallel

          set position = NeutralBuoyancy
        }

        behavior Guidance:Pitch {
          run in parallel

          set elevatorAngle = 0 degree
        }

        behavior Guidance:Point {
          run in parallel

          set rudderAngle = 0 degree
        }

        behavior Guidance:SetSpeed {
          run in parallel

          set speed = 0 meter_per_second
        }

        behavior Guidance:Wait {
          run in sequence

          set duration = MissionTimeout
        }
      }

      aggregate DeeperThanDeepBound {
        run in sequence

        break if (
          Universal:depth <= DeepBound
          or (
            MovingToTargetDepth
            and ( not ( Settled ) )
          )
        )

        behavior Guidance:Mass {
          run in parallel

          set position = MassPosition
        }

        behavior Guidance:Buoyancy {
          run in parallel

          set position = NeutralBuoyancy
        }

        assign in sequence AdjustingBuoyancy = true

        assign in sequence DepthMinusTargetDepth = Universal:depth - Depth

        assign in sequence ElapsedSinceReachingTargetDepth = elapsed ( TimeTargetDepth )

        assign in sequence SinkingOrFloatingRate = DepthMinusTargetDepth / ElapsedSinceReachingTargetDepth

        syslog important "Now depth " + Universal:depth~meter + " is deeper than DeepBound "
             + DeepBound~meter + ". SinkingOrFloatingRate = DepthMinusTargetDepth "
             + DepthMinusTargetDepth~meter + " divided by " + ElapsedSinceReachingTargetDepth~second + " = "
             + SinkingOrFloatingRate~meter_per_second

        aggregate TurnOnFineTuneModeDeeperThanDeepBound {
          run in sequence

          break if (
            ElapsedSinceReachingTargetDepth < ThreshQuietTimeFineTune
            and ( BuoyancyAdjustmentStep >= ( BuoyancyAdjustmentStepMin * 2 none ) )
            or FineTune
          )

          assign in sequence DeepBoundRegainDepth = DeepBoundFineTune

          assign in sequence ShallowBoundRegainDepth = ShallowBoundFineTune

          assign in sequence BuoyancyAdjustmentStep = BuoyancyAdjustmentStepFineTune

          assign in sequence MassPositionAdjustmentStep = MassPositionAdjustmentStepFineTune

          assign in sequence WaitAfterBuoyancyChange = WaitAfterBuoyancyChangeFineTune

          assign in sequence FineTune = true
        }

        aggregate SurfacingDeeperThanDeepBound {
          run when (
            elapsed ( Universal:time_fix ) > SurfacingIntervalDuringListening
          )

          assign in sequence ElapsedSinceLastGPSfix = elapsed ( Universal:time_fix )

          syslog important "Elapsed time since the last GPS fix " + ElapsedSinceLastGPSfix~minute
               + " is longer than SurfacingIntervalDuringListening " + SurfacingIntervalDuringListening~minute
               + ". Surface and optionally transit back to listening location."

          assign in sequence SurfacingStarted = true

          call id="CommsAndOptionalTransitBack" refId="CommsAndOptionallyTransitBackToListeningLocation"

          assign in sequence AdjustingBuoyancy = false
        }

        aggregate IncreaseNeutralBuoyancy {
          run in sequence

          aggregate IncreaseNeutralBuoyancyDecreaseDelta {
            run in sequence

            break if (
              Cnt == 1 count
              or Sinking
              or ( BuoyancyAdjustmentStep < ( BuoyancyAdjustmentStepMin * 2 none ) )
              or FineTune
              or BuoyancyAdjustmentDone
            )

            assign in sequence DeltaReduced = true

            assign in sequence BuoyancyAdjustmentStepPrevious = BuoyancyAdjustmentStep

            assign in sequence MassPositionAdjustmentStepPrevious = MassPositionAdjustmentStep

            assign in sequence NeutralBuoyancyPrevious = NeutralBuoyancy

            assign in sequence MassPositionPrevious = MassPosition

            assign in sequence BuoyancyAdjustmentStepNew = BuoyancyAdjustmentStep / 2 none

            assign in sequence MassPositionAdjustmentStepNew = MassPositionAdjustmentStep / 2 none

            assign in sequence NeutralBuoyancy = NeutralBuoyancy + BuoyancyAdjustmentStepNew

            assign in sequence MassPosition = MassPosition - MassPositionAdjustmentStepNew

            syslog important "Floating previously but sinking now. Decrease BuoyancyAdjustmentStep from "
                 + BuoyancyAdjustmentStepPrevious~cubic_centimeter + " to "
                 + BuoyancyAdjustmentStepNew~cubic_centimeter + ". Decrease MassPositionAdjustmentStep from "
                 + MassPositionAdjustmentStepPrevious~millimeter + " to "
                 + MassPositionAdjustmentStepNew~millimeter + ". Increase neutral buoyancy: "
                 + NeutralBuoyancyPrevious~cubic_centimeter + " + " + BuoyancyAdjustmentStepNew~cubic_centimeter
                 + " = " + NeutralBuoyancy~cubic_centimeter + ". Move mass backwards: "
                 + MassPositionPrevious~millimeter + " - " + MassPositionAdjustmentStepNew~millimeter + " = "
                 + MassPosition~millimeter + ". FineTune = " + FineTune~bool + ". WaitAfterBuoyancyChange = "
                 + WaitAfterBuoyancyChange~minute + "."

            assign in sequence MassPositionAdjustmentStep = MassPositionAdjustmentStepNew

            assign in sequence BuoyancyAdjustmentStep = BuoyancyAdjustmentStepNew

            assign in sequence AdjustingBuoyancy = false

            assign in sequence Sinking = true

            assign in sequence BuoyancyAdjustmentDone = true
          }

          aggregate IncreaseNeutralBuoyancyDoNotChangeDelta {
            run in sequence

            break if (
              not ( FineTune )
              and ( not ( Sinking ) )
              and ( BuoyancyAdjustmentStep >= ( BuoyancyAdjustmentStepMin * 2 none ) )
              and ( Cnt >= 2 count )
              or BuoyancyAdjustmentDone
              or DeltaReduced
            )

            assign in sequence NeutralBuoyancyPrevious = NeutralBuoyancy

            assign in sequence MassPositionPrevious = MassPosition

            assign in sequence NeutralBuoyancy = NeutralBuoyancy + BuoyancyAdjustmentStep

            assign in sequence MassPosition = MassPosition - MassPositionAdjustmentStep

            syslog important "Vehicle is too heavy. Increase neutral buoyancy: "
                 + NeutralBuoyancyPrevious~cubic_centimeter + " + " + BuoyancyAdjustmentStep~cubic_centimeter
                 + " = " + NeutralBuoyancy~cubic_centimeter + ". Move mass backwards: "
                 + MassPositionPrevious~millimeter + " - " + MassPositionAdjustmentStep~millimeter + " = "
                 + MassPosition~millimeter + ". FineTune = " + FineTune~bool + ". WaitAfterBuoyancyChange = "
                 + WaitAfterBuoyancyChange~minute + "."

            assign in sequence AdjustingBuoyancy = false

            assign in sequence Sinking = true

            assign in sequence BuoyancyAdjustmentDone = true
          }
        }
      }

      aggregate ShallowerThanShallowBound {
        run in sequence

        break if (
          Universal:depth >= ShallowBound
          or (
            MovingToTargetDepth
            and ( not ( Settled ) )
          )
          or ( elapsed ( Universal:time_fix ) > NeedCommsMaxWait )
        )

        behavior Guidance:Mass {
          run in parallel

          set position = MassPosition
        }

        behavior Guidance:Buoyancy {
          run in parallel

          set position = NeutralBuoyancy
        }

        assign in sequence AdjustingBuoyancy = true

        assign in sequence TargetDepthMinusDepth = Depth - Universal:depth

        assign in sequence ElapsedSinceReachingTargetDepth = elapsed ( TimeTargetDepth )

        assign in sequence SinkingOrFloatingRate = TargetDepthMinusDepth / ElapsedSinceReachingTargetDepth

        syslog important "Now depth " + Universal:depth~meter + " is shallower than ShallowBound "
             + ShallowBound~meter + ". SinkingOrFloatingRate = TargetDepthMinusDepth "
             + TargetDepthMinusDepth~meter + " divided by " + ElapsedSinceReachingTargetDepth~second + " = "
             + SinkingOrFloatingRate~meter_per_second + "."

        aggregate TurnOnFineTuneModeShallowerThanShallowBound {
          run in sequence

          break if (
            ElapsedSinceReachingTargetDepth < ThreshQuietTimeFineTune
            and ( BuoyancyAdjustmentStep >= ( BuoyancyAdjustmentStepMin * 2 none ) )
            or FineTune
          )

          assign in sequence DeepBoundRegainDepth = DeepBoundFineTune

          assign in sequence ShallowBoundRegainDepth = ShallowBoundFineTune

          assign in sequence BuoyancyAdjustmentStep = BuoyancyAdjustmentStepFineTune

          assign in sequence MassPositionAdjustmentStep = MassPositionAdjustmentStepFineTune

          assign in sequence WaitAfterBuoyancyChange = WaitAfterBuoyancyChangeFineTune

          assign in sequence FineTune = true
        }

        aggregate SurfacingShallowerThanShallowBound {
          run when (
            elapsed ( Universal:time_fix ) > SurfacingIntervalDuringListening
          )

          assign in sequence ElapsedSinceLastGPSfix = elapsed ( Universal:time_fix )

          syslog important "Elapsed time since the last GPS fix " + ElapsedSinceLastGPSfix~minute
               + " is longer than SurfacingIntervalDuringListening " + SurfacingIntervalDuringListening~minute
               + ". Surface and optionally transit back to listening location."

          assign in sequence SurfacingStarted = true

          call id="CommsAndOptionalTransitBack" refId="CommsAndOptionallyTransitBackToListeningLocation"

          assign in sequence AdjustingBuoyancy = false
        }

        aggregate DecreaseNeutralBuoyancy {
          run in sequence

          aggregate DecreaseNeutralBuoyancyDecreaseDelta {
            run in sequence

            break if (
              Cnt == 1 count
              or ( not ( Sinking ) )
              or ( BuoyancyAdjustmentStep < ( BuoyancyAdjustmentStepMin * 2 none ) )
              or FineTune
              or BuoyancyAdjustmentDone
            )

            assign in sequence DeltaReduced = true

            assign in sequence BuoyancyAdjustmentStepPrevious = BuoyancyAdjustmentStep

            assign in sequence MassPositionAdjustmentStepPrevious = MassPositionAdjustmentStep

            assign in sequence NeutralBuoyancyPrevious = NeutralBuoyancy

            assign in sequence MassPositionPrevious = MassPosition

            assign in sequence BuoyancyAdjustmentStepNew = BuoyancyAdjustmentStep / 2 none

            assign in sequence MassPositionAdjustmentStepNew = MassPositionAdjustmentStep / 2 none

            assign in sequence NeutralBuoyancy = NeutralBuoyancy - BuoyancyAdjustmentStepNew

            assign in sequence MassPosition = MassPosition + MassPositionAdjustmentStepNew

            syslog important "Sinking previously but floating now. Decrease BuoyancyAdjustmentStep from "
                 + BuoyancyAdjustmentStepPrevious~cubic_centimeter + " to "
                 + BuoyancyAdjustmentStepNew~cubic_centimeter + ". Decrease MassPositionAdjustmentStep from "
                 + MassPositionAdjustmentStepPrevious~millimeter + " to "
                 + MassPositionAdjustmentStepNew~millimeter + ". Decrease neutral buoyancy: "
                 + NeutralBuoyancyPrevious~cubic_centimeter + " - " + BuoyancyAdjustmentStepNew~cubic_centimeter
                 + " = " + NeutralBuoyancy~cubic_centimeter + ". Move mass forward: "
                 + MassPositionPrevious~millimeter + " + " + MassPositionAdjustmentStepNew~millimeter + " = "
                 + MassPosition~millimeter + ". FineTune = " + FineTune~bool + ". WaitAfterBuoyancyChange = "
                 + WaitAfterBuoyancyChange~minute + "."

            assign in sequence MassPositionAdjustmentStep = MassPositionAdjustmentStepNew

            assign in sequence BuoyancyAdjustmentStep = BuoyancyAdjustmentStepNew

            assign in sequence AdjustingBuoyancy = false

            assign in sequence Sinking = false

            assign in sequence BuoyancyAdjustmentDone = true
          }

          aggregate DecreaseNeutralBuoyancyDoNotChangeDelta {
            run in sequence

            break if (
              not ( FineTune )
              and Sinking
              and ( BuoyancyAdjustmentStep >= ( BuoyancyAdjustmentStepMin * 2 none ) )
              and ( Cnt >= 2 count )
              or BuoyancyAdjustmentDone
              or DeltaReduced
            )

            assign in sequence NeutralBuoyancyPrevious = NeutralBuoyancy

            assign in sequence MassPositionPrevious = MassPosition

            assign in sequence NeutralBuoyancy = NeutralBuoyancy - BuoyancyAdjustmentStep

            assign in sequence MassPosition = MassPosition + MassPositionAdjustmentStep

            syslog important "Vehicle is too light. Decrease neutral buoyancy: "
                 + NeutralBuoyancyPrevious~cubic_centimeter + " - " + BuoyancyAdjustmentStep~cubic_centimeter
                 + " = " + NeutralBuoyancy~cubic_centimeter + ". Move mass forward: "
                 + MassPositionPrevious~millimeter + " + " + MassPositionAdjustmentStep~millimeter + " = "
                 + MassPosition~millimeter + ". FineTune = " + FineTune~bool + ". WaitAfterBuoyancyChange = "
                 + WaitAfterBuoyancyChange~minute + "."

            assign in sequence AdjustingBuoyancy = false

            assign in sequence Sinking = false

            assign in sequence BuoyancyAdjustmentDone = true
          }
        }
      }
    }
  }

  call id="EndingMission" refId="NeedComms"
}