Science/smear_yoyo_camera.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 SmearYoYoCamera {
  """
  After transit, this mission dives to yo-yo min depth, turns on red
  lights and camera, and begins the cylinder. Lights switch between
  red/white every LightDeltaTimeSetting minutes.
  """

  arguments {
    # Almost every mission should start with an overall timeout and a NeedCommsTime. Drift Missions will have an acoustic timeout instead of a NeedCommsTime.

    # You probably need to change these.

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

    NeedCommsTimeInTransit = 30 minute
      """
      On the way transiting to the surveying location, how often to surface
      for commumications
      """

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

    TrackAcousticTarget = true
      """
      Whether to track an acoustic target at all.
      """

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

    TrackAcousticTargetDuringTransit = false
      """
      Whether to track an acoustic target during transit (on condition that
      TrackAcousticTarget is True).
      """

    AcousticTargetContactLabel = 0 count
      """
      The acoustic address of the asset to be tracked
      """

    AcousticTargetDepth = NaN meter
      """
      Depth of acoustic target if known and fixed (or nearly fixed). For
      example, if the acoustic target is a Wave Glider, set it to zero. This
      will improve 2D projected position estimates in the Earth reference
      frame. Defaults to NaN.
      """

    AcousticTargetContactTimeout = 15 minute
      """
      Only use a sufficiently fresh handshake to update the tracking center.
      """

    PeriodOfParameterSetting = 10.0 minute
      """
      Period of taking settings for setting samplers. Set to a large number
      (e.g., 20000 minutes = 14 days) to practically diable this
      functionality.
      """

    LightDeltaTimeSetting = 5.0 minute
      """
      Period of red versus white lights.
      """

    SpeedTransit = 1.0 meter_per_second
      """
      Vehicle speed when transiting to the surveying location.
      """

    SpeedSurveySetting = 1.0 meter_per_second
      """
      Value for setting vehicle speed when surveying on cylinder yoyo.
      """

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

    YoYoMinDepthSetting = 40 meter
      """
      Value for setting minimum depth while performing the YoYo behavior.
      """

    YoYoMaxDepthSetting = 50 meter
      """
      Value for setting maximum depth while performing the YoYo behavior.
      """

    InitDepthTimeout = 20 minute
      """
      Maximum time duration for the vehicle to reach the initial yo-yo depth.
      The vehicle will switch over to attempting yo-yos after the timeout is
      expired.
      """

    TransitYoYoMinDepth = 10 meter
      """
      Minimum yoyo depth while transiting to the surveying location.
      """

    TransitYoYoMaxDepth = 50 meter
      """
      Maximum yoyo depth while transiting to the surveying location.
      """

    TransitCaptureRadius = 100 meter
      """
      Waypoint capture radius for transiting to the surveying location.
      """

    CylinderRadiusSetting = 100 meter
      """
      Value for setting radius of cylinder centered at the surveying location.
      """

    MaxCirclesOnCylinder = 1000 count
      """
      Maximum number of circles on cylinder.
      """

    # debug
    #       </Description><Units:count/><Value>3</Value></DefineArg>

    TransitLatitude = NaN degree
      """
      The latitude of the surveying location. If NaN, will skip transit.
      """

    # debug
    #       </Description><Units:degree/><Value>36.805</Value></DefineArg>

    TransitLongitude = NaN degree
      """
      The longitude of the surveying location. If NaN, will skip transit.
      """

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

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

    # Use caution when changing these.

    YoYoMinAltitude = 9 meter
      """
      Minimum altitude while performing the YoYo behavior (for
      bottom-terminated YoYos).
      """

    MinAltitude = 7 meter
      """
      Minimum altitude for the entire mission.
      """

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

    # You probably do not need to change these.

    TrackingUpdatePeriod = 15 second
      """
      How long to wait between acoustic queries
      """

    NumberOfPings = 1 count
      """
      Number of return pings to request with each acoustic query (more than 1
      will activate oneway mode)
      """

    NumberOfFixesLowPass = 4 count
      """
      Number of fixes to average to produce smoothed lat/lon/dep output
      (initialized to 4).
      """

    NumberOfStartingFixesToIgnore = 8 count
      """
      Number of fixes to ignore at the start of mission (as the vehicle just
      leaves surface, contact's location estimate especailly bearing can be
      erroneous. Initialized to 8, corresponding to 2 minutes if query
      interval TrackingUpdatePeriod = 15 s.
      """

    CylinderCircleMaxError = 100 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.
      """

    CylinderCircleTurnToPort = false
      """
      If true, vehicle turns to the left around the center point. If false,
      vehicle turns to the right.
      """

    KwpHeading = 0.010 radian_per_meter
      """
      Used to relax waypoint cross-track error constant that is adjusted for
      docking. (You can override this setting by passing an argument.)
      """

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

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

    BuoyancyNeutral = Control:VerticalControl.buoyancyNeutral
      """
      Buoyancy bladder position while performing the YoYo behavior. Defaults
      to buoyancyNeutral setting in the Config/Control.cfg file. Set to NaN cc
      for active buoyancy
      """

    MassDefault = Control:VerticalControl.massDefault
      """
      Static setting for mass during the mission. Set to NaN mm for active
      mass position
      """

    # Internal variables that you should not change

    LightDeltaTime = 5.0 minute
      """
      Period of red versus white lights.
      """

    SpeedSurvey = 1.0 meter_per_second
      """
      Vehicle speed when surveying on cylinder yoyo.
      """

    YoYoMinDepth = 1.5 meter
      """
      Minimum depth while performing the YoYo behavior. Initialized to 1.5 m.
      """

    YoYoMaxDepth = 20 meter
      """
      Maximum depth while performing the YoYo behavior. Initialized to 20 m.
      """

    CylinderRadius = 100 meter
      """
      Radius of cylinder centered at the surveying location. Initialized to
      100 m.
      """

    FlagFirstTimeParameterSetting = true
      """
      Flag of first time setting samplers (initialized to True).
      """

    ContactLatitudeLowpass = NaN degree
      """
      Low-pass filtered latitude of contact (initialized to NaN).
      """

    ContactLongitudeLowpass = NaN degree
      """
      Low-pass filtered longitude of contact (initialized to NaN).
      """

    FromLastTimeFixToCenterUpdate = 0 minute
      """
      Elapsed time from last time-fix to center update time (initialized to
      zero).
      """

    FromLastTimeFixToParameterSetting = NaN minute
      """
      Elapsed time from last time-fix to setting of samplers.
      """

    LastLightSwitchTime = NaN minute
      """
      Elapsed time from last time the light color was switched.
      """

    WhiteLightsRequested = false
      """
      Indicates white lights should be turned on (default is red to start so
      we don't scare the fish). If false, red will be turned on.
      """

    TransitWaypointCompleted = false
      """
      Transit waypoint is completed (initialized to false).
      """
  }

  # Missions should almost always start with a timeout and a NeedComms aggregate. Drift missions will not use NeedComms.

  timeout duration=MissionTimeout

  # Missions should almost always start with standard safety envelopes

  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 = BeingInterrogatedTimeout
  }

  # Many missions will keep mass position fixed at the default.

  behavior Guidance:Pitch {
    run in parallel

    set massPosition = MassDefault
  }

  insert Insert/Science.tl

  behavior Guidance:AltitudeEnvelope {
    """
    Another altitude envelope for the YoYo behavior. This envelope
    should fall within the limits of the standard safety envelopes in
    Insert/StandardEnvelopes.tl in order to avoid commanding high pitch
    angles for bottom-terminated YoYos.
    """

    run in parallel

    set minAltitude = YoYoMinAltitude
  }

  aggregate SetAllParameters {
    run when ( called )

    assign in sequence SpeedSurvey = SpeedSurveySetting

    assign in sequence YoYoMinDepth = YoYoMinDepthSetting

    assign in sequence YoYoMaxDepth = YoYoMaxDepthSetting

    assign in sequence CylinderRadius = CylinderRadiusSetting

    assign in sequence LightDeltaTime = LightDeltaTimeSetting

    syslog important "Set SpeedSurvey = " + SpeedSurvey~meter_per_second + ", YoYoMinDepth = "
         + YoYoMinDepth~meter + ", YoYoMaxDepth = " + YoYoMaxDepth~meter + ", CylinderRadius = "
         + CylinderRadius~meter + ", LightingPeriod = " + LightDeltaTime~minute + "."
  }

  aggregate TrackingAcousticTarget {
    run while (
      TrackAcousticTarget
      and TrackAcousticTargetDuringTransit
      or (
        TrackAcousticTarget
        and (
          not ( TrackAcousticTargetDuringTransit )
          and TransitWaypointCompleted
        )
      )
      or (
        TrackAcousticTarget
        and ( isNaN ( TransitLatitude ) )
      )
      or (
        TrackAcousticTarget
        and ( isNaN ( TransitLongitude ) )
      )
    )

    behavior Estimation:Tracking {
      run in parallel

      set contactLabelSetting = AcousticTargetContactLabel
      set numberOfSamplesSetting = NumberOfPings
      set updatePeriodSetting = TrackingUpdatePeriod
      set numFixesLowPassSetting = NumberOfFixesLowPass
      set numStartingFixesToIgnoreSetting = NumberOfStartingFixesToIgnore
      set contactDepthSetting = AcousticTargetDepth
    }
  }

  aggregate MissionStart {
    run in sequence

    insert id="NeedComms" Insert/NeedComms.tl

    call id="MissionStartComms" refId="NeedComms"
  }

  aggregate TransitToSurveyingLocation {
    run in sequence

    break if (
      isNaN ( TransitLatitude )
      or ( isNaN ( TransitLongitude ) )
    )

    insert id="NeedComms" Insert/NeedComms.tl

    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:Buoyancy {
      run in parallel

      set position = BuoyancyNeutral
    }

    behavior Guidance:Waypoint {
      run in sequence

      set latitude = TransitLatitude
      set longitude = TransitLongitude
      set captureRadius = TransitCaptureRadius
    }

    assign in sequence TransitWaypointCompleted = true
  }

  aggregate InitializeWithGPSlatlon {
    run when (
      isNaN ( CenterLatitude )
      or ( isNaN ( CenterLongitude ) )
    )

    assign in sequence CenterLatitude = Universal:latitude_fix

    assign in sequence CenterLongitude = Universal:longitude_fix

    syslog important "Initialize CenterLatitude and CenterLongitude to GPS fix: "
         + CenterLatitude~degree + ", " + CenterLongitude~degree + "."
  }

  aggregate UpdateCenter {
    run when (
      elapsed ( Estimation:Tracking.range_to_contact ) < AcousticTargetContactTimeout
    )

    assign in sequence ContactLatitudeLowpass = Estimation:Tracking.contact_latitude_lowpass

    assign in sequence ContactLongitudeLowpass = Estimation:Tracking.contact_longitude_lowpass

    aggregate UpdateWithContactLowpassLatLon {
      run when (
        not ( isNaN ( ContactLatitudeLowpass ) )
        and ( not ( isNaN ( ContactLongitudeLowpass ) ) )
        and ( elapsed ( Universal:time_fix ) >= ( FromLastTimeFixToCenterUpdate + TrackingUpdatePeriod ) )
      )

      assign in sequence CenterLatitude = ContactLatitudeLowpass

      assign in sequence CenterLongitude = ContactLongitudeLowpass

      syslog info "Updating. FromLastTimeFixToCenterUpdate, CenterLatitude, CenterLongitude = "
           + FromLastTimeFixToCenterUpdate~second + ", " + CenterLatitude~degree + ", "
           + CenterLongitude~degree

      assign in sequence FromLastTimeFixToCenterUpdate = elapsed ( Universal:time_fix )
    }
  }

  aggregate PeriodicSetting {
    run when (
      FlagFirstTimeParameterSetting
      or ( elapsed ( Universal:time_fix ) >= ( FromLastTimeFixToParameterSetting + PeriodOfParameterSetting ) )
    )

    call id="SetLightingAndOtherParameters" refId="SetAllParameters"

    assign in sequence FlagFirstTimeParameterSetting = false

    assign in sequence FromLastTimeFixToParameterSetting = elapsed ( Universal:time_fix )
  }

  aggregate InitialDive {
    """
    Descend from to initial depth prior to beginning yo-yos and turning
    on the lights/camera.
    """

    run in sequence

    behavior Guidance:Buoyancy {
      run in parallel

      set position = BuoyancyNeutral
    }

    behavior Guidance:SetSpeed {
      run in parallel

      set speed = SpeedSurvey
    }

    behavior Guidance:Pitch {
      run in sequence

      timeout duration=InitDepthTimeout {
        syslog important "Timed out at current depth of " + Universal:depth~meter
             + ". Switching to yo-yo command."
      }

      set depth = YoYoMinDepth
    }
  }

  # Now that the initial dive is done, get on with the yo-yos

  aggregate Cylinder {
    run in sequence

    behavior Guidance:SetSpeed {
      run in parallel

      set speed = SpeedSurvey
    }

    behavior Guidance:DepthEnvelope {
      run in parallel

      set minDepth = YoYoMinDepth
      set maxDepth = YoYoMaxDepth
      set downPitch = YoYoDownPitch
      set upPitch = YoYoUpPitch
    }

    behavior Guidance:YoYo {
      run in parallel

      set downPitch = YoYoDownPitch
      set upPitch = YoYoUpPitch
    }

    behavior Guidance:Buoyancy {
      run in parallel

      set position = BuoyancyNeutral
    }

    # Power Camera

    readData id="PowerCamera" {
      Sensor:PowerOnly.sampleLoad1
    }

    syslog info "Activating camera."

    # Wait one minute for camera to start recording

    behavior Guidance:Wait {
      run in sequence

      set duration = 25 second
    }

    # Here we start the light clock and effectively turn on the default (red) lights

    assign in sequence LastLightSwitchTime = elapsed ( Universal:time_fix )

    syslog info "Activating Red Lights."

    # As long as the time hasn't expired and white lights are requested, turn on the white ones

    aggregate WhiteLights {
      run while (
        elapsed ( Universal:time_fix ) <= ( LastLightSwitchTime + LightDeltaTime )
        and WhiteLightsRequested
      )

      aggregate PowerRequest {
        run in sequence

        # Power White Lights

        readData id="PowerWhiteLights" {
          Sensor:PowerOnly.sampleLoad3
        }
      }
    }

    # As long as the time hasn't expired and white lights are NOT requested, turn on the red ones

    aggregate RedLights {
      run while (
        elapsed ( Universal:time_fix ) <= ( LastLightSwitchTime + LightDeltaTime )
        and ( not ( WhiteLightsRequested ) )
      )

      aggregate PowerRequest {
        run in sequence

        # Power Red Lights

        readData id="PowerRedLights" {
          Sensor:PowerOnly.sampleLoad2
        }
      }
    }

    # These two aggregates below handle the logic for turning lights from white to red by setting the boolean and resetting the timer

    aggregate FlipLightsWhite {
      run when (
        elapsed ( Universal:time_fix ) >= ( LastLightSwitchTime + LightDeltaTime )
        and ( not ( WhiteLightsRequested ) )
      )

      aggregate {
        run in sequence

        syslog info "Activating White Lights."

        assign in parallel WhiteLightsRequested = true

        assign in parallel LastLightSwitchTime = elapsed ( Universal:time_fix )
      }
    }

    aggregate FlipLightsRed {
      run when (
        elapsed ( Universal:time_fix ) >= ( LastLightSwitchTime + LightDeltaTime )
        and WhiteLightsRequested
      )

      aggregate {
        run in sequence

        syslog info "Activating Red Lights."

        assign in parallel WhiteLightsRequested = false

        assign in parallel LastLightSwitchTime = elapsed ( Universal:time_fix )
      }
    }

    aggregate CylinderYoYo {
      run in sequence

      syslog important "Yoyo profile on a cylinder around location " + CenterLatitude~degree + ","
           + CenterLongitude~degree + ", at radius = " + CylinderRadius~meter + "."

      aggregate CylinderWrapper {
        run in sequence repeat=MaxCirclesOnCylinder

        assign in parallel Control:HorizontalControl.kwpHeading = KwpHeading

        behavior Guidance:Circle {
          run in sequence

          set latitude = CenterLatitude
          set longitude = CenterLongitude
          set radius = CylinderRadius
          set maxError = CylinderCircleMaxError
          set turnToPort = CylinderCircleTurnToPort
        }
      }
    }
  }
}