Science/sci2_noyo_optim.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 sci2_noyo_optim {
  """
  Vehicle transits to the specified waypoints at the commanded depth
  (flat-and-level, no yoyos) with science, Optim camera, and multiray
  lights on.
  """

  arguments {
    MissionTimeout = 2 hour
      """
      Maximum duration of mission
      """

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

    Depth = 35 meter
      """
      Depth of flat and level flight during the mission.
      """

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

    Lat[1..8] = NaN degree
      """
      Latitude of waypoint {$}. If nan, waypoint is skipped.
      """

    Lon[1..8] = NaN degree
      """
      Longitude of waypoint {$}. If nan, waypoint is skipped.
      """

    FastSpeed = 1 meter_per_second
      """
      Speed while transiting to waypoint.
      """

    ApproachDepth = 30 meter
      """
      Depth for initial descent form the surface.
      """

    ApproachSpeed = 0.7 meter_per_second
      """
      Speed while descending to target depth.
      """

    ApproachDepthRate = 0.05 meter_per_second
      """
      Descent depth rate (Positive depth rate means going down).
      """

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

    ApproachDepthTimeout = 15 minute
      """
      Maximum time duration for the vehicle to reach the target depth
      """

    ApproachSettleTimePreDive = 1 minute
      """
      Time duration for the vehicle to pump down the VBS *BEFORE* leaving the
      surface.
      """

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

    kpDepth = 0.07 radian_per_meter
      """
      Dive loop proportional gain (applied only while the vehicle is
      descending to target depth).
      """

    SlowSpeed = 0.25 meter_per_second
      """
      Set this to something other than Nan if slow speed filming with a
      wide depth deadband is desired.
      """

    DepthDeviationSlow = 3 meter
      """
      Allowable +/- deviation from depth during slow flight. Needs a SlowSpeed
      to be set.
      """

    BuoyancyNeutral = Control:VerticalControl.buoyancyNeutral
      """
      Buoyancy bladder position during the mission. 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.
      """

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

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

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

  output {
    DiveMode = 0 count
      """
      Mission variable (don't change). The mission sets this variable to
      switch between dive modes. Initialized to DoingComms.
      """

    DoingComms = 0 count
      """
      Mission variable (don't change). The mission will run in this to mode
      when doing surface comms.
      """

    InitialDive = 1 count
      """
      Mission variable (don't change). The mission will run in this mode after
      surface comms are done to dive the vehicle to the approach depth.
      """

    SettleAtDepth = 2 count
      """
      Mission variable (don't change). The mission will run in this mode to
      settle the vehicle at the target depth.
      """

    MaintainDepth = 3 count
      """
      Mission variable (don't change). The mission will run in this mode to
      maintain the vehicle at the target depth.
      """
  }

  # Mission timeout

  timeout duration=MissionTimeout

  # Science!

  insert Insert/Science.tl {
    """
    Run the science sensors.
    """
  }

  # 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

  insert id="NeedComms" Insert/NeedComms.tl

  assign in sequence NeedComms:DiveInterval = MissionTimeout

  assign in sequence NeedComms:WaitForPitchUp = 0 minute

  insert Insert/BackseatDriver.tl
  assign in parallel BackseatDriver:EnableBackseat = true

  insert Insert/PowerOnly.tl

  behavior Guidance:Buoyancy {
    run in parallel

    set position = BuoyancyNeutral
  }

  insert Insert/Optim.tl
  assign in parallel Optim:RunOptim = true

  aggregate SurfaceComms {
    run when (
      called
      or ( elapsed ( Universal:time_fix ) > NeedCommsTime )
    )

    assign in sequence DiveMode = DoingComms

    call refId="NeedComms"

    assign in sequence DiveMode = InitialDive
  }

  aggregate DiveCmd {
    """
    Called when the vehicle should be underwater. Arbitrates between 4
    dive modes: 1) descend from the surface to initial depth. 2) settle
    at target depth using the VB. 3) cruise at target depth and go slow
    while within depth deviation bounds if a slow speed is set.
    """

    run when ( called )

    aggregate InitialDive {
      """
      Descend from the surface to target depth, limit pitch and
      depth-rate.
      """

      run while ( DiveMode == InitialDive )

      aggregate dive {
        run in sequence

        assign in parallel Control:VerticalControl.pitchLimit = ApproachPitchLimit

        assign in parallel Control:VerticalControl.kpDepth = kpDepth

        behavior Guidance:Mass {
          run in parallel

          set position = MassDefault
        }

        # behavior Guidance:Buoyancy {
        #   run in parallel

        #   set position = BuoyancyNeutral
        # }

        syslog info "Waiting for "
             + ApproachSettleTimePreDive~minute + "while pumping down to neutral."

        behavior Guidance:Wait {
          """
          Take a few moments to ensure the VBS reaches neutral.
          """

          run in sequence

          set duration = ApproachSettleTimePreDive
        }

        syslog "Moving to " + ApproachDepth~meter

        aggregate ApproachDepth {
          run in sequence

          behavior Guidance:SetSpeed {
            run in parallel

            set speed = ApproachSpeed
          }

          behavior Guidance:Pitch {
            run in sequence

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

              behavior Guidance:Execute {
                run in sequence

                set command = "stop"
              }
            }

            set depth = ApproachDepth
            set depthRate = ApproachDepthRate
          }
        }

        assign in sequence DiveMode = SettleAtDepth
      }
    }

    aggregate SettleAtDepth {
      """
      Take a few more moments here to make sure the vehicle is settled
      at the target depth.
      """

      run while ( DiveMode == SettleAtDepth )

      aggregate dive {
        run in sequence

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

        behavior Guidance:Pitch {
          run in parallel

          set depth = Depth
        }

        behavior Guidance:Wait {
          run in sequence

          set duration = ApproachSettleTimePostDive
        }

        assign in sequence DiveMode = MaintainDepth
      }
    }

    aggregate MaintainDepth {
      """
      Cruise at the target depth.
      """

      run while (
        DiveMode == MaintainDepth
        and ( Universal:depth > ( Depth + DepthDeviationSlow ) )
        or ( Universal:depth < ( Depth - DepthDeviationSlow ) )
      )

      aggregate dive {
        run in parallel

        aggregate ThrusterDive {
          run while ( FastSpeed > 0.0 meter_per_second )

          behavior Guidance:Mass {
            run in parallel

            set position = MassDefault
          }

          # behavior Guidance:Buoyancy {
          #   run in parallel

          #   set position = BuoyancyNeutral
          # }

          behavior Guidance:SetSpeed {
            run in parallel

            set speed = FastSpeed
          }
        }

        behavior Guidance:Pitch {
          run in parallel

          set depth = Depth
        }
      }
    }

    aggregate MaintainDepthSlow {
      """
      Cruise slow at the target depth.
      """

      run while (
        ( not isNaN ( SlowSpeed ) )
        and ( DiveMode == MaintainDepth )
        and ( Universal:depth <= ( Depth + DepthDeviationSlow ) )
        and ( Universal:depth >= ( Depth - DepthDeviationSlow ) )
      )

      behavior Guidance:SetSpeed {
        run in parallel

        set speed = SlowSpeed
      }

      # Let's make sure the vertical control depth deadband doesn't interfere with our mission
      # level version of the deadband. And we'll add an extra 3m so we don't trigger any
      # faults inside vertical control when attempting to regain the commanded depth.
      assign in parallel Control:VerticalControl.depthDeadband = ( DepthDeviationSlow + 3 m )

      behavior Guidance:Buoyancy {
        run in parallel

        set position = NaN cc
      }

      behavior Guidance:Pitch {
        run in parallel

        set depth = Depth
      }
    }
  }

  # Start mission sequence

  call id="StartingMission" refId="SurfaceComms"

  aggregate Lap {
    run in sequence repeat=Repeat

    aggregate Dive {
      run while ( not ( DiveMode == DoingComms ) )

      call refId="DiveCmd"
    }

    aggregate Wpt1 {
      run in sequence

      behavior Guidance:Waypoint {
        run in sequence

        set latitude = Lat[1]
        set longitude = Lon[1]
      }
    }

    macro $i = 2..7 {
      aggregate Wpt$i {
        run in sequence

        behavior Guidance:Waypoint {
          run in sequence

          set latitude = Lat[$i]
          set longitude = Lon[$i]
        }
      }
    }
  }
}