diff --git a/TBF/Rig/TestMethods/iPerlCommunication/common/OptoTelegramRaw.cs b/TBF/Rig/TestMethods/iPerlCommunication/common/OptoTelegramRaw.cs index 8875810bc..5edd36368 100644 --- a/TBF/Rig/TestMethods/iPerlCommunication/common/OptoTelegramRaw.cs +++ b/TBF/Rig/TestMethods/iPerlCommunication/common/OptoTelegramRaw.cs @@ -193,72 +193,75 @@ namespace TBF.Rig.TestMethods.iPerlCommunication.common // -------- TIMESTAMP (seconds) -------- private const double TS_TICKS_PER_SEC = 8192.0; - - // 2^32 ticks converted to seconds => wraps every ~6.07 days - private const double TS_RANGE = 4294967296.0 / TS_TICKS_PER_SEC; // 524288.0 + private const double TS_RANGE = 4294967296.0 / TS_TICKS_PER_SEC; // 2^32 / 8192 = 524288 sec // -------- VOLUME (liters) -------- - private const double GAL_TO_LITER = 3.785411784; - private const double VOL_LITERS_PER_TICK = - (4.0 / 1000.0) / GAL_TO_LITER / 2.0; - - private const double VOL_RANGE = (1 << 24) * VOL_LITERS_PER_TICK; + // vvvvvv is unsigned 24-bit, 1 tick = 1/4 ml = 0.00025 L + private const double VOL_LITERS_PER_TICK = 0.00025; // liters per tick + private const double VOL_RANGE = 16777216.0 * VOL_LITERS_PER_TICK; // 2^24 * 0.00025 = 4194.304 L public void UpdateFromSmart( - DiagnosticLedState4Data data, - int counter, - float refFlow, - ref double volumeRawExtLast, - ref double timestampExtLast) + DiagnosticLedState4Data data, + int counter, + float refFlow, + ref double volumeRawExtLast, + ref double timestampExtLast) { - DateTime = DateTime.Now; - Counter = counter; - RefFlow = refFlow; + DateTime = DateTime.Now; + Counter = counter; + RefFlow = refFlow; - FlowRaw = data.RawFlow; - VolumeRaw = data.RawVolume; + FlowRaw = data.RawFlow; - // ---- TIMESTAMP RAW (seconds, modulo TS_RANGE) ---- - // If upstream conversion ever produced negative values, normalize them. - double ts = data.AsicTimestamp; // already in seconds, but wraps every TS_RANGE - ts = ts % TS_RANGE; - if (ts < 0) ts += TS_RANGE; + // Raw wrapped values coming from device / parser + double v = data.RawVolume; // liters, wraps every VOL_RANGE + double ts = data.AsicTimestamp; // seconds, wraps every TS_RANGE - Timestamp = ts; + // ---- normalize to [0, RANGE) in case upstream gives negative values ---- + v = v % VOL_RANGE; + if (v < 0) v += VOL_RANGE; + VolumeRaw = v; - // ---------- VOLUME UNWRAP ---------- - double v = VolumeRaw; + ts = ts % TS_RANGE; + if (ts < 0) ts += TS_RANGE; + Timestamp = ts; - if (double.IsNaN(volumeRawExtLast)) - { - VolumeRawExt = volumeRawExtLast = v; - } - else - { - // nearest-lap unwrap - double k = Math.Round((volumeRawExtLast - v) / VOL_RANGE); - VolumeRawExt = volumeRawExtLast = v + k * VOL_RANGE; - } + // ---------- VOLUME UNWRAP (24-bit rollover, liters) ---------- + if (double.IsNaN(volumeRawExtLast)) + { + VolumeRawExt = volumeRawExtLast = v; + } + else + { + double lastMod = volumeRawExtLast % VOL_RANGE; + if (lastMod < 0) lastMod += VOL_RANGE; - // ---------- TIMESTAMP UNWRAP (seconds) ---------- - if (double.IsNaN(timestampExtLast)) - { - TimestampExt = timestampExtLast = ts; - } - else - { - // robust unwrap: choose the smallest jump across the modulo boundary - double lastMod = timestampExtLast % TS_RANGE; - if (lastMod < 0) lastMod += TS_RANGE; + double delta = v - lastMod; - double delta = ts - lastMod; + // choose the shortest jump across the modulo boundary + if (delta < -VOL_RANGE / 2.0) delta += VOL_RANGE; + else if (delta > VOL_RANGE / 2.0) delta -= VOL_RANGE; - if (delta < -TS_RANGE / 2.0) delta += TS_RANGE; - else if (delta > TS_RANGE / 2.0) delta -= TS_RANGE; + VolumeRawExt = volumeRawExtLast = volumeRawExtLast + delta; + } - TimestampExt = timestampExtLast = timestampExtLast + delta; - } + // ---------- TIMESTAMP UNWRAP (32-bit rollover, seconds) ---------- + if (double.IsNaN(timestampExtLast)) + { + TimestampExt = timestampExtLast = ts; + } + else + { + double lastMod = timestampExtLast % TS_RANGE; + if (lastMod < 0) lastMod += TS_RANGE; + double delta = ts - lastMod; + + if (delta < -TS_RANGE / 2.0) delta += TS_RANGE; + else if (delta > TS_RANGE / 2.0) delta -= TS_RANGE; + + TimestampExt = timestampExtLast = timestampExtLast + delta; + } } diff --git a/TBFTests/Rig/TestMethods/iPerlCommunication/common/OptoTelegramRawTest.cs b/TBFTests/Rig/TestMethods/iPerlCommunication/common/OptoTelegramRawTest.cs index f1982ed9d..4c01b2090 100644 --- a/TBFTests/Rig/TestMethods/iPerlCommunication/common/OptoTelegramRawTest.cs +++ b/TBFTests/Rig/TestMethods/iPerlCommunication/common/OptoTelegramRawTest.cs @@ -13,6 +13,10 @@ namespace TBFTests.Rig.TestMethods.iPerlCommunication.common { // Must match OptoTelegramRaw constants: private const double TS_RANGE = 4294967296.0 / 8192.0; // 524288.0 + + // Volume: 24-bit counter, 0.25 ml per tick => 0.00025 L per tick + private const double VOL_LITERS_PER_TICK = 0.00025; + private const double VOL_RANGE = 16777216.0 * VOL_LITERS_PER_TICK; // 2^24 * 0.00025 = 4194.304 L private static void AssertAlmostEqual(double expected, double actual, double eps = 1e-6) { @@ -168,5 +172,41 @@ namespace TBFTests.Rig.TestMethods.iPerlCommunication.common } } + [TestMethod] + public void UpdateFromSmart_VolumeRollover_UnwrapsCorrectly() + { + var obj = new OptoTelegramRaw(); + double volLast = double.NaN; + double tsLast = double.NaN; + + // First: near end of 24-bit range (0xFFFFFF ticks) + // This produces a volume close to VOL_RANGE (minus 1 tick). + obj.UpdateFromSmart( + MakeState4( + asicTicks: 0, + rawVolume1to4: 0xFFFFFFu), + 0, 0, ref volLast, ref tsLast); + + double vBefore = obj.VolumeRawExt; + + // Sanity: should be near the end of the lap + Assert.IsTrue(vBefore > (VOL_RANGE - 10 * VOL_LITERS_PER_TICK), + $"Expected VolumeRawExt near end of range. VOL_RANGE={VOL_RANGE}, got={vBefore}"); + + // After rollover: small tick value (e.g., 0x0010 = 16 ticks) + // Expected extended volume = VOL_RANGE + 16*tick + const uint afterTicks = 0x0010u; // 16 ticks + double expectedAfter = VOL_RANGE + afterTicks * VOL_LITERS_PER_TICK; + + obj.UpdateFromSmart( + MakeState4( + asicTicks: 8192, // +1s (timestamp not important here) + rawVolume1to4: afterTicks), + 0, 0, ref volLast, ref tsLast); + + Assert.IsTrue(obj.VolumeRawExt > VOL_RANGE, "VolumeRawExt should extend beyond one wrap range."); + AssertAlmostEqual(expectedAfter, obj.VolumeRawExt, eps: 1e-6); + } + } } \ No newline at end of file