Science/esp_sample_at_threshold.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 esp_sample_at_threshold {
  """
  This mission drives towards a waypoint until chl reading is above a
  threshold for the timeout. Then it stops and takes ESP samples at the
  designated depth. Originally this was meant to be a mashup of sci2 and
  esp_sample_at_depth.
  """

  arguments {
    MissionTimeout = 6 hour
      """
      Maximum duration of mission.
      """

    NeedCommsTime = 60 minute
      """
      How often to surface for commumications during transit to waypoint.
      """

    chlorophyllLowerBound = 3 microgram_per_liter
      """
      When WetLabsBB2FL.bin_median_mass_concentration_of_chlorophyll lies
      between chlorophyllLowerBound and chlorophyllUpperBound for the timeout,
      stop running waypoints and sample at depth.
      """

    chlorophyllUpperBound = 100 microgram_per_liter
      """
      When WetLabsBB2FL.bin_median_mass_concentration_of_chlorophyll lies
      between chlorophyllLowerBound and chlorophyllUpperBound for the timeout,
      stop running waypoints and sample at depth.
      """

    chlorophyllThresholdTimeout = 1 minute
      """
      How long to wait to start sampling while chl is above the threshold.
      """

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

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

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

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

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

    Lat5 = NaN degree
      """
      Latitude of waypoint 5. If nan, waypoint is skipped.
      """

    Lon5 = NaN degree
      """
      Longitude of waypoint 5. If nan, waypoint is skipped.
      """

    Lat6 = NaN degree
      """
      Latitude of waypoint 6. If nan, waypoint is skipped.
      """

    Lon6 = NaN degree
      """
      Longitude of waypoint 6. If nan, waypoint is skipped.
      """

    Lat7 = NaN degree
      """
      Latitude of waypoint 7. If nan, waypoint is skipped.
      """

    Lon7 = NaN degree
      """
      Longitude of waypoint 7. If nan, waypoint is skipped.
      """

    Speed = 1 meter_per_second
      """
      Speed while performing the YoYo behavior.
      """

    CaptureRadius = NaN meter
      """
      Speed while performing the YoYo behavior.
      """

    YoYoMinDepth = 2 meter
      """
      Minimum depth while performing the YoYo behavior.
      """

    YoYoMaxDepth = 200 meter
      """
      Maximum depth while performing the YoYo behavior.
      """

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

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

    TargetDepth = 3 meter
      """
      Depth to sample at. Initialized to 3 m.
      """

    ESPCartridgeType_1 = -4 count
      """
      Specifies ESP cartridge type code for sample# 1. Set to NaN to skip this
      sample.
      """

    ESPCartridgeType_2 = -15 count
      """
      Specifies ESP cartridge type code for sample# 2. Set to NaN to skip this
      sample.
      """

    ESPCartridgeType_3 = NaN count
      """
      Specifies ESP cartridge type code for sample# 3. Set to NaN to skip this
      sample.
      """

    ESPCartridgeType_4 = NaN count
      """
      Specifies ESP cartridge type code for sample# 4. Set to NaN to skip this
      sample.
      """

    SettleTime = 3 minute
      """
      How long to wait between samples. This starts *before* the first sample.
      """

    SpeedSampling = 0 meter_per_second
      """
      Vehicle speed while sampling. Set to zero to maintain depth using VBS
      (drift).
      """

    ApproachSettleTimePostDive = 1 minute
      """
      Time duration for the vehicle to settle *AFTER* reaching the target
      depth.
      """

    DepthDeadband = 0.01 meter
      """
      How much vertical drift from the specified depth is allowed while
      sampling.
      """

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

    MaxDepth = 5 meter
      """
      Maximum allowable depth during the mission.
      """

    MinOffshore = 1 kilometer
      """
      Minimum distance from the shoreline to maintain.
      """
  }

  output {
    ESPCartridgeType = NaN count
      """
      Mission parameter (don't change). Specifies ESP cartridge type code.
      """

    ChlThresholdReached = false
      """
      Indicates internally that the threshold has been reach and we should
      start a sample.
      """

    ChlThresholdReachedFlagSet = false
      """
      Indicates internally that the threshold-met flag is set.
      """
  }

  # Mission timeout

  timeout duration=MissionTimeout

  # Safety nets and surfacing behaviors

  behavior Sample:AbortSample {
    run in parallel
  }

  insert Insert/StandardEnvelopes.tl

  assign in sequence StandardEnvelopes:MaxDepth = MaxDepth

  assign in sequence StandardEnvelopes:MinAltitude = MinAltitude

  assign in sequence StandardEnvelopes:MinOffshore = MinOffshore

  insert id="NeedComms" Insert/NeedComms.tl

  assign in sequence NeedComms:DiveInterval = MissionTimeout

  assign in sequence NeedComms:WaitForPitchUp = 0 minute

  assign in parallel Control:VerticalControl.depthDeadband = DepthDeadband

  # Callable aggregate

  aggregate SampleESP {
    run when ( called )

    aggregate sample {
      run in sequence

      syslog important "At " + Universal:depth~meter + ", settling for " + SettleTime~minute

      behavior Guidance:Wait {
        run in sequence

        set duration = SettleTime
      }

      syslog important "Requesting ESP cartridge type:" + ESPCartridgeType~count

      behavior Science:ESPCartridgeSelect {
        run in sequence

        timeout duration=P1M

        set cartridgeType = ESPCartridgeType
      }

      readDatum id="TriggerESP" {
        Science:ESPComponent.sampling
      }

      syslog important "ESP sampling at " + Universal:depth~meter + " in "
           + Science:CTD_Seabird.bin_mean_sea_water_temperature~celsius + " water with "
           + Science:WetLabsBB2FL.bin_mean_mass_concentration_of_chlorophyll_in_sea_water~microgram_per_liter
           + "chlorophyll fluorescence."

      readDatum id="WaitForESP" {
        Science:ESPComponent.sample_number
      }
    }
  }

  # Start mission sequence

  call id="StartingMission" refId="NeedComms"

  insert Insert/Science.tl

  aggregate CheckChlThreshold {
    run when (
      Science:WetLabsBB2FL.bin_median_mass_concentration_of_chlorophyll_in_sea_water > chlorophyllLowerBound
      and ( Science:WetLabsBB2FL.bin_median_mass_concentration_of_chlorophyll_in_sea_water < chlorophyllUpperBound )
      and ( elapsed ( Universal:time_fix ) > chlorophyllThresholdTimeout )
      and ( not ( ChlThresholdReachedFlagSet ) )
    )

    assign in sequence ChlThresholdReached = true

    assign in sequence ChlThresholdReachedFlagSet = true
  }

  # This first aggregate is the sci2 part

  aggregate Sci2Phase {
    run in sequence

    break if ( ChlThresholdReached )

    assign in sequence NeedComms:DiveInterval = NeedCommsTime

    behavior Guidance:Pitch {
      run in parallel

      set massPosition = MassDefault
    }

    behavior Guidance:Buoyancy {
      run in parallel

      set position = BuoyancyNeutral
    }

    behavior Guidance:SetSpeed {
      run in parallel

      set speed = Speed
    }

    behavior Guidance:DepthEnvelope {
      """
      Another depth 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 depth-terminated YoYos.
      """

      run in parallel

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

    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
    }

    behavior Guidance:YoYo {
      run in parallel

      set downPitch = YoYoDownPitch
    }

    aggregate Lap {
      run in sequence

      aggregate Wpt1 {
        run in sequence

        behavior Guidance:Waypoint {
          run in sequence

          set latitude = Lat1
          set longitude = Lon1
          set captureRadius = CaptureRadius
        }
      }

      aggregate Wpt2 {
        run in sequence

        behavior Guidance:Waypoint {
          run in sequence

          set latitude = Lat2
          set longitude = Lon2
        }
      }

      aggregate Wpt3 {
        run in sequence

        behavior Guidance:Waypoint {
          run in sequence

          set latitude = Lat3
          set longitude = Lon3
        }
      }

      aggregate Wpt4 {
        run in sequence

        behavior Guidance:Waypoint {
          run in sequence

          set latitude = Lat4
          set longitude = Lon4
        }
      }

      aggregate Wpt5 {
        run in sequence

        behavior Guidance:Waypoint {
          run in sequence

          set latitude = Lat5
          set longitude = Lon5
        }
      }

      aggregate Wpt6 {
        run in sequence

        behavior Guidance:Waypoint {
          run in sequence

          set latitude = Lat6
          set longitude = Lon6
        }
      }

      aggregate Wpt7 {
        run in sequence

        behavior Guidance:Waypoint {
          run in sequence

          set latitude = Lat7
          set longitude = Lon7
        }
      }
    }
  }

  # Once the threshold is met for the timeout, we sample

  aggregate SamplingPhase {
    run in sequence

    break if ( not ( ChlThresholdReached ) )

    assign in sequence NeedComms:DiveInterval = MissionTimeout

    aggregate SettleAtDepth {
      """
      Take a few moments here to ensure the vechile is settled at the
      target depth.
      """

      run in sequence

      behavior Guidance:Pitch {
        run in progression

        set depth = TargetDepth
      }

      syslog info "Reached " + Universal:depth~meter + ", settling for "
           + ApproachSettleTimePostDive~minute

      behavior Guidance:Wait {
        run in sequence

        set duration = ApproachSettleTimePostDive
      }
    }

    aggregate SampleAtTargetDepth {
      """
      Maintain target depth and acquire ESP samples.
      """

      run in sequence

      behavior Guidance:SetSpeed {
        run in parallel

        set speed = SpeedSampling
      }

      behavior Guidance:Pitch {
        run in parallel

        set depth = TargetDepth
      }

      aggregate Spinout {
        """
        A speed has been specified, spin in circels while sampling.
        """

        run while ( SpeedSampling > 0.0 meter_per_second )

        behavior Guidance:Pitch id="HoldMassPosition" {
          run in parallel

          set massPosition = Control:VerticalControl.massDefault
        }

        behavior Guidance:Buoyancy id="HoldBuoyancyPosition" {
          run in parallel

          set position = Control:VerticalControl.buoyancyNeutral
        }

        behavior Guidance:Point id="HoldRudderAngle" {
          run in parallel

          set rudderAngle = 9 degree
        }
      }

      aggregate GrabSamples {
        run in sequence

        aggregate Sample1 {
          run in sequence

          break if ( isNaN ( ESPCartridgeType_1 ) )

          assign in sequence ESPCartridgeType = ESPCartridgeType_1

          call refId="SampleESP"
        }

        aggregate Sample2 {
          run in sequence

          break if ( isNaN ( ESPCartridgeType_2 ) )

          assign in sequence ESPCartridgeType = ESPCartridgeType_2

          call refId="SampleESP"
        }

        aggregate Sample3 {
          run in sequence

          break if ( isNaN ( ESPCartridgeType_3 ) )

          assign in sequence ESPCartridgeType = ESPCartridgeType_3

          call refId="SampleESP"
        }

        aggregate Sample4 {
          run in sequence

          break if ( isNaN ( ESPCartridgeType_4 ) )

          assign in sequence ESPCartridgeType = ESPCartridgeType_4

          call refId="SampleESP"
        }
      }
    }
  }
}