Maintenance/line_capture_homing_lab.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 lineCaptureHoming {
  """
  Home to transponder while maintaining altitude. The mission sequence:
  (*) Start (1) Midcourse guidance: nav to target WP while querying
  transponder (2) Terminal guidance: home to target using DUSBL tracking
  data, fire up SCPI when in range (4) Dock: final approach using fixed
  heading (5) Rollout: break off from dock, then nav back to start wp or
  move away from the target at the last commanded heading (*) repeat Note:
  since there's a possibility that no altitude reading will be available
  form the surface (e.g., when the bottom is out of DVL range), the
  vehicle first descends from the surface to a commanded depth and then
  starts following the commanded altitude.
  """

  arguments {
    # Define mission arguments

    MissionTimeout = 4 hour
      """
      Maximum length of mission
      """

    Repeat = 1 count
      """
      Number of times the vehicle should try to cycle through the homing
      sequence.
      """

    DockLat = 36.7436 degree
      """
      Latitude of docking station waypoint.
      """

    DockLon = -121.8800 degree
      """
      Longitude of docking station waypoint.
      """

    StartLat = NaN degree
      """
      Latitude of docking sequance start waypoint. The vehicle navigates back
      to this waypoint at the end of each homing attempt. If set to NaN, the
      vehicle moves away from the target at the last commanded heading until
      *RolloutTime* has passed.
      """

    StartLon = NaN degree
      """
      Longitude of docking sequance start waypoint. The vehicle navigates back
      to this waypoint at the end of each homing attempt. If set to NaN, the
      vehicle moves away from the target at the last commanded heading until
      *RolloutTime* has passed.
      """

    TerminalRange = 650.0 meter
      """
      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 ???.
      """

    ArmRange = 40.0 meter
      """
      Range to deploy the whiskers, open the latch, and activate the camera.
      """

    DockRange = 1.0 meter
      """
      Range from dock to stop commanding heading.
      """

    TerminalGuidanceTimeout = 30 minute
      """
      Maximum time duration for running in terminal guidance mode.
      """

    DockTimeout = 60 second
      """
      Maximum time duration for running in Dock lockout mode.
      """

    UpdateNav = false
      """
      When true, the mission updates the vehicle's position to DockLat/Lon
      when it comes within *DockRange* from the transponder.
      """

    WaitOnDockTimeout = 90 second
      """
      Time duration to wait on the dock after a successful line capture.
      """

    RolloutDistance = 150 meter
      """
      Distance to move away from the target (rollout) in response to missing
      the target.
      """

    RolloutTime = 5 minute
      """
      Drive on your last heading for this long after detaching from the dock.
      This argument is *ignored* if start Lat/Lon are specified.
      """

    InitDepth = 20 meter
      """
      Initial depth. The vehicle will dive to this depth when leaving the
      surface. Necessary if no altitude reading is available form the surface
      (e.g., when the bottom is out of DVL range).
      """

    InitDepthTimeout = 30 minute
      """
      Maximum time duration for the vehicle to reach the initial depth. The
      vehicle will switch over to altitude following after the timeout is
      expired.
      """

    TransponderCode = 2 count
      """
      Transponder Address.
      """

    TrackingUpdatePeriod = 2.5 second
      """
      How long to wait between acoustic queries.
      """

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

    KpHeading = 0.8 none
      """
      Heading proportional gain to use during Terminal Homing.
      """

    KiHeading = 0.002 reciprocal_second
      """
      Heading Integral gain to use during Terminal Homing.
      """

    KpHeadingFinal = 1.6 none
      """
      Heading proportional gain to use during Final Approach.
      """

    KiHeadingFinal = 0.004 reciprocal_second
      """
      Heading Integral gain to use during Final Approach.
      """

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

    Verbose = true
      """
      Send LineCapture/Undock syslog messages to shore.
      """
  }

  output {
    # Mission Variables (don't change)

    HomingActive = true
      """
      Mission variable (don't change). The mission sets this to true when the
      vehicle is homing.
      """

    Docked = false
      """
      Mission variable (don't change). The mission sets this to true to
      indicate a successful line capture.
      """

    DockingDepth = 25 meter
      """
      Mission variable (don't change). The mission sets this to the vehicle's
      depth following a successful line capture.
      """

    DiveMode = 0 count
      """
      Mission variable (don't change). The mission sets this variable to
      switch between dive modes.
      """

    MaintainAltitude = 2 count
      """
      Mission variable (don't change). The mission will run in this mode to
      dive the vehicle at a commanded altitude.
      """

    WaitOnDock = 3 count
      """
      Mission variable (don't change). The mission will run in this mode to
      maintatin depth while the vehicle is latched on the dock.
      """

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

  # Mission timeout

  timeout duration=MissionTimeout

  # Add track acoustic contact directive high in the stack.

  aggregate TrackTransponder {
    run while ( HomingActive )

    behavior Estimation:TrackAcousticContact {
      run in parallel

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

  # Set default mass position

  behavior Guidance:Mass {
    run in parallel

    set position = Control:VerticalControl.massDefault
  }

  # Callable aggregates (priority matters, so these are done below important safety directives

  aggregate DiveCmd {
    """
    Called when the vehicle should be underwater. Arbitrates between 3
    dive modes: 1) descend from the surface to initial depth. 2) follow
    commanded altitude. 3) drift away from dock to initial depth.
    """

    run when ( called )

    aggregate DockHold {
      """
      Wait on dock after vehicle latches.
      """

      run while ( DiveMode == WaitOnDock )

      aggregate dive {
        run in parallel

        behavior Guidance:SetSpeed {
          run in parallel

          set speed = 0 meter_per_second
        }

        behavior Guidance:Pitch {
          run in parallel

          set depth = DockingDepth
        }
      }
    }

    aggregate DriftAwayFromDock {
      """
      Drift away from the dock after vehicle unlatches.
      """

      run while ( DiveMode == DetachFromDock )

      aggregate dive {
        run in sequence

        behavior Guidance:SetSpeed {
          run in parallel

          set speed = 0 meter_per_second
        }

        behavior Guidance:Pitch {
          run in sequence

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

            behavior Guidance:Execute {
              run in sequence

              set command = "stop"
            }
          }

          set depth = InitDepth
        }

        assign in sequence DiveMode = MaintainAltitude
      }
    }
  }

  # Start mission sequence.

  aggregate Lap {
    run in sequence repeat=Repeat

    syslog important "Initiating homing sequence."

    assign in sequence HomingActive = true

    aggregate MidcourseGuidance {
      """
      Navigate toward the dock waypoint while querying the dock
      transponder. Continue until you approach within *TerminalRange*,
      then move on to the next aggregate.
      """

      run in sequence

      break if (
        Estimation:TrackAcousticContact.range_to_contact < TerminalRange
      )

      behavior Guidance:Waypoint {
        run in sequence

        set latitude = DockLat
        set longitude = DockLon
      }
    }

    aggregate TerminalGuidance {
      """
      Move toward the target using DUSBL tracking data and update the
      commanded heading with each cycle, activate SCPI when in range.
      Continue until you're within *DockRange*, then move on to the
      next aggregate.
      """

      run in sequence

      assign in parallel Control:HorizontalControl.rudDeadband = RudderDeadband

      syslog important "Terminal guidance at range: "
           + Estimation:TrackAcousticContact.range_to_contact~meter + "."

      assign in sequence Docked = true

      behavior Dock:LineCapture {
        run in sequence

        timeout duration=TerminalGuidanceTimeout {
          assign in sequence Docked = false

          syslog important "Timed out while trying to dock at range: "
               + Estimation:TrackAcousticContact.range_to_contact~meter + "."
        }

        set armRange = ArmRange
        set lockoutRange = DockRange
        set interceptTimeout = DockTimeout
        set rolloutDistance = RolloutDistance
        set kpHeadingTerminalGuidance = KpHeading
        set kiHeadingTerminalGuidance = KiHeading
        set kpHeadingFinalApproach = KpHeadingFinal
        set kiHeadingFinalApproach = KiHeadingFinal
        set verbose = Verbose
      }

      aggregate Dock {
        run in sequence

        break if ( not ( Docked ) )

        # Hold at the current depth while latched on the line

        assign in sequence DockingDepth = Universal:depth

        syslog important "Holding depth at: " + DockingDepth~meter + "."

        assign in sequence DiveMode = WaitOnDock

        # Dactivate homing while relaxing on the line

        assign in sequence HomingActive = false

        aggregate UpdateNav {
          run in sequence

          break if ( not ( UpdateNav ) )

          behavior Dock:SetNav {
            run in sequence

            set latitude = DockLat
            set longitude = DockLon
          }
        }

        behavior Guidance:Wait {
          run in sequence

          set duration = WaitOnDockTimeout
        }

        aggregate BreakOffDock {
          run in sequence

          assign in sequence HomingActive = true

          syslog important "Breaking off. Range: "
               + Estimation:TrackAcousticContact.range_to_contact~meter + "."

          assign in sequence DiveMode = DetachFromDock

          behavior Dock:Undock {
            run in sequence
          }

          assign in sequence Docked = false
        }
      }
    }

    # Homing sequance complete

    aggregate Rollout {
      """
      Navigate back to the start waypoint or move away from the target
      at the last commanded heading until *RolloutTime* has passed.
      """

      run in sequence

      assign in sequence Docked = false

      assign in sequence HomingActive = false

      # Let's make sure we're in the right dive mode

      assign in sequence DiveMode = MaintainAltitude

      aggregate NavToStart {
        run in sequence

        break if (
          isNaN ( StartLat )
          or ( isNaN ( StartLon ) )
        )

        syslog important "Rollout to start WP at range: "
             + Estimation:TrackAcousticContact.range_to_contact~meter + "."

        behavior Guidance:Waypoint {
          run in sequence

          set latitude = StartLat
          set longitude = StartLon
        }
      }

      aggregate MaintainHeading {
        run in sequence

        break if (
          not ( isNaN ( StartLat ) )
          or ( not ( isNaN ( StartLon ) ) )
        )

        syslog important "Rollout at range: "
             + Estimation:TrackAcousticContact.range_to_contact~meter + "."

        behavior Guidance:Wait {
          run in sequence

          set duration = RolloutTime
        }
      }
    }
  }
}