From 7c733bffabb2c2704dba0b4fa12f41ca3e452d7f Mon Sep 17 00:00:00 2001 From: Kawin Pechetratanapanit <39807451+kawinie@users.noreply.github.com> Date: Sun, 17 May 2026 01:12:56 -0700 Subject: [PATCH] feat(v2): stabilize VBUS readout filtering Display jittered under V_SENSE divider noise and dropped after the output toggle. ADC oversamples 64 samples per read for sqrt(N) noise reduction. EMA gains an optional snap threshold so PDO step transitions bypass smoothing. On OFF->ON, the load reading seeds from the supply value and the next two INA226 samples are discarded while a fresh conversion completes. --- .clang-format | 1 + include/v2/events.h | 24 ++++++++ include/v2/hal/adc_supply_voltage_source.h | 19 +++++-- include/v2/pocketpd.h | 13 +++++ include/v2/stages/energy_stage.h | 27 +++++---- include/v2/stages/normal_stage.h | 64 ++++++++++++++-------- include/v2/util/filter.h | 34 +++++------- src/main.cpp | 9 +-- 8 files changed, 124 insertions(+), 67 deletions(-) diff --git a/.clang-format b/.clang-format index 96fe37c..53818b6 100644 --- a/.clang-format +++ b/.clang-format @@ -10,6 +10,7 @@ AllowShortFunctionsOnASingleLine: Empty AlwaysBreakTemplateDeclarations: Yes BinPackArguments: false BinPackParameters: OnePerLine +PenaltyReturnTypeOnItsOwnLine: 1000 BreakBeforeTernaryOperators: true BreakConstructorInitializers: BeforeColon DerivePointerAlignment: false diff --git a/include/v2/events.h b/include/v2/events.h index 28b0144..7bd8ec2 100644 --- a/include/v2/events.h +++ b/include/v2/events.h @@ -9,6 +9,7 @@ #include #include "v2/pocketpd.h" +#include "v2/util/filter.h" namespace pocketpd { @@ -68,9 +69,21 @@ namespace pocketpd { * @brief Load-side reading from INA226. */ struct LoadReading { + static constexpr uint32_t LOAD_EMA_DEN = 4; + static constexpr uint32_t SNAP_MV = 200; + static constexpr uint32_t SNAP_MA = 20; + uint32_t timestamp_ms = 0; uint32_t vbus_mv = 0; uint32_t current_ma = 0; + + LoadReading ema(const LoadReading& sample ) const { + return { + .timestamp_ms = sample.timestamp_ms, + .vbus_mv = Filter::ema(vbus_mv, sample.vbus_mv, LOAD_EMA_DEN, SNAP_MV), + .current_ma = Filter::ema(current_ma, sample.current_ma, LOAD_EMA_DEN, SNAP_MA), + }; + } }; /** @@ -78,9 +91,20 @@ namespace pocketpd { * VOLTAGE register on earlier board. */ struct SupplyReading { + static constexpr uint32_t SUPPLY_EMA_DEN = 8; + static constexpr uint32_t SNAP_MV = 200; + uint32_t timestamp_ms = 0; uint32_t mv = 0; bool valid = false; + + SupplyReading ema(const SupplyReading& sample) const { + return { + .timestamp_ms = sample.timestamp_ms, + .mv = Filter::ema(mv, sample.mv, SUPPLY_EMA_DEN, SNAP_MV), + .valid = sample.valid, + }; + } }; /** diff --git a/include/v2/hal/adc_supply_voltage_source.h b/include/v2/hal/adc_supply_voltage_source.h index 05eef58..6d2ba24 100644 --- a/include/v2/hal/adc_supply_voltage_source.h +++ b/include/v2/hal/adc_supply_voltage_source.h @@ -4,10 +4,10 @@ */ #pragma once -#include - #include +#include + #include "v2/hal/supply_voltage_source.h" namespace pocketpd { @@ -22,6 +22,9 @@ namespace pocketpd { static constexpr uint32_t DIVIDER_NUM = 13; static constexpr uint32_t DIVIDER_DEN = 2; + // Oversample N raw samples per read(); sqrt(N) noise reduction on uncorrelated ADC noise. + static constexpr uint16_t OVERSAMPLE_N = 64; // tested ~300us loop time at 64 samples + public: explicit AdcSupplyVoltageSource(uint8_t pin) : m_pin(pin) {} @@ -30,9 +33,15 @@ namespace pocketpd { } SupplyVoltageReading read() override { - const uint32_t raw = analogRead(m_pin); - const uint32_t adc_mv = (raw * ADC_REF_MV) / ADC_MAX; - const uint32_t vbus_mv = (adc_mv * DIVIDER_NUM) / DIVIDER_DEN; + uint32_t sum = 0; + for (int i = 0; i < OVERSAMPLE_N; i++) { + sum += analogRead(m_pin); + } + + const uint64_t numerator = (uint64_t) sum * ADC_REF_MV * DIVIDER_NUM; + const uint64_t denominator = (uint64_t) OVERSAMPLE_N * ADC_MAX * DIVIDER_DEN; + const uint32_t vbus_mv = numerator / denominator; + return SupplyVoltageReading{vbus_mv, true}; } }; diff --git a/include/v2/pocketpd.h b/include/v2/pocketpd.h index e5ee831..66f9d91 100644 --- a/include/v2/pocketpd.h +++ b/include/v2/pocketpd.h @@ -43,6 +43,19 @@ namespace pocketpd { // PPS RDO stepping per USB-PD 3.0 spec. constexpr int32_t PPS_VOLTAGE_STEP_MV = 20; constexpr int32_t PPS_CURRENT_STEP_MA = 50; + + constexpr uint8_t pin_encoder_SW = 18; + constexpr uint8_t pin_encoder_A = 19; // CLK + constexpr uint8_t pin_encoder_B = 20; // DATA + + constexpr uint8_t pin_output_Enable = 1; + constexpr uint8_t pin_button_outputSW = 10; + constexpr uint8_t pin_button_selectVI = 11; + + constexpr uint8_t pin_SDA = 4; + constexpr uint8_t pin_SCL = 5; + + constexpr uint8_t pin_VSENSE = 29; // ADC3 — V_SENSE divider (2/13 of VBUS) // —— I2C addresses diff --git a/include/v2/stages/energy_stage.h b/include/v2/stages/energy_stage.h index 3dd8591..20e647b 100644 --- a/include/v2/stages/energy_stage.h +++ b/include/v2/stages/energy_stage.h @@ -22,8 +22,6 @@ #include "v2/hal/output_gate.h" #include "v2/images.h" #include "v2/stages/energy/energy_view.h" -#include "v2/pocketpd.h" -#include "v2/util/filter.h" namespace pocketpd { @@ -41,8 +39,6 @@ namespace pocketpd { uint8_t m_arrow_frame = 0; bool m_locked = false; - static constexpr uint32_t SENSOR_EMA_DEN = 4; - public: static constexpr const char* LOG_TAG = "EnergyStage"; @@ -79,9 +75,10 @@ namespace pocketpd { void on_event(Conductor& conductor, const Event& event, uint32_t) override { auto handler = tempo::overloaded{ - [&](const ButtonEvent& evt) { - // L+R combo always reachable; must precede lock guard so a locked screen can unlock. - if (evt.lr_long()) { + [&](const ButtonEvent& event) { + // L+R combo always reachable; must precede lock guard so a locked screen can + // unlock. + if (event.lr_long()) { m_locked = !m_locked; return; } @@ -89,19 +86,21 @@ namespace pocketpd { return; } - if (evt.r_short()) { + if (event.r_short()) { m_output_gate.toggle(); return; } - if (evt.r_long()) { + if (event.r_long()) { conductor.request(m_active_pdo_index); } }, - [&](const SensorEvent& evt) { - m_load_reading = m_load_init - ? Filter::ema(m_load_reading, evt.load, SENSOR_EMA_DEN) - : evt.load; - m_load_init = true; + [&](const SensorEvent& event) { + if (m_load_init) { + m_load_reading = m_load_reading.ema(event.load); + } else { + m_load_reading = event.load; + m_load_init = true; + } }, [&](const EnergyEvent& evt) { m_energy = evt; }, [](const auto&) {}, diff --git a/include/v2/stages/normal_stage.h b/include/v2/stages/normal_stage.h index d2e8a50..93369d5 100644 --- a/include/v2/stages/normal_stage.h +++ b/include/v2/stages/normal_stage.h @@ -18,7 +18,6 @@ #include "v2/stages/normal/fixed_mode.h" #include "v2/stages/normal/normal_view.h" #include "v2/stages/normal/pps_mode.h" -#include "v2/util/filter.h" namespace pocketpd { @@ -48,8 +47,12 @@ namespace pocketpd { uint32_t m_last_draw_ms = 0; bool m_blink_visible = true; - static constexpr uint32_t SENSOR_EMA_DEN = 4; - static constexpr uint32_t SUPPLY_EMA_DEN = 8; + // After OFF->ON, the first INA226 read returns a stale conversion (latched while FET + // was off; observed ~200 mV instead of true VBUS). Discard N load samples and hold the + // seeded supply value until the sensor has a fresh conversion in hand. + uint8_t m_postenable_discard_left = 0; + static constexpr uint8_t POSTENABLE_DISCARD_SAMPLES = 2; + static constexpr uint32_t READOUT_BLINK_ON_MS = 1200; static constexpr uint32_t READOUT_BLINK_OFF_MS = 400; static constexpr uint32_t READOUT_BLINK_CYCLE_MS = @@ -132,7 +135,7 @@ namespace pocketpd { if (m_last_draw_ms != 0) { const uint32_t period = now_ms - m_last_draw_ms; const uint32_t hz = period == 0 ? 0 : 1000 / period; - log.debug("draw period={}ms (~{}Hz)", period, hz); + // log.debug("draw period={}ms (~{}Hz)", period, hz); } m_last_draw_ms = now_ms; m_blink_visible = m_output_gate.is_enabled() || @@ -143,38 +146,48 @@ namespace pocketpd { void on_event(Conductor& conductor, const Event& event, uint32_t) override { auto handler = tempo::overloaded{ - [&](const ButtonEvent& evt) { + [&](const ButtonEvent& event) { // L+R combo always reachable; must precede lock guard so a locked screen can // unlock. - if (evt.lr_long()) { + if (event.lr_long()) { m_locked = !m_locked; return; } + if (m_locked) { return; } - if (evt.r_short()) { + if (event.r_short()) { + const bool was_enabled = m_output_gate.is_enabled(); m_output_gate.toggle(); + // Display source flips from supply-side to load-side on enable. Seed the + // load EMA with the current supply value so the first frame after toggle + // shows the known VBUS rather than a stale or zero load_reading. + if (!was_enabled && m_output_gate.is_enabled() && m_supply_init) { + m_load_reading.vbus_mv = m_supply_reading.mv; + m_load_init = true; + m_postenable_discard_left = POSTENABLE_DISCARD_SAMPLES; + } return; } - if (evt.r_long()) { + if (event.r_long()) { conductor.request(m_active_pdo_index); return; } - if (evt.l_long()) { + if (event.l_long()) { conductor.request(); return; } // V/I are adjustable in PPS mode if (auto* pps = std::get_if(&m_mode)) { - pps->on_button(evt); + pps->on_button(event); } }, - [&](const EncoderEvent& evt) { + [&](const EncoderEvent& event) { if (m_locked) { return; } @@ -183,21 +196,28 @@ namespace pocketpd { return; } - if (!pps->on_encoder(evt)) { + if (!pps->on_encoder(event)) { const auto msg = "set_pps_pdo({}, {}, {}) failed"; log.error(msg, m_active_pdo_index, pps->target_mv, pps->target_ma); } }, - [&](const SensorEvent& evt) { - m_load_reading = m_load_init - ? Filter::ema(m_load_reading, evt.load, SENSOR_EMA_DEN) - : evt.load; - m_load_init = true; - if (evt.supply.valid) { - m_supply_reading = m_supply_init - ? Filter::ema(m_supply_reading, evt.supply, SUPPLY_EMA_DEN) - : evt.supply; - m_supply_init = true; + [&](const SensorEvent& event) { + if (m_postenable_discard_left > 0) { + --m_postenable_discard_left; + } else if (m_load_init) { + m_load_reading = m_load_reading.ema(event.load); + } else { + m_load_reading = event.load; + m_load_init = true; + } + + if (event.supply.valid) { + if (m_supply_init) { + m_supply_reading = m_supply_reading.ema(event.supply); + } else { + m_supply_reading = event.supply; + m_supply_init = true; + } } }, [](const auto&) {}, diff --git a/include/v2/util/filter.h b/include/v2/util/filter.h index 674a9fe..6cf5d4d 100644 --- a/include/v2/util/filter.h +++ b/include/v2/util/filter.h @@ -5,6 +5,7 @@ #pragma once #include +#include #include "v2/events.h" @@ -15,29 +16,22 @@ namespace pocketpd { /** * @brief One EMA step. `new = ((den-1)*prev + sample) / den`. * For `den = 4`: weights 0.75 prev / 0.25 sample. + * + * @param prev The previous value. + * @param sample The new value. + * @param den The denominator of the EMA. + * @param snap The snap threshold. + * @return The EMA value. */ - static uint32_t ema(uint32_t prev, uint32_t sample, uint32_t den) { + static uint32_t ema( + uint32_t prev, uint32_t sample, uint32_t den, uint32_t snap = UINT32_MAX + ) { + uint32_t diff = sample > prev ? sample - prev : prev - sample; + if (diff >= snap) { + return sample; + } return (prev * (den - 1) + sample) / den; } - - /** @brief Per-field EMA across a `LoadReading`. `timestamp_ms` tracks the latest. */ - static LoadReading ema(const LoadReading& prev, const LoadReading& sample, uint32_t den) { - return LoadReading{ - .timestamp_ms = sample.timestamp_ms, - .vbus_mv = ema(prev.vbus_mv, sample.vbus_mv, den), - .current_ma = ema(prev.current_ma, sample.current_ma, den), - }; - } - - /** @brief Per-field EMA across a `SupplyReading`. Caller filters invalid samples. */ - static SupplyReading - ema(const SupplyReading& prev, const SupplyReading& sample, uint32_t den) { - return { - .timestamp_ms = sample.timestamp_ms, - .mv = ema(prev.mv, sample.mv, den), - .valid = sample.valid, - }; - } }; } // namespace pocketpd diff --git a/src/main.cpp b/src/main.cpp index a181bb0..4ec6b24 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -2,14 +2,13 @@ #include #include #include -#include #include #include #include "v2/app.h" +#include "v2/hal/adc_supply_voltage_source.h" #include "v2/hal/ap33772_pd_sink.h" #include "v2/hal/ap33772_supply_voltage_source.h" -#include "v2/hal/adc_supply_voltage_source.h" #include "v2/hal/arduino_clock.h" #include "v2/hal/arduino_output_gate.h" #include "v2/hal/arduino_stream_reader.h" @@ -26,8 +25,6 @@ namespace pocketpd { - - // —— Hardware adapters ArduinoClock arduino_clock; @@ -76,14 +73,14 @@ namespace pocketpd { } // namespace pocketpd void setup() { + using namespace pocketpd; + Serial.begin(115200); Wire.setSDA(pin_SDA); Wire.setSCL(pin_SCL); Wire.begin(); - using namespace pocketpd; - ina226_driver.begin(); power_monitor.begin(); supply_voltage_source.begin();