From 4bd84ddf7e095b3e2579c55ed4217937e1e3279e Mon Sep 17 00:00:00 2001 From: liquidraver <504870+liquidraver@users.noreply.github.com> Date: Sat, 14 Mar 2026 13:45:45 +0100 Subject: [PATCH] APC first test --- .gitignore | 2 + zephcore/CMakeLists.txt | 1 + zephcore/Kconfig | 12 + zephcore/adapters/radio/LoRaRadioBase.cpp | 5 + zephcore/adapters/radio/LoRaRadioBase.h | 5 + zephcore/app/CompanionMesh.cpp | 6 + zephcore/app/RepeaterMesh.cpp | 3 + zephcore/app/RepeaterMesh.h | 16 ++ zephcore/boards/example_board/README.md | 2 + zephcore/helpers/CommonCLI.cpp | 35 ++- zephcore/helpers/CommonCLI.h | 6 + zephcore/include/mesh/Mesh.h | 8 + zephcore/include/mesh/PowerController.h | 109 +++++++++ zephcore/include/mesh/Radio.h | 4 + zephcore/src/Mesh.cpp | 26 ++- zephcore/src/PowerController.cpp | 256 ++++++++++++++++++++++ 16 files changed, 487 insertions(+), 9 deletions(-) create mode 100644 zephcore/include/mesh/PowerController.h create mode 100644 zephcore/src/PowerController.cpp diff --git a/.gitignore b/.gitignore index 9a6edb9..eccc7bf 100644 --- a/.gitignore +++ b/.gitignore @@ -89,3 +89,5 @@ WEST_UPDATE.md /doom/ THINKNODE_M1_HANDOVER.md CLAUDE.md +zephcore/apc_checklist.md +zephcore/apc.md diff --git a/zephcore/CMakeLists.txt b/zephcore/CMakeLists.txt index ea81219..a074910 100644 --- a/zephcore/CMakeLists.txt +++ b/zephcore/CMakeLists.txt @@ -361,6 +361,7 @@ target_sources(app PRIVATE src/ContentionTracker.cpp src/Dispatcher.cpp src/Identity.cpp + $<$:src/PowerController.cpp> src/Mesh.cpp src/Packet.cpp src/StaticPoolPacketManager.cpp diff --git a/zephcore/Kconfig b/zephcore/Kconfig index ba12a1e..d8badff 100644 --- a/zephcore/Kconfig +++ b/zephcore/Kconfig @@ -349,6 +349,18 @@ config ZEPHCORE_LORA_RX_DUTY_CYCLE Can be toggled at runtime via CLI "set rxduty on/off". +config ZEPHCORE_APC + bool "Adaptive Power Control (APC)" + default y + help + Automatically reduce TX power when echo packets from neighbors + indicate excess SNR margin. Saves battery and reduces channel + congestion. Ramps back to full power within 2 minutes if no + echoes are heard. + + Uses rogue-filtering: clusters echo SNRs to avoid one badly + placed high-SNR neighbor from over-reducing power. + endmenu menu "GPS Configuration" diff --git a/zephcore/adapters/radio/LoRaRadioBase.cpp b/zephcore/adapters/radio/LoRaRadioBase.cpp index 59b2d38..99cb57c 100644 --- a/zephcore/adapters/radio/LoRaRadioBase.cpp +++ b/zephcore/adapters/radio/LoRaRadioBase.cpp @@ -28,6 +28,7 @@ LoRaRadioBase::LoRaRadioBase(const struct device *lora_dev, MainBoard &board, _noise_floor(DEFAULT_NOISE_FLOOR), _calibration_threshold(0), _ema_unguarded(0), _rx_duty_cycle_enabled(IS_ENABLED(CONFIG_ZEPHCORE_LORA_RX_DUTY_CYCLE)), _rx_boost_enabled(true), + _tx_power_reduction_db(0), _config_cached(false), _rx_cb(nullptr), _rx_cb_user_data(nullptr), _tx_done_cb(nullptr), _tx_done_cb_user_data(nullptr), @@ -201,6 +202,10 @@ void LoRaRadioBase::buildModemConfig(struct lora_modem_config &cfg, bool tx) cfg.tx_power = CONFIG_ZEPHCORE_MAX_TX_POWER_DBM; } #endif + /* APC reduction (applied after all clamps) */ + cfg.tx_power -= _tx_power_reduction_db; + if (cfg.tx_power < -9) cfg.tx_power = -9; + cfg.tx = tx; cfg.iq_inverted = false; cfg.public_network = false; diff --git a/zephcore/adapters/radio/LoRaRadioBase.h b/zephcore/adapters/radio/LoRaRadioBase.h index 5561841..223bcd7 100644 --- a/zephcore/adapters/radio/LoRaRadioBase.h +++ b/zephcore/adapters/radio/LoRaRadioBase.h @@ -73,6 +73,10 @@ public: void setRxBoost(bool enable); bool isRxBoostEnabled() const { return _rx_boost_enabled; } + /* Adaptive Power Control */ + void setTxPowerReduction(int8_t reduction_db) override { _tx_power_reduction_db = reduction_db; } + int8_t getTxPowerReduction() const override { return _tx_power_reduction_db; } + protected: /* ── Hardware primitives — subclass MUST implement ─────────── */ @@ -149,6 +153,7 @@ protected: /* Power saving */ bool _rx_duty_cycle_enabled; bool _rx_boost_enabled; + int8_t _tx_power_reduction_db; /* Config cache — skip redundant hwConfigure() on TX↔RX transitions */ struct lora_modem_config _last_cfg; diff --git a/zephcore/app/CompanionMesh.cpp b/zephcore/app/CompanionMesh.cpp index 662efca..f0d1ba4 100644 --- a/zephcore/app/CompanionMesh.cpp +++ b/zephcore/app/CompanionMesh.cpp @@ -183,6 +183,9 @@ CompanionMesh::CompanionMesh(mesh::Radio &radio, mesh::MillisecondClock &ms, mes void CompanionMesh::begin() { BaseChatMesh::begin(); +#ifdef CONFIG_ZEPHCORE_APC + _power_ctrl.setSF(prefs.sf); +#endif } bool CompanionMesh::allowPacketForward(const mesh::Packet *packet) @@ -1777,6 +1780,9 @@ bool CompanionMesh::handleProtocolFrame(const uint8_t *data, size_t len) prefs.cr = cr; prefs.client_repeat = repeat; _store->savePrefs(prefs); +#ifdef CONFIG_ZEPHCORE_APC + _power_ctrl.setSF(sf); +#endif if (_radio_reconfig_cb) _radio_reconfig_cb(); LOG_INF("SET_RADIO_PARAMS: client_repeat=%d", repeat); sendPacketOk(); diff --git a/zephcore/app/RepeaterMesh.cpp b/zephcore/app/RepeaterMesh.cpp index cfb86b0..2462dc5 100644 --- a/zephcore/app/RepeaterMesh.cpp +++ b/zephcore/app/RepeaterMesh.cpp @@ -807,6 +807,9 @@ void RepeaterMesh::begin(RepeaterDataStore* store) { * so we skip loading it here (self_id should already be set). */ _store->loadPrefs(_prefs); _contention.setBackoffMultiplier(_prefs.backoff_multiplier); +#ifdef CONFIG_ZEPHCORE_APC + _power_ctrl.setSF(_prefs.sf); +#endif acl.load(_store->getAclPath(), self_id); region_map.load(_store->getRegionsPath()); diff --git a/zephcore/app/RepeaterMesh.h b/zephcore/app/RepeaterMesh.h index 0bc8f5d..2187f91 100644 --- a/zephcore/app/RepeaterMesh.h +++ b/zephcore/app/RepeaterMesh.h @@ -197,6 +197,22 @@ public: getContentionTracker().setBackoffMultiplier(m); } +#ifdef CONFIG_ZEPHCORE_APC + /* Adaptive Power Control callbacks */ + int8_t getAPCReduction() const override { + return getPowerController().getPowerReduction(); + } + float getAPCMargin() const override { + return getPowerController().getMarginEstimate(); + } + bool isAPCEnabled() const override { + return getPowerController().isEnabled(); + } + void setAPCEnabled(bool en) override { + getPowerController().setEnabled(en); + } +#endif + void handleCommand(uint32_t sender_timestamp, char* command, char* reply); void loop(); diff --git a/zephcore/boards/example_board/README.md b/zephcore/boards/example_board/README.md index 1365766..3b7bd39 100644 --- a/zephcore/boards/example_board/README.md +++ b/zephcore/boards/example_board/README.md @@ -186,6 +186,8 @@ should ONLY contain settings that can't be inferred from hardware: CONFIG_SPI Auto from ZEPHCORE_RADIO_LR1110 CONFIG_NORDIC_QSPI_NOR Auto from DT nordic,qspi-nor node CONFIG_ZEPHCORE_LORA_RX_DUTY_CYCLE Auto: ON for companion+SX1262, OFF for repeater/LR1110 + CONFIG_ZEPHCORE_APC Adaptive Power Control — ON by default for all boards/roles. + Set to n in board.conf to disable for a specific board. Config Inheritance diff --git a/zephcore/helpers/CommonCLI.cpp b/zephcore/helpers/CommonCLI.cpp index 937afff..1856312 100644 --- a/zephcore/helpers/CommonCLI.cpp +++ b/zephcore/helpers/CommonCLI.cpp @@ -428,6 +428,17 @@ void CommonCLI::handleCommand(uint32_t sender_timestamp, const char* command, ch float ff = _callbacks->getFloodDelayFactor(); snprintf(reply, CLI_REPLY_SIZE, "> adaptive (est=%.1f flood=%.2f)", (double)est, (double)ff); + } else if (memcmp(config, "txpower", 7) == 0) { + if (_callbacks->isAPCEnabled()) { + int8_t apc = _callbacks->getAPCReduction(); + float margin = _callbacks->getAPCMargin(); + int effective = (int)_prefs->tx_power_dbm - (int)apc; + snprintf(reply, CLI_REPLY_SIZE, "> %ddBm (max=%d apc=-%d margin=%.1f)", + effective, (int)_prefs->tx_power_dbm, (int)apc, (double)margin); + } else { + snprintf(reply, CLI_REPLY_SIZE, "> %ddBm (apc=off)", + (int)_prefs->tx_power_dbm); + } } else if (memcmp(config, "flood.max", 9) == 0) { snprintf(reply, CLI_REPLY_SIZE, "> %u", (uint32_t)_prefs->flood_max); } else if (memcmp(config, "direct.txdelay", 14) == 0) { @@ -659,16 +670,24 @@ void CommonCLI::handleCommand(uint32_t sender_timestamp, const char* command, ch strcpy(reply, "OK"); } } else if (memcmp(config, "tx ", 3) == 0) { - int val = atoi(&config[3]); + if (memcmp(&config[3], "apc", 3) == 0) { + _callbacks->setAPCEnabled(true); + snprintf(reply, CLI_REPLY_SIZE, "OK - tx power=%d dBm (apc=on)", + (int)_prefs->tx_power_dbm); + } else { + int val = atoi(&config[3]); #ifdef CONFIG_ZEPHCORE_MAX_TX_POWER_DBM - if (val > CONFIG_ZEPHCORE_MAX_TX_POWER_DBM) { - val = CONFIG_ZEPHCORE_MAX_TX_POWER_DBM; - } + if (val > CONFIG_ZEPHCORE_MAX_TX_POWER_DBM) { + val = CONFIG_ZEPHCORE_MAX_TX_POWER_DBM; + } #endif - _prefs->tx_power_dbm = (int8_t)val; - savePrefs(); - _callbacks->setTxPower(_prefs->tx_power_dbm); - snprintf(reply, CLI_REPLY_SIZE, "OK - tx power=%d dBm", (int)_prefs->tx_power_dbm); + _prefs->tx_power_dbm = (int8_t)val; + savePrefs(); + _callbacks->setAPCEnabled(false); + _callbacks->setTxPower(_prefs->tx_power_dbm); + snprintf(reply, CLI_REPLY_SIZE, "OK - tx power=%d dBm (apc=off)", + (int)_prefs->tx_power_dbm); + } } else if (sender_timestamp == 0 && memcmp(config, "freq ", 5) == 0) { _prefs->freq = atof(&config[5]); savePrefs(); diff --git a/zephcore/helpers/CommonCLI.h b/zephcore/helpers/CommonCLI.h index 6aeee9c..d085095 100644 --- a/zephcore/helpers/CommonCLI.h +++ b/zephcore/helpers/CommonCLI.h @@ -52,6 +52,12 @@ public: virtual float getFloodDelayFactor() const { return 0.5f; } virtual void setBackoffMultiplier(float m) { (void)m; } + // Adaptive Power Control + virtual int8_t getAPCReduction() const { return 0; } + virtual float getAPCMargin() const { return 0.0f; } + virtual bool isAPCEnabled() const { return true; } + virtual void setAPCEnabled(bool en) { (void)en; } + // Sensor manager interface (for GPS) virtual double getNodeLat() const { return 0.0; } virtual double getNodeLon() const { return 0.0; } diff --git a/zephcore/include/mesh/Mesh.h b/zephcore/include/mesh/Mesh.h index 7b9a29b..aaaf328 100644 --- a/zephcore/include/mesh/Mesh.h +++ b/zephcore/include/mesh/Mesh.h @@ -7,6 +7,9 @@ #include #include +#ifdef CONFIG_ZEPHCORE_APC +#include +#endif #include namespace mesh { @@ -35,6 +38,11 @@ protected: ContentionTracker _contention; ContentionTracker& getContentionTracker() { return _contention; } const ContentionTracker& getContentionTracker() const { return _contention; } +#ifdef CONFIG_ZEPHCORE_APC + PowerController _power_ctrl; + PowerController& getPowerController() { return _power_ctrl; } + const PowerController& getPowerController() const { return _power_ctrl; } +#endif void extendPendingRetransmit(uint32_t hash32); DispatcherAction onRecvPacket(Packet *pkt) override; diff --git a/zephcore/include/mesh/PowerController.h b/zephcore/include/mesh/PowerController.h new file mode 100644 index 0000000..c388f92 --- /dev/null +++ b/zephcore/include/mesh/PowerController.h @@ -0,0 +1,109 @@ +/* + * SPDX-License-Identifier: Apache-2.0 + * Adaptive Power Control (APC) — echo-based TX power reduction + * + * Measures link margin by tracking echo packets (flood dupes of + * packets we sent or retransmitted, heard back from neighbors). + * Feeds per-packet margins into a rolling EMA to produce an + * adaptive TX power reduction in dBm. + * + * Rogue filtering: when 2+ distinct neighbors echo the same packet, + * clusters their SNRs within 6 dB of the best. An isolated high + * outlier (rogue, badly placed neighbor) is dropped. + */ + +#pragma once + +#include + +namespace mesh { + +class Packet; + +class PowerController { +public: + PowerController(); + + /* Enable/disable APC. When disabled, getPowerReduction() returns 0. */ + void setEnabled(bool en) { _enabled = en; } + bool isEnabled() const { return _enabled; } + + /* Set current spreading factor (needed for margin calculation). */ + void setSF(uint8_t sf) { _sf = sf; } + + /* Called when we send or retransmit a flood packet. */ + void trackTransmit(uint32_t hash32, uint32_t now_ms); + + /* Called for every received flood dupe. Updates per-source best + * SNR and source diversity. Returns true if the dupe matched a + * tracked transmit. */ + bool recordEcho(uint32_t hash32, int8_t snr_x4, + uint8_t first_hop_hash, uint32_t now_ms); + + /* Finalize expired entries, update EMA, adjust power, handle + * staleness. Call from maintenanceLoop (~5 s). */ + void tick(uint32_t now_ms); + + /* Current TX power reduction in dBm (0 to MAX_REDUCTION_DB). + * Returns 0 when disabled. */ + int8_t getPowerReduction() const { return _enabled ? _power_reduction_db : 0; } + + /* Current margin estimate in dB (for diagnostics). */ + float getMarginEstimate() const; + + /* Source count from most recently finalized entry (diagnostics). */ + uint8_t getLastSourceCount() const { return _last_source_count; } + + bool isWarmedUp() const { return _finalized_count >= WARMUP_COUNT; } + bool isStale(uint32_t now_ms) const; + +private: + static constexpr int RING_SIZE = 16; + static constexpr uint32_t ECHO_WINDOW_MS = 10000; /* 10s: covers SF12 2-hop echo */ + static constexpr uint32_t STALE_MS = 120000; /* 2 min */ + static constexpr int EMA_SHIFT = 2; /* alpha = 1/4 */ + static constexpr int WARMUP_COUNT = 3; + static constexpr int MAX_SOURCES = 3; + static constexpr int8_t STEP_DOWN_DB = 3; + static constexpr int8_t STEP_UP_DB = 6; + static constexpr int8_t MAX_REDUCTION_DB = 12; + static constexpr int8_t MIN_TX_POWER_DBM = -9; /* SX1262 hw min */ + static constexpr int CLUSTER_WIDTH_X4 = 24; /* 6 dB in x4 */ + /* TARGET_MARGIN: 16 dB above SF threshold. + * For SF8 (threshold -10 dB): reduce at SNR > +7, increase at SNR < +5 */ + static constexpr int TARGET_MARGIN_X4 = 64; /* 16 dB * 4 */ + static constexpr int HYSTERESIS_X4 = 4; /* 1 dB * 4 */ + + struct Source { + uint8_t hash; + int8_t snr_x4; + }; + + struct EchoEntry { + uint32_t hash32; + uint32_t timestamp_ms; + uint8_t source_count; + uint8_t sf_at_track; /* SF when packet was transmitted */ + Source sources[MAX_SOURCES]; + bool active; + }; + + EchoEntry _ring[RING_SIZE]; + int _next_idx; + int32_t _margin_ema_x256; /* fixed-point EMA (x4 * 256) */ + int _finalized_count; + uint32_t _last_echo_ms; + int8_t _power_reduction_db; + bool _enabled; + uint8_t _sf; + uint8_t _last_source_count; + + void finalizeEntry(int idx); + int findEntry(uint32_t hash32) const; + int8_t computeRobustSNR(const EchoEntry &entry) const; + + /* SNR threshold for a given SF (x4 fixed point). */ + static int8_t sfThresholdX4(uint8_t sf); +}; + +} /* namespace mesh */ diff --git a/zephcore/include/mesh/Radio.h b/zephcore/include/mesh/Radio.h index 933c534..f9a1203 100644 --- a/zephcore/include/mesh/Radio.h +++ b/zephcore/include/mesh/Radio.h @@ -29,6 +29,10 @@ public: virtual float getLastRSSI() const { return 0; } virtual float getLastSNR() const { return 0; } + /* Adaptive Power Control */ + virtual void setTxPowerReduction(int8_t reduction_db) { (void)reduction_db; } + virtual int8_t getTxPowerReduction() const { return 0; } + /* Packet statistics */ virtual uint32_t getPacketsRecv() const { return 0; } virtual uint32_t getPacketsSent() const { return 0; } diff --git a/zephcore/src/Mesh.cpp b/zephcore/src/Mesh.cpp index 7185c58..ea23ee1 100644 --- a/zephcore/src/Mesh.cpp +++ b/zephcore/src/Mesh.cpp @@ -30,7 +30,12 @@ void Mesh::loop() void Mesh::maintenanceLoop() { Dispatcher::maintenanceLoop(); - _contention.tick((uint32_t)_ms->getMillis()); + uint32_t now = (uint32_t)_ms->getMillis(); + _contention.tick(now); +#ifdef CONFIG_ZEPHCORE_APC + _power_ctrl.tick(now); + _radio->setTxPowerReduction(_power_ctrl.getPowerReduction()); +#endif } void Mesh::extendPendingRetransmit(uint32_t hash32) @@ -93,6 +98,9 @@ DispatcherAction Mesh::routeRecvPacket(Packet *packet) packet->setPathHashCount(n + 1); uint32_t h = ContentionTracker::computePacketHash32(packet); _contention.trackRetransmit(h, (uint32_t)_ms->getMillis()); +#ifdef CONFIG_ZEPHCORE_APC + _power_ctrl.trackTransmit(h, (uint32_t)_ms->getMillis()); +#endif uint32_t d = getRetransmitDelay(packet); return ACTION_RETRANSMIT_DELAYED(packet->getPathHashCount(), d); // give priority to closer sources } @@ -204,6 +212,10 @@ DispatcherAction Mesh::onRecvPacket(Packet *pkt) /* Record dupes for contention tracking + reactive backoff */ if (pkt->isRouteFlood()) { uint32_t h = ContentionTracker::computePacketHash32(pkt); +#ifdef CONFIG_ZEPHCORE_APC + uint8_t first_hop = (pkt->getPathHashCount() > 0) ? pkt->path[0] : 0; + _power_ctrl.recordEcho(h, pkt->_snr, first_hop, (uint32_t)_ms->getMillis()); +#endif if (_contention.recordDupeIfTracked(h, (uint32_t)_ms->getMillis())) { extendPendingRetransmit(h); } @@ -460,6 +472,12 @@ void Mesh::sendFlood(Packet *packet, uint32_t delay_millis, uint8_t path_hash_si packet->header |= ROUTE_TYPE_FLOOD; packet->setPathHashSizeAndCount(path_hash_size, 0); _tables->hasSeen(packet); +#ifdef CONFIG_ZEPHCORE_APC + { + uint32_t h = ContentionTracker::computePacketHash32(packet); + _power_ctrl.trackTransmit(h, (uint32_t)_ms->getMillis()); + } +#endif uint8_t pri; if (packet->getPayloadType() == PAYLOAD_TYPE_PATH) { @@ -489,6 +507,12 @@ void Mesh::sendFlood(Packet *packet, uint16_t *transport_codes, uint32_t delay_m packet->transport_codes[1] = transport_codes[1]; packet->setPathHashSizeAndCount(path_hash_size, 0); _tables->hasSeen(packet); +#ifdef CONFIG_ZEPHCORE_APC + { + uint32_t h = ContentionTracker::computePacketHash32(packet); + _power_ctrl.trackTransmit(h, (uint32_t)_ms->getMillis()); + } +#endif uint8_t pri; if (packet->getPayloadType() == PAYLOAD_TYPE_PATH) { diff --git a/zephcore/src/PowerController.cpp b/zephcore/src/PowerController.cpp new file mode 100644 index 0000000..35ac895 --- /dev/null +++ b/zephcore/src/PowerController.cpp @@ -0,0 +1,256 @@ +/* + * SPDX-License-Identifier: Apache-2.0 + * Adaptive Power Control — echo-based TX power reduction + */ + +#include +#include +#include + +#include +LOG_MODULE_REGISTER(zephcore_apc, CONFIG_ZEPHCORE_MAIN_LOG_LEVEL); + +/* SNR thresholds per SF (x4 fixed point, matching radio_common.h) */ +static constexpr int8_t snr_threshold_x4[] = { + -30, /* SF7: -7.5 dB */ + -40, /* SF8: -10.0 dB */ + -50, /* SF9: -12.5 dB */ + -60, /* SF10: -15.0 dB */ + -70, /* SF11: -17.5 dB */ + -80, /* SF12: -20.0 dB */ +}; + +namespace mesh { + +PowerController::PowerController() + : _next_idx(0), _margin_ema_x256(0), _finalized_count(0), + _last_echo_ms(0), _power_reduction_db(0), _enabled(true), + _sf(8), _last_source_count(0) +{ + memset(_ring, 0, sizeof(_ring)); +} + +int8_t PowerController::sfThresholdX4(uint8_t sf) +{ + int idx = (int)sf - 7; + if (idx < 0) idx = 0; + if (idx > 5) idx = 5; + return snr_threshold_x4[idx]; +} + +int PowerController::findEntry(uint32_t hash32) const +{ + for (int i = 0; i < RING_SIZE; i++) { + if (_ring[i].active && _ring[i].hash32 == hash32) { + return i; + } + } + return -1; +} + +void PowerController::trackTransmit(uint32_t hash32, uint32_t now_ms) +{ + /* If ring slot is occupied, finalize it first */ + if (_ring[_next_idx].active) { + finalizeEntry(_next_idx); + } + + EchoEntry &e = _ring[_next_idx]; + e.hash32 = hash32; + e.timestamp_ms = now_ms; + e.source_count = 0; + e.sf_at_track = _sf; + memset(e.sources, 0, sizeof(e.sources)); + e.active = true; + + _next_idx = (_next_idx + 1) % RING_SIZE; +} + +bool PowerController::recordEcho(uint32_t hash32, int8_t snr_x4, + uint8_t first_hop_hash, uint32_t now_ms) +{ + int idx = findEntry(hash32); + if (idx < 0) return false; + + EchoEntry &e = _ring[idx]; + + /* Check if entry has expired */ + if (now_ms - e.timestamp_ms > ECHO_WINDOW_MS) { + finalizeEntry(idx); + return false; + } + + /* Update existing source or add new one */ + for (int i = 0; i < e.source_count; i++) { + if (e.sources[i].hash == first_hop_hash) { + if (snr_x4 > e.sources[i].snr_x4) { + e.sources[i].snr_x4 = snr_x4; + } + _last_echo_ms = now_ms; + return true; + } + } + + if (e.source_count < MAX_SOURCES) { + e.sources[e.source_count].hash = first_hop_hash; + e.sources[e.source_count].snr_x4 = snr_x4; + e.source_count++; + } + + _last_echo_ms = now_ms; + return true; +} + +int8_t PowerController::computeRobustSNR(const EchoEntry &entry) const +{ + if (entry.source_count == 0) { + return sfThresholdX4(entry.sf_at_track); /* no echo = margin 0 */ + } + + if (entry.source_count == 1) { + return entry.sources[0].snr_x4; + } + + /* 2-3 sources: sort descending, then cluster + rogue filter */ + int8_t sorted[MAX_SOURCES]; + int n = entry.source_count; + for (int i = 0; i < n; i++) { + sorted[i] = entry.sources[i].snr_x4; + } + /* Simple insertion sort (max 3 elements) */ + for (int i = 1; i < n; i++) { + int8_t key = sorted[i]; + int j = i - 1; + while (j >= 0 && sorted[j] < key) { + sorted[j + 1] = sorted[j]; + j--; + } + sorted[j + 1] = key; + } + + /* Count how many are within CLUSTER_WIDTH of the best */ + int cluster_count = 1; + for (int i = 1; i < n; i++) { + if (sorted[0] - sorted[i] <= CLUSTER_WIDTH_X4) { + cluster_count++; + } + } + + if (cluster_count >= 2) { + /* 2+ in cluster: median of the cluster values */ + /* For 2 values: average. For 3 values: middle one. */ + if (cluster_count == 2) { + return (int8_t)(((int)sorted[0] + (int)sorted[1]) / 2); + } + /* cluster_count == 3 (all 3 within 6 dB) */ + return sorted[1]; /* median */ + } + + /* Only 1 in top cluster → rogue. Drop it, use next. */ + if (n >= 3 && sorted[1] - sorted[2] <= CLUSTER_WIDTH_X4) { + /* sources[1] and [2] cluster together — median them */ + return (int8_t)(((int)sorted[1] + (int)sorted[2]) / 2); + } + /* Fall back to second-best */ + return sorted[1]; +} + +void PowerController::finalizeEntry(int idx) +{ + if (!_ring[idx].active) return; + + EchoEntry &e = _ring[idx]; + _last_source_count = e.source_count; + + int8_t robust_snr = computeRobustSNR(e); + int32_t margin_x4 = (int32_t)robust_snr - (int32_t)sfThresholdX4(e.sf_at_track); + /* margin_x4 is in x4 units. Convert to x256 for EMA. */ + int32_t sample_x256 = margin_x4 << 6; /* x4 * 64 = x256 */ + + int32_t diff = sample_x256 - _margin_ema_x256; + + if (_finalized_count < WARMUP_COUNT) { + /* During warmup, seed the EMA faster */ + if (_finalized_count == 0) { + _margin_ema_x256 = sample_x256; + } else { + _margin_ema_x256 += diff >> 1; + } + } else { + /* Normal EMA update: ema += (sample - ema) >> shift */ + _margin_ema_x256 += diff >> EMA_SHIFT; + } + + _finalized_count++; + e.active = false; + + LOG_DBG("APC finalize: sources=%d robust_snr=%.1f margin=%.1f ema=%.1f", + (int)_last_source_count, + (double)(robust_snr / 4.0f), + (double)(margin_x4 / 4.0f), + (double)getMarginEstimate()); +} + +void PowerController::tick(uint32_t now_ms) +{ + /* Finalize expired entries */ + for (int i = 0; i < RING_SIZE; i++) { + if (_ring[i].active && now_ms - _ring[i].timestamp_ms > ECHO_WINDOW_MS) { + finalizeEntry(i); + } + } + + if (!isWarmedUp()) return; + + int32_t margin_x256 = _margin_ema_x256; + int32_t target_x256 = TARGET_MARGIN_X4 << 6; + int32_t hyst_x256 = HYSTERESIS_X4 << 6; + + int8_t old_reduction = _power_reduction_db; + + /* Staleness takes priority: ramp back to full power if no echoes. + * When stale, never increase reduction — old EMA data is unreliable. */ + if (isStale(now_ms)) { + if (_power_reduction_db > 0) { + _power_reduction_db -= STEP_DOWN_DB; + if (_power_reduction_db < 0) { + _power_reduction_db = 0; + } + } + } else if (margin_x256 > target_x256 + hyst_x256) { + /* Margin very good — step down */ + if (_power_reduction_db < MAX_REDUCTION_DB) { + _power_reduction_db += STEP_DOWN_DB; + if (_power_reduction_db > MAX_REDUCTION_DB) { + _power_reduction_db = MAX_REDUCTION_DB; + } + } + } else if (margin_x256 < target_x256 - hyst_x256) { + /* Margin too low — step up (reduce the reduction) */ + if (_power_reduction_db > 0) { + _power_reduction_db -= STEP_UP_DB; + if (_power_reduction_db < 0) { + _power_reduction_db = 0; + } + } + } + + if (_power_reduction_db != old_reduction) { + LOG_INF("APC: reduction %d -> %d dBm (margin=%.1f)", + (int)old_reduction, (int)_power_reduction_db, + (double)getMarginEstimate()); + } +} + +float PowerController::getMarginEstimate() const +{ + return (float)_margin_ema_x256 / 256.0f; +} + +bool PowerController::isStale(uint32_t now_ms) const +{ + if (_last_echo_ms == 0) return false; + return now_ms - _last_echo_ms > STALE_MS; +} + +} /* namespace mesh */