speedandcurrent 1.0.1 → 1.3.0
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- package/CHANGELOG.md +19 -1
- package/correctionTable.js +26 -14
- package/index.js +220 -174
- package/package.json +3 -3
- package/public/TableRenderer.js +8 -6
- package/public/app.js +30 -20
- package/public/index.html +22 -14
- package/public/main.css +177 -0
- package/public/vectors.html +1 -1
- package/public/vectors.js +30 -50
- package/public/style.css +0 -184
- package/public/vectors.css +0 -59
package/index.js
CHANGED
|
@@ -1,4 +1,4 @@
|
|
|
1
|
-
const {
|
|
1
|
+
const { MessageHandler, Polar, PolarDamped, SI, Reporter } = require('signalkutilities');
|
|
2
2
|
const { CorrectionTable } = require('./correctionTable.js');
|
|
3
3
|
const { KalmanFilter } = require('kalman-filter');
|
|
4
4
|
|
|
@@ -7,13 +7,25 @@ const { KalmanFilter } = require('kalman-filter');
|
|
|
7
7
|
|
|
8
8
|
module.exports = function (app) {
|
|
9
9
|
|
|
10
|
-
let unsubscribes = [];
|
|
11
10
|
let options = {};
|
|
12
11
|
let isRunning = false;
|
|
13
12
|
let corrTable = null;
|
|
14
13
|
let lastSave = null;
|
|
15
|
-
|
|
16
|
-
|
|
14
|
+
reportFullheading = null;
|
|
15
|
+
reportFullattitude = null;
|
|
16
|
+
reportFullcurrent = null;
|
|
17
|
+
reportFullcurrentStat = null;
|
|
18
|
+
reportFullboatSpeed = null;
|
|
19
|
+
reportFullboatSpeedStat = null;
|
|
20
|
+
reportFullcorrectedBoatSpeed = null;
|
|
21
|
+
reportFullboatSpeedRefGround = null;
|
|
22
|
+
reportFullgroundSpeed = null;
|
|
23
|
+
reportFullgroundSpeedStat = null;
|
|
24
|
+
reportFullspeedCorrection = null;
|
|
25
|
+
reportFullresidual = null;
|
|
26
|
+
reportFullreportFull = null;
|
|
27
|
+
var reportVector = null;
|
|
28
|
+
var table = null;
|
|
17
29
|
|
|
18
30
|
const plugin = {};
|
|
19
31
|
plugin.id = "SpeedAndCurrent";
|
|
@@ -63,6 +75,42 @@ module.exports = function (app) {
|
|
|
63
75
|
description: "Correction table maximum speed.",
|
|
64
76
|
default: 9
|
|
65
77
|
},
|
|
78
|
+
|
|
79
|
+
headingSource: {
|
|
80
|
+
type: "string",
|
|
81
|
+
title: "Heading Source",
|
|
82
|
+
description: "Source to use for navigation.heading (optional).",
|
|
83
|
+
default: " "
|
|
84
|
+
},
|
|
85
|
+
|
|
86
|
+
boatSpeedSource: {
|
|
87
|
+
type: "string",
|
|
88
|
+
title: "Boat Speed Source",
|
|
89
|
+
description: "Source to use for navigation.speedThroughWater (optional).",
|
|
90
|
+
default: " "
|
|
91
|
+
},
|
|
92
|
+
|
|
93
|
+
COGSource: {
|
|
94
|
+
type: "string",
|
|
95
|
+
title: "Course over ground",
|
|
96
|
+
description: "Source to use for navigation.courseOverGroundTrue (optional)",
|
|
97
|
+
default: " "
|
|
98
|
+
},
|
|
99
|
+
|
|
100
|
+
SOGSource: {
|
|
101
|
+
type: "string",
|
|
102
|
+
title: "Ground Speed Source",
|
|
103
|
+
description: "Source to use for navigation.speedOverGround (optional)",
|
|
104
|
+
default: " "
|
|
105
|
+
},
|
|
106
|
+
|
|
107
|
+
attitudeSource: {
|
|
108
|
+
type: "string",
|
|
109
|
+
title: "Attitude Source",
|
|
110
|
+
description: "Source to use for navigation.attitude (optional)",
|
|
111
|
+
default: " "
|
|
112
|
+
},
|
|
113
|
+
|
|
66
114
|
preventDuplication: {
|
|
67
115
|
type: "boolean",
|
|
68
116
|
title: "Prevent duplication of boat speed",
|
|
@@ -119,10 +167,10 @@ module.exports = function (app) {
|
|
|
119
167
|
maximum: 8
|
|
120
168
|
}
|
|
121
169
|
}
|
|
122
|
-
};
|
|
123
|
-
|
|
124
|
-
|
|
125
|
-
|
|
170
|
+
};
|
|
171
|
+
|
|
172
|
+
plugin.uiSchema = {
|
|
173
|
+
'ui:order': ["mode", "maxSpeed", "speedStep", "maxHeel", "heelStep", "headingSource", "boatSpeedSource", "COGSource", "SOGSource", "attitudeSource", "preventDuplication", "updateCorrectionTable", "estimateBoatSpeed", "correctionStability", "doStartFresh", "assumeCurrent", "estimateCurrent", "currentStability"],
|
|
126
174
|
doStartFresh: {
|
|
127
175
|
"ui:widget": "hidden",
|
|
128
176
|
},
|
|
@@ -141,7 +189,7 @@ module.exports = function (app) {
|
|
|
141
189
|
mode: {
|
|
142
190
|
"ui:widget": "select",
|
|
143
191
|
},
|
|
144
|
-
|
|
192
|
+
updateCorrectionTable: {
|
|
145
193
|
"ui:widget": "hidden",
|
|
146
194
|
},
|
|
147
195
|
estimateBoatSpeed: {
|
|
@@ -163,7 +211,8 @@ module.exports = function (app) {
|
|
|
163
211
|
"ui:widget": "hidden",
|
|
164
212
|
}
|
|
165
213
|
};
|
|
166
|
-
|
|
214
|
+
|
|
215
|
+
|
|
167
216
|
plugin.registerWithRouter = function (router) {
|
|
168
217
|
app.debug('registerWithRouter');
|
|
169
218
|
|
|
@@ -172,9 +221,8 @@ module.exports = function (app) {
|
|
|
172
221
|
res.status(503).json({ error: "Plugin is not running" });
|
|
173
222
|
}
|
|
174
223
|
else {
|
|
175
|
-
res.json(
|
|
224
|
+
res.json(reportFull.report());
|
|
176
225
|
}
|
|
177
|
-
|
|
178
226
|
});
|
|
179
227
|
|
|
180
228
|
router.get('/getVectors', (req, res) => {
|
|
@@ -182,30 +230,14 @@ module.exports = function (app) {
|
|
|
182
230
|
res.status(503).json({ error: "Plugin is not running" });
|
|
183
231
|
}
|
|
184
232
|
else {
|
|
185
|
-
|
|
186
|
-
const d = [heading, attitude];
|
|
187
|
-
const p = [groundSpeed, boatSpeedPolar, correctedBoatSpeed, current, speedCorrection, residual];
|
|
188
|
-
p.forEach(polar => {
|
|
189
|
-
v.polars.push({
|
|
190
|
-
id: polar.id,
|
|
191
|
-
plane: polar.plane,
|
|
192
|
-
label: polar.label,
|
|
193
|
-
speed: polar.pValue.speed,
|
|
194
|
-
angle: polar.pValue.angle,
|
|
195
|
-
});
|
|
196
|
-
});
|
|
197
|
-
d.forEach(delta => {
|
|
198
|
-
v.deltas.push({
|
|
199
|
-
id: delta.id,
|
|
200
|
-
value: delta.value,
|
|
201
|
-
});
|
|
202
|
-
});
|
|
203
|
-
res.json(v);
|
|
233
|
+
res.json(reportVector.report());
|
|
204
234
|
}
|
|
205
235
|
});
|
|
206
236
|
|
|
207
237
|
}
|
|
208
238
|
|
|
239
|
+
// ...existing code...
|
|
240
|
+
|
|
209
241
|
function saveTable(options, correctionTable) {
|
|
210
242
|
options.correctionTable = correctionTable.toJSON();
|
|
211
243
|
app.savePluginOptions(options, () => { app.debug('Correction table saved') });
|
|
@@ -214,125 +246,161 @@ module.exports = function (app) {
|
|
|
214
246
|
|
|
215
247
|
|
|
216
248
|
plugin.start = (opts) => {
|
|
217
|
-
app.
|
|
218
|
-
|
|
249
|
+
app.setPluginStatus("starting");
|
|
250
|
+
app.debug("plugin starting");
|
|
251
|
+
|
|
219
252
|
// make options survive to the stop function
|
|
220
253
|
options = opts;
|
|
221
254
|
loadPresets();
|
|
222
255
|
|
|
223
256
|
// correction table
|
|
224
|
-
|
|
225
|
-
|
|
257
|
+
table = loadTable(options);
|
|
258
|
+
|
|
226
259
|
|
|
227
260
|
// heading
|
|
228
|
-
|
|
229
|
-
heading
|
|
230
|
-
heading.
|
|
231
|
-
heading.setDisplayAttributes("heading","heading");
|
|
261
|
+
heading = new MessageHandler("heading", "navigation.headingTrue", options.headingSource);
|
|
262
|
+
heading.subscribe(app, plugin.id, true);
|
|
263
|
+
heading.setDisplayAttributes({ label: "heading" });
|
|
232
264
|
|
|
233
|
-
//attitude
|
|
234
|
-
attitude = new
|
|
265
|
+
// attitude
|
|
266
|
+
attitude = new MessageHandler("attitude", "navigation.attitude", options.attitudeSource);
|
|
235
267
|
attitude.value = { pitch: 0, roll: 0, yaw: 0 }; //prevents errors when there is no attitude sensor
|
|
236
|
-
attitude.setDisplayAttributes("attitude"
|
|
237
|
-
attitude.subscribe(app, plugin.id,
|
|
238
|
-
|
|
239
|
-
// current
|
|
240
|
-
currentDelta = new DeltaBase("environment.current");
|
|
241
|
-
current = new PolarDeltaBase();
|
|
242
|
-
current.setDisplayAttributes("current", "ref_ground", "current");
|
|
243
|
-
|
|
244
|
-
const currentStat = new PolarStat(current);
|
|
268
|
+
attitude.setDisplayAttributes({ label: "attitude" });
|
|
269
|
+
attitude.subscribe(app, plugin.id, true);
|
|
245
270
|
|
|
246
|
-
//
|
|
247
|
-
|
|
271
|
+
// current
|
|
272
|
+
current = new Polar("current", "self.environment.current.drift", "self.environment.current.setTrue");
|
|
273
|
+
current.setDisplayAttributes({ label: "current", plane: "Ground" });
|
|
274
|
+
current.setAngleRange('0to2pi');
|
|
275
|
+
currentStat = new PolarDamped("currentDamped", current);
|
|
276
|
+
currentStat.setAngleRange('0to2pi');
|
|
277
|
+
currentStat.timeConstant = 60; // Set time constant for damping
|
|
278
|
+
// Current should be initialised as no current
|
|
279
|
+
current.setVectorValue({ x: 0, y: 0 });
|
|
280
|
+
// There should be at least two samples, otherwise we can't calculate a valid speed
|
|
281
|
+
currentStat.sample();
|
|
282
|
+
currentStat.sample();
|
|
248
283
|
|
|
249
284
|
// boat speed
|
|
250
|
-
|
|
251
|
-
|
|
252
|
-
|
|
253
|
-
|
|
254
|
-
}
|
|
255
|
-
else {
|
|
256
|
-
boatSpeed = new DeltaSubscribe("navigation.speedThroughWater");
|
|
257
|
-
boatSpeed.subscribe(app, plugin.id, unsubscribes, calculate);
|
|
258
|
-
}
|
|
259
|
-
|
|
260
|
-
boatSpeedPolar = new PolarDeltaBase("navigation.speedThroughWater", "environment.wind.directionTruenavigation.leewayAngle");
|
|
261
|
-
boatSpeedPolar.setDisplayAttributes("boatSpeed", "ref_boat", "observed boat speed");
|
|
262
|
-
const boatSpeedStat = new PolarStat(boatSpeedPolar);
|
|
263
|
-
|
|
264
|
-
correctedBoatSpeed = new PolarDeltaBase("navigation.speedThroughWater", "environment.wind.directionTruenavigation.leewayAngle");
|
|
265
|
-
correctedBoatSpeed.setDisplayAttributes("correctedSpeed", "ref_boat", "estimated boat Speed");
|
|
285
|
+
boatSpeed = new Polar("boatSpeed", "navigation.speedThroughWater", options.boatSpeedSource);
|
|
286
|
+
boatSpeed.subscribe(app, plugin.id, true, false, !options.preventDuplication || !options.estimateBoatSpeed);
|
|
287
|
+
boatSpeed.setDisplayAttributes({ label: "boat speed", plane: "Boat" });
|
|
288
|
+
boatSpeedStat = new PolarDamped("boatSpeedDamped", boatSpeed);
|
|
289
|
+
boatSpeedStat.setDisplayAttributes({ label: "boat speed (damped)", plane: "Boat" });
|
|
266
290
|
|
|
267
|
-
|
|
268
|
-
|
|
291
|
+
correctedBoatSpeed = new Polar("correctedBoatSpeed", "navigation.speedThroughWater", "environment.wind.directionTruenavigation.leewayAngle");
|
|
292
|
+
correctedBoatSpeed.setDisplayAttributes({ label: "corrected boat speed", plane: "Boat" });
|
|
293
|
+
//const correctedBoatSpeedStat = new PolarDamped("correctedBoatSpeedDamped", correctedBoatSpeed);
|
|
269
294
|
|
|
295
|
+
boatSpeedRefGround = new Polar("boatSpeedRefGround");
|
|
296
|
+
boatSpeedRefGround.setDisplayAttributes("boat speed over ground", "Ground");
|
|
270
297
|
|
|
271
298
|
// ground speed
|
|
272
|
-
groundSpeed = new
|
|
273
|
-
groundSpeed.subscribe(app, plugin.id,
|
|
274
|
-
groundSpeed.setDisplayAttributes(
|
|
275
|
-
|
|
299
|
+
groundSpeed = new Polar("groundSpeed", "navigation.speedOverGround", "navigation.courseOverGroundTrue", options.SOGSource, options.COGSource);
|
|
300
|
+
groundSpeed.subscribe(app, plugin.id, true, true, true);
|
|
301
|
+
groundSpeed.setDisplayAttributes({ label: "ground speed", plane: "Ground" });
|
|
302
|
+
groundSpeed.setAngleRange('0to2pi');
|
|
303
|
+
groundSpeedStat = new PolarDamped("groundSpeedDamped", groundSpeed);
|
|
304
|
+
groundSpeedStat.setDisplayAttributes({ label: "ground speed (damped)", plane: "Ground" });
|
|
305
|
+
groundSpeedStat.setAngleRange('0to2pi');
|
|
276
306
|
|
|
277
307
|
// correction vector
|
|
278
|
-
speedCorrection = new
|
|
279
|
-
speedCorrection.setDisplayAttributes("correction", "
|
|
308
|
+
speedCorrection = new Polar("speedCorrection");
|
|
309
|
+
speedCorrection.setDisplayAttributes("speed correction", "Boat");
|
|
310
|
+
speedCorrection.setDisplayAttributes({ label: "speed correction", plane: "Boat" });
|
|
280
311
|
|
|
281
312
|
// residual
|
|
282
|
-
residual = new
|
|
283
|
-
residual.setDisplayAttributes("residual", "
|
|
313
|
+
residual = new Polar("residual");
|
|
314
|
+
residual.setDisplayAttributes({ label: "residual", plane: "Ground" });
|
|
284
315
|
|
|
285
316
|
// Make reporting object for webApp
|
|
286
|
-
|
|
287
|
-
|
|
288
|
-
|
|
289
|
-
|
|
290
|
-
|
|
291
|
-
|
|
292
|
-
|
|
293
|
-
|
|
294
|
-
|
|
295
|
-
|
|
296
|
-
|
|
297
|
-
|
|
298
|
-
|
|
299
|
-
|
|
300
|
-
|
|
301
|
-
|
|
317
|
+
reportFull = new Reporter();
|
|
318
|
+
reportFull.addDelta(heading);
|
|
319
|
+
reportFull.addAttitude(attitude);
|
|
320
|
+
reportFull.addPolar(groundSpeed);
|
|
321
|
+
reportFull.addPolar(boatSpeed);
|
|
322
|
+
//reportFull.addPolar(boatSpeedStat);
|
|
323
|
+
//reportFull.addPolar(groundSpeedStat);
|
|
324
|
+
reportFull.addPolar(speedCorrection);
|
|
325
|
+
reportFull.addPolar(correctedBoatSpeed);
|
|
326
|
+
reportFull.addPolar(current);
|
|
327
|
+
reportFull.addPolar(residual);
|
|
328
|
+
reportFull.addTable(table);
|
|
329
|
+
|
|
330
|
+
reportVector = new Reporter();
|
|
331
|
+
reportVector.addDelta(heading);
|
|
332
|
+
reportVector.addPolar(residual);
|
|
333
|
+
reportVector.addPolar(speedCorrection);
|
|
334
|
+
reportVector.addPolar(current);
|
|
335
|
+
reportVector.addPolar(correctedBoatSpeed);
|
|
336
|
+
reportVector.addPolar(boatSpeed);
|
|
337
|
+
reportVector.addPolar(groundSpeed);
|
|
338
|
+
|
|
339
|
+
|
|
340
|
+
|
|
341
|
+
app.debug("Analyzing input data");
|
|
342
|
+
let candidates = [heading, attitude, boatSpeed, groundSpeed];
|
|
343
|
+
// wait for all to settle
|
|
344
|
+
setTimeout(() => {
|
|
345
|
+
//app.debug(groundSpeed.magnitudeHandler);
|
|
346
|
+
//app.debug(groundSpeed.angleHandler);
|
|
347
|
+
// Check if all candidates get input data
|
|
348
|
+
candidates.forEach(candidate => {
|
|
349
|
+
if (candidate.lackingInputData()) {
|
|
350
|
+
app.setPluginStatus("Halted: insufficient input data");
|
|
351
|
+
app.debug("Insufficient input data for " + candidate.displayAttributes.label );
|
|
352
|
+
return;
|
|
302
353
|
}
|
|
354
|
+
});
|
|
355
|
+
|
|
356
|
+
// determing slowest input path and use this as a heartbeat for calculations
|
|
357
|
+
candidates.sort((a, b) => (a.frequency || 0) - (b.frequency || 0));
|
|
358
|
+
|
|
359
|
+
|
|
360
|
+
|
|
361
|
+
if (!candidates[0].lackingInputData()) {
|
|
362
|
+
slowestInput = candidates[0];
|
|
363
|
+
app.debug("Using " + slowestInput.displayAttributes.label + " as heartbeat for calculations ( " + Math.round(candidates[0].frequency) + " updates per second)");
|
|
364
|
+
slowestInput.onChange = calculate;
|
|
365
|
+
if (slowestInput == groundSpeed) {
|
|
366
|
+
groundSpeed.onChange = () => {
|
|
367
|
+
groundSpeedStat.sample();
|
|
368
|
+
calculate();
|
|
369
|
+
};
|
|
370
|
+
}
|
|
371
|
+
else {
|
|
372
|
+
groundSpeed.onChange = () => {
|
|
373
|
+
groundSpeedStat.sample();
|
|
374
|
+
};
|
|
375
|
+
}
|
|
376
|
+
if (slowestInput == boatSpeed) {
|
|
377
|
+
boatSpeed.onChange = () => {
|
|
378
|
+
boatSpeedStat.sample();
|
|
379
|
+
calculate();
|
|
380
|
+
};
|
|
381
|
+
}
|
|
382
|
+
else {
|
|
383
|
+
boatSpeed.onChange = () => {
|
|
384
|
+
boatSpeedStat.sample();
|
|
385
|
+
};
|
|
386
|
+
}
|
|
387
|
+
|
|
388
|
+
isRunning = true;
|
|
389
|
+
app.setPluginStatus("Running");
|
|
390
|
+
app.debug("Running");
|
|
303
391
|
}
|
|
304
|
-
});
|
|
305
|
-
let currentEstimation = null;
|
|
306
|
-
|
|
307
|
-
// filter for boat speed
|
|
308
|
-
stability = 10 ** (1);
|
|
309
|
-
const boatSpeedEstimator = new KalmanFilter({
|
|
310
|
-
observation: {
|
|
311
|
-
stateProjection: [[1, 0], [0, 1]], // observation matrix H
|
|
312
|
-
covariance: [[1 * stability, 0], [0, 1 * stability]], //measurement noise R
|
|
313
|
-
dimension: 2
|
|
314
|
-
},
|
|
315
|
-
dynamic: {
|
|
316
|
-
transition: [[1, 0], [0, 1]], // state transition matrix F
|
|
317
|
-
covariance: [1, 1],// process noise covariance matrix Q
|
|
318
|
-
}
|
|
319
|
-
});
|
|
320
|
-
let boatSpeedEstimation = null;
|
|
321
392
|
|
|
393
|
+
}, 5000);
|
|
322
394
|
|
|
323
|
-
|
|
395
|
+
|
|
324
396
|
|
|
325
397
|
|
|
326
398
|
|
|
327
399
|
// Main function, estimates boatSpeed, leeway and current
|
|
328
|
-
function calculate(
|
|
400
|
+
function calculate() {
|
|
329
401
|
// prepare iteration
|
|
330
|
-
boatSpeedPolar.setVValue({ x: boatSpeed.value, y: 0 });
|
|
331
|
-
groundSpeedStat.sample();
|
|
332
|
-
currentStat.sample();
|
|
333
|
-
boatSpeedStat.sample();
|
|
334
402
|
heel = attitude.value.roll;
|
|
335
|
-
speed = boatSpeed.
|
|
403
|
+
speed = boatSpeed.magnitude;
|
|
336
404
|
theta = heading.value;
|
|
337
405
|
if (!Number.isFinite(heel)) {
|
|
338
406
|
app.debug("Heel is not a valid number, skipping calculation");
|
|
@@ -346,18 +414,11 @@ module.exports = function (app) {
|
|
|
346
414
|
app.debug("Heading is not a valid number, skipping calculation");
|
|
347
415
|
return;
|
|
348
416
|
}
|
|
349
|
-
reporter.newReport();
|
|
350
|
-
reporter.addOptions(options);
|
|
351
|
-
reporter.addPolar( groundSpeed);
|
|
352
|
-
reporter.addPolar( boatSpeedPolar);
|
|
353
|
-
reporter.addAttitude( attitude);
|
|
354
|
-
reporter.addDelta( heading);
|
|
355
417
|
|
|
356
418
|
// update correction table
|
|
357
419
|
if (options.updateCorrectionTable) {
|
|
358
420
|
table.update(speed, heel, groundSpeedStat, currentStat, boatSpeedStat, theta);
|
|
359
421
|
}
|
|
360
|
-
reporter.addTable(table, speed, heel);
|
|
361
422
|
|
|
362
423
|
// correct and estimate boat speed
|
|
363
424
|
estimateBoatSpeed();
|
|
@@ -370,14 +431,10 @@ module.exports = function (app) {
|
|
|
370
431
|
|
|
371
432
|
|
|
372
433
|
// Send calculated delta's
|
|
373
|
-
|
|
374
|
-
|
|
375
|
-
|
|
376
|
-
|
|
377
|
-
currentDelta.setValue(cc);
|
|
378
|
-
DeltaBase.sendDeltas(app, plugin.id, [currentDelta]);
|
|
379
|
-
}
|
|
380
|
-
if (options.estimateBoatSpeed) PolarDeltaBase.sendDeltas(app, plugin.id, [correctedBoatSpeed]);
|
|
434
|
+
updates = [];
|
|
435
|
+
if (options.estimateCurrent) updates.push(current);
|
|
436
|
+
if (options.estimateBoatSpeed) updates.push(correctedBoatSpeed);
|
|
437
|
+
if (updates.length) Polar.send(app, plugin.id, updates);
|
|
381
438
|
|
|
382
439
|
// Save correction table
|
|
383
440
|
// periodically
|
|
@@ -389,31 +446,19 @@ module.exports = function (app) {
|
|
|
389
446
|
// estimates boat speed from observed boatSpeed and correction
|
|
390
447
|
function estimateBoatSpeed() {
|
|
391
448
|
cor = table.getKalmanCorrection(speed, heel);
|
|
392
|
-
|
|
393
|
-
|
|
394
|
-
speedCorrection.setVValue(cor);
|
|
395
|
-
|
|
396
|
-
reporter.addPolar(speedCorrection);
|
|
449
|
+
correctedBoatSpeed.copyFrom(boatSpeed);
|
|
450
|
+
speedCorrection.setVectorValue(cor);
|
|
397
451
|
correctedBoatSpeed.add(speedCorrection);
|
|
398
|
-
const v = correctedBoatSpeed.getVValue();
|
|
399
|
-
const observation = [v.x, v.y];
|
|
400
|
-
boatSpeedEstimation = boatSpeedEstimator.filter({ previousCorrected: boatSpeedEstimation, observation });
|
|
401
|
-
correctedBoatSpeed.setKValue(boatSpeedEstimation);
|
|
402
|
-
reporter.addPolar(correctedBoatSpeed);
|
|
403
452
|
}
|
|
404
453
|
|
|
405
454
|
// estimates current from observed groundspeed and estimated boatspeed
|
|
406
455
|
function estimateCurrent() {
|
|
407
456
|
boatSpeedRefGround.copyFrom(correctedBoatSpeed);
|
|
408
457
|
boatSpeedRefGround.rotate(theta);
|
|
409
|
-
reporter.addPolar( boatSpeedRefGround);
|
|
410
458
|
current.copyFrom(groundSpeed);
|
|
411
459
|
current.substract(boatSpeedRefGround);
|
|
412
|
-
|
|
413
|
-
|
|
414
|
-
currentEstimation = currentEstimator.filter({ previousCorrected: currentEstimation, observation });
|
|
415
|
-
current.setKValue(currentEstimation);
|
|
416
|
-
reporter.addPolar( current);
|
|
460
|
+
currentStat.sample();
|
|
461
|
+
current.copyFrom(currentStat);
|
|
417
462
|
}
|
|
418
463
|
|
|
419
464
|
// Calculate what ground speed is observed that is not attributed to estimated boat speed or estimated current
|
|
@@ -423,14 +468,13 @@ module.exports = function (app) {
|
|
|
423
468
|
boatSpeedRefGround.rotate(theta);
|
|
424
469
|
residual.substract(boatSpeedRefGround);
|
|
425
470
|
residual.substract(current);
|
|
426
|
-
reporter.addPolar(residual);
|
|
427
471
|
}
|
|
428
472
|
|
|
429
473
|
// Make sure the table matches the settings
|
|
430
|
-
function enforceConsistancy(row, col
|
|
474
|
+
function enforceConsistancy(row, col) {
|
|
431
475
|
// function is bugged due to IS units vs knots and degrees
|
|
432
476
|
if (!options?.correctionTable) return false;
|
|
433
|
-
|
|
477
|
+
table = options.correctionTable;
|
|
434
478
|
if (!table) return false;
|
|
435
479
|
if (!table.row) return false;
|
|
436
480
|
if (table.row.min != row.min) return false;
|
|
@@ -446,28 +490,28 @@ module.exports = function (app) {
|
|
|
446
490
|
const row = { min: 0, max: SI.fromKnots(options.maxSpeed), step: SI.fromKnots(options.speedStep) };
|
|
447
491
|
const col = { min: -SI.fromDegrees(options.maxHeel), max: SI.fromDegrees(options.maxHeel), step: SI.fromDegrees(options.heelStep) };
|
|
448
492
|
let table;
|
|
449
|
-
if (
|
|
493
|
+
if (enforceConsistancy(row, col) && !options.doStartFresh) {
|
|
450
494
|
table = CorrectionTable.fromJSON(options.correctionTable, options.correctionStability);
|
|
451
495
|
app.debug("Correction table loaded");
|
|
452
496
|
lastSave = new Date();
|
|
453
497
|
}
|
|
454
498
|
else {
|
|
455
|
-
table = new CorrectionTable(row, col, options.correctionStability);
|
|
499
|
+
table = new CorrectionTable("correctionTable", row, col, options.correctionStability);
|
|
456
500
|
app.debug("Correction table created");
|
|
457
501
|
options.doStartFresh = false;
|
|
458
502
|
lastSave = saveTable(options, table);
|
|
459
503
|
}
|
|
460
|
-
table.setDisplayAttributes("
|
|
504
|
+
table.setDisplayAttributes({ label: "correction table" });
|
|
461
505
|
return table;
|
|
462
506
|
}
|
|
463
507
|
|
|
464
508
|
|
|
465
|
-
function loadPresets() {
|
|
509
|
+
function loadPresets() {
|
|
466
510
|
const modeEnum = plugin.schema.properties.mode.enum;
|
|
467
511
|
app.debug("Loading presests for mode: " + options.mode);
|
|
468
512
|
|
|
469
513
|
switch (modeEnum.indexOf(options.mode)) {
|
|
470
|
-
case
|
|
514
|
+
case -1:
|
|
471
515
|
app.debug("Invalid mode");
|
|
472
516
|
app.debug(options);
|
|
473
517
|
//options.mode = modeEnum[2];
|
|
@@ -480,7 +524,7 @@ module.exports = function (app) {
|
|
|
480
524
|
options.assumeCurrent = false;
|
|
481
525
|
options.estimateCurrent = false;
|
|
482
526
|
options.correctionStability = 5;
|
|
483
|
-
options.mode = modeEnum[2];
|
|
527
|
+
options.mode = modeEnum[2];
|
|
484
528
|
break;
|
|
485
529
|
case 1:
|
|
486
530
|
// new table, with current
|
|
@@ -491,7 +535,7 @@ module.exports = function (app) {
|
|
|
491
535
|
options.estimateCurrent = false;
|
|
492
536
|
options.correctionStability = 5;
|
|
493
537
|
options.currentStability = 7;
|
|
494
|
-
options.mode = modeEnum[3];
|
|
538
|
+
options.mode = modeEnum[3];
|
|
495
539
|
break;
|
|
496
540
|
case 2:
|
|
497
541
|
// fresh table, no current
|
|
@@ -501,7 +545,7 @@ module.exports = function (app) {
|
|
|
501
545
|
options.assumeCurrent = false;
|
|
502
546
|
options.estimateCurrent = false;
|
|
503
547
|
options.correctionStability = 6;
|
|
504
|
-
|
|
548
|
+
break;
|
|
505
549
|
case 3:
|
|
506
550
|
// fresh table, with current
|
|
507
551
|
options.doStartFresh = false;
|
|
@@ -535,11 +579,11 @@ module.exports = function (app) {
|
|
|
535
579
|
// locked table
|
|
536
580
|
options.doStartFresh = false;
|
|
537
581
|
options.updateCorrectionTable = false;
|
|
538
|
-
options.estimateBoatSpeed =
|
|
582
|
+
options.estimateBoatSpeed = true;
|
|
539
583
|
options.assumeCurrent = true;
|
|
540
584
|
options.estimateCurrent = true;
|
|
541
585
|
options.currentStability = 2;
|
|
542
|
-
break;
|
|
586
|
+
break;
|
|
543
587
|
case 7:
|
|
544
588
|
// all manual configuration;
|
|
545
589
|
break;
|
|
@@ -557,31 +601,33 @@ module.exports = function (app) {
|
|
|
557
601
|
}
|
|
558
602
|
|
|
559
603
|
|
|
560
|
-
|
|
604
|
+
plugin.stop = () => {
|
|
561
605
|
return new Promise((resolve, reject) => {
|
|
562
606
|
try {
|
|
563
|
-
unsubscribes.forEach(f => f());
|
|
564
|
-
unsubscribes = [];
|
|
565
607
|
//corrTable.save(app);
|
|
566
608
|
if (corrTable != null) {
|
|
567
609
|
lastSave = saveTable(options, corrTable);
|
|
568
610
|
}
|
|
569
|
-
|
|
570
|
-
|
|
571
|
-
residual = null;
|
|
611
|
+
|
|
572
612
|
heading = null;
|
|
573
613
|
attitude = null;
|
|
614
|
+
current = null;
|
|
615
|
+
currentStat = null;
|
|
574
616
|
boatSpeed = null;
|
|
575
|
-
|
|
617
|
+
boatSpeedStat = null;
|
|
576
618
|
correctedBoatSpeed = null;
|
|
619
|
+
boatSpeedRefGround = null;
|
|
577
620
|
groundSpeed = null;
|
|
578
|
-
|
|
579
|
-
currentDelta = null;
|
|
621
|
+
groundSpeedStat = null;
|
|
580
622
|
speedCorrection = null;
|
|
581
|
-
|
|
623
|
+
residual = null;
|
|
624
|
+
reportFull = null;
|
|
625
|
+
reportVector = null;
|
|
626
|
+
table =
|
|
627
|
+
app.setPluginStatus("Stopped");
|
|
628
|
+
app.debug("Stopped");
|
|
629
|
+
|
|
582
630
|
isRunning = false;
|
|
583
|
-
corrTable = null;
|
|
584
|
-
|
|
585
631
|
resolve();
|
|
586
632
|
} catch (error) {
|
|
587
633
|
reject(error);
|
package/package.json
CHANGED
|
@@ -1,7 +1,7 @@
|
|
|
1
1
|
{
|
|
2
2
|
"name": "speedandcurrent",
|
|
3
|
-
"version": "1.0
|
|
4
|
-
"description": "
|
|
3
|
+
"version": "1.3.0",
|
|
4
|
+
"description": "A plugin that automatically corrects boat speed and calculates current and leeway using sensor fusion.",
|
|
5
5
|
"main": "index.js",
|
|
6
6
|
"scripts": {
|
|
7
7
|
"test": "echo \"Error: no test specified\" && exit 1"
|
|
@@ -29,6 +29,6 @@
|
|
|
29
29
|
},
|
|
30
30
|
"dependencies": {
|
|
31
31
|
"kalman-filter": "^2.3.0",
|
|
32
|
-
"signalkutilities": "^1.
|
|
32
|
+
"signalkutilities": "^1.3.0"
|
|
33
33
|
}
|
|
34
34
|
}
|
package/public/TableRenderer.js
CHANGED
|
@@ -28,11 +28,13 @@ class TableRenderer {
|
|
|
28
28
|
}
|
|
29
29
|
|
|
30
30
|
render(data) {
|
|
31
|
-
const { row, col, table,
|
|
31
|
+
const {id, row, col, table, displayAttributes } = data;
|
|
32
32
|
this.data = data;
|
|
33
|
-
|
|
33
|
+
const label = displayAttributes && displayAttributes.label ? displayAttributes.label : "";
|
|
34
|
+
|
|
34
35
|
// Create a table element
|
|
35
36
|
const tableElement = document.createElement('table');
|
|
37
|
+
|
|
36
38
|
tableElement.id = id;
|
|
37
39
|
tableElement.classList.add('Table2D');
|
|
38
40
|
// create header row
|
|
@@ -41,7 +43,7 @@ class TableRenderer {
|
|
|
41
43
|
// Create other rows
|
|
42
44
|
var rIndex = 0;
|
|
43
45
|
for (let r = row.min; r <= row.max + 0.01; r += row.step) {
|
|
44
|
-
tableElement.appendChild(this.tableRow(r, rIndex, col, table
|
|
46
|
+
tableElement.appendChild(this.tableRow(r, rIndex, col, table));
|
|
45
47
|
rIndex++;
|
|
46
48
|
}
|
|
47
49
|
return tableElement;
|
|
@@ -54,7 +56,7 @@ class TableRenderer {
|
|
|
54
56
|
firstHeaderCell.innerText = label ? label : '';
|
|
55
57
|
firstHeaderCell.classList.add('TableRowHeader');
|
|
56
58
|
headerRow.appendChild(firstHeaderCell);
|
|
57
|
-
|
|
59
|
+
|
|
58
60
|
// Create header row
|
|
59
61
|
for (let c = col.min; c <= col.max + 0.01; c += col.step) {
|
|
60
62
|
const headerCell = document.createElement('th');
|
|
@@ -73,7 +75,7 @@ class TableRenderer {
|
|
|
73
75
|
return headerCell;
|
|
74
76
|
}
|
|
75
77
|
|
|
76
|
-
tableRow(r, rIndex, col, table
|
|
78
|
+
tableRow(r, rIndex, col, table) {
|
|
77
79
|
var cIndex = 0;
|
|
78
80
|
const rowElement = document.createElement('tr');
|
|
79
81
|
rowElement.appendChild(this.rowLabel(r));
|
|
@@ -88,7 +90,7 @@ class TableRenderer {
|
|
|
88
90
|
else {
|
|
89
91
|
cell.classList.add('emptyCell');
|
|
90
92
|
}
|
|
91
|
-
if (
|
|
93
|
+
if (value.displayAttributes && value.displayAttributes.selected) cell.classList.add('selectedCell');
|
|
92
94
|
|
|
93
95
|
rowElement.appendChild(cell);
|
|
94
96
|
cIndex++;
|