Insert/LineCapture.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.

aggregate LineCapture {
  """
  Home to transponder sequence: (0) Undock (1) Midcourse guidance: nav to
  dock WP while querying transponder, execute search pattern if WP is
  reached with no response from x-ponder. (2) Terminal guidance: home to
  target using DUSBL tracking data, reset nav, and dock
  """

  arguments {
    # Dock

    DockLat = Dock:Dock.lat
      """
      Latitude of docking station waypoint (defaults to
      Config/workSite.beaconLat).
      """

    DockLon = Dock:Dock.lon
      """
      Longitude of docking station waypoint (defaults to
      Config/workSite.beaconLon).
      """

    # Undock arguments

    DetachTimeout = 30 minute
      """
      Maximum time duration for the vehicle to detach form the dock. The
      vehicle will switch over to midcourse guidance after the timeout is
      expired.
      """

    # Midcourse guidance arguments

    MidcourseDepth = Dock:LineCapture.midcourseDepth
      """
      Desired vehicle depth when navigating to dock WP (during midcourse
      phase).
      """

    MidcourseSpeed = Dock:LineCapture.midcourseSpeed
      """
      Thruster speed during Midcourse guidance.
      """

    MidcourseTimeout = Dock:LineCapture.midcourseTimeout
      """
      Maximum time duration for running in midcourse guidance mode.
      """

    # Terminal guidance arguments

    TerminalRange = Dock:LineCapture.terminalRange
      """
      Range at which the vehicle should switch from navigating to the dock wp
      and start homing to the target using DUSBL tracking data. Max is ??
      meters due to DUSBL ???.
      """

    AcousticTimeout = Dock:LineCapture.acousticTimeout
      """
      Maximum time duration allowed between pings during terminal guidance. If
      expired, the vehicle will regress to Midcourse guidance.
      """

    losRateIIRFilterDecay = 0.0 none
      """
      LineCapture single-pole IIR filter decay value.
      Filters line-of-sight (LOS) angle rotation rate.
      The Decay value should be btween 0 to 1, set to 0 disable filtering.
      """

    # Search pattern arguments

    SearchTimeout = Dock:LineCapture.searchTimeout
      """
      Maximum time duration for running in search guidance mode.
      """

    CircleRadius = Dock:LineCapture.searchCircleRadius
      """
      Radius of circle around the dock waypoint.
      """

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

    # General settings

    DepthDeadband = 0.01 meter
      """
      Vertical drift from the specified depth that's allowed during depth
      keeping.
      """

    RudderDeadband = 0.001 arcdeg
      """
      Degree of rounding in rudder command output values to use during
      Terminal Homing.
      """

    MinAltitude = 2 meter
      """
      Minimum height above the sea floor for the entire mission.
      """

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

    MinOffshore = 1 meter
      """
      Minimum offshore distance for the entire mission.
      """

    DockingAltitude = NaN meter
      """
      Docking altitude. Ignored when NaN and used over depth when a valid altitude is set.
      """

    InvalidAltitudeTimeout = NaN minute
      """
      The mission will use the last good depth command if no valid altitude readings are received within
      this specified time duration.
      """

    AltitudeDepthLimit = NaN meter
      """
      Vehicle will not be commanded deeper than this regardless of altitude command.
      """

    AltitudeIIRFilterDecay = 0.0 none
      """
      AltitudeServo single-pole IIR filter decay value.
      Low-pass filters the AltitudeServo depth command.
      The Decay value should be btween 0 to 1, set to 0 disable filtering.
      """

    # Mission Variables (don't change!)

    ElapsedRange = 0 minute
      """
      Mission variable (don't change). Time since last range to acoustic contact.
      """

    Range = NaN meter
      """
      Mission variable (don't change). Most recent range to acoustic contact.
      """
  }

  output {
    # Mission Variables (don't change!)

    DoneTimeout = -1 count
      """
      Mission variable (don't change). The mission will run in this mode when
      the insert timed out.
      """

    Done = 0 count
      """
      Mission variable (don't change). The mission will run in this mode when
      the insert is done (docked).
      """

    DetachFromDock = 1 count
      """
      Mission variable (don't change). The mission will run in this mode to
      drift away from the dock after is unlatched.
      """

    Midcourse = 2 count
      """
      Mission variable (don't change). The mission will run in this mode when
      approaching the dock under Midcourse guidance.
      """

    Terminal = 3 count
      """
      Mission variable (don't change). The mission will run in this mode when
      approaching the dock under Terminal guidance.
      """

    GuidanceMode = DetachFromDock
      """
      Mission variable (don't change). The mission sets this variable to
      switch between guidance modes.
      """

    DockingDepth = 1 meter
      """
      Mission variable (don't change). Actual docking depth.
      """
  }

  run when ( called )

  assign in sequence GuidanceMode = DetachFromDock

  assign in sequence DockingDepth = Dock:Dock.depth + Dock:Docked.dockingDepthOffset

  # Safety nets

  insert Insert/StandardEnvelopes.tl

  assign in sequence StandardEnvelopes:MinAltitude = MinAltitude

  assign in sequence StandardEnvelopes:MaxDepth = MaxDepth

  assign in sequence StandardEnvelopes:MinOffshore = MinOffshore

  # Update internal range arguments

  assign in parallel ElapsedRange = elapsed ( Universal:acoustic_contact_range )

  assign in parallel Range = Universal:acoustic_contact_range

  # Set default mass and buoyancy positions

  aggregate HoldMassAndBuoyancy {
    """
    Maintain VB and mass at neutral/default position while underway
    (during Midcourse and Terminal guidance).
    """

    run while (
      abs ( Control:SpeedControl.speedCmd ) > 0 meter_per_second
    )

    assign in parallel Control:VerticalControl.depthDeadband = DepthDeadband

    behavior Guidance:Mass {
      run in parallel

      set position = Control:VerticalControl.massDefault
    }

    behavior Guidance:Buoyancy {
      run in parallel

      set position = Control:VerticalControl.buoyancyNeutral
    }
  }

  aggregate HomingSequence {
    """
    This is the insert's guidance section, which arbitrates between 3
    modes: 1) DetachFromDock: unlatch and drift away from the dock. 2)
    Midcourse: navigate toward the dock waypoint, and enter search
    pattern if waypoint has been reached with no range data. 2)
    Terminal: home toward the dock using DUSBL tracking data.
    """

    run in sequence

    break if ( GuidanceMode <= Done )

    aggregate DetachFromDock {
      """
      Unlatch and drift away from the dock.
      """

      run while ( GuidanceMode == DetachFromDock )

      aggregate guide {
        run in sequence

        behavior Dock:Undock {
          run in sequence

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

        syslog info "Transitioning to midcourse guidance."

        assign in sequence GuidanceMode = Midcourse
      }
    }

    aggregate MidcourseGuidance {
      """
      (1) Navigate toward the dock waypoint while querying the dock
      transponder. (2) Continue until within *TerminalRange*, then
      move on to terminal guidance. (3) Enter search pattern if
      waypoint has been reached with no range data.
      """

      run while ( GuidanceMode == Midcourse )

      aggregate guide {
        run in sequence

        syslog info "Navigating to dock waypoint (" + DockLat~degree + "," + DockLon~degree + ")"

        aggregate CheckRange {
          run when (
            Range < TerminalRange
            and ( ElapsedRange <= AcousticTimeout )
          )

          syslog info "Transitioning to terminal guidance at range" + Range~meter

          assign in sequence GuidanceMode = Terminal
        }

        behavior Guidance:SetSpeed {
          run in parallel

          set speed = MidcourseSpeed
        }

        behavior Guidance:Pitch {
          run in parallel

          set depth = MidcourseDepth
        }

        behavior Guidance:Waypoint {
          run in sequence

          timeout duration=MidcourseTimeout {
            syslog important "Terminating homing sequence: timed out while navigating to the dock waypoint."

            assign in sequence GuidanceMode = DoneTimeout
          }

          set latitude = DockLat
          set longitude = DockLon
        }

        syslog info "Executing search pattern."

        aggregate SearchPattern {
          run in sequence

          timeout duration=SearchTimeout {
            syslog important "Timed out while executing search pattern around dock waypoint."
          }

          behavior Guidance:Circle {
            run in parallel

            set latitude = DockLat
            set longitude = DockLon
            set radius = CircleRadius
            set maxError = CircleMaxError
            set turnToPort = CircleTurnToPort
          }

          behavior Guidance:Wait {
            run in sequence

            set duration = SearchTimeout
          }
        }

        syslog important "Homing sequence timed out prior to docking."

        assign in sequence GuidanceMode = DoneTimeout
      }
    }

    aggregate TerminalGuidance {
      """
      Home toward the target using USBL tracking data.
      """

      run while ( GuidanceMode == Terminal )

      aggregate guide {
        run in sequence

        syslog info "Terminal guidance."

        aggregate CheckRange {
          run when (
            Range >= TerminalRange
            or ( ElapsedRange > AcousticTimeout )
          )

          syslog important "Transitioning to midcourse guidance. Received last range of"
               + Range~meter + ElapsedRange~minute + "ago."

          assign in sequence GuidanceMode = Midcourse
        }

        aggregate altitudeControl {
          run while ( not ( isNaN ( DockingAltitude ) ) )

          behavior Guidance:AltitudeServo {
            run in parallel

            set targetAltitude = DockingAltitude
            set initialDepth = DockingDepth
            set invalidAltitudeTimeout = InvalidAltitudeTimeout
            set maxDepthLimit = AltitudeDepthLimit
            set iirFilterDecay = AltitudeIIRFilterDecay
          }
        }

        aggregate depthControl {
          run while ( isNaN ( DockingAltitude ) )

          behavior Guidance:Pitch {
            run in parallel

            set depth = DockingDepth
          }
        }

        assign in parallel Servo:RudderServo.deviationAngle = RudderDeadband

        assign in parallel Control:HorizontalControl.rudDeadband = RudderDeadband

        behavior Dock:LineCapture {
          run in sequence

          set iirFilterDecay = losRateIIRFilterDecay
        }

        syslog info "Docked. Homing sequence complete."

        assign in sequence GuidanceMode = Done
      }
    }
  }

  behavior Guidance:Wait {
    run in sequence

    set duration = 3 second
  }

  syslog info "GuidanceMode" + GuidanceMode~count
}