Engineering/zoomies_and_homies.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 zoomies_and_homies_and_sampling {
  """
  Depart the dock and zoom over to a location while homing and sampling. Once close enough, grab the fix end let the mission end.
  """

  arguments {
    # Define mission arguments

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

    AcousticContactTimeout = 3 hour
      """
      If the vehicle does not receive an acoustic signal for more than this
      length of time, it will surface for communications with shore.
      """

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

    ContactDepth = 1 meter
      """
      The depth of the asset to be tracked
      """

    ContactDistance = 150 meter
      """
      How close to get before stopping
      """

    SetNavFix = true
      """
      Whether to take a fix from the acoustic beason upon
      acheiving contact distance.
      """

    NavFixLatitude = 36.696 degree
      """
      The latitude of the beacon being homed to. This is the position used
      in the fix if setnavfix is true and the vehicle is within contact
      distance to the beacon.
      """

    NavFixLongitude = -121.9415 degree
      """
      The longitude of the beacon being homed to. This is the position used
      in the fix if setnavfix is true and the vehicle is within contact
      distance to the beacon.
      """

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

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

    Speed = 1 meter_per_second
      """
      Vehicle speed.
      """

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

    Depth = 50 meter
      """
      Depth of flat and level flight during zooming/smearing
      """

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

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

    SettleTime = 2 minute
      """
      Wait this long to settle after stopping the prop
      """

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

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

    EnableFiring = true
      """
      Enable firing. Initialized to true.
      """

    NumESPSamplers = 2 count
      """
      Total number of ESP cartridges (Maximum 60. Set to a smaller number if
      not using all of them. Set to 0 to disable ESP sampling).
      """

    CircleRadius = 25 meter
      """
      Radius of circle around acoustic contact during wait time
      """

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

  output {
    # Internal variables that you should not change

    DetachTimeout = 20 minute
      """
      Maximum time to spend getting off the dock
      """

    CenterTouched = false
      """
      Flag to indicate center lon/lat was updated.
      """

    DepthLogged = 0.0 meter

    TempLogged = 0.0 celsius

    ChlLogged = 0.0 microgram_per_liter

    CntSamples = 1 count
      """
      Count of water samples (initilized to 1).
      """

    FlagSamplingOngoing = false
      """
      Flag of water sampling in process (initialized to false).
      """

    FlagFired = false
      """
      Flag of sample fired (initialized to false).
      """

    SampleCompleted = false
      """
      Sampling completed. Initialized to false.
      """

    SetNavFixCompleted = false
      """
      The nav fix has been completed (initialized to false).
      """
  }

  # Mission timeout

  timeout duration=MissionTimeout

  insert Insert/AbortDrift.tl {
    redefineArg AcousticTimeout = AcousticContactTimeout
  }

  insert Insert/Science.tl {
    """
    Most missions will run the science sensors. If you don't place this
    aggregate above NeedComms, science instruments get turned off on the
    last upcast and while floating on the surface.
    """
  }

  # Safety nets and surfacing behaviors

  insert Insert/StandardEnvelopes.tl

  assign in sequence StandardEnvelopes:MinAltitude = MinAltitude

  assign in sequence StandardEnvelopes:MaxDepth = MaxDepth

  assign in sequence StandardEnvelopes:MinOffshore = MinOffshore

  aggregate ESPSampling {
    run when ( called )

    aggregate TriggerESP {
      run in sequence

      assign in sequence DepthLogged = Universal:depth

      assign in sequence TempLogged = Universal:sea_water_temperature

      assign in sequence ChlLogged = Universal:mass_concentration_of_chlorophyll_in_sea_water

      syslog important "Trigger ESP sampling. DepthLogged, TempLogged, ChlLogged = "
           + DepthLogged~meter + ", " + TempLogged~celsius + ", " + ChlLogged~microgram_per_liter + "."

      readDatum {
        Science:ESPComponent.sampling
      }

      syslog important "Wait for ESP sampling to complete."

      readDatum {
        Science:ESPComponent.sample_number
      }
    }
  }

  aggregate Firing {
    run when (
      ( Universal:depth > 10 meter )
      and ( not ( FlagSamplingOngoing ) )
      and ( CntSamples <= NumESPSamplers )
      and ( not ( SampleCompleted ) )
    )

    assign in sequence FlagSamplingOngoing = true

    syslog important "Taking ESP sample No." + CntSamples~count + "."

    assign in sequence FlagFired = true

    call id="Sampling" priorityHere=false refId="ESPSampling"

    syslog important " ESP sample No." + CntSamples~count + " completed."

    assign in sequence FlagSamplingOngoing = false

    assign in sequence SampleCompleted = true

    assign in sequence EnableFiring = false
  }

  # Add track acoustic contact directive high in the stack.
  behavior Estimation:TrackAcousticContact {
    run in parallel

    set contactLabelSetting = ContactLabel
    set contactDepthSetting = ContactDepth
    set numberOfSamplesSetting = NumberOfPings
    set updatePeriodSetting = TrackingUpdatePeriod
  }

  aggregate UpdateCenter {
    run when (
      elapsed ( Estimation:TrackAcousticContact.range_to_contact ) < ( elapsed ( CenterTouched ) )
    )

    assign in sequence CenterLatitude = Estimation:TrackAcousticContact.contact_latitude

    assign in sequence CenterLongitude = Estimation:TrackAcousticContact.contact_longitude

    assign in sequence CenterTouched = true
  }

  aggregate Detach {
    run in sequence

    timeout duration=DetachTimeout {
      syslog important "Timed out trying to detach from dock. Stopping mission."

      behavior Guidance:Execute {
        run in sequence

        set command = "stop"
      }
    }

    syslog important "Detaching from dock."

    # Cast off for the great unknown!

    behavior Dock:Undock {
      run in sequence
    }
  }

  aggregate SetNavFix {
    run when (
      SetNavFix
      and Universal:acoustic_contact_range < ContactDistance
      and ( not ( SetNavFixCompleted ) )
    )

    syslog important "Setting position to:" + NavFixLatitude~degree + "," + NavFixLongitude~degree

    behavior Dock:SetNav {
      run in sequence

      set latitude = NavFixLatitude

      set longitude = NavFixLongitude
    }

    assign in sequence SetNavFixCompleted = true
  }

  # Start mission sequence.

  aggregate Home {
    """
    Head towards the target.
    """

    run in sequence

    break if (
      Universal:acoustic_contact_range < ContactDistance
    )

    behavior Guidance:Pitch {
      run in parallel

      set massPosition = Control:VerticalControl.massDefault
    }

    behavior Guidance:Buoyancy {
      run in parallel

      set position = Control:VerticalControl.buoyancyNeutral
    }

    behavior Guidance:SetSpeed {
      run in parallel

      set speed = Speed
    }

    behavior Guidance:Pitch {
      run in parallel

      set depth = Depth
    }

    behavior Guidance:Circle {
      run in parallel

      set latitude = CenterLatitude
      set longitude = CenterLongitude
      set radius = CircleRadius
      set maxError = CircleMaxError
      set turnToPort = CircleTurnToPort
    }
  }

  aggregate Hockey_Stop {
    run in sequence

    syslog important "Hockey stop! Range: " + Universal:acoustic_contact_range~meter

    behavior Guidance:SetSpeed {
      run in parallel

      set speed = 0 meter_per_second
    }

    behavior Guidance:Point {
      run in parallel

      set rudderAngle = 10 degree
    }

    behavior Guidance:Pitch {
      run in parallel

      set depth = Depth
    }

    behavior Guidance:Wait {
      run in sequence

      set duration = SettleTime
    }
  }
}