Science/smear_sampling.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 Smear {
  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.

    NumESPSamplers = 3 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).
      """

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

    NeedCommsTimeInTransect = 60 minute
      """
      In transit, how often to surface for commumications
      """

    AcousticTrackingTimeout = 21 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.
      """

    MaxWaitNoFiring = 4 hour
      """
      If no firing after more than MaxWaitNoFiring, terminate mission.
      """

    CylinderTimeout = 30 minute
      """
      Maximum duration of cylinder yoyo.
      """

    CylinderYoYo = false
      """
      Whether to yoyo on cylinder centered at the sampling location.
      Initialized to True.
      """

    SpeedTransit = 1.0 meter_per_second
      """
      Vehicle speed when transiting.
      """

    SpeedSmearSampling = 1.0 meter_per_second
      """
      Vehicle speed for smear sampling.
      """

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

    ShallowBoundSmear = 5.0 meter
      """
      Shallow depth bound for smear sampling.
      """

    DeepBoundSmear = 25 meter
      """
      Deep depth bound for smear sampling.
      """

    TransitYoYoMinDepth = 1.5 meter
      """
      Minimum depth while performing the YoYo behavior during transit.
      """

    TransitYoYoMaxDepth = 30 meter
      """
      Maximum depth while performing the YoYo behavior during transit.
      """

    CylinderYoYoMinDep = 1.5 meter
      """
      Minimum depth while performing the YoYo behavior on cylinder centered at
      the sampling location.
      """

    CylinderYoYoMaxDep = 30 meter
      """
      Maximum depth while performing the YoYo behavior on cylinder centered at
      the sampling location.
      """

    CylinderRadius = 200 meter
      """
      Radius of cylinder centered at the sampling location.
      """

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

    RepeatTransects = 1 count
      """
      Number of times the vehicle should try to cycle through waypoints..
      """

    Lat1 = NaN degree
      """
      Latitude of waypoint 1. If nan, waypoint is skipped.
      """

    # debug
    #       </Description><Units:degree/><Value>36.815</Value></DefineArg>
    #       </Description><Units:degree/><Value>36.810</Value></DefineArg>

    Lon1 = NaN degree
      """
      Longitude of waypoint 1. If nan, waypoint is skipped.
      """

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

    Lat2 = NaN degree
      """
      Latitude of waypoint 2. If nan, waypoint is skipped.
      """

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

    Lon2 = NaN degree
      """
      Longitude of waypoint 2. If nan, waypoint is skipped.
      """

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

    Lat3 = NaN degree
      """
      Latitude of waypoint 3. If nan, waypoint is skipped.
      """

    Lon3 = NaN degree
      """
      Longitude of waypoint 3. If nan, waypoint is skipped.
      """

    Lat4 = NaN degree
      """
      Latitude of waypoint 4. If nan, waypoint is skipped.
      """

    Lon4 = NaN degree
      """
      Longitude of waypoint 4. If nan, waypoint is skipped.
      """

    # Use caution when changing these.

    UpRate = -0.04 meter_per_second
      """
      Go-up depth rate (Negative depth rate means going up).
      """

    DownRate = 0.04 meter_per_second
      """
      Go-down depth rate (Positive depth rate means going down).
      """

    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.

    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.
      """

    RudderAngle = 13 degree
      """
      Rudder angle to use while performing the donut cast.
      """

    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

    YoYoMinDepth = 1.5 meter
      """
      Minimum depth while performing the YoYo behavior. Program will set to
      TransitYoYoMinDepth, and then CylinderYoYoMinDep.
      """

    YoYoMaxDepth = 30 meter
      """
      Maximum depth while performing the YoYo behavior. Program will set to
      TransitYoYoMaxDepth, and then CylinderYoYoMaxDep.
      """

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

    StoppedForNoFiringForTooLong = false
      """
      Mission stopped because of no firing for too long. Initialized to false.
      """

    DepthLogged = 0.0 meter

    TempLogged = 0.0 celsius

    ChlLogged = 0.0 microgram_per_liter

    MissionStartCommsCompleted = false
      """
      MissionStartComms is completed (initialized to false).
      """

    StartSmear = false
      """
      Start vertical smear (initialized to false).
      """

    FlagGoUp = false
      """
      Go-up flag (set to true when going up. Initialized to false).
      """

    FlagGoDown = false
      """
      Go-down flag (set to true when going down. Initialized to false).
      """

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

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

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

    FromLastTimeFixToStartSmear = NaN minute
      """
      Elapsed time from last time-fix to StartSmear.
      """

    FromLastTimeFixToNow = NaN minute
      """
      Elapsed time from last time-fix to now.
      """

    FromLastTimeFixToLastFiring = NaN minute
      """
      Elapsed time from last time-fix to last firing.
      """

    CylinderLat = NaN degree
      """
      Latitude of cylinder center.
      """

    CylinderLon = NaN degree
      """
      Longitude of cylinder center.
      """

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

  # 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 = AcousticTrackingTimeout
  }

  # Many missions will keep mass position fixed at the default. This mission allows the buoyancy volume to change to track the chlorophyll peak while drifting.

  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 MissionStart {
    run in sequence

    insert id="NeedComms" Insert/NeedComms.tl

    call id="MissionStartComms" refId="NeedComms"

    assign in sequence MissionStartCommsCompleted = true
  }

  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 TransitToSamplingLocation {
    run in sequence

    insert id="NeedComms" Insert/NeedComms.tl

    assign in sequence NeedComms:DiveInterval = NeedCommsTimeInTransect

    behavior Guidance:SetSpeed {
      run in parallel

      set speed = SpeedTransit
    }

    assign in sequence YoYoMinDepth = TransitYoYoMinDepth

    assign in sequence YoYoMaxDepth = TransitYoYoMaxDepth

    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
    }

    aggregate Lap {
      run in sequence repeat=RepeatTransects

      aggregate Wpt1 {
        run in sequence

        break if (
          isNaN ( Lat1 )
          or ( isNaN ( Lon1 ) )
        )

        behavior Guidance:Waypoint {
          run in sequence

          set latitude = Lat1
          set longitude = Lon1
        }
      }

      aggregate Wpt2 {
        run in sequence

        break if (
          isNaN ( Lat2 )
          or ( isNaN ( Lon2 ) )
        )

        behavior Guidance:Waypoint {
          run in sequence

          set latitude = Lat2
          set longitude = Lon2
        }
      }

      aggregate Wpt3 {
        run in sequence

        break if (
          isNaN ( Lat3 )
          or ( isNaN ( Lon3 ) )
        )

        behavior Guidance:Waypoint {
          run in sequence

          set latitude = Lat3
          set longitude = Lon3
        }
      }

      aggregate Wpt4 {
        run in sequence

        break if (
          isNaN ( Lat4 )
          or ( isNaN ( Lon4 ) )
        )

        behavior Guidance:Waypoint {
          run in sequence

          set latitude = Lat4
          set longitude = Lon4
        }
      }
    }

    aggregate SetSamplingLocationtoWaypoint1 {
      run in sequence

      break if (
        not ( CylinderYoYo )
        or ( isNaN ( Lat1 ) )
        or ( isNaN ( Lon1 ) )
      )

      assign in sequence CylinderLat = Lat1

      assign in sequence CylinderLon = Lon1

      syslog important "Set cylinder center to Waypoint1 " + CylinderLat~degree + ","
           + CylinderLon~degree + "."
    }

    aggregate CylinderYoYoAroundSamplingLocation {
      run in sequence

      timeout duration=CylinderTimeout

      aggregate CylinderIfNonNaNLatLon {
        run in sequence

        break if (
          not ( CylinderYoYo )
          or ( isNaN ( CylinderLat ) )
          or ( isNaN ( CylinderLon ) )
        )

        syslog important "Yoyo profile on a cylinder around sampling location " + CylinderLat~degree
             + "," + CylinderLon~degree + ", at radius = " + CylinderRadius~meter

        assign in sequence YoYoMinDepth = CylinderYoYoMinDep

        assign in sequence YoYoMaxDepth = CylinderYoYoMaxDep

        aggregate CylinderWrapper {
          run in sequence repeat=MaxCirclesOnCylinder

          assign in parallel Control:HorizontalControl.kwpHeading = KwpHeading

          behavior Guidance:Circle {
            run in sequence

            set latitude = CylinderLat
            set longitude = CylinderLon
            set radius = CylinderRadius
            set maxError = CylinderCircleMaxError
            set turnToPort = CylinderCircleTurnToPort
          }
        }
      }
    }

    aggregate GoToCylinderCenter {
      run in sequence

      break if (
        isNaN ( CylinderLat )
        or ( isNaN ( CylinderLon ) )
      )

      syslog important "Go to cylinder center (i.e., sampling location): " + CylinderLat~degree
           + "," + CylinderLon~degree

      behavior Guidance:Waypoint id="ToCylinderCenter" {
        run in sequence

        set latitude = CylinderLat
        set longitude = CylinderLon
      }

      call id="GoToSurfaceBeforeDiving" refId="NeedComms"
    }

    assign in sequence FlagGoDown = true

    syslog important "Go down at " + Universal:depth~meter + "."

    assign in sequence StartSmear = true

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

  # debug
  #         <Syslog Severity="Important">FromLastTimeFixToStartSmear = <Arg Name="FromLastTimeFixToStartSmear"/><Units:minute/></Syslog>


  aggregate GoDownGoUp {
    run in sequence

    aggregate GoDownInSpiralMode {
      run while (
        FlagGoDown
        and StartSmear
      )

      behavior Guidance:Pitch {
        run in parallel

        set depthRate = DownRate
      }

      behavior Guidance:SetSpeed {
        run in parallel

        set speed = SpeedSmearSampling
      }

      behavior Guidance:Point {
        run in parallel

        set rudderAngle = RudderAngle
      }

      behavior Guidance:Buoyancy {
        run in parallel

        set position = BuoyancyNeutral
      }
    }

    aggregate GoUpInSpiralMode {
      run while (
        FlagGoUp
        and StartSmear
      )

      behavior Guidance:Pitch {
        run in parallel

        set depthRate = UpRate
      }

      behavior Guidance:SetSpeed {
        run in parallel

        set speed = SpeedSmearSampling
      }

      behavior Guidance:Point {
        run in parallel

        set rudderAngle = RudderAngle
      }

      behavior Guidance:Buoyancy {
        run in parallel

        set position = BuoyancyNeutral
      }
    }

    aggregate ResetFlagGoDownSetFlagGoUp {
      run when (
        Universal:depth >= DeepBoundSmear
        and StartSmear
        and ( not ( FlagGoUp ) )
      )

      assign in sequence FlagGoDown = false

      assign in sequence FlagGoUp = true

      assign in sequence EnableFiring = true

      syslog important "Go up at " + Universal:depth~meter + "."
    }

    aggregate ResetFlagGoUpSetFlagGoDown {
      run when (
        Universal:depth <= ShallowBoundSmear
        and StartSmear
        and ( not ( FlagGoDown ) )
      )

      assign in sequence FlagGoUp = false

      assign in sequence FlagGoDown = true

      assign in sequence EnableFiring = true

      syslog important "Go down at " + Universal:depth~meter + "."
    }
  }

  aggregate TakeSamples {
    run in parallel

    aggregate NoFiringForTooLong {
      run when (
        elapsed ( Universal:time_fix ) > ( FromLastTimeFixToStartSmear + MaxWaitNoFiring )
        and ( not ( FlagSamplingOngoing ) )
        and ( CntSamples == 1 count )
        and ( not ( FlagFired ) )
        and ( not ( StoppedForNoFiringForTooLong ) )
        or (
          elapsed ( Universal:time_fix ) > ( FromLastTimeFixToLastFiring + MaxWaitNoFiring )
          and ( not ( FlagSamplingOngoing ) )
          and ( CntSamples >= 2 count )
          and ( not ( StoppedForNoFiringForTooLong ) )
        )
      )

      assign in sequence FromLastTimeFixToNow = elapsed ( Universal:time_fix )

      syslog important "No firing for too long. Stopping mission. FromLastTimeFixToNow, FromLastTimeFixToLastFiring = "
           + FromLastTimeFixToNow~minute + ", " + FromLastTimeFixToLastFiring~minute

      behavior Guidance:Execute {
        run in sequence

        set command = "stop"
      }

      assign in sequence StoppedForNoFiringForTooLong = true
    }

    aggregate Firing {
      run when (
        MissionStartCommsCompleted
        and StartSmear
        and EnableFiring
        and FlagGoDown
        and ( Universal:depth >= ShallowBoundSmear )
        and ( not ( FlagSamplingOngoing ) )
        and ( CntSamples <= NumESPSamplers )
        and ( not ( SampleCompleted ) )
        or (
          StartSmear
          and EnableFiring
          and FlagGoUp
          and ( Universal:depth <= DeepBoundSmear )
          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

      assign in sequence FromLastTimeFixToLastFiring = elapsed ( Universal:time_fix )

      # debug
      #             <Syslog Severity="Important">FromLastTimeFixToLastFiring = <Arg Name="FromLastTimeFixToLastFiring"/><Units:minute/></Syslog>

      call id="Sampling" 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
    }

    aggregate IncrementSampleCounter {
      run when ( SampleCompleted )

      assign in sequence CntSamples = CntSamples + 1 count

      assign in sequence SampleCompleted = false
    }
  }

  aggregate MissionCompleted {
    run when (
      CntSamples == ( NumESPSamplers + 1 count )
    )

    syslog important "Completed sampling. Stopping mission."

    behavior Guidance:Execute {
      run in sequence

      set command = "stop"
    }

    assign in sequence CntSamples = CntSamples + 1 count
  }
}