DockingTankTest.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/Demo/DockingTankTest.xml:161:7
     | 
 157 | 
 158 |     # Let's fire up the DockingServo component here just so it's ready to go
 159 | 
 160 |     readData strategy="MinError" {
 161 |       in parallel
     |       ^^
 162 |       Dock:Dock.docking_state
 163 |     }
 164 | 
 165 |     # Start with the arm closed if it isn't already
     | 
Unexpected: `in`
One of the following is possible (total 9):
  """
  while
  timeout
  break
  interval
  period
  someID
  Universal:...
  universal
  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
# -   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 DockingTankTest {
  """
  1.) Wait timeout for loading in launcher. 2.) Turn on prop and arm for
  docking. 3.) Wait timeout for either cable present or timeout. 4.) Cable
  present == latch for timeout then release, back up and end mission. 5.)
  No cable present == speed 0 and end mission. 6.) end mission == float up
  at 0 speed
  """

  arguments {
    MissionTimeout = 15 minute
      """
      Maximum length of mission
      """

    Speed = 1 meter_per_second
      """
      Thruster speed for docking.
      """

    LatchingSpeed = 0.5 meter_per_second
      """
      Thruster speed while closing the latch (ideally slower) than docking.
      """

    WaitForLoadTimeout = 90 second
      """
      Time to wait prior to turning on the prop.
      """

    LaunchTimeout = 70 second
      """
      Time to wait with the prop on driving to the dock.
      """

    LatchTimeout = 10 second
      """
      Time to wait before closing the latch after seeing the cable.
      """

    LatchCloseTimeout = 10 second
      """
      Time to wait while the latch closes.
      """

    DockTimeout = 15 second
      """
      Time to wait on the dock.
      """

    DetachTimeout = 7 second
      """
      Time to wait while performing dock detach.
      """

    FloatTimeout = 120 second
      """
      How long to wait for the vehicle to float to the surface.
      """

    ElevatorDefault = -4.0 degree
      """
      Static setting for elevator.
      """

    RudderDefault = 3.0 degree
      """
      Static setting for rudder.
      """
  }

  output {
    DockingDepth = 1.22 meter
      """
      How deep in the tank do you want to swim?
      """

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

    LatchSequence = 1 count
      """
      Mission variable (don't change). The mission will run in this mode to
      initiate the debouncing latch sequence.
      """

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

    WaitOnDock = 3 count
      """
      Mission variable (don't change). The mission will run in this mode to
      maintatin depth while the vehicle is latched on the dock.
      """

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

    FloatUp = 5 count
      """
      Mission variable (don't change). The mission will run in this mode to
      float back to the surface rather than drive.
      """
  }

  timeout duration=MissionTimeout

  aggregate TankDocking {
    run in sequence

    # Be neutral the entire time
    #        <Guidance:Buoyancy>
    #            <Parallel/>
    #            <Setting><Guidance:Buoyancy.position/><Control:VerticalControl.buoyancyNeutral/></Setting>
    #        </Guidance:Buoyancy>

    # set up the vertical control

    behavior Guidance:Pitch {
      run in parallel
      set Guidance:Pitch.depth = DockingDepth
      set Guidance:Pitch.elevatorAngle = ElevatorDefault
      set Guidance:Pitch.massPosition = Control:VerticalControl.massDefault
    }

    # set up the horizontal control

    behavior Guidance:Point {
      run in parallel
      set Guidance:Point.rudderAngle = RudderDefault
    }

    # Let's fire up the DockingServo component here just so it's ready to go

    readData strategy="MinError" {
      in parallel
      Dock:Dock.docking_state
    }

    # Start with the arm closed if it isn't already

    assign in sequence Dock:Dock.docking_state_cmd = 1 enum

    aggregate LatchSequence {
      """
      Vehicle hit cable and now needs to debounce.
      """

      syslog important "Initiating latching sequence. Waiting for debounce"

      run while ( DiveMode == LatchSequence )

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

      aggregate dive {
        run in sequence

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

        assign in sequence DiveMode = ArmClose
      }
    }

    aggregate ArmClose {
      """
      Change speed and close arm.
      """

      syslog important "Closing and changing speed."

      run while ( DiveMode == ArmClose )

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

      # <Aggregate Id="dive">

      assign in sequence Dock:Dock.docking_state_cmd = 1 enum

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

      aggregate assign {
        run when (
          Dock:Dock.dock_cable_present == true
          and ( Servo:DockingServo.armAngle < ( -12 degree ) )
        )

        syslog important "Latch confirmed closed."

        assign in sequence DiveMode = WaitOnDock
      }

      # If we didn't change the mode above, let's detach

      syslog important "Could not confirm latch closed. Detaching."

      assign in sequence DiveMode = DetachFromDock
    }
    # </Aggregate>


    aggregate DockHold {
      """
      Wait on dock after vehicle latches.
      """

      syslog important "Holding on dock."

      run while ( DiveMode == WaitOnDock )

      aggregate dive {
        run in sequence

        assign in sequence Dock:Dock.docking_state_cmd = 1 enum

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

        # <ReadData Id="PowerRedLights">
        #                       <Parallel/>
        #     <Sensor:PowerOnly.sampleLoad3/>
        # </ReadData>

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

        assign in sequence DiveMode = DetachFromDock
      }
    }

    aggregate Detach {
      """
      Reverse off the dock.
      """

      syslog important "Detaching From Dock."

      run while ( DiveMode == DetachFromDock )

      aggregate dive {
        run in sequence

        # open to detach

        assign in sequence Dock:Dock.docking_state_cmd = 2 enum

        # And reverse

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

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

        assign in sequence DiveMode = FloatUp
      }
    }

    aggregate Float_Up {
      run while ( DiveMode == FloatUp )

      syslog important "Floating Up."

      # keep open while floating up

      assign in sequence Dock:Dock.docking_state_cmd = 2 enum

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

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

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

      assign in sequence DiveMode = DoingComms
    }

    aggregate Wait_for_loading {
      run in sequence

      syslog info "Waiting to load for "
           + WaitForLoadTimeout ~ second

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

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

    aggregate LaunchAndDock {
      run in sequence

      timeout duration=LaunchTimeout {
        syslog important "Timed out while driving to the dock."
      }


      break if (
        DiveMode > DoingComms
      )

      syslog info "Prop on and arming!"

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

      # Arm the....arm. Hmm....

      assign in sequence Dock:Dock.docking_state_cmd = 2 enum

      aggregate Latch {
        run when (
          Dock:Dock.dock_cable_present == true
          and ( not ( DiveMode == LatchSequence ) )
        )

        assign in sequence DiveMode = LatchSequence
      }

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

    behavior Guidance:Wait {
      run in sequence
      set Guidance:Wait.duration = MissionTimeout
    }
  }
}
  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
<?xml version="1.0" encoding="UTF-8"?>
<!--
-   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 xmlns="Tethys"
       xmlns:Sensor="Tethys/Sensor" 
       xmlns:Units="Tethys/Units"
       xmlns:Estimation="Tethys/Estimation"
       xmlns:Dock="Tethys/Dock"       
       xmlns:Guidance="Tethys/Guidance"
       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/Dock http://okeanids.mbari.org/tethys/Xml/Dock.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="DockingTankTest">

    <Description>
        1.) Wait timeout for loading in launcher. 2.) Turn on prop and arm for
        docking. 3.) Wait timeout for either cable present or timeout. 4.) Cable
        present == latch for timeout then release, back up and end mission. 5.)
        No cable present == speed 0 and end mission. 6.) end mission == float up
        at 0 speed
    </Description>

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

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

    <DefineArg Name="LatchingSpeed"><Description>
        Thruster speed while closing the latch (ideally slower) than docking.
    </Description><Units:meter_per_second/><Value>0.5</Value></DefineArg>

    <DefineOutput Name="DockingDepth"><Description>
        How deep in the tank do you want to swim?
    </Description><Units:meter/><Value>1.22</Value></DefineOutput>

    <DefineArg Name="WaitForLoadTimeout"><Description>
        Time to wait prior to turning on the prop.
    </Description><Units:second/><Value>90</Value></DefineArg>

    <DefineArg Name="LaunchTimeout"><Description>
        Time to wait with the prop on driving to the dock.
    </Description><Units:second/><Value>70</Value></DefineArg>

    <DefineArg Name="LatchTimeout"><Description>
        Time to wait before closing the latch after seeing the cable.
    </Description><Units:second/><Value>10</Value></DefineArg>

    <DefineArg Name="LatchCloseTimeout"><Description>
        Time to wait while the latch closes.
    </Description><Units:second/><Value>10</Value></DefineArg>

    <DefineArg Name="DockTimeout"><Description>
        Time to wait on the dock.
    </Description><Units:second/><Value>15</Value></DefineArg>

    <DefineArg Name="DetachTimeout"><Description>
        Time to wait while performing dock detach.
    </Description><Units:second/><Value>7</Value></DefineArg>

    <DefineArg Name="FloatTimeout"><Description>
        How long to wait for the vehicle to float to the surface.
    </Description><Units:second/><Value>120</Value></DefineArg>

    <DefineArg Name="ElevatorDefault"><Description>
        Static setting for elevator.
    </Description><Units:degree/><Value>-4.0</Value></DefineArg>

    <DefineArg Name="RudderDefault"><Description>
        Static setting for rudder.
    </Description><Units:degree/><Value>3.0</Value></DefineArg>

    <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="LatchSequence"><Description>
        Mission variable (don't change). The mission will run in this mode to
        initiate the debouncing latch sequence.
    </Description><Units:count/><Value>1</Value></DefineOutput>

    <DefineOutput Name="ArmClose"><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>2</Value></DefineOutput>

    <DefineOutput Name="WaitOnDock"><Description>
        Mission variable (don't change). The mission will run in this mode to
        maintatin depth while the vehicle is latched on the dock.
    </Description><Units:count/><Value>3</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 unlatched.
    </Description><Units:count/><Value>4</Value></DefineOutput>

    <DefineOutput Name="FloatUp"><Description>
        Mission variable (don't change). The mission will run in this mode to
        float back to the surface rather than drive.
    </Description><Units:count/><Value>5</Value></DefineOutput>

    <Timeout Duration="MissionTimeout"/>

    <Aggregate Id="TankDocking">

<!-- Be neutral the entire time    
        <Guidance:Buoyancy>
            <Parallel/>
            <Setting><Guidance:Buoyancy.position/><Control:VerticalControl.buoyancyNeutral/></Setting>
        </Guidance:Buoyancy> -->

<!-- set up the vertical control -->

        <Guidance:Pitch>
            <Parallel/>
            <Setting><Guidance:Pitch.depth/><Arg Name="DockingDepth"/></Setting>
            <Setting><Guidance:Pitch.elevatorAngle/><Arg Name="ElevatorDefault"/></Setting>
            <Setting><Guidance:Pitch.massPosition/><Control:VerticalControl.massDefault/></Setting>
        </Guidance:Pitch>

<!-- set up the horizontal control -->

        <Guidance:Point>
            <Parallel/>
            <Setting><Guidance:Point.rudderAngle/><Arg Name="RudderDefault"/></Setting>
        </Guidance:Point>

<!-- Let's fire up the DockingServo component here just so it's ready to go -->

        <ReadData Strategy="MinError">
            <Parallel/>
            <Dock:Dock.docking_state/>
        </ReadData>

<!-- Start with the arm closed if it isn't already -->

        <Assign><Sequence/><Dock:Dock.docking_state_cmd/><Units:enum/><Value>1</Value></Assign>

        <Aggregate Id="LatchSequence">

            <Description>
                Vehicle hit cable and now needs to debounce.
            </Description>

            <Syslog Severity="Important">Initiating latching sequence. Waiting for debounce</Syslog>

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

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

            <Aggregate Id="dive">

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

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

            </Aggregate>

        </Aggregate>

        <Aggregate Id="ArmClose">

            <Description>
                Change speed and close arm.
            </Description>

            <Syslog Severity="Important">Closing and changing speed.</Syslog>

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

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

<!--  <Aggregate Id="dive">-->

            <Assign><Sequence/><Dock:Dock.docking_state_cmd/><Units:enum/><Value>1</Value></Assign>

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

            <Aggregate Id="assign">

                <When>
                    <Dock:Dock.dock_cable_present/>
                    <Eq><True/></Eq>
                    <And><Servo:DockingServo.armAngle/><Lt><Units:degree/><Value>-12</Value></Lt></And>
                </When>

                <Syslog Severity="Important">Latch confirmed closed.</Syslog>

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

            </Aggregate>

<!-- If we didn't change the mode above, let's detach -->

            <Syslog Severity="Important">Could not confirm latch closed. Detaching.</Syslog>

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

<!--</Aggregate>-->

        </Aggregate>

        <Aggregate Id="DockHold">

            <Description>
                Wait on dock after vehicle latches.
            </Description>

            <Syslog Severity="Important">Holding on dock.</Syslog>

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

            <Aggregate Id="dive">

                <Assign><Sequence/><Dock:Dock.docking_state_cmd/><Units:enum/><Value>1</Value></Assign>

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

<!--                <ReadData Id="PowerRedLights">
                    <Parallel/>
                    <Sensor:PowerOnly.sampleLoad3/>
                </ReadData>
-->

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

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

            </Aggregate>

        </Aggregate>

        <Aggregate Id="Detach">

            <Description>
                Reverse off the dock.
            </Description>

            <Syslog Severity="Important">Detaching From Dock.</Syslog>

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

            <Aggregate Id="dive">

<!-- open to detach  -->

                <Assign><Sequence/><Dock:Dock.docking_state_cmd/><Units:enum/><Value>2</Value></Assign>

<!-- And reverse -->

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

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

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

            </Aggregate>

        </Aggregate>

        <Aggregate Id="Float Up">

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

            <Syslog Severity="Important">Floating Up.</Syslog>

<!-- keep open while floating up  -->

            <Assign><Sequence/><Dock:Dock.docking_state_cmd/><Units:enum/><Value>2</Value></Assign>

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

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

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

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

        </Aggregate>

        <Aggregate Id="Wait for loading">

            <Syslog Severity="Info">Waiting to load for <Arg Name="WaitForLoadTimeout"/><Units:second/></Syslog>

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

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

        </Aggregate>

        <Aggregate Id="LaunchAndDock">

            <Timeout Duration="LaunchTimeout">
                <Syslog Severity="Important">Timed out while driving to the dock.</Syslog>
            </Timeout>

            <Break><Arg Name="DiveMode"/>
                <Gt><Arg Name="DoingComms"/></Gt>
            </Break>

            <Syslog Severity="Info">Prop on and arming!</Syslog>

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

<!-- Arm the....arm. Hmm....  -->

            <Assign><Sequence/><Dock:Dock.docking_state_cmd/><Units:enum/><Value>2</Value></Assign>

            <Aggregate Id="Latch">

                <When>
                    <Dock:Dock.dock_cable_present/>
                    <Eq><True/></Eq>
                    <And><Not><Arg Name="DiveMode"/><Eq><Arg Name="LatchSequence"/></Eq></Not></And>
                </When>

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

            </Aggregate>

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

        </Aggregate>

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

    </Aggregate>

</Mission>