Science/sci2_flat_and_level.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_flat_and_level {
  """
  Vehicle transits to the specified waypoints at the commanded depth
  (flat-and-level) with science turned on.
  """

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

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

    Depth1 = 3 meter
      """
      Depth of flat and level flight during the mission for the transit of the waypoints
      """

    Depth[2..10] = NaN meter
      """
      Depth of flat and level flight during the mission for subsequent repetitions of the waypoints.
      Skipped if NaN.
      """

    Repeat = 1 count
      """
      Number of times the vehicle should try to cycle through waypoints and depths.
      One run of all waypoints at all target depths is one repetition.
      """

    Lat[1..7] = NaN degree
      """
      Latitude of waypoint 1. If nan, waypoint is skipped.
      """

    Lon[1..7] = NaN degree
      """
      Longitude of waypoint 1. If nan, waypoint is skipped.
      """

    Speed = 1 meter_per_second
      """
      Speed while transiting to waypoints.
      """

    ApproachDepth = 2 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).
      """

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

    LegDepth = Depth1
  }

  # 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

  insert Insert/PowerOnly.tl

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

    assign in sequence DiveMode = DoingComms

    call refId="NeedComms"

    assign in sequence DiveMode = InitialDive
  }

  assign in sequence LegDepth = Depth1

  aggregate DiveCmd {
    """
    Called when the vehicle should be underwater. Arbitrates between 3
    dive modes: 1) descend from the surface to initial depth. 2) settle
    at target depth using the VB. 3) cruise at target depth.
    """

    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 vechile 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 = ApproachDepth
        }

        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 )

      aggregate dive {
        run in parallel

        aggregate ThrusterDive {
          run while ( Speed > 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 = Speed
        }

        behavior Guidance:Pitch {
          run in parallel

          set depth = LegDepth
        }
      }
    }
  }

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

      assign in sequence LegDepth = Depth1

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

          behavior Guidance:Waypoint {
            run in sequence

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

    aggregate Leg2 {
      run in sequence

      break if ( isNaN ( Depth[2] ) )

      syslog important "Transiting waypoints at depth " + Depth[2]~meter
      assign in sequence LegDepth = Depth[2]

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

          behavior Guidance:Waypoint {
            run in sequence

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

    aggregate Leg3 {
      run in sequence

      break if ( isNaN ( Depth[3] ) )

      syslog important "Transiting waypoints at depth " + Depth[3]~meter
      assign in sequence LegDepth = Depth[3]

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

          behavior Guidance:Waypoint {
            run in sequence

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

    aggregate Leg4 {
      run in sequence

      break if ( isNaN ( Depth[4] ) )

      syslog important "Transiting waypoints at depth " + Depth[4]~meter
      assign in sequence LegDepth = Depth[4]

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

          behavior Guidance:Waypoint {
            run in sequence

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

    aggregate Leg5 {
      run in sequence

      break if ( isNaN ( Depth[5] ) )

      syslog important "Transiting waypoints at depth " + Depth[5]~meter
      assign in sequence LegDepth = Depth[5]
      macro $i = 1..7 {
        aggregate Wpt$i {
          run in sequence

          behavior Guidance:Waypoint {
            run in sequence

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

    aggregate Leg6 {
      run in sequence

      break if ( isNaN ( Depth[6] ) )

      syslog important "Transiting waypoints at depth " + Depth[6]~meter
      assign in sequence LegDepth = Depth[6]
      macro $i = 1..7 {
        aggregate Wpt$i {
          run in sequence

          behavior Guidance:Waypoint {
            run in sequence

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

    aggregate Leg7 {
      run in sequence

      break if ( isNaN ( Depth[7] ) )

      syslog important "Transiting waypoints at depth " + Depth[7]~meter
      assign in sequence LegDepth = Depth[7]
      macro $i = 1..7 {
        aggregate Wpt$i {
          run in sequence

          behavior Guidance:Waypoint {
            run in sequence

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

    aggregate Leg8 {
      run in sequence

      break if ( isNaN ( Depth[8] ) )

      syslog important "Transiting waypoints at depth " + Depth[8]~meter
      assign in sequence LegDepth = Depth[8]
      macro $i = 1..7 {
        aggregate Wpt$i {
          run in sequence

          behavior Guidance:Waypoint {
            run in sequence

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

    aggregate Leg9 {
      run in sequence

      break if ( isNaN ( Depth[9] ) )

      syslog important "Transiting waypoints at depth " + Depth[9]~meter
      assign in sequence LegDepth = Depth[9]
      macro $i = 1..7 {
        aggregate Wpt$i {
          run in sequence

          behavior Guidance:Waypoint {
            run in sequence

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

    aggregate Leg10 {
      run in sequence

      break if ( isNaN ( Depth[10] ) )

      syslog important "Transiting waypoints at depth " + Depth[10]~meter
      assign in sequence LegDepth = Depth[10]
      macro $i = 1..7 {
        aggregate Wpt$i {
          run in sequence

          behavior Guidance:Waypoint {
            run in sequence

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