Engineering/sci2_slow_and_flat.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_slow_and_flat {
  """
  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
      """

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

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

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

    Lon1 = -122.8 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 = 0.25 meter_per_second
      """
      Speed while transiting to waypoints.
      """

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

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

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

    ApproachDepthTimeout = 20 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 = 40 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

  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 3 dive modes:
        1) descend from the surface to initial depth.
        2) settle at target depth using the VB.
        3) cruise at target depth at 0 pitch using mass shifter and VB for vertical control.
    """

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

        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 = 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 )

      aggregate dive {
        run in parallel

        behavior Guidance:SetSpeed {
          run in parallel

          set speed = Speed
        }

        behavior Guidance:Pitch {
          run in parallel

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

  # 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 = Lat1
        set longitude = Lon1
      }
    }

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