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
  }
  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
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
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"
}
  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
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
<?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>