underIce/DefaultDockNav.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 DefaultDockNav {
  arguments {
    ElapsedSinceDefaultStarted = 0 minute

    DetachTimeout = 30 minute
      """
      Maximum time duration for the vehicle to detach form the dock.
      """

    TransponderCode = Dock:Dock.transponderCode
      """
      Dock transponder address.
      """

    TrackingPeriodUndock = 10 second
      """
      Time duration between acoustic range queries while the mission is
      detaching from the dock.
      """

    TrackingPeriodMidcourse = 60 second
      """
      Time duration between acoustic range queries while the mission is
      navigating to the dock.
      """

    TrackingPeriodCircle = 20 second
      """
      Time duration between acoustic range queries while the mission is
      searching for the dock.
      """

    NumberOfPings = 1 count
      """
      Number of pings requested each time.
      """

    DriftTimeout = 3 minute
      """
      How long to allow vehicle to drift away from dock before retrying reverse
      """

    LineCaptureTimeout = 30 minute
      """
      Maximum time duration to run LineCapture when we have good navigation.
      """

    SearchCircleRadius = 300 meter
      """
      Radius of circle to search when we don't have good navigation.
      """

    SearchTimeout = 10 minute
      """
      Maximum time duration to search for acoustic contact when we don't have
      good navigation.
      """

    AcousticContactTimeout = 15 minute
      """
      Time duration after which acoustic ranges are considered stale.
      """

    MidcourseDepthOverride = 10 meter
      """
      Depth to use for LineCapture midcourse phase.
      """

    MaxDepth = 50 meter
      """
      Maximum depth for the entire mission.
      """
  }

  output {
    TrackingUpdatePeriod = 10 second
      """
      Mission variable (don't change). The mission sets this variable to
      update how long to wait between acoustic queries.
      """
  }

  behavior Guidance:Wait {
    """
    Wait a moment to see if the scheduler starts a new mission before
    starting to actually run Default.
    """

    run in sequence

    set duration = 30 second
  }

  # Add LineCapture directives.

  insert Insert/LineCapture.tl
  assign in sequence LineCapture:MaxDepth = MaxDepth

  # Add SetNav directives.

  insert Insert/SetNavAcoustic.tl

  # Acoustic tracking directives

  behavior Estimation:TrackAcousticContact {
    run while (
      Universal:depth > Control:VerticalControl.surfaceThreshold
    )

    set contactLabelSetting = TransponderCode
    set numberOfSamplesSetting = NumberOfPings
    set updatePeriodSetting = TrackingUpdatePeriod
  }

  aggregate LeaveDock {
    run in sequence

    timeout duration=DetachTimeout {
      syslog important "Timed out while trying to detach from dock at current depth of "
           + Universal:depth~meter + "."
    }

    # Leave the dock
    assign in sequence TrackingUpdatePeriod = TrackingPeriodUndock

    behavior Dock:Undock {
      run in sequence

      set driftTimeout = DriftTimeout
    }
  }

  aggregate PostUndockNavigation {
    """
    After leaving dock, navigate based on whether we have good navigation.
    If we have good nav, use LineCapture. If not, search locally for acoustic
    contact and use relative navigation.
    """

    run in sequence

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

    aggregate NavToDock {
      """
      If we have good navigation (received GPS fix and dead reckoning has run),
      use LineCapture to home to the dock.
      """

      run in sequence

      break if (
        isNaN ( Universal:horizontal_path_length_since_last_fix )
        or Universal:horizontal_path_length_since_last_fix < 0 meter
      )

      timeout duration=LineCaptureTimeout {
        syslog important "LineCapture timed out after " + LineCaptureTimeout~minute
      }

      assign in sequence TrackingUpdatePeriod = TrackingPeriodMidcourse

      syslog important "Have valid DR'd position, running LineCapture."

      assign in sequence LineCapture:MidcourseDepth = MidcourseDepthOverride

      call priorityHere=false refId="LineCapture"

      aggregate UndockIfDocked {
        """
        If LineCapture successfully docked, undock before falling through to
        surface default.
        """

        run in sequence

        break if (
          LineCapture:GuidanceMode != LineCapture:Done
        )

        syslog important "Docked, undocking before surfacing."

        behavior Dock:Undock {
          run in sequence

          set driftTimeout = DriftTimeout
        }
      }
    }

    aggregate SearchForDock {
      """
      If we don't have good navigation, search locally for acoustic contact
      using a circle pattern.
      """

      run in sequence

      break if (
        Universal:horizontal_path_length_since_last_fix >= 0 meter
      )

      assign in sequence TrackingUpdatePeriod = TrackingPeriodCircle

      syslog important "No DR'd position, searching for acoustic contact."

      behavior Guidance:Mass {
        run in parallel

        set position = Control:VerticalControl.massDefault
      }

      behavior Guidance:Buoyancy {
        run in parallel

        set position = Control:VerticalControl.buoyancyNeutral
      }

      behavior Guidance:Pitch {
        run in parallel

        set depth = MidcourseDepthOverride
      }

      aggregate CircleSearch {
        """
        Drive a circle at current position while searching for acoustic contact.
        """

        run in sequence

        break if (
          elapsed ( Estimation:TrackAcousticContact.range_to_contact ) < AcousticContactTimeout
        )

        timeout duration=SearchTimeout {
          syslog important "Acoustic search timed out after " + SearchTimeout~minute
        }

        behavior Guidance:Circle {
          run in sequence

          set latitude = Universal:latitude
          set longitude = Universal:longitude
          set radius = SearchCircleRadius
          set maxError = 100 meter
          set turnToPort = false
        }
      }

      aggregate AcousticWaypointNav {
        """
        If we got an acoustic hit during search, navigate towards it using
        waypoint navigation (except don't because we can't yet)
        """

        run in sequence

        break if (
          elapsed ( Estimation:TrackAcousticContact.range_to_contact ) >= AcousticContactTimeout
        )

        syslog important "Acoustic target found! At range "
             + Estimation:TrackAcousticContact.range_to_contact~meter + " meters. And estimatedposition "
             + Estimation:TrackAcousticContact.contact_latitude~degree + ", "
             + Estimation:TrackAcousticContact.contact_longitude~degree + "."
        syslog important "But giving up and continuing with surface default."
      }
    }
  }

  aggregate SurfaceDefault {
    run in sequence

    # This mission assumes open water so from here on it's the same old default that's always used

    behavior Guidance:GoToSurface {
      run in progression
    }

    aggregate CheckIn {
      run in sequence repeat=288

      readDatum id="Read_GPS" {
        timeout duration=P5M

        Universal:time_fix
      }

      readDatum id="Read_Iridium" {
        timeout duration=P2H {
          behavior Guidance:Execute {
            run in sequence

            set command = "Burn on"
          }

          syslog critical "Dropped weight due to communications timeout."
        }

        Universal:platform_communications
      }

      behavior Guidance:Wait {
        run in sequence

        set duration = 5 minute
      }

      assign in sequence ElapsedSinceDefaultStarted = elapsed ( Universal:mission_started )

      syslog important "Default mission has been running for " + ElapsedSinceDefaultStarted~minute
    }
  }

  syslog important "Restarting logs and Default mission."

  behavior Guidance:Execute {
    run in sequence

    set command = "restart logs"
  }
}