@depz/sensor-sdk 0.2.2 → 0.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/README.md +13 -5
- package/dist/dataset.d.ts.map +1 -1
- package/dist/dataset.js +66 -2
- package/dist/dataset.js.map +1 -1
- package/dist/discovery.d.ts.map +1 -1
- package/dist/discovery.js +25 -2
- package/dist/discovery.js.map +1 -1
- package/dist/index.d.ts +21 -1
- package/dist/index.d.ts.map +1 -1
- package/dist/index.js +16 -1
- package/dist/index.js.map +1 -1
- package/dist/protocol/bno055.d.ts +101 -0
- package/dist/protocol/bno055.d.ts.map +1 -0
- package/dist/protocol/bno055.js +105 -0
- package/dist/protocol/bno055.js.map +1 -0
- package/dist/protocol/identity.d.ts +1 -1
- package/dist/protocol/identity.d.ts.map +1 -1
- package/dist/protocol/identity.js +6 -0
- package/dist/protocol/identity.js.map +1 -1
- package/dist/protocol/vl53l7.d.ts +77 -0
- package/dist/protocol/vl53l7.d.ts.map +1 -0
- package/dist/protocol/vl53l7.js +99 -0
- package/dist/protocol/vl53l7.js.map +1 -0
- package/dist/protocol/vl53lx.d.ts +72 -0
- package/dist/protocol/vl53lx.d.ts.map +1 -0
- package/dist/protocol/vl53lx.js +87 -0
- package/dist/protocol/vl53lx.js.map +1 -0
- package/dist/sensors/bno055/bno055.d.ts +207 -0
- package/dist/sensors/bno055/bno055.d.ts.map +1 -0
- package/dist/sensors/bno055/bno055.js +547 -0
- package/dist/sensors/bno055/bno055.js.map +1 -0
- package/dist/sensors/bno055/regs.d.ts +271 -0
- package/dist/sensors/bno055/regs.d.ts.map +1 -0
- package/dist/sensors/bno055/regs.js +366 -0
- package/dist/sensors/bno055/regs.js.map +1 -0
- package/dist/sensors/bno086/bno086.d.ts +13 -0
- package/dist/sensors/bno086/bno086.d.ts.map +1 -1
- package/dist/sensors/bno086/bno086.js +26 -1
- package/dist/sensors/bno086/bno086.js.map +1 -1
- package/dist/sensors/bno086/sh2.d.ts.map +1 -1
- package/dist/sensors/bno086/sh2.js +5 -2
- package/dist/sensors/bno086/sh2.js.map +1 -1
- package/dist/sensors/vl53l7/vl53l7.d.ts +97 -0
- package/dist/sensors/vl53l7/vl53l7.d.ts.map +1 -0
- package/dist/sensors/vl53l7/vl53l7.js +138 -0
- package/dist/sensors/vl53l7/vl53l7.js.map +1 -0
- package/dist/sensors/vl53l8/assets/index.d.ts +5 -1
- package/dist/sensors/vl53l8/assets/index.d.ts.map +1 -1
- package/dist/sensors/vl53l8/assets/index.js +7 -1
- package/dist/sensors/vl53l8/assets/index.js.map +1 -1
- package/dist/sensors/vl53l8/assets/l7ch.d.ts +12 -0
- package/dist/sensors/vl53l8/assets/l7ch.d.ts.map +1 -0
- package/dist/sensors/vl53l8/assets/l7ch.js +18 -0
- package/dist/sensors/vl53l8/assets/l7ch.js.map +1 -0
- package/dist/sensors/vl53l8/assets/l7cx.d.ts +14 -0
- package/dist/sensors/vl53l8/assets/l7cx.d.ts.map +1 -0
- package/dist/sensors/vl53l8/assets/l7cx.js +20 -0
- package/dist/sensors/vl53l8/assets/l7cx.js.map +1 -0
- package/dist/sensors/vl53l8/cnh.d.ts +4 -3
- package/dist/sensors/vl53l8/cnh.d.ts.map +1 -1
- package/dist/sensors/vl53l8/cnh.js +4 -3
- package/dist/sensors/vl53l8/cnh.js.map +1 -1
- package/dist/sensors/vl53l8/uld.d.ts +50 -9
- package/dist/sensors/vl53l8/uld.d.ts.map +1 -1
- package/dist/sensors/vl53l8/uld.js +241 -35
- package/dist/sensors/vl53l8/uld.js.map +1 -1
- package/dist/sensors/vl53l8/vl53l8.d.ts +8 -3
- package/dist/sensors/vl53l8/vl53l8.d.ts.map +1 -1
- package/dist/sensors/vl53l8/vl53l8.js +12 -7
- package/dist/sensors/vl53l8/vl53l8.js.map +1 -1
- package/dist/sensors/vl53lx/decode.d.ts +70 -0
- package/dist/sensors/vl53lx/decode.d.ts.map +1 -0
- package/dist/sensors/vl53lx/decode.js +108 -0
- package/dist/sensors/vl53lx/decode.js.map +1 -0
- package/dist/sensors/vl53lx/uld/arith.d.ts +23 -0
- package/dist/sensors/vl53lx/uld/arith.d.ts.map +1 -0
- package/dist/sensors/vl53lx/uld/arith.js +42 -0
- package/dist/sensors/vl53lx/uld/arith.js.map +1 -0
- package/dist/sensors/vl53lx/uld/bare/core.d.ts +345 -0
- package/dist/sensors/vl53lx/uld/bare/core.d.ts.map +1 -0
- package/dist/sensors/vl53lx/uld/bare/core.js +1085 -0
- package/dist/sensors/vl53lx/uld/bare/core.js.map +1 -0
- package/dist/sensors/vl53lx/uld/bare/driver.d.ts +91 -0
- package/dist/sensors/vl53lx/uld/bare/driver.d.ts.map +1 -0
- package/dist/sensors/vl53lx/uld/bare/driver.js +206 -0
- package/dist/sensors/vl53lx/uld/bare/driver.js.map +1 -0
- package/dist/sensors/vl53lx/uld/bare/hist.d.ts +180 -0
- package/dist/sensors/vl53lx/uld/bare/hist.d.ts.map +1 -0
- package/dist/sensors/vl53lx/uld/bare/hist.js +949 -0
- package/dist/sensors/vl53lx/uld/bare/hist.js.map +1 -0
- package/dist/sensors/vl53lx/uld/bare/image.d.ts +53 -0
- package/dist/sensors/vl53lx/uld/bare/image.d.ts.map +1 -0
- package/dist/sensors/vl53lx/uld/bare/image.js +108 -0
- package/dist/sensors/vl53lx/uld/bare/image.js.map +1 -0
- package/dist/sensors/vl53lx/uld/bare/imath.d.ts +39 -0
- package/dist/sensors/vl53lx/uld/bare/imath.d.ts.map +1 -0
- package/dist/sensors/vl53lx/uld/bare/imath.js +99 -0
- package/dist/sensors/vl53lx/uld/bare/imath.js.map +1 -0
- package/dist/sensors/vl53lx/uld/bare/index.d.ts +22 -0
- package/dist/sensors/vl53lx/uld/bare/index.d.ts.map +1 -0
- package/dist/sensors/vl53lx/uld/bare/index.js +22 -0
- package/dist/sensors/vl53lx/uld/bare/index.js.map +1 -0
- package/dist/sensors/vl53lx/uld/bare/nvm.d.ts +85 -0
- package/dist/sensors/vl53lx/uld/bare/nvm.d.ts.map +1 -0
- package/dist/sensors/vl53lx/uld/bare/nvm.js +148 -0
- package/dist/sensors/vl53lx/uld/bare/nvm.js.map +1 -0
- package/dist/sensors/vl53lx/uld/bare/regs.d.ts +54 -0
- package/dist/sensors/vl53lx/uld/bare/regs.d.ts.map +1 -0
- package/dist/sensors/vl53lx/uld/bare/regs.js +496 -0
- package/dist/sensors/vl53lx/uld/bare/regs.js.map +1 -0
- package/dist/sensors/vl53lx/uld/bare/tuning.d.ts +15 -0
- package/dist/sensors/vl53lx/uld/bare/tuning.d.ts.map +1 -0
- package/dist/sensors/vl53lx/uld/bare/tuning.js +303 -0
- package/dist/sensors/vl53lx/uld/bare/tuning.js.map +1 -0
- package/dist/sensors/vl53lx/uld/base.d.ts +129 -0
- package/dist/sensors/vl53lx/uld/base.d.ts.map +1 -0
- package/dist/sensors/vl53lx/uld/base.js +166 -0
- package/dist/sensors/vl53lx/uld/base.js.map +1 -0
- package/dist/sensors/vl53lx/uld/l0x.d.ts +306 -0
- package/dist/sensors/vl53lx/uld/l0x.d.ts.map +1 -0
- package/dist/sensors/vl53lx/uld/l0x.js +1492 -0
- package/dist/sensors/vl53lx/uld/l0x.js.map +1 -0
- package/dist/sensors/vl53lx/uld/l1.d.ts +69 -0
- package/dist/sensors/vl53lx/uld/l1.d.ts.map +1 -0
- package/dist/sensors/vl53lx/uld/l1.js +262 -0
- package/dist/sensors/vl53lx/uld/l1.js.map +1 -0
- package/dist/sensors/vl53lx/uld/l3.d.ts +25 -0
- package/dist/sensors/vl53lx/uld/l3.d.ts.map +1 -0
- package/dist/sensors/vl53lx/uld/l3.js +51 -0
- package/dist/sensors/vl53lx/uld/l3.js.map +1 -0
- package/dist/sensors/vl53lx/uld/l4.d.ts +22 -0
- package/dist/sensors/vl53lx/uld/l4.d.ts.map +1 -0
- package/dist/sensors/vl53lx/uld/l4.js +99 -0
- package/dist/sensors/vl53lx/uld/l4.js.map +1 -0
- package/dist/sensors/vl53lx/uld/link.d.ts +42 -0
- package/dist/sensors/vl53lx/uld/link.d.ts.map +1 -0
- package/dist/sensors/vl53lx/uld/link.js +28 -0
- package/dist/sensors/vl53lx/uld/link.js.map +1 -0
- package/dist/sensors/vl53lx/uld/registry.d.ts +68 -0
- package/dist/sensors/vl53lx/uld/registry.d.ts.map +1 -0
- package/dist/sensors/vl53lx/uld/registry.js +139 -0
- package/dist/sensors/vl53lx/uld/registry.js.map +1 -0
- package/dist/sensors/vl53lx/uld/vl53l1-die.d.ts +172 -0
- package/dist/sensors/vl53lx/uld/vl53l1-die.d.ts.map +1 -0
- package/dist/sensors/vl53lx/uld/vl53l1-die.js +431 -0
- package/dist/sensors/vl53lx/uld/vl53l1-die.js.map +1 -0
- package/dist/sensors/vl53lx/vl53lx.d.ts +323 -0
- package/dist/sensors/vl53lx/vl53lx.d.ts.map +1 -0
- package/dist/sensors/vl53lx/vl53lx.js +688 -0
- package/dist/sensors/vl53lx/vl53lx.js.map +1 -0
- package/package.json +9 -2
|
@@ -0,0 +1,949 @@
|
|
|
1
|
+
/**
|
|
2
|
+
* uld/bare/hist — histogram post-processing: 24 bins → targets.
|
|
3
|
+
* Mirror of the Python `depz_sensor_sdk.vl53lx.uld.bare.hist` (a port of
|
|
4
|
+
* `VL53LX_hist_process_data()`; ST ships the C obfuscated, the C names are in
|
|
5
|
+
* the comments next to ours).
|
|
6
|
+
*
|
|
7
|
+
* In one paragraph: fold repeated bin codes together, estimate the ambient
|
|
8
|
+
* floor, drop the ambient-only bins, work out per-bin detection thresholds,
|
|
9
|
+
* and mark every bin above its threshold. Runs of marked bins are pulses.
|
|
10
|
+
* Each pulse is isolated, filtered with a three-tap window, and the zero
|
|
11
|
+
* crossing between the two halves is interpolated to a sub-bin phase. Phase
|
|
12
|
+
* converts to millimetres against `zero_distance_phase`; event sums convert
|
|
13
|
+
* to rates; the filter sums to sigma. `FrameHistory` compares consecutive
|
|
14
|
+
* frames (phase and event consistency), which is what turns "no wrap check
|
|
15
|
+
* done" into "valid" or "wrapped target".
|
|
16
|
+
*
|
|
17
|
+
* Not here (as in Python): crosstalk compensation, the ambient dmax estimate
|
|
18
|
+
* (a stub on purpose — our boards ship the FMT region blank) and the
|
|
19
|
+
* crosstalk-monitor consistency check.
|
|
20
|
+
*
|
|
21
|
+
* **Integers.** Python floors (`//`), C truncates (`cdiv`); both are kept
|
|
22
|
+
* exactly where the Python has them. Three places can leave 2^53 and run in
|
|
23
|
+
* BigInt: the sigma estimate (64-bit saturation ceilings), the phase
|
|
24
|
+
* interpolation (4096² scaling) and the per-SPAD / events-consistency maths.
|
|
25
|
+
*/
|
|
26
|
+
import { measurement } from "../base.js";
|
|
27
|
+
import { DEVICESTATE_RANGING_OUTPUT_DATA, HISTOGRAM_BUFFER_SIZE, RANGING_WINDOW_VCSEL_PERIODS, calcPllPeriodUs, decodeVcselPeriod, durationMaths, } from "./core.js";
|
|
28
|
+
import { babs, bcdiv, bfloorDiv, bmin, cdiv, floorDiv, isqrtBig, isqrtNum, pymod, shl, shr, } from "./imath.js";
|
|
29
|
+
// vl53lx_hist_structs.h / vl53lx_platform_user_config.h
|
|
30
|
+
export const MAX_BIN_SEQUENCE_LENGTH = 6;
|
|
31
|
+
export const MAX_BIN_SEQUENCE_CODE = 15;
|
|
32
|
+
export const MAX_PULSES = 8; // VL53LX_D_001
|
|
33
|
+
export const MAX_RANGE_RESULTS = 4;
|
|
34
|
+
// vl53lx_ll_device.h
|
|
35
|
+
export const MAX_ALLOWED_PHASE = 0xffff;
|
|
36
|
+
export const SPAD_TOTAL_COUNT_MAX = 2 ** 29 - 1;
|
|
37
|
+
export const SPAD_TOTAL_COUNT_RES_THRES = 2 ** 24;
|
|
38
|
+
export const SPEED_OF_LIGHT_IN_AIR_DIV_8 = 299704 >> 3;
|
|
39
|
+
// vl53lx_sigma_estimate.h — the saturation ceilings of the sigma estimate.
|
|
40
|
+
export const SIGMA_INVALID = 0xffff; // VL53LX_D_002
|
|
41
|
+
const D_003 = 0xffffffn;
|
|
42
|
+
const D_004 = 0xffffffffffffffn;
|
|
43
|
+
const D_005 = 0x7fffffffffn;
|
|
44
|
+
const D_006 = 0x7fffffffffffffffn;
|
|
45
|
+
const D_007 = 0xffffffffn;
|
|
46
|
+
// VL53LX_DeviceError (vl53lx_ll_device.h), the few the histogram path sets.
|
|
47
|
+
export const DEVICEERROR_NOUPDATE = 0;
|
|
48
|
+
export const DEVICEERROR_RANGEPHASECHECK = 5;
|
|
49
|
+
export const DEVICEERROR_SIGMATHRESHOLDCHECK = 6;
|
|
50
|
+
export const DEVICEERROR_PHASECONSISTENCY = 7;
|
|
51
|
+
export const DEVICEERROR_RANGECOMPLETE = 9;
|
|
52
|
+
export const DEVICEERROR_RANGECOMPLETE_NO_WRAP_CHECK = 19;
|
|
53
|
+
export const DEVICEERROR_EVENTCONSISTENCY = 20;
|
|
54
|
+
export const DEVICEERROR_RANGECOMPLETE_MERGED_PULSE = 22;
|
|
55
|
+
export const DEVICEERROR_PREV_RANGE_NO_TARGETS = 23;
|
|
56
|
+
// VL53LX_HIST_TARGET_ORDER__*
|
|
57
|
+
export const HIST_TARGET_ORDER_STRONGEST_FIRST = 1;
|
|
58
|
+
export const HIST_TARGET_ORDER_CLOSEST_FIRST = 2;
|
|
59
|
+
// VL53LX_RangeStatus (vl53lx_def.h) — the user-facing status.
|
|
60
|
+
export const RANGESTATUS_RANGE_VALID = 0;
|
|
61
|
+
export const RANGESTATUS_SIGMA_FAIL = 1;
|
|
62
|
+
export const RANGESTATUS_OUTOFBOUNDS_FAIL = 4;
|
|
63
|
+
export const RANGESTATUS_RANGE_VALID_NO_WRAP_CHECK = 6;
|
|
64
|
+
export const RANGESTATUS_WRAP_TARGET_FAIL = 7;
|
|
65
|
+
export const RANGESTATUS_RANGE_VALID_MERGED_PULSE = 11;
|
|
66
|
+
export const RANGESTATUS_TARGET_PRESENT_LACK_OF_SIGNAL = 12;
|
|
67
|
+
export const RANGESTATUS_NONE = 255;
|
|
68
|
+
// ── C arithmetic ─────────────────────────────────────────────────────────────
|
|
69
|
+
/** VL53LX_isqrt() — floor of the square root, on a uint32 argument. */
|
|
70
|
+
export function isqrt(num) {
|
|
71
|
+
return isqrtNum(pymod(num, 2 ** 32));
|
|
72
|
+
}
|
|
73
|
+
/** VL53LX_calc_pll_period_mm(). */
|
|
74
|
+
export function calcPllPeriodMm(fastOscFrequency) {
|
|
75
|
+
const pllPeriodUs = calcPllPeriodUs(fastOscFrequency);
|
|
76
|
+
const pllPeriodMm = SPEED_OF_LIGHT_IN_AIR_DIV_8 * shr(pllPeriodUs, 2);
|
|
77
|
+
return shr(pllPeriodMm + 2 ** 15, 16);
|
|
78
|
+
}
|
|
79
|
+
/** VL53LX_rate_maths(): events over a duration → count rate in 9.7 Mcps. */
|
|
80
|
+
export function rateMaths(events, timeUs) {
|
|
81
|
+
let tmp = 0;
|
|
82
|
+
if (events > SPAD_TOTAL_COUNT_MAX)
|
|
83
|
+
tmp = SPAD_TOTAL_COUNT_MAX;
|
|
84
|
+
else if (events > 0)
|
|
85
|
+
tmp = events;
|
|
86
|
+
const fracBits = events > SPAD_TOTAL_COUNT_RES_THRES ? 3 : 7;
|
|
87
|
+
if (timeUs > 0)
|
|
88
|
+
tmp = floorDiv(shl(tmp, fracBits) + floorDiv(timeUs, 2), timeUs);
|
|
89
|
+
if (events > SPAD_TOTAL_COUNT_RES_THRES)
|
|
90
|
+
tmp = shl(tmp, 4);
|
|
91
|
+
return Math.min(tmp, 0xffff);
|
|
92
|
+
}
|
|
93
|
+
/** VL53LX_rate_per_spad_maths(). */
|
|
94
|
+
export function ratePerSpadMaths(fracBits, peakCountRate, numSpads, maxOutputValue) {
|
|
95
|
+
let tmp;
|
|
96
|
+
if (numSpads > 0) {
|
|
97
|
+
tmp = shl(shl(peakCountRate, 8), fracBits);
|
|
98
|
+
tmp = floorDiv(tmp + floorDiv(numSpads, 2), numSpads);
|
|
99
|
+
}
|
|
100
|
+
else {
|
|
101
|
+
tmp = shl(peakCountRate, fracBits);
|
|
102
|
+
}
|
|
103
|
+
return Math.min(tmp, maxOutputValue);
|
|
104
|
+
}
|
|
105
|
+
/** VL53LX_events_per_spad_maths() → kcps per SPAD (BigInt: `total << 11`). */
|
|
106
|
+
export function eventsPerSpadMaths(events, numSpads, duration) {
|
|
107
|
+
let total = 0n;
|
|
108
|
+
if (numSpads !== 0)
|
|
109
|
+
total = bfloorDiv(BigInt(events) * 1000n * 256n, BigInt(numSpads));
|
|
110
|
+
let perSpad;
|
|
111
|
+
if (duration > 0) {
|
|
112
|
+
const d = BigInt(duration);
|
|
113
|
+
perSpad = bfloorDiv((total << 11n) + d / 2n, d);
|
|
114
|
+
}
|
|
115
|
+
else {
|
|
116
|
+
perSpad = total << 11n;
|
|
117
|
+
}
|
|
118
|
+
return Number(perSpad & 0xffffffffn);
|
|
119
|
+
}
|
|
120
|
+
/** VL53LX_range_maths(): phase → millimetres. */
|
|
121
|
+
export function rangeMaths(fastOscFrequency, phase, zeroDistancePhase, fractionalBits, gainFactor, rangeOffsetMm) {
|
|
122
|
+
const pllPeriodUs = calcPllPeriodUs(fastOscFrequency);
|
|
123
|
+
let tmp = phase - zeroDistancePhase;
|
|
124
|
+
tmp = tmp * pllPeriodUs;
|
|
125
|
+
tmp = cdiv(tmp, 2 ** 9);
|
|
126
|
+
tmp = tmp * SPEED_OF_LIGHT_IN_AIR_DIV_8;
|
|
127
|
+
tmp = cdiv(tmp, 2 ** 22);
|
|
128
|
+
let rangeMm = tmp + rangeOffsetMm;
|
|
129
|
+
rangeMm *= gainFactor;
|
|
130
|
+
rangeMm += 0x0400;
|
|
131
|
+
rangeMm = cdiv(rangeMm, 0x0800);
|
|
132
|
+
if (fractionalBits === 0) {
|
|
133
|
+
const rangeMm10 = cdiv(rangeMm * 10, 2 ** 2);
|
|
134
|
+
if (Math.abs(rangeMm10 - cdiv(rangeMm10, 10) * 10) < 5)
|
|
135
|
+
rangeMm = cdiv(rangeMm10, 10);
|
|
136
|
+
else
|
|
137
|
+
rangeMm = cdiv(rangeMm10, 10) + 1;
|
|
138
|
+
}
|
|
139
|
+
else if (fractionalBits === 1) {
|
|
140
|
+
rangeMm = cdiv(rangeMm, 2 ** 1);
|
|
141
|
+
}
|
|
142
|
+
return rangeMm;
|
|
143
|
+
}
|
|
144
|
+
// ── results ──────────────────────────────────────────────────────────────────
|
|
145
|
+
/** `VL53LX_range_data_t`: one target. C field names kept. */
|
|
146
|
+
export class RangeData {
|
|
147
|
+
range_id = 0;
|
|
148
|
+
start_bin = 0; // VL53LX_p_012, window start
|
|
149
|
+
first_bin = 0; // VL53LX_p_019, first bin over threshold
|
|
150
|
+
peak_bin = 0; // VL53LX_p_023
|
|
151
|
+
last_bin = 0; // VL53LX_p_024
|
|
152
|
+
end_bin = 0; // VL53LX_p_013
|
|
153
|
+
width_bins = 0; // VL53LX_p_025, bins over threshold
|
|
154
|
+
window_bins = 0; // VL53LX_p_029, bins in the window
|
|
155
|
+
vcsel_width = 0;
|
|
156
|
+
fast_osc_frequency = 0;
|
|
157
|
+
zero_distance_phase = 0;
|
|
158
|
+
spads = 0; // VL53LX_p_004
|
|
159
|
+
total_periods_elapsed = 0;
|
|
160
|
+
peak_duration_us = 0;
|
|
161
|
+
woi_duration_us = 0;
|
|
162
|
+
ambient_events = 0; // VL53LX_p_016
|
|
163
|
+
total_events = 0; // VL53LX_p_017
|
|
164
|
+
signal_events = 0; // VL53LX_p_010
|
|
165
|
+
peak_signal_count_rate_mcps = 0;
|
|
166
|
+
avg_signal_count_rate_mcps = 0;
|
|
167
|
+
ambient_count_rate_mcps = 0;
|
|
168
|
+
total_rate_per_spad_mcps = 0;
|
|
169
|
+
signal_events_per_spad_kcps = 0; // VL53LX_p_009
|
|
170
|
+
sigma = 0; // VL53LX_p_002, 9.7 fixed point mm
|
|
171
|
+
phase_start = 0; // VL53LX_p_026
|
|
172
|
+
phase_mean = 0; // VL53LX_p_011
|
|
173
|
+
phase_end = 0; // VL53LX_p_027
|
|
174
|
+
min_range_mm = 0;
|
|
175
|
+
median_range_mm = 0;
|
|
176
|
+
max_range_mm = 0;
|
|
177
|
+
range_status = DEVICEERROR_NOUPDATE;
|
|
178
|
+
get sigma_mm() {
|
|
179
|
+
return shr(this.sigma, 7);
|
|
180
|
+
}
|
|
181
|
+
/** peak_signal_count_rate_mcps is 9.7 Mcps. */
|
|
182
|
+
get signal_kcps() {
|
|
183
|
+
return floorDiv(this.peak_signal_count_rate_mcps * 1000, 128);
|
|
184
|
+
}
|
|
185
|
+
get ambient_kcps() {
|
|
186
|
+
return floorDiv(this.ambient_count_rate_mcps * 1000, 128);
|
|
187
|
+
}
|
|
188
|
+
}
|
|
189
|
+
/** `VL53LX_range_results_t`, reduced to the single-zone case. */
|
|
190
|
+
export class RangeResults {
|
|
191
|
+
stream_count = 0;
|
|
192
|
+
targets = []; // VL53LX_p_003, active_results entries
|
|
193
|
+
ambient_dmax_mm = [0, 0, 0, 0, 0]; // VL53LX_p_022
|
|
194
|
+
wrap_dmax_mm = 0;
|
|
195
|
+
}
|
|
196
|
+
/** `VL53LX_hist_pulse_data_t`: one run of bins above the threshold. */
|
|
197
|
+
export class PulseData {
|
|
198
|
+
start_bin = 0; // VL53LX_p_012
|
|
199
|
+
first_bin = 0; // VL53LX_p_019
|
|
200
|
+
peak_bin = 0xff; // VL53LX_p_023
|
|
201
|
+
last_bin = 0; // VL53LX_p_024
|
|
202
|
+
end_bin = 0; // VL53LX_p_013
|
|
203
|
+
width_bins = 0; // VL53LX_p_025
|
|
204
|
+
filter_woi = 0; // VL53LX_p_051, filter half width
|
|
205
|
+
ambient_events = 0; // VL53LX_p_016
|
|
206
|
+
total_events = 0; // VL53LX_p_017
|
|
207
|
+
signal_events = 0; // VL53LX_p_010
|
|
208
|
+
phase_start = 0; // VL53LX_p_026
|
|
209
|
+
phase_mean = 0; // VL53LX_p_011
|
|
210
|
+
phase_end = 0; // VL53LX_p_027
|
|
211
|
+
sigma = 0; // VL53LX_p_002
|
|
212
|
+
}
|
|
213
|
+
const zeros = (n) => new Array(n).fill(0);
|
|
214
|
+
/** `VL53LX_hist_gen3_algo_private_data_t` (reset = VL53LX_f_003). */
|
|
215
|
+
class Gen3Algo {
|
|
216
|
+
first_bin = 0; // VL53LX_p_019
|
|
217
|
+
buffer_size = HISTOGRAM_BUFFER_SIZE; // VL53LX_p_020
|
|
218
|
+
bins_in_data = 0; // VL53LX_p_021
|
|
219
|
+
vcsel_period = 0; // VL53LX_p_030, bins per period
|
|
220
|
+
bins_over_threshold = 0; // VL53LX_p_039
|
|
221
|
+
ambient_per_bin = 0; // VL53LX_p_028
|
|
222
|
+
ambient_threshold = 0; // VL53LX_p_031
|
|
223
|
+
over_threshold = zeros(HISTOGRAM_BUFFER_SIZE); // VL53LX_p_040
|
|
224
|
+
pulse_mask = zeros(HISTOGRAM_BUFFER_SIZE); // VL53LX_p_041
|
|
225
|
+
pulse_no = zeros(HISTOGRAM_BUFFER_SIZE); // VL53LX_p_042
|
|
226
|
+
threshold = zeros(HISTOGRAM_BUFFER_SIZE); // VL53LX_p_052
|
|
227
|
+
first_rising_bin = 0; // VL53LX_p_044
|
|
228
|
+
max_pulses = MAX_PULSES; // VL53LX_p_045
|
|
229
|
+
pulse_count = 0; // VL53LX_p_046
|
|
230
|
+
pulses = Array.from({ length: MAX_PULSES }, () => new PulseData());
|
|
231
|
+
bins = null; // VL53LX_p_006
|
|
232
|
+
xtalk; // VL53LX_p_047
|
|
233
|
+
pulse_amb; // VL53LX_p_048
|
|
234
|
+
pulse_zero; // VL53LX_p_049
|
|
235
|
+
pulse_xtalk; // VL53LX_p_050
|
|
236
|
+
}
|
|
237
|
+
/** `VL53LX_hist_gen4_algo_filtered_data_t`. */
|
|
238
|
+
class Filtered {
|
|
239
|
+
a = zeros(HISTOGRAM_BUFFER_SIZE); // VL53LX_p_007
|
|
240
|
+
b = zeros(HISTOGRAM_BUFFER_SIZE); // VL53LX_p_032
|
|
241
|
+
c = zeros(HISTOGRAM_BUFFER_SIZE); // VL53LX_p_001
|
|
242
|
+
left = zeros(HISTOGRAM_BUFFER_SIZE); // VL53LX_p_053
|
|
243
|
+
right = zeros(HISTOGRAM_BUFFER_SIZE); // VL53LX_p_054
|
|
244
|
+
is_peak = zeros(HISTOGRAM_BUFFER_SIZE); // VL53LX_p_040
|
|
245
|
+
}
|
|
246
|
+
function sumOf(xs) {
|
|
247
|
+
let s = 0;
|
|
248
|
+
for (const x of xs)
|
|
249
|
+
s += x;
|
|
250
|
+
return s;
|
|
251
|
+
}
|
|
252
|
+
// ── bin housekeeping (vl53lx_hist_core.c, vl53lx_core_support.c) ─────────────
|
|
253
|
+
/** VL53LX_f_031: fold the six bin-sequence codes together (repeats averaged). */
|
|
254
|
+
export function averageRepeatedBins(src) {
|
|
255
|
+
const dst = src.clone();
|
|
256
|
+
dst.bins_in_data = 0;
|
|
257
|
+
dst.bin_seq = new Array(MAX_BIN_SEQUENCE_LENGTH).fill(MAX_BIN_SEQUENCE_CODE + 1);
|
|
258
|
+
dst.bin_data = zeros(dst.number_of_bins);
|
|
259
|
+
const initialIndex = zeros(MAX_BIN_SEQUENCE_CODE + 2);
|
|
260
|
+
const repeatCount = zeros(MAX_BIN_SEQUENCE_CODE + 2);
|
|
261
|
+
let seqLength = 0;
|
|
262
|
+
for (let lc = 0; lc < MAX_BIN_SEQUENCE_LENGTH; lc++) {
|
|
263
|
+
const binCfg = src.bin_seq[lc];
|
|
264
|
+
if (repeatCount[binCfg] === 0) {
|
|
265
|
+
initialIndex[binCfg] = seqLength * 4;
|
|
266
|
+
dst.bin_seq[seqLength] = binCfg;
|
|
267
|
+
seqLength += 1;
|
|
268
|
+
}
|
|
269
|
+
repeatCount[binCfg] += 1;
|
|
270
|
+
const base = initialIndex[binCfg];
|
|
271
|
+
for (let i = 0; i < 4; i++)
|
|
272
|
+
dst.bin_data[base + i] += src.bin_data[lc * 4 + i];
|
|
273
|
+
}
|
|
274
|
+
dst.bin_rep = dst.bin_seq.map((c) => (c <= MAX_BIN_SEQUENCE_CODE ? repeatCount[c] : 0));
|
|
275
|
+
dst.bins_in_data = seqLength * 4;
|
|
276
|
+
for (let code = 0; code <= MAX_BIN_SEQUENCE_CODE; code++) {
|
|
277
|
+
const reps = repeatCount[code];
|
|
278
|
+
if (reps > 0) {
|
|
279
|
+
const base = initialIndex[code];
|
|
280
|
+
for (let i = 0; i < 4; i++) {
|
|
281
|
+
dst.bin_data[base + i] = floorDiv(dst.bin_data[base + i] + floorDiv(reps, 2), reps);
|
|
282
|
+
}
|
|
283
|
+
}
|
|
284
|
+
}
|
|
285
|
+
// Codes 7 and 15 are the ambient-only sequence entries.
|
|
286
|
+
dst.number_of_ambient_bins = repeatCount[7] || repeatCount[15] ? 4 : 0;
|
|
287
|
+
return dst;
|
|
288
|
+
}
|
|
289
|
+
/** VL53LX_hist_calc_zero_distance_phase */
|
|
290
|
+
export function calcZeroDistancePhase(bins) {
|
|
291
|
+
const period = 2048 * decodeVcselPeriod(bins.vcsel_period);
|
|
292
|
+
const phase = period + bins.phasecal_result__reference_phase +
|
|
293
|
+
2048 * bins.phasecal_result__vcsel_start - 2048 * bins.cal_config__vcsel_start;
|
|
294
|
+
bins.zero_distance_phase = period ? pymod(phase, period) : 0;
|
|
295
|
+
}
|
|
296
|
+
/** VL53LX_hist_estimate_ambient_from_thresholded_bins() */
|
|
297
|
+
export function estimateAmbientFromThresholdedBins(ambientThresholdSigma, bins) {
|
|
298
|
+
const data = bins.bin_data.slice(0, bins.bins_in_data);
|
|
299
|
+
bins.min_bin_value = data.length ? Math.min(...data) : 0;
|
|
300
|
+
bins.max_bin_value = data.length ? Math.max(...data) : 0;
|
|
301
|
+
let threshold = isqrt(bins.min_bin_value);
|
|
302
|
+
threshold *= ambientThresholdSigma;
|
|
303
|
+
threshold += 0x07;
|
|
304
|
+
threshold = shr(threshold, 4);
|
|
305
|
+
threshold += bins.min_bin_value;
|
|
306
|
+
bins.number_of_ambient_samples = 0;
|
|
307
|
+
bins.ambient_events_sum = 0;
|
|
308
|
+
for (const value of data) {
|
|
309
|
+
if (value < threshold) {
|
|
310
|
+
bins.ambient_events_sum += value;
|
|
311
|
+
bins.number_of_ambient_samples += 1;
|
|
312
|
+
}
|
|
313
|
+
}
|
|
314
|
+
if (bins.number_of_ambient_samples > 0) {
|
|
315
|
+
bins.ambient_per_bin = cdiv(bins.ambient_events_sum + floorDiv(bins.number_of_ambient_samples, 2), bins.number_of_ambient_samples);
|
|
316
|
+
}
|
|
317
|
+
}
|
|
318
|
+
/** VL53LX_hist_estimate_ambient_from_ambient_bins() */
|
|
319
|
+
export function estimateAmbientFromAmbientBins(bins) {
|
|
320
|
+
if (bins.number_of_ambient_bins > 0) {
|
|
321
|
+
bins.number_of_ambient_samples = bins.number_of_ambient_bins;
|
|
322
|
+
bins.ambient_events_sum = sumOf(bins.bin_data.slice(0, bins.number_of_ambient_bins));
|
|
323
|
+
bins.ambient_per_bin = cdiv(bins.ambient_events_sum + floorDiv(bins.number_of_ambient_bins, 2), bins.number_of_ambient_bins);
|
|
324
|
+
}
|
|
325
|
+
}
|
|
326
|
+
/** VL53LX_hist_remove_ambient_bins: drop the ambient-only bins off the front. */
|
|
327
|
+
export function removeAmbientBins(bins) {
|
|
328
|
+
if ((bins.bin_seq[0] & 0x07) === 0x07) {
|
|
329
|
+
const keptSeq = [];
|
|
330
|
+
const keptRep = [];
|
|
331
|
+
for (let lc = 0; lc < MAX_BIN_SEQUENCE_LENGTH; lc++) {
|
|
332
|
+
if ((bins.bin_seq[lc] & 0x07) !== 0x07) {
|
|
333
|
+
keptSeq.push(bins.bin_seq[lc]);
|
|
334
|
+
keptRep.push(bins.bin_rep[lc]);
|
|
335
|
+
}
|
|
336
|
+
}
|
|
337
|
+
const pad = MAX_BIN_SEQUENCE_LENGTH - keptSeq.length;
|
|
338
|
+
bins.bin_seq = keptSeq.concat(new Array(pad).fill(MAX_BIN_SEQUENCE_CODE + 1));
|
|
339
|
+
bins.bin_rep = keptRep.concat(zeros(pad));
|
|
340
|
+
}
|
|
341
|
+
const n = bins.number_of_ambient_bins;
|
|
342
|
+
if (n > 0) {
|
|
343
|
+
bins.bin_data = bins.bin_data.slice(n).concat(zeros(n));
|
|
344
|
+
bins.bins_in_data -= n;
|
|
345
|
+
bins.number_of_ambient_bins = 0;
|
|
346
|
+
}
|
|
347
|
+
}
|
|
348
|
+
/** VL53LX_f_022: the three-tap window sums around `binIndex`. */
|
|
349
|
+
function woiSums(binIndex, filterWoi, bins) {
|
|
350
|
+
let a = 0;
|
|
351
|
+
const b = bins.bin_data[binIndex];
|
|
352
|
+
let c = 0;
|
|
353
|
+
for (let w = 0; w < shl(filterWoi, 1) + 1; w++) {
|
|
354
|
+
const j = pymod(binIndex + w + bins.bins_in_data - filterWoi, bins.bins_in_data);
|
|
355
|
+
if (w < filterWoi)
|
|
356
|
+
a += bins.bin_data[j];
|
|
357
|
+
else if (w > filterWoi)
|
|
358
|
+
c += bins.bin_data[j];
|
|
359
|
+
}
|
|
360
|
+
return [a, b, c];
|
|
361
|
+
}
|
|
362
|
+
/** VL53LX_f_018: event sums → count rates. */
|
|
363
|
+
function calcRates(target, vcselWidth, fastOscFrequency, totalPeriodsElapsed, spads, mergeNb) {
|
|
364
|
+
target.vcsel_width = vcselWidth;
|
|
365
|
+
target.fast_osc_frequency = fastOscFrequency;
|
|
366
|
+
target.total_periods_elapsed = totalPeriodsElapsed;
|
|
367
|
+
target.spads = spads;
|
|
368
|
+
if (fastOscFrequency === 0 || totalPeriodsElapsed === 0)
|
|
369
|
+
return false;
|
|
370
|
+
const pllPeriodUs = calcPllPeriodUs(fastOscFrequency);
|
|
371
|
+
const periodsElapsed = totalPeriodsElapsed + 1;
|
|
372
|
+
target.peak_duration_us = durationMaths(pllPeriodUs, vcselWidth, RANGING_WINDOW_VCSEL_PERIODS, periodsElapsed);
|
|
373
|
+
target.woi_duration_us = durationMaths(pllPeriodUs, shl(target.window_bins, 4), RANGING_WINDOW_VCSEL_PERIODS, periodsElapsed);
|
|
374
|
+
target.peak_signal_count_rate_mcps = rateMaths(target.signal_events, target.peak_duration_us);
|
|
375
|
+
target.avg_signal_count_rate_mcps = rateMaths(target.signal_events, target.woi_duration_us);
|
|
376
|
+
target.ambient_count_rate_mcps = rateMaths(target.ambient_events, target.woi_duration_us);
|
|
377
|
+
let countRateTotal = target.peak_signal_count_rate_mcps + target.ambient_count_rate_mcps;
|
|
378
|
+
if (mergeNb > 1)
|
|
379
|
+
countRateTotal = floorDiv(countRateTotal, mergeNb);
|
|
380
|
+
target.total_rate_per_spad_mcps = ratePerSpadMaths(0x06, countRateTotal, spads, 0xffff);
|
|
381
|
+
target.signal_events_per_spad_kcps = eventsPerSpadMaths(target.signal_events, spads, target.peak_duration_us);
|
|
382
|
+
return true;
|
|
383
|
+
}
|
|
384
|
+
/** VL53LX_f_019 */
|
|
385
|
+
function calcRanges(gainFactor, rangeOffsetMm, target) {
|
|
386
|
+
const r = (phase) => rangeMaths(target.fast_osc_frequency, phase, target.zero_distance_phase, 0, gainFactor, rangeOffsetMm);
|
|
387
|
+
target.min_range_mm = r(target.phase_start);
|
|
388
|
+
target.median_range_mm = r(target.phase_mean);
|
|
389
|
+
target.max_range_mm = r(target.phase_end);
|
|
390
|
+
}
|
|
391
|
+
// ── sigma (vl53lx_sigma_estimate.c) ──────────────────────────────────────────
|
|
392
|
+
/**
|
|
393
|
+
* VL53LX_f_023: the width of the pulse, in 9.7 fixed point millimetres. All
|
|
394
|
+
* in BigInt: the intermediates run up to the 63-bit saturation ceilings.
|
|
395
|
+
*/
|
|
396
|
+
function sigmaEstimate(sigmaRefMm, a, b, c, aZp, cZp, bx, axZp, cxZp, ambientPerBin, fastOscFrequency) {
|
|
397
|
+
if (fastOscFrequency === 0)
|
|
398
|
+
return SIGMA_INVALID;
|
|
399
|
+
const pllPeriodMm = BigInt(calcPllPeriodMm(fastOscFrequency));
|
|
400
|
+
const bMinusAmb = babs(BigInt(ambientPerBin) - BigInt(b));
|
|
401
|
+
const aMinusC = babs(BigInt(a) - BigInt(c));
|
|
402
|
+
if (bMinusAmb === 0n)
|
|
403
|
+
return SIGMA_INVALID;
|
|
404
|
+
let tmp0 = bmin(BigInt(b) + BigInt(bx) + BigInt(ambientPerBin), D_003);
|
|
405
|
+
let tmp1 = (aMinusC * aMinusC) << 8n;
|
|
406
|
+
tmp1 = bmin(tmp1, D_004);
|
|
407
|
+
tmp1 = bfloorDiv(tmp1, bMinusAmb);
|
|
408
|
+
tmp1 = bfloorDiv(tmp1, bMinusAmb);
|
|
409
|
+
tmp1 = bmin(tmp1, D_005);
|
|
410
|
+
tmp0 = tmp1 * tmp0;
|
|
411
|
+
tmp1 = bmin(BigInt(cZp) + BigInt(cxZp) + BigInt(aZp) + BigInt(axZp), D_003) << 8n;
|
|
412
|
+
tmp0 = bmin(tmp1 + tmp0, D_006);
|
|
413
|
+
if (tmp0 > D_007)
|
|
414
|
+
tmp0 = bfloorDiv(tmp0, bMinusAmb) * pllPeriodMm;
|
|
415
|
+
else
|
|
416
|
+
tmp0 = bfloorDiv(tmp0 * pllPeriodMm, bMinusAmb);
|
|
417
|
+
tmp0 = bmin(tmp0, D_006);
|
|
418
|
+
if (tmp0 > D_007)
|
|
419
|
+
tmp0 = bfloorDiv(bfloorDiv(tmp0, bMinusAmb), 4n) * pllPeriodMm;
|
|
420
|
+
else
|
|
421
|
+
tmp0 = bfloorDiv(bfloorDiv(tmp0 * pllPeriodMm, bMinusAmb), 4n);
|
|
422
|
+
tmp0 = bmin(tmp0, D_006);
|
|
423
|
+
tmp0 = bmin(tmp0 >> 2n, D_007);
|
|
424
|
+
tmp1 = BigInt(sigmaRefMm) << 7n;
|
|
425
|
+
tmp0 = bmin(tmp0 + tmp1 * tmp1, D_007);
|
|
426
|
+
// isqrt() masks to uint32; tmp0 is already <= D_007 and non-negative.
|
|
427
|
+
return Number(isqrtBig(tmp0 & 0xffffffffn));
|
|
428
|
+
}
|
|
429
|
+
/** VL53LX_f_014: the sigma of one pulse, from its padded copies. */
|
|
430
|
+
function pulseSigma(pulse, sigmaRefMm, algo, xtalkEnable) {
|
|
431
|
+
if (algo.vcsel_period === 0)
|
|
432
|
+
return SIGMA_INVALID;
|
|
433
|
+
const i = pymod(pulse.peak_bin, algo.vcsel_period);
|
|
434
|
+
const [aZp, , cZp] = woiSums(i, pulse.filter_woi, algo.pulse_zero);
|
|
435
|
+
const [a, b, c] = woiSums(i, pulse.filter_woi, algo.pulse_amb);
|
|
436
|
+
let ax = 0;
|
|
437
|
+
let bx = 0;
|
|
438
|
+
let cx = 0;
|
|
439
|
+
if (xtalkEnable)
|
|
440
|
+
[ax, bx, cx] = woiSums(i, pulse.filter_woi, algo.pulse_xtalk);
|
|
441
|
+
return sigmaEstimate(sigmaRefMm, a, b, c, aZp, cZp, bx, ax, cx, algo.pulse_amb.ambient_per_bin, algo.pulse_amb.fast_osc_frequency);
|
|
442
|
+
}
|
|
443
|
+
// ── pulse detection (vl53lx_hist_algos_gen3.c) ───────────────────────────────
|
|
444
|
+
/** VL53LX_f_006: a detection threshold per bin, and the over-threshold flags. */
|
|
445
|
+
function ambientThresholds(scaler, thresholdSigma, minThresholdEvents, xtalkEnable, bins, algo) {
|
|
446
|
+
algo.buffer_size = bins.number_of_bins;
|
|
447
|
+
algo.first_bin = bins.first_bin;
|
|
448
|
+
algo.bins_in_data = bins.bins_in_data;
|
|
449
|
+
algo.ambient_per_bin = bins.ambient_per_bin;
|
|
450
|
+
algo.vcsel_period = decodeVcselPeriod(bins.vcsel_period);
|
|
451
|
+
const ambEvents = cdiv(bins.ambient_per_bin * scaler + 2048, 4096);
|
|
452
|
+
for (let lb = 0; lb < bins.bins_in_data; lb++) {
|
|
453
|
+
const samples = bins.bin_rep[lb >> 2];
|
|
454
|
+
if (samples <= 0)
|
|
455
|
+
continue;
|
|
456
|
+
let value;
|
|
457
|
+
if (lb < algo.xtalk.bins_in_data && xtalkEnable) {
|
|
458
|
+
value = samples * (ambEvents + algo.xtalk.bin_data[lb]);
|
|
459
|
+
}
|
|
460
|
+
else {
|
|
461
|
+
value = samples * ambEvents;
|
|
462
|
+
}
|
|
463
|
+
value = isqrt(value);
|
|
464
|
+
value += floorDiv(samples, 2);
|
|
465
|
+
value = floorDiv(value, samples);
|
|
466
|
+
value *= thresholdSigma;
|
|
467
|
+
value += 8;
|
|
468
|
+
value = floorDiv(value, 16);
|
|
469
|
+
value += ambEvents;
|
|
470
|
+
value = Math.max(value, minThresholdEvents);
|
|
471
|
+
algo.threshold[lb] = value;
|
|
472
|
+
algo.ambient_threshold = value;
|
|
473
|
+
}
|
|
474
|
+
algo.bins_over_threshold = 0;
|
|
475
|
+
for (let lb = bins.first_bin; lb < bins.bins_in_data; lb++) {
|
|
476
|
+
const over = bins.bin_data[lb] > algo.threshold[lb] ? 1 : 0;
|
|
477
|
+
algo.over_threshold[lb] = over;
|
|
478
|
+
algo.pulse_mask[lb] = over;
|
|
479
|
+
algo.bins_over_threshold += over;
|
|
480
|
+
}
|
|
481
|
+
}
|
|
482
|
+
/** VL53LX_f_007: the first quiet-to-loud step. */
|
|
483
|
+
function findFirstRisingEdge(algo) {
|
|
484
|
+
algo.first_rising_bin = 0;
|
|
485
|
+
let found = false;
|
|
486
|
+
for (let i = 0; i < algo.vcsel_period; i++) {
|
|
487
|
+
const j = pymod(i + 1, algo.vcsel_period);
|
|
488
|
+
if (i < algo.bins_in_data && j < algo.bins_in_data) {
|
|
489
|
+
if (algo.pulse_mask[i] === 0 && algo.pulse_mask[j] === 1 && !found) {
|
|
490
|
+
algo.first_rising_bin = i;
|
|
491
|
+
found = true;
|
|
492
|
+
}
|
|
493
|
+
}
|
|
494
|
+
}
|
|
495
|
+
}
|
|
496
|
+
/** VL53LX_f_008: number the runs of over-threshold bins. */
|
|
497
|
+
function assignPulseNumbers(algo) {
|
|
498
|
+
for (let lb = algo.first_rising_bin; lb < algo.first_rising_bin + algo.vcsel_period; lb++) {
|
|
499
|
+
const i = pymod(lb, algo.vcsel_period);
|
|
500
|
+
const j = pymod(lb + 1, algo.vcsel_period);
|
|
501
|
+
if (!(i < algo.bins_in_data && j < algo.bins_in_data))
|
|
502
|
+
continue;
|
|
503
|
+
if (algo.pulse_mask[i] === 0 && algo.pulse_mask[j] === 1)
|
|
504
|
+
algo.pulse_count += 1;
|
|
505
|
+
algo.pulse_count = Math.min(algo.pulse_count, algo.max_pulses);
|
|
506
|
+
algo.pulse_no[i] = algo.pulse_mask[i] > 0 ? algo.pulse_count : 0;
|
|
507
|
+
}
|
|
508
|
+
}
|
|
509
|
+
/** VL53LX_f_009: each numbered run → a pulse window. */
|
|
510
|
+
function pulseExtents(algo) {
|
|
511
|
+
const maxFilterHalfWidth = shr(algo.vcsel_period - 1, 1);
|
|
512
|
+
for (let blb = algo.first_rising_bin; blb < algo.first_rising_bin + algo.vcsel_period; blb++) {
|
|
513
|
+
const i = pymod(blb, algo.vcsel_period);
|
|
514
|
+
const j = pymod(blb + 1, algo.vcsel_period);
|
|
515
|
+
if (!(i < algo.bins_in_data && j < algo.bins_in_data))
|
|
516
|
+
continue;
|
|
517
|
+
if (algo.pulse_no[i] === 0 && algo.pulse_no[j] > 0) {
|
|
518
|
+
const no = algo.pulse_no[j] - 1;
|
|
519
|
+
if (no < algo.max_pulses) {
|
|
520
|
+
const pulse = algo.pulses[no];
|
|
521
|
+
pulse.start_bin = blb;
|
|
522
|
+
pulse.first_bin = blb + 1;
|
|
523
|
+
pulse.peak_bin = 0xff;
|
|
524
|
+
pulse.last_bin = 0;
|
|
525
|
+
pulse.end_bin = 0;
|
|
526
|
+
}
|
|
527
|
+
}
|
|
528
|
+
if (algo.pulse_no[i] > 0 && algo.pulse_no[j] === 0) {
|
|
529
|
+
const no = algo.pulse_no[i] - 1;
|
|
530
|
+
if (no < algo.max_pulses) {
|
|
531
|
+
const pulse = algo.pulses[no];
|
|
532
|
+
pulse.last_bin = blb;
|
|
533
|
+
pulse.end_bin = blb + 1;
|
|
534
|
+
pulse.width_bins = pulse.last_bin + 1 - pulse.first_bin;
|
|
535
|
+
pulse.filter_woi = Math.min(pulse.end_bin + 1 - pulse.start_bin, maxFilterHalfWidth);
|
|
536
|
+
}
|
|
537
|
+
}
|
|
538
|
+
}
|
|
539
|
+
}
|
|
540
|
+
/** VL53LX_f_010: total and ambient events inside the pulse window. */
|
|
541
|
+
function pulseEventSums(pulseNo, bins, algo) {
|
|
542
|
+
const pulse = algo.pulses[pulseNo];
|
|
543
|
+
pulse.total_events = 0;
|
|
544
|
+
pulse.ambient_events = 0;
|
|
545
|
+
for (let lb = pulse.start_bin; lb <= pulse.end_bin; lb++) {
|
|
546
|
+
const i = pymod(lb, algo.vcsel_period);
|
|
547
|
+
pulse.total_events += bins.bin_data[i];
|
|
548
|
+
pulse.ambient_events += algo.ambient_per_bin;
|
|
549
|
+
}
|
|
550
|
+
pulse.signal_events = pulse.total_events - pulse.ambient_events;
|
|
551
|
+
}
|
|
552
|
+
/** VL53LX_f_011: a copy of the frame with everything outside the pulse padded. */
|
|
553
|
+
function isolatePulse(pulseNo, bins, algo, padValue) {
|
|
554
|
+
const pulse = algo.pulses[pulseNo];
|
|
555
|
+
const out = bins.clone();
|
|
556
|
+
for (let lb = algo.first_rising_bin; lb < algo.first_rising_bin + algo.vcsel_period; lb++) {
|
|
557
|
+
if (lb < pulse.start_bin || lb > pulse.end_bin) {
|
|
558
|
+
const i = pymod(lb, algo.vcsel_period);
|
|
559
|
+
if (i < out.bins_in_data)
|
|
560
|
+
out.bin_data[i] = padValue;
|
|
561
|
+
}
|
|
562
|
+
}
|
|
563
|
+
return out;
|
|
564
|
+
}
|
|
565
|
+
/** VL53LX_f_020: centre of mass of the bins in [start, end], 1/2048 bin. */
|
|
566
|
+
function weightedPhase(start, end, vcselPeriod, clipEvents, bins) {
|
|
567
|
+
let eventSum = 0;
|
|
568
|
+
let weightedSum = 0;
|
|
569
|
+
if (vcselPeriod === 0)
|
|
570
|
+
return MAX_ALLOWED_PHASE;
|
|
571
|
+
for (let lb = start; lb <= end; lb++) {
|
|
572
|
+
const i = lb < 0 ? lb + vcselPeriod : pymod(lb, vcselPeriod);
|
|
573
|
+
if (i >= 0 && i < HISTOGRAM_BUFFER_SIZE) {
|
|
574
|
+
let value = bins.bin_data[i] - bins.ambient_per_bin;
|
|
575
|
+
if (clipEvents && value < 0)
|
|
576
|
+
value = 0;
|
|
577
|
+
eventSum += value;
|
|
578
|
+
weightedSum += value * (1024 + 2048 * lb);
|
|
579
|
+
}
|
|
580
|
+
}
|
|
581
|
+
if (eventSum > 0) {
|
|
582
|
+
weightedSum += cdiv(eventSum, 2);
|
|
583
|
+
weightedSum = cdiv(weightedSum, eventSum);
|
|
584
|
+
return Math.max(weightedSum, 0);
|
|
585
|
+
}
|
|
586
|
+
return MAX_ALLOWED_PHASE;
|
|
587
|
+
}
|
|
588
|
+
/** VL53LX_f_015: the near and far phase of the pulse. */
|
|
589
|
+
function pulsePhaseLimits(pulseNo, clipEvents, bins, algo) {
|
|
590
|
+
const pulse = algo.pulses[pulseNo];
|
|
591
|
+
if (pulse.peak_bin === 0xff)
|
|
592
|
+
pulse.peak_bin = 1;
|
|
593
|
+
const i = pymod(pulse.peak_bin, algo.vcsel_period);
|
|
594
|
+
const start = i + pulse.start_bin - pulse.peak_bin;
|
|
595
|
+
const end = i + pulse.end_bin - pulse.peak_bin;
|
|
596
|
+
const windowWidth = Math.min(end - start, 3);
|
|
597
|
+
pulse.phase_start = weightedPhase(start, start + windowWidth, algo.vcsel_period, clipEvents, bins);
|
|
598
|
+
pulse.phase_end = weightedPhase(end - windowWidth, end, algo.vcsel_period, clipEvents, bins);
|
|
599
|
+
if (pulse.phase_start > pulse.phase_end) {
|
|
600
|
+
[pulse.phase_start, pulse.phase_end] = [pulse.phase_end, pulse.phase_start];
|
|
601
|
+
}
|
|
602
|
+
pulse.phase_start = Math.min(pulse.phase_start, pulse.phase_mean);
|
|
603
|
+
pulse.phase_end = Math.max(pulse.phase_end, pulse.phase_mean);
|
|
604
|
+
}
|
|
605
|
+
/** VL53LX_f_016: strongest first or closest first (stable, as Python's sort). */
|
|
606
|
+
function sortPulses(targetOrder, algo) {
|
|
607
|
+
if (algo.pulse_count <= 1)
|
|
608
|
+
return;
|
|
609
|
+
const pulses = algo.pulses.slice(0, algo.pulse_count);
|
|
610
|
+
if (targetOrder === HIST_TARGET_ORDER_STRONGEST_FIRST) {
|
|
611
|
+
pulses.sort((p, q) => q.signal_events - p.signal_events);
|
|
612
|
+
}
|
|
613
|
+
else {
|
|
614
|
+
pulses.sort((p, q) => p.phase_mean - q.phase_mean);
|
|
615
|
+
}
|
|
616
|
+
algo.pulses.splice(0, algo.pulse_count, ...pulses);
|
|
617
|
+
}
|
|
618
|
+
/** VL53LX_f_017: pulse → target, plus the sigma and phase-window checks. */
|
|
619
|
+
function fillTarget(rangeId, validPhaseLow, validPhaseHigh, sigmaThresh, bins, pulse, target) {
|
|
620
|
+
target.range_id = rangeId;
|
|
621
|
+
target.start_bin = pulse.start_bin;
|
|
622
|
+
target.first_bin = pulse.first_bin;
|
|
623
|
+
target.peak_bin = pulse.peak_bin;
|
|
624
|
+
target.last_bin = pulse.last_bin;
|
|
625
|
+
target.end_bin = pulse.end_bin;
|
|
626
|
+
target.width_bins = pulse.width_bins;
|
|
627
|
+
target.window_bins = pulse.end_bin + 1 - pulse.start_bin;
|
|
628
|
+
target.zero_distance_phase = bins.zero_distance_phase;
|
|
629
|
+
target.sigma = pulse.sigma;
|
|
630
|
+
target.phase_start = pymod(pulse.phase_start, 0x10000);
|
|
631
|
+
target.phase_mean = pymod(pulse.phase_mean, 0x10000);
|
|
632
|
+
target.phase_end = pymod(pulse.phase_end, 0x10000);
|
|
633
|
+
target.total_events = pulse.total_events;
|
|
634
|
+
target.signal_events = pulse.signal_events;
|
|
635
|
+
target.ambient_events = pulse.ambient_events;
|
|
636
|
+
target.total_periods_elapsed = bins.total_periods_elapsed;
|
|
637
|
+
target.range_status = DEVICEERROR_RANGECOMPLETE_NO_WRAP_CHECK;
|
|
638
|
+
if (sigmaThresh > 0 && pulse.sigma > shl(sigmaThresh, 5)) {
|
|
639
|
+
target.range_status = DEVICEERROR_SIGMATHRESHOLDCHECK;
|
|
640
|
+
}
|
|
641
|
+
let lower = pymod(shl(validPhaseLow, 8), 0x10000);
|
|
642
|
+
lower = lower < target.zero_distance_phase ? target.zero_distance_phase - lower : 0;
|
|
643
|
+
const upper = pymod(shl(validPhaseHigh, 8) + bins.zero_distance_phase, 0x10000);
|
|
644
|
+
if (target.phase_mean < lower || target.phase_mean > upper) {
|
|
645
|
+
target.range_status = DEVICEERROR_RANGEPHASECHECK;
|
|
646
|
+
}
|
|
647
|
+
}
|
|
648
|
+
// ── peak location (vl53lx_hist_algos_gen4.c) ─────────────────────────────────
|
|
649
|
+
/** VL53LX_f_026: the three-tap window over the pulse. */
|
|
650
|
+
function filterPulse(pulseNo, pulseBins, algo, filtered) {
|
|
651
|
+
const pulse = algo.pulses[pulseNo];
|
|
652
|
+
for (let lb = pulse.start_bin; lb <= pulse.end_bin; lb++) {
|
|
653
|
+
const i = pymod(lb, algo.vcsel_period);
|
|
654
|
+
const [a, b, c] = woiSums(i, pulse.filter_woi, pulseBins);
|
|
655
|
+
filtered.a[i] = a;
|
|
656
|
+
filtered.b[i] = b;
|
|
657
|
+
filtered.c[i] = c;
|
|
658
|
+
filtered.left[i] = a + b - (c + algo.ambient_per_bin);
|
|
659
|
+
filtered.right[i] = b + c - (a + algo.ambient_per_bin);
|
|
660
|
+
}
|
|
661
|
+
}
|
|
662
|
+
/**
|
|
663
|
+
* VL53LX_f_028: sub-bin phase from the window sums. null when the pulse has
|
|
664
|
+
* no height (the C division by zero → "not a peak after all"). BigInt: the
|
|
665
|
+
* 4096 * 4096 scaling can pass 2^53.
|
|
666
|
+
*/
|
|
667
|
+
function interpolatePhase(binIndex, a, b, c, ambientPerBin, vcselPeriod) {
|
|
668
|
+
const numerator = 4096n * (BigInt(c) - BigInt(a));
|
|
669
|
+
const halfBMinusAmb = 4096n * (BigInt(b) - BigInt(ambientPerBin));
|
|
670
|
+
if (halfBMinusAmb === 0n)
|
|
671
|
+
return null;
|
|
672
|
+
let meanPhase = 4096n * numerator + halfBMinusAmb;
|
|
673
|
+
meanPhase = bcdiv(meanPhase, halfBMinusAmb * 2n);
|
|
674
|
+
meanPhase += 2048n;
|
|
675
|
+
meanPhase += 4096n * BigInt(binIndex);
|
|
676
|
+
meanPhase = bcdiv(meanPhase + 1n, 2n);
|
|
677
|
+
if (meanPhase < 0n)
|
|
678
|
+
meanPhase = 0n;
|
|
679
|
+
if (meanPhase > BigInt(MAX_ALLOWED_PHASE))
|
|
680
|
+
meanPhase = BigInt(MAX_ALLOWED_PHASE);
|
|
681
|
+
return pymod(Number(meanPhase), vcselPeriod * 2048);
|
|
682
|
+
}
|
|
683
|
+
/** VL53LX_f_027: keep the last bin where the filter says the peak is here. */
|
|
684
|
+
function findPeakBin(pulseNo, filtered, algo) {
|
|
685
|
+
const pulse = algo.pulses[pulseNo];
|
|
686
|
+
for (let lb = pulse.start_bin; lb < pulse.end_bin; lb++) {
|
|
687
|
+
const i = pymod(lb, algo.vcsel_period);
|
|
688
|
+
const j = pymod(lb + 1, algo.vcsel_period);
|
|
689
|
+
if (!(i < algo.bins_in_data && j < algo.bins_in_data))
|
|
690
|
+
continue;
|
|
691
|
+
const L = filtered.left;
|
|
692
|
+
const R = filtered.right;
|
|
693
|
+
let isPeak;
|
|
694
|
+
if (L[i] === 0 && R[i] === 0)
|
|
695
|
+
isPeak = 0;
|
|
696
|
+
else if (L[i] >= 0 && R[i] >= 0)
|
|
697
|
+
isPeak = 1;
|
|
698
|
+
else if (L[i] < 0 && R[i] >= 0 && L[j] >= 0 && R[j] < 0)
|
|
699
|
+
isPeak = 1;
|
|
700
|
+
else
|
|
701
|
+
isPeak = 0;
|
|
702
|
+
filtered.is_peak[i] = isPeak;
|
|
703
|
+
if (isPeak) {
|
|
704
|
+
pulse.peak_bin = lb;
|
|
705
|
+
const phase = interpolatePhase(lb, filtered.a[i], filtered.b[i], filtered.c[i], algo.ambient_per_bin, algo.vcsel_period);
|
|
706
|
+
if (phase === null)
|
|
707
|
+
filtered.is_peak[i] = 0;
|
|
708
|
+
else
|
|
709
|
+
pulse.phase_mean = phase;
|
|
710
|
+
}
|
|
711
|
+
}
|
|
712
|
+
}
|
|
713
|
+
/**
|
|
714
|
+
* `VL53LX_f_001` — the ambient-limited maximum range — deliberately a stub
|
|
715
|
+
* (see the Python docstring): our boards ship the FMT 140MM_DARK region
|
|
716
|
+
* blank, so a faithful port returns this same zero on every frame.
|
|
717
|
+
*/
|
|
718
|
+
function ambientDmax(results) {
|
|
719
|
+
results.ambient_dmax_mm = [0, 0, 0, 0, 0];
|
|
720
|
+
}
|
|
721
|
+
// ── entry point ──────────────────────────────────────────────────────────────
|
|
722
|
+
/**
|
|
723
|
+
* `VL53LX_hist_process_data()` + `VL53LX_f_025`, with crosstalk off.
|
|
724
|
+
* `binsInput` is what `BareDriver.getHistogramBinData()` returns; `hpp` is the
|
|
725
|
+
* driver's `HistPostProcessConfig`. Pure (does not modify `binsInput`).
|
|
726
|
+
*/
|
|
727
|
+
export function processData(binsInput, hpp, mergeNb = 1) {
|
|
728
|
+
if (hpp.algo__crosstalk_compensation_enable) {
|
|
729
|
+
throw new Error("crosstalk compensation is not implemented (E8.6)");
|
|
730
|
+
}
|
|
731
|
+
const algo = new Gen3Algo();
|
|
732
|
+
const filtered = new Filtered();
|
|
733
|
+
const results = new RangeResults();
|
|
734
|
+
const bins = averageRepeatedBins(binsInput); // VL53LX_f_031
|
|
735
|
+
algo.bins = bins;
|
|
736
|
+
// The crosstalk histogram, zero everywhere while compensation is off.
|
|
737
|
+
algo.xtalk = bins.clone();
|
|
738
|
+
algo.xtalk.bin_data = zeros(bins.number_of_bins);
|
|
739
|
+
results.stream_count = binsInput.result__stream_count;
|
|
740
|
+
calcZeroDistancePhase(bins);
|
|
741
|
+
estimateAmbientFromThresholdedBins(hpp.ambient_thresh_sigma0, bins);
|
|
742
|
+
estimateAmbientFromAmbientBins(bins);
|
|
743
|
+
removeAmbientBins(bins);
|
|
744
|
+
ambientDmax(results);
|
|
745
|
+
ambientThresholds(hpp.ambient_thresh_events_scaler, hpp.ambient_thresh_sigma1, hpp.min_ambient_thresh_events, 0, bins, algo);
|
|
746
|
+
findFirstRisingEdge(algo);
|
|
747
|
+
assignPulseNumbers(algo);
|
|
748
|
+
pulseExtents(algo);
|
|
749
|
+
for (let p = 0; p < algo.pulse_count; p++) {
|
|
750
|
+
const pulse = algo.pulses[p];
|
|
751
|
+
pulseEventSums(p, bins, algo);
|
|
752
|
+
algo.pulse_amb = isolatePulse(p, bins, algo, bins.ambient_per_bin);
|
|
753
|
+
algo.pulse_zero = isolatePulse(p, bins, algo, 0);
|
|
754
|
+
algo.pulse_xtalk = isolatePulse(p, algo.xtalk, algo, 0);
|
|
755
|
+
filterPulse(p, algo.pulse_amb, algo, filtered);
|
|
756
|
+
findPeakBin(p, filtered, algo);
|
|
757
|
+
pulse.sigma = pulseSigma(pulse, hpp.sigma_estimator__sigma_ref_mm, algo, 0);
|
|
758
|
+
pulsePhaseLimits(p, 1, bins, algo);
|
|
759
|
+
}
|
|
760
|
+
sortPulses(hpp.hist_target_order, algo);
|
|
761
|
+
for (let p = 0; p < algo.pulse_count; p++) {
|
|
762
|
+
if (results.targets.length >= MAX_RANGE_RESULTS)
|
|
763
|
+
break;
|
|
764
|
+
const pulse = algo.pulses[p];
|
|
765
|
+
if (!(pulse.signal_events > hpp.signal_total_events_limit && pulse.peak_bin < 0xff))
|
|
766
|
+
continue;
|
|
767
|
+
const target = new RangeData();
|
|
768
|
+
fillTarget(results.targets.length, hpp.valid_phase_low, hpp.valid_phase_high, hpp.sigma_thresh, bins, pulse, target);
|
|
769
|
+
calcRates(target, bins.vcsel_width, bins.fast_osc_frequency, bins.total_periods_elapsed, bins.result__dss_actual_effective_spads, mergeNb);
|
|
770
|
+
calcRanges(hpp.gain_factor, hpp.range_offset_mm, target);
|
|
771
|
+
results.targets.push(target);
|
|
772
|
+
}
|
|
773
|
+
return results;
|
|
774
|
+
}
|
|
775
|
+
/**
|
|
776
|
+
* The frame before this one, reduced to what the consistency checks read
|
|
777
|
+
* (`VL53LX_zone_hist_info_t` / `VL53LX_zone_objects_t` for our single zone).
|
|
778
|
+
* The driver owns one and hands it every frame, exactly once.
|
|
779
|
+
*/
|
|
780
|
+
export class FrameHistory {
|
|
781
|
+
rd_device_state = null;
|
|
782
|
+
total_periods_elapsed = 0;
|
|
783
|
+
spads = 0;
|
|
784
|
+
targets = [];
|
|
785
|
+
/** Forget the stream so far; the next `apply()` only remembers. */
|
|
786
|
+
reset() {
|
|
787
|
+
this.rd_device_state = null;
|
|
788
|
+
this.total_periods_elapsed = 0;
|
|
789
|
+
this.spads = 0;
|
|
790
|
+
this.targets = [];
|
|
791
|
+
}
|
|
792
|
+
/** Check `results` against the remembered frame, then remember it (C order). */
|
|
793
|
+
apply(results, bins, rdDeviceState, hpp) {
|
|
794
|
+
this.phaseConsistencyCheck(results, hpp);
|
|
795
|
+
this.rd_device_state = rdDeviceState;
|
|
796
|
+
this.total_periods_elapsed = bins.total_periods_elapsed;
|
|
797
|
+
this.spads = bins.result__dss_actual_effective_spads;
|
|
798
|
+
this.targets = results.targets.map((t) => ({
|
|
799
|
+
ambient_events: t.ambient_events,
|
|
800
|
+
total_events: t.total_events,
|
|
801
|
+
phase_mean: t.phase_mean,
|
|
802
|
+
range_status: t.range_status,
|
|
803
|
+
}));
|
|
804
|
+
}
|
|
805
|
+
/** `VL53LX_hist_phase_consistency_check()`. */
|
|
806
|
+
phaseConsistencyCheck(results, hpp) {
|
|
807
|
+
// The tolerance is a whole phase unit, and phases here are 8.8.
|
|
808
|
+
const phaseTolerance = shl(hpp.algo__consistency_check__phase_tolerance, 8);
|
|
809
|
+
if (this.rd_device_state !== DEVICESTATE_RANGING_OUTPUT_DATA)
|
|
810
|
+
return;
|
|
811
|
+
if (phaseTolerance === 0)
|
|
812
|
+
return;
|
|
813
|
+
const eventSigma = hpp.algo__consistency_check__event_sigma;
|
|
814
|
+
const minSpads = hpp.algo__consistency_check__event_min_spad_count;
|
|
815
|
+
const minMaxTolerance = hpp.algo__consistency_check__min_max_tolerance;
|
|
816
|
+
for (const target of results.targets) {
|
|
817
|
+
if (target.range_status !== DEVICEERROR_RANGECOMPLETE &&
|
|
818
|
+
target.range_status !== DEVICEERROR_RANGECOMPLETE_NO_WRAP_CHECK) {
|
|
819
|
+
continue;
|
|
820
|
+
}
|
|
821
|
+
target.range_status = this.targets.length
|
|
822
|
+
? DEVICEERROR_PHASECONSISTENCY
|
|
823
|
+
: DEVICEERROR_PREV_RANGE_NO_TARGETS;
|
|
824
|
+
for (const prev of this.targets) {
|
|
825
|
+
if (Math.abs(target.phase_mean - prev.phase_mean) >= phaseTolerance)
|
|
826
|
+
continue;
|
|
827
|
+
let status = eventsConsistencyCheck(eventSigma, minSpads, this, prev, target);
|
|
828
|
+
if (status === DEVICEERROR_RANGECOMPLETE)
|
|
829
|
+
status = mergedPulseCheck(minMaxTolerance, target);
|
|
830
|
+
target.range_status = status;
|
|
831
|
+
}
|
|
832
|
+
}
|
|
833
|
+
}
|
|
834
|
+
}
|
|
835
|
+
/**
|
|
836
|
+
* `VL53LX_hist_events_consistency_check()` → a device error code. BigInt
|
|
837
|
+
* throughout: the scaler squared is unbounded in Python.
|
|
838
|
+
*/
|
|
839
|
+
function eventsConsistencyCheck(eventSigma, minEffectiveSpadCount, prevFrame, prev, target) {
|
|
840
|
+
if (eventSigma === 0)
|
|
841
|
+
return DEVICEERROR_RANGECOMPLETE;
|
|
842
|
+
const tmpp = BigInt(1 + prevFrame.total_periods_elapsed) * BigInt(prevFrame.spads);
|
|
843
|
+
const tmpc = BigInt(1 + target.total_periods_elapsed) * BigInt(target.spads);
|
|
844
|
+
let eventsScaler = tmpp * 4096n;
|
|
845
|
+
if (tmpc !== 0n)
|
|
846
|
+
eventsScaler = bcdiv(eventsScaler + bfloorDiv(tmpc, 2n), tmpc);
|
|
847
|
+
const eventsScalerSq = bcdiv(eventsScaler * eventsScaler + 2048n, 4096n);
|
|
848
|
+
const cSignalEvents = bcdiv((BigInt(target.total_events) - BigInt(target.ambient_events)) * eventsScaler + 2048n, 4096n);
|
|
849
|
+
const cSigNoiseSq = bcdiv(eventsScalerSq * BigInt(target.total_events) + 2048n, 4096n);
|
|
850
|
+
let cAmbNoiseSq = bcdiv(eventsScalerSq * BigInt(target.ambient_events) + 2048n, 4096n);
|
|
851
|
+
// Ambient lands in the estimate a quarter of its weight.
|
|
852
|
+
cAmbNoiseSq = bcdiv(cAmbNoiseSq + 2n, 4n);
|
|
853
|
+
const pAmbNoiseSq = bcdiv(BigInt(prev.ambient_events) + 2n, 4n);
|
|
854
|
+
// Python `& 0xFFFFFFFF` on a possibly negative int: two's complement view.
|
|
855
|
+
const noiseSqSum = (BigInt(prev.total_events) + cSigNoiseSq + pAmbNoiseSq + cAmbNoiseSq) & 0xffffffffn;
|
|
856
|
+
// isqrt() masks its argument to uint32.
|
|
857
|
+
let tolerance = isqrtBig((noiseSqSum * 16n) & 0xffffffffn);
|
|
858
|
+
tolerance = bcdiv(tolerance * BigInt(eventSigma) + 32n, 64n);
|
|
859
|
+
const pSignalEvents = BigInt(prev.total_events) - BigInt(prev.ambient_events);
|
|
860
|
+
const delta = babs(cSignalEvents - pSignalEvents);
|
|
861
|
+
if (delta > tolerance && target.spads > minEffectiveSpadCount) {
|
|
862
|
+
return DEVICEERROR_EVENTCONSISTENCY;
|
|
863
|
+
}
|
|
864
|
+
return DEVICEERROR_RANGECOMPLETE;
|
|
865
|
+
}
|
|
866
|
+
/** `VL53LX_hist_merged_pulse_check()`. */
|
|
867
|
+
function mergedPulseCheck(minMaxToleranceMm, target) {
|
|
868
|
+
const deltaMm = Math.abs(target.max_range_mm - target.min_range_mm);
|
|
869
|
+
if (minMaxToleranceMm > 0 && deltaMm > minMaxToleranceMm) {
|
|
870
|
+
return DEVICEERROR_RANGECOMPLETE_MERGED_PULSE;
|
|
871
|
+
}
|
|
872
|
+
return DEVICEERROR_RANGECOMPLETE;
|
|
873
|
+
}
|
|
874
|
+
// ── the family-wide result shape ─────────────────────────────────────────────
|
|
875
|
+
const CONVERT_STATUS = {
|
|
876
|
+
[DEVICEERROR_RANGEPHASECHECK]: RANGESTATUS_OUTOFBOUNDS_FAIL,
|
|
877
|
+
[DEVICEERROR_SIGMATHRESHOLDCHECK]: RANGESTATUS_SIGMA_FAIL,
|
|
878
|
+
[DEVICEERROR_RANGECOMPLETE_NO_WRAP_CHECK]: RANGESTATUS_RANGE_VALID_NO_WRAP_CHECK,
|
|
879
|
+
[DEVICEERROR_PHASECONSISTENCY]: RANGESTATUS_WRAP_TARGET_FAIL,
|
|
880
|
+
[DEVICEERROR_EVENTCONSISTENCY]: RANGESTATUS_WRAP_TARGET_FAIL,
|
|
881
|
+
[DEVICEERROR_PREV_RANGE_NO_TARGETS]: RANGESTATUS_TARGET_PRESENT_LACK_OF_SIGNAL,
|
|
882
|
+
[DEVICEERROR_RANGECOMPLETE_MERGED_PULSE]: RANGESTATUS_RANGE_VALID_MERGED_PULSE,
|
|
883
|
+
[DEVICEERROR_RANGECOMPLETE]: RANGESTATUS_RANGE_VALID,
|
|
884
|
+
};
|
|
885
|
+
/** ConvertStatusHisto() (vl53lx_api.c), the cases the histogram path produces. */
|
|
886
|
+
export function convertStatus(deviceError) {
|
|
887
|
+
return CONVERT_STATUS[deviceError] ?? RANGESTATUS_NONE;
|
|
888
|
+
}
|
|
889
|
+
/** UM2931 range status names, as uld/l4 spells them. */
|
|
890
|
+
export const STATUS_NAMES = {
|
|
891
|
+
[RANGESTATUS_RANGE_VALID]: "valid",
|
|
892
|
+
[RANGESTATUS_SIGMA_FAIL]: "sigma above threshold",
|
|
893
|
+
[RANGESTATUS_OUTOFBOUNDS_FAIL]: "phase out of valid limit",
|
|
894
|
+
[RANGESTATUS_RANGE_VALID_NO_WRAP_CHECK]: "no wrap-around check done",
|
|
895
|
+
[RANGESTATUS_WRAP_TARGET_FAIL]: "wrapped target",
|
|
896
|
+
[RANGESTATUS_RANGE_VALID_MERGED_PULSE]: "valid, merged pulse",
|
|
897
|
+
[RANGESTATUS_TARGET_PRESENT_LACK_OF_SIGNAL]: "no target in the frame before",
|
|
898
|
+
[RANGESTATUS_NONE]: "no target",
|
|
899
|
+
};
|
|
900
|
+
function toTarget(data) {
|
|
901
|
+
const status = convertStatus(data.range_status);
|
|
902
|
+
return {
|
|
903
|
+
distanceMm: data.median_range_mm,
|
|
904
|
+
status,
|
|
905
|
+
statusText: STATUS_NAMES[status] ?? `unknown (${status})`,
|
|
906
|
+
signalKcps: data.signal_kcps,
|
|
907
|
+
ambientKcps: data.ambient_kcps,
|
|
908
|
+
sigmaMm: data.sigma_mm,
|
|
909
|
+
minRangeMm: data.min_range_mm,
|
|
910
|
+
maxRangeMm: data.max_range_mm,
|
|
911
|
+
};
|
|
912
|
+
}
|
|
913
|
+
/**
|
|
914
|
+
* RangeResults → the family-wide Measurement. Every return goes into
|
|
915
|
+
* `targets`; the first one is also the measurement itself. `extra` keeps the
|
|
916
|
+
* Python key names (display only).
|
|
917
|
+
*/
|
|
918
|
+
export function toMeasurement(results, bins) {
|
|
919
|
+
if (results.targets.length === 0) {
|
|
920
|
+
return measurement({
|
|
921
|
+
distanceMm: 8191,
|
|
922
|
+
status: RANGESTATUS_NONE,
|
|
923
|
+
statusText: STATUS_NAMES[RANGESTATUS_NONE],
|
|
924
|
+
spads: shr(bins.result__dss_actual_effective_spads, 8),
|
|
925
|
+
extra: { stream_count: results.stream_count, ambient_per_bin: bins.ambient_per_bin },
|
|
926
|
+
});
|
|
927
|
+
}
|
|
928
|
+
const first = results.targets[0];
|
|
929
|
+
const t0 = toTarget(first);
|
|
930
|
+
return measurement({
|
|
931
|
+
distanceMm: t0.distanceMm,
|
|
932
|
+
status: t0.status,
|
|
933
|
+
statusText: t0.statusText,
|
|
934
|
+
signalKcps: t0.signalKcps,
|
|
935
|
+
ambientKcps: t0.ambientKcps,
|
|
936
|
+
sigmaMm: t0.sigmaMm,
|
|
937
|
+
// The device counts effective SPADs in 1/256; the family reports whole ones.
|
|
938
|
+
spads: shr(first.spads, 8),
|
|
939
|
+
targets: results.targets.map(toTarget),
|
|
940
|
+
extra: {
|
|
941
|
+
stream_count: results.stream_count,
|
|
942
|
+
min_range_mm: first.min_range_mm,
|
|
943
|
+
max_range_mm: first.max_range_mm,
|
|
944
|
+
peak_bin: first.peak_bin,
|
|
945
|
+
ambient_per_bin: bins.ambient_per_bin,
|
|
946
|
+
},
|
|
947
|
+
});
|
|
948
|
+
}
|
|
949
|
+
//# sourceMappingURL=hist.js.map
|