feat: implement modular stylus solver components and device runtime handlers

Author: awarson2233

EGoTouchService/Device/btmcu/PenUsbTypes.h

@@ -84,6 +84,9 @@ struct PenSemanticState {
 
     bool hasEraserToggle = false;
     uint8_t eraserToggle = 0;
+
+    bool hasCurrentFunc = false;
+    uint8_t currentFunc = 0;
 };
 
 struct PenUsbHeader {

EGoTouchService/Device/penevt/PenEventBridge.cpp

@@ -474,6 +474,10 @@ void PenEventBridge::OnPacketReceived(const std::vector<uint8_t>& packet) {
                 ev.semantic.hasEraserToggle = true;
                 ev.semantic.eraserToggle = ev.payload[0];
                 break;
+            case PenUsbEventCode::PenCurrentFunc:
+                ev.semantic.hasCurrentFunc = true;
+                ev.semantic.currentFunc = ev.payload[0];
+                break;
             default:
                 break;
             }

EGoTouchService/Device/runtime/DeviceRuntime.cpp

@@ -483,6 +483,22 @@ void DeviceRuntime::IngestPenEvent(const Himax::Pen::PenEvent& ev) {
         break;
     }
 
+    case EC::PenCurrentFunc: {
+        uint8_t func = 0;
+        {
+            std::lock_guard<std::mutex> lk(m_penStateMu);
+            m_penState.hasCurrentFunc = ev.semantic.hasCurrentFunc;
+            m_penState.currentFunc = ev.semantic.hasCurrentFunc ? ev.semantic.currentFunc : 0;
+            func = m_penState.currentFunc;
+        }
+
+        // 笔双击触控按键 → 触发 Barrel Button (momentary, 下一帧自动复位)
+        m_vhfReporter.SetBarrelButtonState(true);
+        LOG_INFO("Runtime", __func__, "MCU",
+                 "PenCurrentFunc: func={} → barrel button pressed (momentary).", func);
+        break;
+    }
+
     case EC::EraserToggle: {
         uint8_t eraserState = 0;
         {

EGoTouchService/Device/runtime/DeviceRuntime.h

@@ -108,6 +108,9 @@ struct RuntimePenState {
 
     bool hasEraserToggle = false;
     uint8_t eraserToggle = 0;
+
+    bool hasCurrentFunc = false;
+    uint8_t currentFunc = 0;
 };
 
 // --------------- DeviceRuntime ---------------

EGoTouchService/Device/vhf/VhfReporter.cpp

@@ -207,6 +207,8 @@ VhfReporter::StylusDispatchPacket VhfReporter::BuildStylusPacket(
         config.sensorCols = m_stylusSensorCols;
         config.emitWhenInvalid = m_emitStylusPacketWhenInvalid;
     }
+    config.barrelButton = m_barrelButtonState.exchange(
+        false, std::memory_order_relaxed);
 
     StylusDispatchPacket built{};
     built.packet = VhfStylusPacket::Build(stylus, config);

EGoTouchService/Device/vhf/VhfReporter.h

@@ -51,6 +51,10 @@ class VhfReporter {
         m_eraserState.store(v, std::memory_order_relaxed);
     }
 
+    void SetBarrelButtonState(bool pressed) {
+        m_barrelButtonState.store(pressed, std::memory_order_relaxed);
+    }
+
     bool IsDeviceOpen() const;
     void Close();
 
@@ -84,6 +88,7 @@ class VhfReporter {
     std::atomic<bool> m_transpose{false};
     std::atomic<bool> m_hadTouchLastFrame{false};
     std::atomic<uint8_t> m_eraserState{0};
+    std::atomic<bool> m_barrelButtonState{false};
 
     int m_stylusSensorRows = 40;
     int m_stylusSensorCols = 60;

EGoTouchService/Device/vhf/VhfReporterStylusPacketHelper.h

@@ -14,6 +14,7 @@ struct Config {
     int sensorRows = 40;
     int sensorCols = 60;
     bool emitWhenInvalid = true;
+    bool barrelButton = false;
 };
 
 namespace detail {
@@ -78,10 +79,13 @@ inline Solvers::StylusPacket Build(const Solvers::StylusFrameData& stylus,
 
     uint8_t penState = 0;
     if (stylus.output.tipDown) {
-        penState |= 0x01u;
+        penState |= 0x01u;   // Tip Switch
+    }
+    if (config.barrelButton) {
+        penState |= 0x02u;   // Barrel Switch
     }
     if (stylus.output.inRange) {
-        penState |= 0x20u;
+        penState |= 0x20u;   // In Range
     }
     packet.bytes[1] = penState;
     packet.bytes[2] = 0x00u;

EGoTouchService/Solvers/StylusFrameTypes.h

@@ -213,6 +213,18 @@ struct StylusRuntimePost {
     bool linearFilterActive = false;
     int32_t linearFilterDeltaDim1 = 0;
     int32_t linearFilterDeltaDim2 = 0;
+
+    // ── CoorSpeedProcess outputs ──
+    int32_t speedValue = 0;
+    int32_t speedAvgDx = 0;
+    int32_t speedAvgDy = 0;
+    int32_t speedShortAvgDist = 0;
+    int32_t speedFullAvgDist = 0;
+
+    // ── CoorIIRProcess output ──
+    uint16_t iirCoef = 0;
+    bool iirFilterActive = false;
+
 #if EGOTOUCH_DIAG
     float lfLineFitSlopeA = 0.f;
     float lfLineFitInterceptB = 0.f;
@@ -223,6 +235,14 @@ struct StylusRuntimePost {
     bool coorReviseActive = false;
     int16_t coorReviseCorrectionDim1 = 0;
     int16_t coorReviseCorrectionDim2 = 0;
+
+    // ── AftCoorProcess diagnostics ──
+    bool lockActiveX = false;
+    bool lockActiveY = false;
+    int32_t lockOffsetX = 0;
+    int32_t lockOffsetY = 0;
+    int32_t lockThresholdX = 0;
+    int32_t lockThresholdY = 0;
 #endif
 };
 

EGoTouchService/Solvers/StylusSolver/AftCoorProcess.hpp

@@ -0,0 +1,162 @@
+#pragma once
+
+#include "AsaTypes.hpp"
+#include "SolverTypes.h"
+
+#include <algorithm>
+#include <cstdint>
+
+namespace Solvers::Stylus {
+
+class AftCoorProcess {
+public:
+    bool m_enabled = true;
+
+    // ── Lock thresholds (flash params equivalent) ──
+    // In-band thresholds (coordinate within sensor interior)
+    uint8_t m_lockFlashInBandX = 10;  // flash[0xA5A]
+    uint8_t m_lockFlashInBandY = 10;  // flash[0xA5B]
+
+    // Edge thresholds (coordinate near sensor boundary)
+    uint8_t m_lockFlashEdgeX = 20;  // flash[0xA58]
+    uint8_t m_lockFlashEdgeY = 20;  // flash[0xA59]
+
+    // Sensor dimensions (from asaPrmt)
+    int m_sensorTxCount = 60;   // bTxCount
+    int m_sensorRxCount = 40;   // bRxCount
+    int m_sensorDim1 = 60;      // wField00
+    int m_sensorDim2 = 40;      // wField02
+
+    // Bypass gate: skip lock when this ASA-style flag is active
+    bool m_bypassLock = false;
+
+    inline void Reset() {
+        m_startX = 0;
+        m_startY = 0;
+        m_lockOffsetX = 0;
+        m_lockOffsetY = 0;
+        m_flagLockX = false;
+        m_flagLockY = false;
+        m_prevPressure = 0;
+    }
+
+    inline void Process(HeatmapFrame& frame) {
+        auto& runtime = frame.stylus.runtime;
+        auto& coor = runtime.post.finalCoor;
+        const uint16_t curPressure = runtime.pressure.outputPressure;
+
+        if (!m_enabled || m_bypassLock) {
+            // Pass-through: just clamp to sensor bounds
+            if (coor.valid) {
+                ClampToSensor(coor);
+            }
+            m_prevPressure = curPressure;
+            return;
+        }
+
+        if (!coor.valid) {
+            Reset();
+            m_prevPressure = curPressure;
+            return;
+        }
+
+        // ── Select thresholds based on coordinate position ──
+        // TSACore: if coor < 0x401 or coor >= (count-1)*0x400 → edge threshold
+        const int32_t minBound = 0x401;
+        const int32_t maxBoundX = (m_sensorTxCount - 1) * Asa::kCoorUnit;
+        const int32_t maxBoundY = (m_sensorRxCount - 1) * Asa::kCoorUnit;
+
+        uint32_t thresholdX;
+        uint32_t thresholdY;
+
+        if (coor.dim1 < minBound || coor.dim2 < minBound ||
+            coor.dim1 >= maxBoundX || coor.dim2 >= maxBoundY) {
+            // Near boundary → use edge thresholds (more tolerant)
+            thresholdX = (static_cast<uint32_t>(m_lockFlashEdgeX) *
+                          static_cast<uint32_t>(m_sensorTxCount) * Asa::kCoorUnit) /
+                         static_cast<uint32_t>(std::max(1, m_sensorDim1));
+            thresholdY = (static_cast<uint32_t>(m_lockFlashEdgeY) *
+                          static_cast<uint32_t>(m_sensorRxCount) * Asa::kCoorUnit) /
+                         static_cast<uint32_t>(std::max(1, m_sensorDim2));
+        } else {
+            // Interior → use in-band thresholds
+            thresholdX = (static_cast<uint32_t>(m_lockFlashInBandX) *
+                          static_cast<uint32_t>(m_sensorTxCount) * Asa::kCoorUnit) /
+                         static_cast<uint32_t>(std::max(1, m_sensorDim1));
+            thresholdY = (static_cast<uint32_t>(m_lockFlashInBandY) *
+                          static_cast<uint32_t>(m_sensorRxCount) * Asa::kCoorUnit) /
+                         static_cast<uint32_t>(std::max(1, m_sensorDim2));
+        }
+
+        // ── Pen-down transition: lock to start position ──
+        if (curPressure != 0 && m_prevPressure == 0) {
+            m_startX = coor.dim1;
+            m_startY = coor.dim2;
+            m_flagLockX = true;
+            m_flagLockY = true;
+            m_lockOffsetX = 0;
+            m_lockOffsetY = 0;
+        }
+
+        // ── X-axis lock ──
+        if (m_flagLockX) {
+            const int32_t diff = coor.dim1 - m_startX;
+            const uint32_t absDiff = static_cast<uint32_t>(std::abs(diff));
+            if (absDiff > thresholdX) {
+                m_flagLockX = false;  // movement exceeded threshold → release lock
+            }
+            if (m_flagLockX) {
+                m_lockOffsetX = diff;  // track offset while locked
+            }
+        }
+
+        // ── Y-axis lock ──
+        if (m_flagLockY) {
+            const int32_t diff = coor.dim2 - m_startY;
+            const uint32_t absDiff = static_cast<uint32_t>(std::abs(diff));
+            if (absDiff > thresholdY) {
+                m_flagLockY = false;
+            }
+            if (m_flagLockY) {
+                m_lockOffsetY = diff;
+            }
+        }
+
+        // ── Apply offset and clamp ──
+        int32_t finalX = coor.dim1 - m_lockOffsetX;
+        int32_t finalY = coor.dim2 - m_lockOffsetY;
+
+        finalX = std::clamp(finalX, 0, m_sensorTxCount * Asa::kCoorUnit);
+        finalY = std::clamp(finalY, 0, m_sensorRxCount * Asa::kCoorUnit);
+
+        coor.dim1 = finalX;
+        coor.dim2 = finalY;
+
+#if EGOTOUCH_DIAG
+        runtime.post.lockActiveX = m_flagLockX;
+        runtime.post.lockActiveY = m_flagLockY;
+        runtime.post.lockOffsetX = m_lockOffsetX;
+        runtime.post.lockOffsetY = m_lockOffsetY;
+        runtime.post.lockThresholdX = static_cast<int32_t>(thresholdX);
+        runtime.post.lockThresholdY = static_cast<int32_t>(thresholdY);
+#endif
+
+        m_prevPressure = curPressure;
+    }
+
+private:
+    int32_t m_startX = 0;
+    int32_t m_startY = 0;
+    int32_t m_lockOffsetX = 0;
+    int32_t m_lockOffsetY = 0;
+    bool m_flagLockX = false;
+    bool m_flagLockY = false;
+    uint16_t m_prevPressure = 0;
+
+    inline void ClampToSensor(Asa::AsaCoorResult& coor) const {
+        coor.dim1 = std::clamp(coor.dim1, 0, m_sensorTxCount * Asa::kCoorUnit);
+        coor.dim2 = std::clamp(coor.dim2, 0, m_sensorRxCount * Asa::kCoorUnit);
+    }
+};
+
+} // namespace Solvers::Stylus