lineCaptureHomingUpdate.xml

Page auto-generated from XML file.

---

Failure

The generated TethysL source could not be parsed, likely due to source XML not being well-formed.

Below is the compile error at the TethysL level, as well as the source XML.

The TethysL file name shown below does not necessarily indicate the file exists, but hints about the actual corresponding .xml file.

Syntax error:
    --> lrauv-application/Missions/Engineering/lineCaptureHomingUpdate.xml:439:7
     | 
 435 |       )
 436 | 
 437 |       # Even if we're getting acoustic contact, let's not just do this forever
 438 | 
 439 |       timeout duration=TerminalGuidanceTimeout
     |       ^^^^^^^
 440 | 
 441 |       syslog important "Terminal guidance at range: "
 442 |            + Estimation:TrackAcousticContact.range_to_contact ~ meter
 443 |            + "."
     | 
Unexpected: `timeout`
Possible items include (total 13):
  aggregate
  assign
  behavior
  call
  [...5 more...]
  touch
  macro
  preemptive
  }

Generated TethysL

mission lineCaptureHoming {
  """
  Home to transponder while maintaining altitude. The mission sequence:
  (*) Start (1) Midcourse guidance: nav to target WP while querying
  transponder (2) Terminal guidance: home to target using DUSBL tracking
  data, fire up SCPI when in range (4) Dock: final approach using fixed
  heading (5) Rollout: break off from dock, then nav back to start wp or
  move away from the target at the last commanded heading (*) repeat Note:
  since there's a possibility that no altitude reading will be available
  form the surface (e.g., when the bottom is out of DVL range), the
  vehicle first descends from the surface to a commanded depth and then
  starts following the commanded altitude.
  """

  arguments {
    # Define mission arguments

    MissionTimeout = 4 hour
      """
      Maximum length of mission
      """

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

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

    DockLat = 36.7436 degree
      """
      Latitude of docking station waypoint.
      """

    DockLon = -121.8800 degree
      """
      Longitude of docking station waypoint.
      """

    StartLat = NaN degree
      """
      Latitude of docking sequance start waypoint. The vehicle navigates back
      to this waypoint at the end of each homing attempt. If set to NaN, the
      vehicle moves away from the target at the last commanded heading until
      *RolloutTimeout* has passed.
      """

    StartLon = NaN degree
      """
      Longitude of docking sequance start waypoint. The vehicle navigates back
      to this waypoint at the end of each homing attempt. If set to NaN, the
      vehicle moves away from the target at the last commanded heading until
      *RolloutTimeout* has passed.
      """

    TerminalRange = 650.0 meter
      """
      Range at which the vehicle should switch from navigating to the dock wp
      and start homing to the target using DUSBL tracking data. Max is ??
      meters due to DUSBL ???.
      """

    CameraRange = 25.0 meter
      """
      Range to activate the camera payload.
      """

    DockRange = 4.0 meter
      """
      Minimum range to continue to update heading being driven.
      """

    UpdateNav = false bool
      """
      When true, the mission updates the vehicle's position to DockLat/Lon
      when it comes within *DockRange* from the transponder.
      """

    AcousticContactTimeout = 15 minute
      """
      If the vehicle does not receive an acoustic signal for more than this
      length of time, it will boot out of the homing sequance.
      """

    TerminalGuidanceTimeout = 30 minute
      """
      Maximum time duration for running in terminal guidance mode.
      """

    DockTimeout = 60 second
      """
      Time duration for running in Dock mode.
      """

    RolloutTimeout = 12 minute
      """
      Drive on your last heading for this long after approaching within
      minimum (dock) range.
      """

    HoldAltitude = 7 meter
      """
      Desired vehicle altitude to maintain.
      """

    InvalidAltitudeTimeout = NaN minute
      """
      The mission will abort if no valid altitude readings are received within
      this specified time duration. Set this variable to NaN to disable this
      timeout functionality.
      """

    InitDepth = 30 meter
      """
      Initial depth. The vehicle will dive to this depth when leaving the
      surface. Necessary if no altitude reading is available form the surface
      (e.g., when the bottom is out of DVL range).
      """

    InitDepthTimeout = 30 minute
      """
      Maximum time duration for the vehicle to reach the initial depth. The
      vehicle will switch over to altitude following after the timeout is
      expired.
      """

    TransponderCode = 2 count
      """
      Transponder Address.
      """

    TrackingUpdatePeriod = 0.1 second
      """
      How long to wait between acoustic queries
      """

    NumberOfPings = 1 count
      """
      Number of pings requested each time.
      """

    Speed = 1 meter_per_second
      """
      Thruster speed for the entier mission.
      """

    DepthDeadband = 0.0 meter

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

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

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

  output {
    HomingActive = true
      """
      Mission variable (don't change). The mission sets this to true when the
      vehicle is homing.
      """

    CameraRequested = false
      """
      Mission variable (don't change). The mission sets this to true after
      camera is requested (prevents multipul req).
      """

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

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

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

    MaintainAltitude = 2 count
      """
      Mission variable (don't change). The mission will run in this mode to
      dive the vehicle at a commanded altitude.
      """

    DetachFromDock = 3 count
      """
      Mission variable (don't change). The mission will run in this mode to
      drift away from the dock after is unlatches.
      """
  }

  # Mission timeout

  timeout duration=MissionTimeout

  # 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 Insert/NeedComms.tl id="NeedComms"


  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
  }

  # Add track acoustic contact directive high in the stack.

  aggregate TrackTransponder {
    run while ( HomingActive )

    behavior Estimation:TrackAcousticContact {
      run in parallel
      set Estimation:TrackAcousticContact.contactLabelSetting = TransponderCode
      set Estimation:TrackAcousticContact.numberOfSamplesSetting = NumberOfPings
      set Estimation:TrackAcousticContact.updatePeriodSetting = TrackingUpdatePeriod
    }
  }

  # Set default mass position

  behavior Guidance:Mass {
    run in parallel
    set Guidance:Mass.position = Control:VerticalControl.massDefault
  }

  # Callable aggregates (priority matters, so these are done below important safety directives

  aggregate DiveCmd {
    """
    Called when the vehicle should be underwater. Arbitrates between 3
    dive modes: 1) descend from the surface to initial depth. 2) follow
    commanded altitude. 3) drift away from dock to initial depth.
    """

    run when (
      called
    )

    aggregate InitialDive {
      """
      Descend from the surface to initial depth. Necessary when no
      altitude reading is available form the surface.
      """

      run while ( DiveMode == InitialDive )

      aggregate dive {
        run in sequence

        behavior Guidance:Buoyancy {
          run in parallel
          set Guidance:Buoyancy.position = Control:VerticalControl.buoyancyNeutral
        }

        behavior Guidance:SetSpeed {
          run in parallel
          set Guidance:SetSpeed.speed = Speed
        }

        behavior Guidance:Pitch {
          run in sequence
          timeout duration=InitDepthTimeout {
            syslog important "Timed out at current depth of "
                 + Universal:depth ~ meter
                 + ". Switching to altitude servo."
          }

          set Guidance:Pitch.depth = InitDepth
        }

        assign in sequence DiveMode = MaintainAltitude
      }
    }

    aggregate AltitudeServo {
      """
      Follow commanded altitude.
      """

      run while ( DiveMode == MaintainAltitude )

      aggregate dive {
        run in parallel

        assign in parallel Control:VerticalControl.depthDeadband = DepthDeadband

        behavior Guidance:Buoyancy {
          run in parallel
          set Guidance:Buoyancy.position = Control:VerticalControl.buoyancyNeutral
        }

        behavior Guidance:SetSpeed {
          run in parallel
          set Guidance:SetSpeed.speed = Speed
        }

        behavior Guidance:AltitudeServo {
          run in parallel
          set Guidance:AltitudeServo.targetAltitude = HoldAltitude
          set Guidance:AltitudeServo.invalidAltitudeTimeout = InvalidAltitudeTimeout
        }
      }
    }

    aggregate DriftAwayFromDock {
      """
      Drift away from the dock after vehicle unlatches.
      """

      run while ( DiveMode == DetachFromDock )

      aggregate dive {
        run in sequence

        behavior Guidance:SetSpeed {
          run in parallel
          set Guidance:SetSpeed.speed = 0 meter_per_second
        }

        behavior Guidance:Pitch {
          run in sequence
          timeout duration=InitDepthTimeout {
            syslog important "Timed out while trying to detach from dock at current depth of "
                 + Universal:depth ~ meter
                 + "."
            behavior Guidance:Execute {
              run in sequence
              set Guidance:Execute.command = "stop"
            }
          }

          set Guidance:Pitch.depth = InitDepth
        }

        assign in sequence DiveMode = MaintainAltitude
      }
    }
  }

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

    syslog important "Initiating homing sequence."

    assign in sequence HomingActive = true

    aggregate MidcourseGuidance {
      """
      Navigate toward the dock waypoint while querying the dock
      transponder. Continue until you approach within *TerminalRange*,
      then move on to the next aggregate.
      """

      run in sequence

      break if (
        Estimation:TrackAcousticContact.range_to_contact < TerminalRange
      )

      behavior Guidance:Waypoint {
        run in sequence
        set Guidance:Waypoint.latitude = DockLat
        set Guidance:Waypoint.longitude = DockLon
      }
    }

    syslog info "Transition to TerminalGuidance at range: "
         + Estimation:TrackAcousticContact.range_to_contact ~ meter
         + "."

    aggregate TerminalGuidance {
      """
      Move toward the target using DUSBL tracking data and update the
      commanded heading with each cycle, activate SCPI when in range.
      Continue until you're within *DockRange*, then move on to the
      next aggregate.
      """

      run in sequence

      break if (
        Estimation:TrackAcousticContact.range_to_contact < DockRange
      )

      # Even if we're getting acoustic contact, let's not just do this forever

      timeout duration=TerminalGuidanceTimeout

      syslog important "Terminal guidance at range: "
           + Estimation:TrackAcousticContact.range_to_contact ~ meter
           + "."

      behavior Guidance:Point {
        run in parallel
        set Guidance:Point.forceUpdate = true
        # keep adjusting the heading
        set Guidance:Point.heading = Estimation:TrackAcousticContact.heading_to_contact
      }

      aggregate ActivateCamera {
        run when (
          not ( CameraRequested )
          and ( Estimation:TrackAcousticContact.range_to_contact <= CameraRange )
        )

        syslog important "Lights, Camera on at range: "
             + Estimation:TrackAcousticContact.range_to_contact ~ meter
             + "."

        readData strategy="MinError" {
          Sensor:PowerOnly.samplePowerOnly
        }

        assign in sequence CameraRequested = true
      }

      # Keep running terminal guidance until we're within dock range (or until we time out)

      behavior Guidance:Wait {
        run in sequence
        set Guidance:Wait.duration = TerminalGuidanceTimeout
      }
    }

    syslog info "Transition to dock at range: "
         + Estimation:TrackAcousticContact.range_to_contact ~ meter
         + "."

    aggregate Dock {
      """
      Final approach. Close in on the target at the last commanded
      heading until *DockTimeout* has passed. No DUSBL heading
      updates.
      """

      run in sequence

      # No reson to dock if we haven't come within camara range

      break if (
        not ( CameraRequested )
      )

      syslog important "Final approach. Range: "
           + Estimation:TrackAcousticContact.range_to_contact ~ meter
           + "."

      aggregate UpdateNav {
        run in sequence

        break if (
          not ( UpdateNav )
        )

        behavior Dock:SetNav {
          run in sequence
          set Dock:SetNav.latitude = DockLat
          set Dock:SetNav.longitude = DockLon
        }
      }

      behavior Guidance:Wait {
        run in sequence
        set Guidance:Wait.duration = DockTimeout
      }

      aggregate BreakOffDock {
        run in sequence

        syslog info "Breaking off. Range: "
             + Estimation:TrackAcousticContact.range_to_contact ~ meter
             + "."

        assign in sequence DiveMode = DetachFromDock

        aggregate WaitForBreakOff {
          run when (
            DiveMode == DetachFromDock
          )

          behavior Guidance:Wait {
            run in sequence
            set Guidance:Wait.duration = InitDepthTimeout
          }
        }
      }
    }

    syslog info "Rollout at range: "
         + Estimation:TrackAcousticContact.range_to_contact ~ meter
         + "."

    # Homing sequance complete

    aggregate Rollout {
      """
      Navigate back to the start waypoint or move away from the target
      at the last commanded heading until *RolloutTimeout* has passed.
      """

      run in sequence

      assign in sequence CameraRequested = false

      assign in sequence HomingActive = false

      # Let's make sure we're in the right dive mode

      assign in sequence DiveMode = MaintainAltitude

      aggregate NavToStart {
        run in sequence

        break if (
          isNaN ( StartLat )
          or ( isNaN ( StartLon ) )
        )

        behavior Guidance:Waypoint {
          run in sequence
          set Guidance:Waypoint.latitude = StartLat
          set Guidance:Waypoint.longitude = StartLon
        }
      }

      aggregate MaintainHeading {
        run in sequence

        break if (
          not ( isNaN ( StartLat ) )
          or ( not ( isNaN ( StartLon ) ) )
        )

        behavior Guidance:Wait {
          run in sequence
          set Guidance:Wait.duration = RolloutTimeout
        }
      }
    }
  }

  call id="PhoneHome" refId="SurfaceComms"
}

Source XML

<?xml version="1.0" encoding="UTF-8"?>
<Mission xmlns="Tethys"
    xmlns:Control="Tethys/Control"
    xmlns:Estimation="Tethys/Estimation"
    xmlns:Guidance="Tethys/Guidance"
    xmlns:Sensor="Tethys/Sensor"
    xmlns:Units="Tethys/Units"
    xmlns:Universal="Tethys/Universal"
    xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
    xsi:schemaLocation="Tethys http://okeanids.mbari.org/tethys/Xml/Tethys.xsd
    Tethys/Control http://okeanids.mbari.org/tethys/Xml/Control.xsd
    Tethys/Estimation http://okeanids.mbari.org/tethys/Xml/Estimation.xsd
    Tethys/Guidance http://okeanids.mbari.org/tethys/Xml/Guidance.xsd
    Tethys/Sensor http://okeanids.mbari.org/tethys/Xml/Sensor.xsd
    Tethys/Units http://okeanids.mbari.org/tethys/Xml/Units.xsd
    Tethys/Universal http://okeanids.mbari.org/tethys/Xml/Universal.xsd"
    Id="lineCaptureHoming">

    <Description>
        Home to transponder while maintaining altitude. The mission sequence:
        (*) Start (1) Midcourse guidance: nav to target WP while querying
        transponder (2) Terminal guidance: home to target using DUSBL tracking
        data, fire up SCPI when in range (4) Dock: final approach using fixed
        heading (5) Rollout: break off from dock, then nav back to start wp or
        move away from the target at the last commanded heading (*) repeat Note:
        since there's a possibility that no altitude reading will be available
        form the surface (e.g., when the bottom is out of DVL range), the
        vehicle first descends from the surface to a commanded depth and then
        starts following the commanded altitude.
    </Description>

<!-- Define mission arguments -->

    <DefineArg Name="MissionTimeout"><Description>
        Maximum length of mission
    </Description><Units:hour/><Value>4</Value></DefineArg>

    <DefineArg Name="NeedCommsTime"><Description>
        How often to surface for commumications
    </Description><Units:minute/><Value>60</Value></DefineArg>

    <DefineArg Name="Repeat"><Description>
        Number of times the vehicle should try to cycle through the homing
        sequence.
    </Description><Units:count/><Value>1</Value></DefineArg>

    <DefineArg Name="DockLat"><Description>
        Latitude of docking station waypoint.
    </Description><Units:degree/><Value>36.7436</Value></DefineArg>

    <DefineArg Name="DockLon"><Description>
        Longitude of docking station waypoint.
    </Description><Units:degree/><Value>-121.8800</Value></DefineArg>

    <DefineArg Name="StartLat"><Description>
        Latitude of docking sequance start waypoint. The vehicle navigates back
        to this waypoint at the end of each homing attempt. If set to NaN, the
        vehicle moves away from the target at the last commanded heading until
        *RolloutTimeout* has passed.
    </Description><Units:degree/><Value>NaN</Value></DefineArg>

    <DefineArg Name="StartLon"><Description>
        Longitude of docking sequance start waypoint. The vehicle navigates back
        to this waypoint at the end of each homing attempt. If set to NaN, the
        vehicle moves away from the target at the last commanded heading until
        *RolloutTimeout* has passed.
    </Description><Units:degree/><Value>NaN</Value></DefineArg>

    <DefineArg Name="TerminalRange"><Description>
        Range at which the vehicle should switch from navigating to the dock wp
        and start homing to the target using DUSBL tracking data. Max is ??
        meters due to DUSBL ???.
    </Description><Units:meter/><Value>650.0</Value></DefineArg>

    <DefineArg Name="CameraRange"><Description>
        Range to activate the camera payload.
    </Description><Units:meter/><Value>25.0</Value></DefineArg>

    <DefineArg Name="DockRange"><Description>
        Minimum range to continue to update heading being driven.
    </Description><Units:meter/><Value>4.0</Value></DefineArg>

    <DefineArg Name="UpdateNav"><Description>
        When true, the mission updates the vehicle's position to DockLat/Lon
        when it comes within *DockRange* from the transponder.
    </Description><Units:bool/><Value><False/></Value></DefineArg>

    <DefineArg Name="AcousticContactTimeout"><Description>
        If the vehicle does not receive an acoustic signal for more than this
        length of time, it will boot out of the homing sequance.
    </Description><Units:minute/><Value>15</Value></DefineArg>

    <DefineArg Name="TerminalGuidanceTimeout"><Description>
        Maximum time duration for running in terminal guidance mode.
    </Description><Units:minute/><Value>30</Value></DefineArg>

    <DefineArg Name="DockTimeout"><Description>
        Time duration for running in Dock mode.
    </Description><Units:second/><Value>60</Value></DefineArg>

    <DefineArg Name="RolloutTimeout"><Description>
        Drive on your last heading for this long after approaching within
        minimum (dock) range.
    </Description><Units:minute/><Value>12</Value></DefineArg>

    <DefineArg Name="HoldAltitude"><Description>
        Desired vehicle altitude to maintain.
    </Description><Units:meter/><Value>7</Value></DefineArg>

    <DefineArg Name="InvalidAltitudeTimeout"><Description>
        The mission will abort if no valid altitude readings are received within
        this specified time duration. Set this variable to NaN to disable this
        timeout functionality.
    </Description><Units:minute/><Value>NaN</Value></DefineArg>

    <DefineArg Name="InitDepth"><Description>
        Initial depth. The vehicle will dive to this depth when leaving the
        surface. Necessary if no altitude reading is available form the surface
        (e.g., when the bottom is out of DVL range).
    </Description><Units:meter/><Value>30</Value></DefineArg>

    <DefineArg Name="InitDepthTimeout"><Description>
        Maximum time duration for the vehicle to reach the initial depth. The
        vehicle will switch over to altitude following after the timeout is
        expired.
    </Description><Units:minute/><Value>30</Value></DefineArg>

    <DefineArg Name="TransponderCode"><Description>
        Transponder Address.
    </Description><Units:count/><Value>2</Value></DefineArg>

    <DefineArg Name="TrackingUpdatePeriod"><Description>
        How long to wait between acoustic queries
    </Description><Units:second/><Value>0.1</Value></DefineArg>

    <DefineArg Name="NumberOfPings"><Description>
        Number of pings requested each time.
    </Description><Units:count/><Value>1</Value></DefineArg>

    <DefineArg Name="Speed"><Description>
        Thruster speed for the entier mission.
    </Description><Units:meter_per_second/><Value>1</Value></DefineArg>

    <DefineArg Name="DepthDeadband"><Units:meter/><Value>0.0</Value></DefineArg>

    <DefineArg Name="MinAltitude"><Description>
        Minimum height above the sea floor for the entire mission.
    </Description><Units:meter/><Value>5</Value></DefineArg>

    <DefineArg Name="MaxDepth"><Description>
        Maximum depth for the entire mission.
    </Description><Units:meter/><Value>205</Value></DefineArg>

    <DefineArg Name="MinOffshore"><Description>
        Minimum offshore distance for the entire mission.
    </Description><Units:kilometer/><Value>2</Value></DefineArg>

    <DefineOutput Name="HomingActive"><Description>
        Mission variable (don't change). The mission sets this to true when the
        vehicle is homing.
    </Description><True/></DefineOutput>

    <DefineOutput Name="CameraRequested"><Description>
        Mission variable (don't change). The mission sets this to true after
        camera is requested (prevents multipul req).
    </Description><False/></DefineOutput>

    <DefineOutput Name="DiveMode"><Description>
        Mission variable (don't change). The mission sets this variable to
        switch between dive modes.
    </Description><Units:count/><Value>0</Value></DefineOutput>

    <DefineOutput Name="DoingComms"><Description>
        Mission variable (don't change). The mission will run in this to mode
        when running surface comms.
    </Description><Units:count/><Value>0</Value></DefineOutput>

    <DefineOutput Name="InitialDive"><Description>
        Mission variable (don't change). The mission will run in this mode when
        surface comms are done to dive the vehicle to DVL bottom range.
    </Description><Units:count/><Value>1</Value></DefineOutput>

    <DefineOutput Name="MaintainAltitude"><Description>
        Mission variable (don't change). The mission will run in this mode to
        dive the vehicle at a commanded altitude.
    </Description><Units:count/><Value>2</Value></DefineOutput>

    <DefineOutput Name="DetachFromDock"><Description>
        Mission variable (don't change). The mission will run in this mode to
        drift away from the dock after is unlatches.
    </Description><Units:count/><Value>3</Value></DefineOutput>

<!-- Mission timeout -->

    <Timeout Duration="MissionTimeout"/>

<!-- Safety nets and surfacing behaviors -->

    <Insert Filename="Insert/StandardEnvelopes.tl"/>

    <Assign><Sequence/><Arg Name="StandardEnvelopes:MinAltitude"/><Arg Name="MinAltitude"/></Assign>

    <Assign><Sequence/><Arg Name="StandardEnvelopes:MaxDepth"/><Arg Name="MaxDepth"/></Assign>

    <Assign><Sequence/><Arg Name="StandardEnvelopes:MinOffshore"/><Arg Name="MinOffshore"/></Assign>

    <Insert Filename="Insert/NeedComms.tl" Id="NeedComms"/>

    <Assign><Sequence/><Arg Name="NeedComms:DiveInterval"/><Arg Name="MissionTimeout"/></Assign>

    <Assign><Sequence/><Arg Name="NeedComms:WaitForPitchUp"/><Units:minute/><Value>0</Value></Assign>

    <Aggregate Id="SurfaceComms">

        <When>
            <Called/>
            <Or><Elapsed><Universal:time_fix/></Elapsed><Gt><Arg Name="NeedCommsTime"/></Gt></Or>
        </When>

        <Assign><Sequence/><Arg Name="DiveMode"/><Arg Name="DoingComms"/></Assign>

        <Call RefId="NeedComms"/>

        <Assign><Sequence/><Arg Name="DiveMode"/><Arg Name="InitialDive"/></Assign>

    </Aggregate>

<!-- Add track acoustic contact directive high in the stack. -->

    <Aggregate Id="TrackTransponder">

        <While>
            <Arg Name="HomingActive"/>
        </While>

        <Estimation:TrackAcousticContact>
            <Parallel/>
            <Setting><Estimation:TrackAcousticContact.contactLabelSetting/><Arg Name="TransponderCode"/></Setting>
            <Setting><Estimation:TrackAcousticContact.numberOfSamplesSetting/><Arg Name="NumberOfPings"/></Setting>
            <Setting><Estimation:TrackAcousticContact.updatePeriodSetting/><Arg Name="TrackingUpdatePeriod"/></Setting>
        </Estimation:TrackAcousticContact>

    </Aggregate>

<!-- Set default mass position -->

    <Guidance:Mass>
        <Parallel/>
        <Setting><Guidance:Mass.position/><Control:VerticalControl.massDefault/></Setting>
    </Guidance:Mass>

<!-- Callable aggregates (priority matters, so these are done below important safety directives -->

    <Aggregate Id="DiveCmd">

        <Description>
            Called when the vehicle should be underwater. Arbitrates between 3
            dive modes: 1) descend from the surface to initial depth. 2) follow
            commanded altitude. 3) drift away from dock to initial depth.
        </Description>

        <When>
            <Called/>
        </When>

        <Aggregate Id="InitialDive">

            <Description>
                Descend from the surface to initial depth. Necessary when no
                altitude reading is available form the surface.
            </Description>

            <While>
                <Arg Name="DiveMode"/>
                <Eq><Arg Name="InitialDive"/></Eq>
            </While>

            <Aggregate Id="dive">

                <Sequence/>

                <Guidance:Buoyancy>
                    <Parallel/>
                    <Setting><Guidance:Buoyancy.position/><Control:VerticalControl.buoyancyNeutral/></Setting>
                </Guidance:Buoyancy>

                <Guidance:SetSpeed>
                    <Parallel/>
                    <Setting><Guidance:SetSpeed.speed/><Arg Name="Speed"/></Setting>
                </Guidance:SetSpeed>

                <Guidance:Pitch>
                    <Sequence/>
                    <Timeout Duration="InitDepthTimeout">
                        <Syslog Severity="Important">Timed out at current depth of <Universal:depth/><Units:meter/>. Switching to altitude servo.</Syslog>
                    </Timeout>
                    <Setting><Guidance:Pitch.depth/><Arg Name="InitDepth"/></Setting>
                </Guidance:Pitch>

                <Assign><Sequence/><Arg Name="DiveMode"/><Arg Name="MaintainAltitude"/></Assign>

            </Aggregate>

        </Aggregate>

        <Aggregate Id="AltitudeServo">

            <Description>
                Follow commanded altitude.
            </Description>

            <While>
                <Arg Name="DiveMode"/>
                <Eq><Arg Name="MaintainAltitude"/></Eq>
            </While>

            <Aggregate Id="dive">

                <Parallel/>

                <Assign><Parallel/><Control:VerticalControl.depthDeadband/><Arg Name="DepthDeadband"/></Assign>

                <Guidance:Buoyancy>
                    <Parallel/>
                    <Setting><Guidance:Buoyancy.position/><Control:VerticalControl.buoyancyNeutral/></Setting>
                </Guidance:Buoyancy>

                <Guidance:SetSpeed>
                    <Parallel/>
                    <Setting><Guidance:SetSpeed.speed/><Arg Name="Speed"/></Setting>
                </Guidance:SetSpeed>

                <Guidance:AltitudeServo>
                    <Parallel/>
                    <Setting><Guidance:AltitudeServo.targetAltitude/><Arg Name="HoldAltitude"/></Setting>
                    <Setting><Guidance:AltitudeServo.invalidAltitudeTimeout/><Arg Name="InvalidAltitudeTimeout"/></Setting>
                </Guidance:AltitudeServo>

            </Aggregate>

        </Aggregate>

        <Aggregate Id="DriftAwayFromDock">

            <Description>
                Drift away from the dock after vehicle unlatches.
            </Description>

            <While>
                <Arg Name="DiveMode"/>
                <Eq><Arg Name="DetachFromDock"/></Eq>
            </While>

            <Aggregate Id="dive">

                <Sequence/>

                <Guidance:SetSpeed>
                    <Parallel/>
                    <Setting><Guidance:SetSpeed.speed/><Units:meter_per_second/><Value>0</Value></Setting>
                </Guidance:SetSpeed>

                <Guidance:Pitch>
                    <Sequence/>
                    <Timeout Duration="InitDepthTimeout">
                        <Syslog Severity="Important">Timed out while trying to detach from dock at current depth of <Universal:depth/><Units:meter/>.</Syslog>
                        <Guidance:Execute>
                            <Sequence/>
                            <Setting><Guidance:Execute.command/><String>stop</String></Setting>
                        </Guidance:Execute>
                    </Timeout>
                    <Setting><Guidance:Pitch.depth/><Arg Name="InitDepth"/></Setting>
                </Guidance:Pitch>

                <Assign><Sequence/><Arg Name="DiveMode"/><Arg Name="MaintainAltitude"/></Assign>

            </Aggregate>

        </Aggregate>

    </Aggregate>

<!-- Start mission sequence. -->

    <Call Id="StartingMission" RefId="SurfaceComms"/>

    <Aggregate Id="Lap">

        <Sequence Repeat="Repeat"/>

        <Aggregate Id="Dive">

            <While>
                <Not><Arg Name="DiveMode"/><Eq><Arg Name="DoingComms"/></Eq></Not>
            </While>

            <Call RefId="DiveCmd"/>

        </Aggregate>

        <Syslog Severity="Important">Initiating homing sequence.</Syslog>

        <Assign><Sequence/><Arg Name="HomingActive"/><True/></Assign>

        <Aggregate Id="MidcourseGuidance">

            <Description>
                Navigate toward the dock waypoint while querying the dock
                transponder. Continue until you approach within *TerminalRange*,
                then move on to the next aggregate.
            </Description>

            <Sequence/>

            <Break>
                <Estimation:TrackAcousticContact.range_to_contact/>
                <Lt><Arg Name="TerminalRange"/></Lt>
            </Break>

            <Guidance:Waypoint>
                <Sequence/>
                <Setting><Guidance:Waypoint.latitude/><Arg Name="DockLat"/></Setting>
                <Setting><Guidance:Waypoint.longitude/><Arg Name="DockLon"/></Setting>
            </Guidance:Waypoint>

        </Aggregate>

        <Syslog Severity="Info">Transition to TerminalGuidance at range: <Estimation:TrackAcousticContact.range_to_contact/><Units:meter/>.</Syslog>

        <Aggregate Id="TerminalGuidance">

            <Description>
                Move toward the target using DUSBL tracking data and update the
                commanded heading with each cycle, activate SCPI when in range.
                Continue until you're within *DockRange*, then move on to the
                next aggregate.
            </Description>

            <Sequence/>

            <Break>
                <Estimation:TrackAcousticContact.range_to_contact/>
                <Lt><Arg Name="DockRange"/></Lt>
            </Break>

<!-- Even if we're getting acoustic contact, let's not just do this forever -->

            <Timeout Duration="TerminalGuidanceTimeout"/>

            <Syslog Severity="Important">Terminal guidance at range: <Estimation:TrackAcousticContact.range_to_contact/><Units:meter/>.</Syslog>

            <Guidance:Point>
                <Parallel/>
                <Setting><Guidance:Point.forceUpdate/><True/></Setting>
<!-- keep adjusting the heading -->
                <Setting><Guidance:Point.heading/><Estimation:TrackAcousticContact.heading_to_contact/></Setting>
            </Guidance:Point>

            <Aggregate Id="ActivateCamera">

                <When>
                    <Not><Arg Name="CameraRequested"/></Not>
                    <And><Estimation:TrackAcousticContact.range_to_contact/><Le><Arg Name="CameraRange"/></Le></And>
                </When>

                <Syslog Severity="Important">Lights, Camera on at range: <Estimation:TrackAcousticContact.range_to_contact/><Units:meter/>.</Syslog>

                <ReadData Strategy="MinError">
                    <Sensor:PowerOnly.samplePowerOnly/>
                </ReadData>

                <Assign><Sequence/><Arg Name="CameraRequested"/><True/></Assign>

            </Aggregate>

<!-- Keep running terminal guidance until we're within dock range (or until we time out) -->

            <Guidance:Wait>
                <Sequence/>
                <Setting><Guidance:Wait.duration/><Arg Name="TerminalGuidanceTimeout"/></Setting>
            </Guidance:Wait>

        </Aggregate>

        <Syslog Severity="Info">Transition to dock at range: <Estimation:TrackAcousticContact.range_to_contact/><Units:meter/>.</Syslog>

        <Aggregate Id="Dock">

            <Description>
                Final approach. Close in on the target at the last commanded
                heading until *DockTimeout* has passed. No DUSBL heading
                updates.
            </Description>

            <Sequence/>

<!-- No reson to dock if we haven't come within camara range -->

            <Break>
                <Not><Arg Name="CameraRequested"/></Not>
            </Break>

            <Syslog Severity="Important">Final approach. Range: <Estimation:TrackAcousticContact.range_to_contact/><Units:meter/>.</Syslog>

            <Aggregate Id="UpdateNav">

                <Sequence/>

                <Break>
                    <Not><Arg Name="UpdateNav"/></Not>
                </Break>

                <Dock:SetNav>
                    <Sequence/>
                    <Setting><Dock:SetNav.latitude/><Arg Name="DockLat"/></Setting>
                    <Setting><Dock:SetNav.longitude/><Arg Name="DockLon"/></Setting>
                </Dock:SetNav>

            </Aggregate>

            <Guidance:Wait>
                <Sequence/>
                <Setting><Guidance:Wait.duration/><Arg Name="DockTimeout" /></Setting>
            </Guidance:Wait>

            <Aggregate Id="BreakOffDock">

                <Sequence/>

                <Syslog Severity="Info">Breaking off. Range: <Estimation:TrackAcousticContact.range_to_contact/><Units:meter/>.</Syslog>

                <Assign><Sequence/><Arg Name="DiveMode"/><Arg Name="DetachFromDock"/></Assign>

                <Aggregate Id="WaitForBreakOff">

                    <When>
                        <Arg Name="DiveMode"/>
                        <Eq><Arg Name="DetachFromDock"/></Eq>
                    </When>

                    <Guidance:Wait>
                        <Sequence/>
                        <Setting><Guidance:Wait.duration/><Arg Name="InitDepthTimeout" /></Setting>
                    </Guidance:Wait>

                </Aggregate>

            </Aggregate>

        </Aggregate>

        <Syslog Severity="Info">Rollout at range: <Estimation:TrackAcousticContact.range_to_contact/><Units:meter/>.</Syslog>

<!-- Homing sequance complete -->

        <Aggregate Id="Rollout">

            <Description>
                Navigate back to the start waypoint or move away from the target
                at the last commanded heading until *RolloutTimeout* has passed.
            </Description>

            <Sequence/>

            <Assign><Sequence/><Arg Name="CameraRequested"/><False/></Assign>

            <Assign><Sequence/><Arg Name="HomingActive"/><False/></Assign>

<!-- Let's make sure we're in the right dive mode -->

            <Assign><Sequence/><Arg Name="DiveMode"/><Arg Name="MaintainAltitude"/></Assign>

            <Aggregate Id="NavToStart">

                <Sequence/>

                <Break>
                    <IsNaN><Arg Name="StartLat"/>
                    </IsNaN>
                    <Or><IsNaN><Arg Name="StartLon"/>
                    </IsNaN></Or>
                </Break>

                <Guidance:Waypoint>
                    <Sequence/>
                    <Setting><Guidance:Waypoint.latitude/><Arg Name="StartLat"/></Setting>
                    <Setting><Guidance:Waypoint.longitude/><Arg Name="StartLon"/></Setting>
                </Guidance:Waypoint>

            </Aggregate>

            <Aggregate Id="MaintainHeading">

                <Sequence/>

                <Break>
                    <Not><IsNaN><Arg Name="StartLat"/>
                    </IsNaN></Not>
                    <Or><Not><IsNaN><Arg Name="StartLon"/>
                    </IsNaN></Not></Or>
                </Break>

                <Guidance:Wait>
                    <Sequence/>
                    <Setting><Guidance:Wait.duration/><Arg Name="RolloutTimeout" /></Setting>
                </Guidance:Wait>

            </Aggregate>

        </Aggregate>

    </Aggregate>

    <Call Id="PhoneHome" RefId="SurfaceComms"/>

</Mission>