Science/mind_the_drift.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 {
  """
  This mission drifts level at a depth while running the backseat and
  sending acomms data. Originally it was written for the Big Business of
  getting ayeris images transferred over the acomms link. The Drift: You
  start sinking, better grab an oar and row. And please take a path that
  you know You start thinking, making up the images as you go, as you go.
  """

  arguments {
    MissionTimeout = 120 minute
      """
      Maximum duration of mission
      """

    NeedCommsTime = 60 minute
      """
      How often to surface for commumications
      """

    AcousticModemCommsInterval = 5 minute
      """
      How often to trigger acoustic modem communications
      """

    Depth = 35 meter
      """
      How deep to drift.
      """

    DepthDeadband = 1 meter
      """
      How much vertical drift from the specified depth is allowed durnig the
      mission.
      """

    ApproachSpeed = 1 meter_per_second
      """
      Enables vehicle thruster at the commanded speed to reach target depth
      (set to 0 m/s to drift).
      """

    ApproachDepthTimeout = 20 minute
      """
      Maximum wait time for the vehicle to reach the targeted depth.
      """

    ApproachPitchLimit = 20 degree
      """
      Max vehicle pitch (+/-) when decending from the surface.
      """

    MinAltitude = 5 meter

    MaxDepth = 52 meter

    MinOffshore = 2 kilometer

    CheckDepths = 1 bool

    PowerBackseat = 1 bool
      """
      Set to false to disable backseat
      """
  }

  timeout duration=MissionTimeout

  insert Insert/Science.tl

  insert id="NeedComms" Insert/NeedComms.tl

  assign in sequence NeedComms:DiveInterval = NeedCommsTime

  insert Insert/StandardEnvelopes.tl

  assign in sequence StandardEnvelopes:MinAltitude = MinAltitude

  assign in sequence StandardEnvelopes:MaxDepth = MaxDepth

  assign in sequence StandardEnvelopes:MinOffshore = MinOffshore

  insert id="AcousticComms" Insert/AcousticModemComms.tl {
    redefineArg CommsInterval = AcousticModemCommsInterval
  }

  aggregate CheckTargetDepths {
    run in sequence

    break if ( not ( CheckDepths ) )

    aggregate CheckDepth {
      run in sequence

      break if ( not ( Depth > MaxDepth ) )

      syslog important "Aborting mission. Target depth exceeds the mission's MaxDepth setting (review mission parameters)."

      behavior Guidance:Execute {
        run in sequence

        set command = "stop"
      }
    }

    assign in sequence CheckDepths = 0 bool
  }

  call id="StartingMission" refId="NeedComms"

  insert Insert/BackseatDriver.tl

  assign in sequence BackseatDriver:EnableBackseat = PowerBackseat

  insert Insert/PowerOnly.tl

  aggregate TheDrift {
    run in sequence

    break if ( isNaN ( Depth ) )

    assign in parallel Control:VerticalControl.kpPitchMass = 0.005 ratio

    assign in parallel Control:VerticalControl.kiPitchMass = 0.00075 reciprocal_second

    assign in parallel Control:VerticalControl.kdPitchMass = 0 second

    assign in parallel Control:VerticalControl.depthDeadband = DepthDeadband

    # DO NOT REMOVE the syslog entry below. The MissionManager seems to ignore the value of ApproachSpeed and does not evaluate the SetSpeed statement properly. Adding the syslog call somehow fixes it.

    syslog info "Going to target depth. Speed set to "
         + ApproachSpeed~meter_per_second

    aggregate GoToTargetDepth {
      run in sequence

      assign in parallel Control:VerticalControl.pitchLimit = ApproachPitchLimit

      aggregate ActuatorHold {
        """
        Hold the positions of the mass, VBS, and rudder while going
        to the target depth with the thruster on.
        """

        run while ( ApproachSpeed > 0 meter_per_second )

        behavior Guidance:Point {
          run in parallel

          set rudderAngle = 13 degree
        }

        behavior Guidance:Buoyancy {
          run in parallel

          set position = Control:VerticalControl.buoyancyNeutral
        }

        behavior Guidance:Pitch {
          run in parallel

          set massPosition = Control:VerticalControl.massDefault
        }
      }

      aggregate ApproachDepth {
        run in sequence

        syslog "Moving to " + Depth~meter

        behavior Guidance:SetSpeed {
          run in parallel

          set speed = ApproachSpeed
        }

        behavior Guidance:Pitch {
          run in sequence

          timeout duration=ApproachDepthTimeout {
            syslog important "Timed out trying to reach the target depth. Stopping mission at current depth of "
                 + Universal:depth~meter

            behavior Guidance:Execute {
              run in sequence

              set command = "stop"
            }
          }

          set depth = Depth
        }
      }
    }

    aggregate DriftAtDepth {
      """
      Drift at 0 speed at target depth.
      """

      run in sequence

      assign in parallel Control:VerticalControl.depthDeadband = DepthDeadband

      behavior Guidance:SetSpeed {
        run in parallel

        set speed = 0 meter_per_second
      }

      behavior Guidance:Pitch {
        run in parallel

        set depth = Depth
        set elevatorAngle = 0 degree
        set pitch = 0 degree
      }

      syslog info "Holding at depth. Depth = " + Universal:depth~meter

      behavior Guidance:Wait {
        run in sequence

        set duration = MissionTimeout
      }
    }
  }
}