mirror of
https://github.com/liquidraver/ZephCore.git
synced 2026-09-03 07:23:54 +00:00
micro-tweak: skip APC math when APC is off
This commit is contained in:
liquidraver
@@ -256,6 +256,9 @@ public:
|
||||
}
|
||||
void setAPCEnabled(bool en) override {
|
||||
getPowerController().setEnabled(en);
|
||||
if (!en) {
|
||||
_radio->setTxPowerReduction(0);
|
||||
}
|
||||
}
|
||||
uint8_t getAPCTargetMargin() const override {
|
||||
return getPowerController().getTargetMargin();
|
||||
|
||||
@@ -1,113 +1,113 @@
|
||||
/*
|
||||
* 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 <stdint.h>
|
||||
|
||||
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; }
|
||||
|
||||
/* Set target link margin in dB. Higher = more conservative
|
||||
* (better for networks with poor-RX hardware). Default 16 dB. */
|
||||
void setTargetMargin(uint8_t margin_db) { _target_margin_x4 = (int)margin_db * 4; }
|
||||
uint8_t getTargetMargin() const { return (uint8_t)(_target_margin_x4 / 4); }
|
||||
|
||||
/* 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 */
|
||||
static constexpr int DEFAULT_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;
|
||||
int _target_margin_x4;
|
||||
|
||||
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 */
|
||||
/*
|
||||
* 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 <stdint.h>
|
||||
|
||||
namespace mesh {
|
||||
|
||||
class Packet;
|
||||
|
||||
class PowerController {
|
||||
public:
|
||||
PowerController();
|
||||
|
||||
/* Enable/disable APC. When disabled, APC tracking/math is bypassed. */
|
||||
void setEnabled(bool en);
|
||||
bool isEnabled() const { return _enabled; }
|
||||
|
||||
/* Set current spreading factor (needed for margin calculation). */
|
||||
void setSF(uint8_t sf) { _sf = sf; }
|
||||
|
||||
/* Set target link margin in dB. Higher = more conservative
|
||||
* (better for networks with poor-RX hardware). Default 16 dB. */
|
||||
void setTargetMargin(uint8_t margin_db) { _target_margin_x4 = (int)margin_db * 4; }
|
||||
uint8_t getTargetMargin() const { return (uint8_t)(_target_margin_x4 / 4); }
|
||||
|
||||
/* 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 */
|
||||
static constexpr int DEFAULT_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;
|
||||
int _target_margin_x4;
|
||||
|
||||
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 */
|
||||
|
||||
+278
-256
@@ -1,256 +1,278 @@
|
||||
/*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
* Adaptive Power Control — echo-based TX power reduction
|
||||
*/
|
||||
|
||||
#include <mesh/PowerController.h>
|
||||
#include <mesh/Packet.h>
|
||||
#include <string.h>
|
||||
|
||||
#include <zephyr/logging/log.h>
|
||||
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), _target_margin_x4(DEFAULT_TARGET_MARGIN_X4)
|
||||
{
|
||||
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 */
|
||||
/*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
* Adaptive Power Control — echo-based TX power reduction
|
||||
*/
|
||||
|
||||
#include <mesh/PowerController.h>
|
||||
#include <mesh/Packet.h>
|
||||
#include <string.h>
|
||||
|
||||
#include <zephyr/logging/log.h>
|
||||
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), _target_margin_x4(DEFAULT_TARGET_MARGIN_X4)
|
||||
{
|
||||
memset(_ring, 0, sizeof(_ring));
|
||||
}
|
||||
|
||||
void PowerController::setEnabled(bool en)
|
||||
{
|
||||
if (_enabled == en) return;
|
||||
_enabled = en;
|
||||
if (!_enabled) {
|
||||
/* Drop APC runtime state while disabled so no tracking work runs. */
|
||||
memset(_ring, 0, sizeof(_ring));
|
||||
_next_idx = 0;
|
||||
_margin_ema_x256 = 0;
|
||||
_finalized_count = 0;
|
||||
_last_echo_ms = 0;
|
||||
_last_source_count = 0;
|
||||
_power_reduction_db = 0;
|
||||
}
|
||||
}
|
||||
|
||||
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 (!_enabled) return;
|
||||
|
||||
/* 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)
|
||||
{
|
||||
if (!_enabled) return false;
|
||||
|
||||
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)
|
||||
{
|
||||
if (!_enabled) return;
|
||||
|
||||
/* 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 */
|
||||
|
||||
Reference in New Issue
Block a user