Transport/keepstation_approach.tl

mission keepstation_approach {
  """
  Vehicle transits to desired waypoint (or stays put if none is commanded)
  and stays within the specified radius. After each surface interval,
  performs an approach dive to settle at the approach depth before
  transitioning to station keeping.
  """

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

    NeedCommsTime = 45 minute
      """
      How often to surface for communications
      """

    Latitude = NaN degree
      """
      Latitude of waypoint to hold. If NaN, hold the latitude at start of
      mission.
      """

    Longitude = NaN degree
      """
      Longitude of waypoint to hold. If NaN, hold the longitude at start of
      mission.
      """

    Depth = 30 meter
      """
      Depth held during drift mode
      """

    TransitDepth = 15 meter
      """
      Depth for initial transit to station.
      """

    ApproachDepth = 29 meter
      """
      Depth for initial dive during station keeping
      """

    SettleTime = 5 minute
      """
      Time to hold at approach depth before transitioning to station keeping
      """

    DepthDeadband = 4 meter
      """
      How much vertical drift from the specified depth is allowed in drift
      mode
      """

    Speed = 0.75 meter_per_second
      """
      Vehicle transit speed.
      """

    ApproachSpeed = 1 meter_per_second
      """
      Speed for initial dive during station keeping
      """

    Radius = 200 meter
      """
      Radius of circle around waypoint to hold.
      """

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

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

  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 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 station keeping depth.
      """
  }

  timeout duration=MissionTimeout

  insert id="NeedComms" Insert/NeedComms.tl

  assign in sequence NeedComms:DiveInterval = NeedCommsTime

  insert Insert/StandardEnvelopes.tl

  assign in sequence StandardEnvelopes:MaxDepth = MaxDepth

  assign in sequence StandardEnvelopes:MinOffshore = MinOffshore

  insert Insert/BackseatDriver.tl

  insert Insert/PowerOnly.tl

  behavior Guidance:Pitch {
    run in parallel

    set massPosition = Control:VerticalControl.massDefault
  }

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

    assign in sequence DiveMode = DoingComms

    call refId="NeedComms"

    assign in sequence DiveMode = InitialDive
  }

  call id="StartingMission" refId="SurfaceComms"

  aggregate TransitToStation {
    """
    Need a separate aggregate for transit if we want to specify a
    different depth for the approach.
    """

    run in sequence

    syslog important "Transiting to station"

    behavior Guidance:Buoyancy {
      run in parallel

      set position = Control:VerticalControl.buoyancyNeutral
    }

    behavior Guidance:Pitch {
      run in parallel

      set depth = TransitDepth
    }

    behavior Guidance:SetSpeed {
      run in parallel

      set speed = Speed
    }

    behavior Guidance:Waypoint id="Wpt1" {
      run in sequence

      set latitude = Latitude
      set longitude = Longitude
    }
  }

  call id="OnStation" refId="SurfaceComms"

  aggregate DiveCmd {
    """
    Arbitrates between 3 dive modes: 1) descend from the surface
    to approach depth. 2) settle at approach depth. 3) maintain
    station keeping at target depth.
    """

    run when ( called )

    aggregate InitialDive {
      """
      Descend from the surface to approach depth
      """

      run while ( DiveMode == InitialDive )

      aggregate dive {
        run in sequence

        syslog important "Moving to " + ApproachDepth~meter

        behavior Guidance:Buoyancy {
          run in parallel

          set position = Control:VerticalControl.buoyancyNeutral
        }

        behavior Guidance:SetSpeed {
          run in parallel

          set speed = ApproachSpeed
        }

        behavior Guidance:Pitch {
          run in sequence

          set depth = ApproachDepth
        }

        assign in sequence DiveMode = SettleAtDepth
      }
    }

    aggregate SettleAtDepth {
      """
      Take time to settle at the approach depth with zero speed
      """

      run while ( DiveMode == SettleAtDepth )

      aggregate dive {
        run in sequence

        syslog info "Reached " + ApproachDepth~meter + ", settling for " + SettleTime~minute

        behavior Guidance:SetSpeed {
          run in parallel

          set speed = 0 meter_per_second
        }

        behavior Guidance:Pitch {
          run in parallel

          set depth = ApproachDepth
        }

        behavior Guidance:Wait {
          run in sequence

          set duration = SettleTime
        }

        assign in sequence DiveMode = MaintainDepth
      }
    }

    aggregate MaintainDepth {
      run while ( DiveMode == MaintainDepth )

      aggregate keepstation {
        run in parallel

        assign in parallel Control:VerticalControl.depthDeadband = DepthDeadband

        behavior Guidance:Pitch {
          run in parallel

          set depth = Depth
        }

        behavior Guidance:KeepStation {
          run in parallel

          set latitude = Latitude
          set longitude = Longitude
          set radius = Radius
          set speed = Speed
        }
      }
    }
  }

  aggregate Station {
    run in sequence

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

      call refId="DiveCmd"
    }

    behavior Guidance:Wait {
      """
      Due to the way the KeepStation behavior is currently
      implemented, we must run it in parallel and use a Wait to keep
      from completing the mission before the timeout expires. However,
      this new version of the mission uses a top-level timeout so that
      the entire mission will not run for longer then MissionTimeout.
      """

      run in sequence

      set duration = MissionTimeout
    }
  }
}