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 | # 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.
aggregate BallastAndTrim {
"""
Control depth within deadband, settle, and then estimate until
measurements converge and report.
"""
arguments {
InsertDepth = 25 meter
"""
Depth to run ballast and trim at.
"""
InsertApproachSpeed = 0 meter_per_second
"""
Enables vehicle thruster at the commanded speed to reach target depth
(set to 0 m/s to drift).
"""
InsertApproachDepthRate = 0.4 meter_per_second
"""
Descent depth rate (Positive depth rate means going down).
"""
InsertApproachPitchLimit = 20 degree
"""
Max vehicle pitch (+/-) when decending from the surface.
"""
InsertApproachSettleTimePreDive = 0 minute
"""
Time duration for the vehicle to pump down the VBS *BEFORE* leaving the
surface.
"""
InsertApproachDepthTimeout = 20 minute
"""
Maximum wait time for the vehicle to reach the targeted depth.
"""
InsertSettleTime = 10 minute
"""
How long to wait after reaching target depth before starting ballast and
trim.
"""
EstTimeout = 45 minute
"""
Timeout for ballast and trim estimation.
"""
MinEstTime = 10 minute
"""
Minimum time to run ballast and trim estimation before determining
convergence (must be shorter than timeout).
"""
MassEstErrorBound = 0.25 millimeter
"""
Desired range of the estimated sample mean from the true mean statistic
(smaller values take longer to converge).
"""
BuoyEstErrorBound = 15 cubic_centimeter
"""
Desired range of the estimated sample mean from the true mean statistic
(smaller values take longer to converge).
"""
EstConfidence = 99.9 percent
"""
Confidence level that the estimator has reached the desired error range
(larger values take longer to converge).
"""
InsertDepthDeadband = 1 meter
"""
How much vertical drift from the specified depth is allowed.
"""
InsertMassDeadband = 0.25 millimeter
"""
Degree of rounding in mass-shifter command output values.
"""
}
run when ( called )
assign in parallel Control:VerticalControl.kpPitchMass = 0.005 ratio
assign in parallel Control:VerticalControl.kiPitchMass = 0.00075 reciprocal_second
assign in parallel Control:VerticalControl.kdPitchMass = 0 second
assign in parallel Control:VerticalControl.massDeadband = InsertMassDeadband
assign in parallel Control:VerticalControl.depthDeadband = InsertDepthDeadband
# DO NOT REMOVE the syslog entry below. The MissionManager seems to ignore the value of InsertApproachSpeed and does not evaluate the SetSpeed statement properly. Adding the syslog call somehow fixes it.
syslog info "Going to target depth. Speed set to "
+ InsertApproachSpeed~meter_per_second
aggregate GoToTargetDepth {
run in sequence
assign in parallel Control:VerticalControl.pitchLimit = InsertApproachPitchLimit
aggregate ActuatorHold {
"""
Hold the positions of the mass, VBS, and rudder while going to
the target depth with the thruster on.
"""
run while ( InsertApproachSpeed > 0 meter_per_second )
behavior Guidance:Point {
run in parallel
set rudderAngle = 13 degree
}
behavior Guidance:Buoyancy {
run in parallel
set position = Control:VerticalControl.buoyancyNeutral
}
behavior Guidance:Pitch {
run in parallel
set massPosition = Control:VerticalControl.massDefault
}
}
aggregate WaitPreDive {
run in sequence
break if (
InsertApproachSettleTimePreDive <= 0 minute
)
syslog info "Waiting for " + InsertApproachSettleTimePreDive~minute
+ "while pumping down to neutral."
behavior Guidance:Wait {
"""
Take a few moments to ensure the VBS reaches neutral.
"""
run in sequence
set duration = InsertApproachSettleTimePreDive
}
}
aggregate ApproachDepth {
run in sequence
syslog "Moving to " + InsertDepth~meter
behavior Guidance:SetSpeed {
run in parallel
set speed = InsertApproachSpeed
}
behavior Guidance:Pitch {
run in sequence
timeout duration=InsertApproachDepthTimeout {
syslog important "Timed out trying to reach the target depth. Stopping mission at current depth of "
+ Universal:depth~meter
behavior Guidance:Execute {
run in sequence
set command = "stop"
}
}
set depth = InsertDepth
set depthRate = InsertApproachDepthRate
}
}
}
aggregate BallastAndTrimAtTargetDepth {
"""
Execute ballast and trim estimation at target depth.
"""
run in sequence
behavior Guidance:SetSpeed {
run in parallel
set speed = 0 meter_per_second
}
behavior Guidance:Pitch {
run in parallel
set depth = InsertDepth
}
syslog info "Waiting for the vehicle to settle. Depth = " + Universal:depth~meter
behavior Guidance:Wait {
run in sequence
set duration = InsertSettleTime
}
behavior Guidance:Execute {
run in sequence
set command = "gfscan"
}
syslog info "Running ballast and trim. Depth = " + Universal:depth~meter
behavior Estimation:BallastAndTrim {
run in sequence
set estimationTimeout = EstTimeout
set minEstimationTime = MinEstTime
set massErrorBound = MassEstErrorBound
set buoyErrorBound = BuoyEstErrorBound
set confidenceLevel = EstConfidence
}
}
}
|