mirror of
https://github.com/pelgraine/Meck.git
synced 2026-10-31 04:02:01 +01:00
Pelgraine
973d02df60
T-Watch: So many UI changes. Also swapped render battery indicator to use the AXP2101 gauge for hopefully higher accuracy. Also implemnted meck rx duty cycle for watch builds only to test out power saving effects first on watch build before implementing elsewhere.
427 lines
15 KiB
C++
427 lines
15 KiB
C++
#include "Dispatcher.h"
|
|
|
|
#if MESH_PACKET_LOGGING
|
|
#include <Arduino.h>
|
|
#endif
|
|
|
|
#include <math.h>
|
|
|
|
namespace mesh {
|
|
|
|
#define MAX_RX_DELAY_MILLIS 32000 // 32 seconds
|
|
|
|
// Rolling-window duty-cycle (token bucket) tuning -- see issue #20
|
|
#define MIN_TX_BUDGET_RESERVE_MS 100 // min budget (ms) required before allowing the next TX
|
|
#define MIN_TX_BUDGET_AIRTIME_DIV 2 // require at least 1/N of estimated airtime as budget before TX
|
|
|
|
#ifndef NOISE_FLOOR_CALIB_INTERVAL
|
|
#define NOISE_FLOOR_CALIB_INTERVAL 2000 // 2 seconds
|
|
#endif
|
|
|
|
void Dispatcher::begin() {
|
|
n_sent_flood = n_sent_direct = 0;
|
|
n_recv_flood = n_recv_direct = 0;
|
|
_err_flags = 0;
|
|
radio_nonrx_start = _ms->getMillis();
|
|
|
|
// Initialise the rolling-window TX budget (token bucket) to a full window's
|
|
// worth of airtime. duty_cycle = 1 / (1 + airtime_factor): factor 2.0 -> 1/3 (33%).
|
|
duty_cycle_window_ms = getDutyCycleWindowMs();
|
|
float duty_cycle = 1.0f / (1.0f + getAirtimeBudgetFactor());
|
|
tx_budget_ms = (unsigned long)(duty_cycle_window_ms * duty_cycle);
|
|
last_budget_update = _ms->getMillis();
|
|
|
|
_radio->begin();
|
|
prev_isrecv_mode = _radio->isInRecvMode();
|
|
}
|
|
|
|
float Dispatcher::getAirtimeBudgetFactor() const {
|
|
return 2.0; // default, 33.3% (1/3rd)
|
|
}
|
|
|
|
// Refill the rolling-window TX budget proportionally to idle time elapsed since
|
|
// the last update, capped at one window's worth. Cheap; safe to call often.
|
|
void Dispatcher::updateTxBudget() {
|
|
unsigned long now = _ms->getMillis();
|
|
unsigned long elapsed = now - last_budget_update;
|
|
|
|
float duty_cycle = 1.0f / (1.0f + getAirtimeBudgetFactor());
|
|
unsigned long max_budget = (unsigned long)(getDutyCycleWindowMs() * duty_cycle);
|
|
unsigned long refill = (unsigned long)(elapsed * duty_cycle);
|
|
|
|
if (refill > 0) {
|
|
tx_budget_ms += refill;
|
|
if (tx_budget_ms > max_budget) {
|
|
tx_budget_ms = max_budget;
|
|
}
|
|
last_budget_update = now;
|
|
}
|
|
}
|
|
|
|
int Dispatcher::calcRxDelay(float score, uint32_t air_time) const {
|
|
return (int) ((pow(10, 0.85f - score) - 1.0) * air_time);
|
|
}
|
|
|
|
uint32_t Dispatcher::getCADFailRetryDelay() const {
|
|
return 200;
|
|
}
|
|
uint32_t Dispatcher::getCADFailMaxDuration() const {
|
|
return 6000; // 6 seconds
|
|
}
|
|
|
|
void Dispatcher::loop() {
|
|
if (millisHasNowPassed(next_floor_calib_time)) {
|
|
_radio->triggerNoiseFloorCalibrate(getInterferenceThreshold());
|
|
next_floor_calib_time = futureMillis(NOISE_FLOOR_CALIB_INTERVAL);
|
|
}
|
|
_radio->loop();
|
|
|
|
// check for radio 'stuck' in mode other than Rx
|
|
bool is_recv = _radio->isInRecvMode();
|
|
if (is_recv != prev_isrecv_mode) {
|
|
prev_isrecv_mode = is_recv;
|
|
if (!is_recv) {
|
|
radio_nonrx_start = _ms->getMillis();
|
|
} else {
|
|
rx_stuck_count = 0; // radio recovered — reset counter
|
|
}
|
|
}
|
|
#if !defined(MECK_RX_DUTY_CYCLE)
|
|
if (!is_recv && _ms->getMillis() - radio_nonrx_start > 8000) { // radio has not been in Rx mode for 8 seconds!
|
|
_err_flags |= ERR_EVENT_STARTRX_TIMEOUT;
|
|
|
|
rx_stuck_count++;
|
|
MESH_DEBUG_PRINTLN("%s Dispatcher::loop(): RX stuck (attempt %d), calling onRxStuck()", getLogDateTime(), rx_stuck_count);
|
|
onRxStuck();
|
|
|
|
uint8_t reboot_threshold = getRxFailRebootThreshold();
|
|
if (reboot_threshold > 0 && rx_stuck_count >= reboot_threshold) {
|
|
MESH_DEBUG_PRINTLN("%s Dispatcher::loop(): RX unrecoverable after %d attempts", getLogDateTime(), rx_stuck_count);
|
|
onRxUnrecoverable();
|
|
}
|
|
|
|
// Reset state to give recovery the full 8s window before re-triggering
|
|
radio_nonrx_start = _ms->getMillis();
|
|
prev_isrecv_mode = true;
|
|
cad_busy_start = 0;
|
|
next_agc_reset_time = futureMillis(getAGCResetInterval());
|
|
}
|
|
#endif
|
|
|
|
if (outbound) { // waiting for outbound send to be completed
|
|
if (_radio->isSendComplete()) {
|
|
long t = _ms->getMillis() - outbound_start;
|
|
total_air_time += t; // keep track of how much air time we are using
|
|
//Serial.print(" airtime="); Serial.println(t);
|
|
|
|
// Spend this transmission's airtime from the rolling-window budget.
|
|
updateTxBudget();
|
|
if (t > (long)tx_budget_ms) {
|
|
tx_budget_ms = 0;
|
|
} else {
|
|
tx_budget_ms -= t;
|
|
}
|
|
if (tx_budget_ms < MIN_TX_BUDGET_RESERVE_MS) {
|
|
float duty_cycle = 1.0f / (1.0f + getAirtimeBudgetFactor());
|
|
unsigned long needed = MIN_TX_BUDGET_RESERVE_MS - tx_budget_ms;
|
|
next_tx_time = futureMillis((unsigned long)(needed / duty_cycle));
|
|
} else {
|
|
next_tx_time = _ms->getMillis();
|
|
}
|
|
|
|
_radio->onSendFinished();
|
|
logTx(outbound, 2 + outbound->getPathByteLen() + outbound->payload_len);
|
|
if (outbound->isRouteFlood()) {
|
|
n_sent_flood++;
|
|
} else {
|
|
n_sent_direct++;
|
|
}
|
|
releasePacket(outbound); // return to pool
|
|
outbound = NULL;
|
|
} else if (millisHasNowPassed(outbound_expiry)) {
|
|
MESH_DEBUG_PRINTLN("%s Dispatcher::loop(): WARNING: outbound packed send timed out!", getLogDateTime());
|
|
|
|
_radio->onSendFinished();
|
|
logTxFail(outbound, 2 + outbound->getPathByteLen() + outbound->payload_len);
|
|
|
|
releasePacket(outbound); // return to pool
|
|
outbound = NULL;
|
|
} else {
|
|
return; // can't do any more radio activity until send is complete or timed out
|
|
}
|
|
|
|
// going back into receive mode now...
|
|
next_agc_reset_time = futureMillis(getAGCResetInterval());
|
|
}
|
|
|
|
#if !defined(MECK_RX_DUTY_CYCLE)
|
|
if (getAGCResetInterval() > 0 && millisHasNowPassed(next_agc_reset_time)) {
|
|
_radio->resetAGC();
|
|
next_agc_reset_time = futureMillis(getAGCResetInterval());
|
|
}
|
|
#endif
|
|
|
|
// check inbound (delayed) queue
|
|
{
|
|
Packet* pkt = _mgr->getNextInbound(_ms->getMillis());
|
|
if (pkt) {
|
|
processRecvPacket(pkt);
|
|
}
|
|
}
|
|
checkRecv();
|
|
checkSend();
|
|
}
|
|
|
|
void Dispatcher::checkRecv() {
|
|
Packet* pkt;
|
|
float score;
|
|
uint32_t air_time;
|
|
{
|
|
uint8_t raw[MAX_TRANS_UNIT+1];
|
|
int len = _radio->recvRaw(raw, MAX_TRANS_UNIT);
|
|
if (len > 0) {
|
|
logRxRaw(_radio->getLastSNR(), _radio->getLastRSSI(), raw, len);
|
|
|
|
pkt = _mgr->allocNew();
|
|
if (pkt == NULL) {
|
|
MESH_DEBUG_PRINTLN("%s Dispatcher::checkRecv(): WARNING: received data, no unused packets available!", getLogDateTime());
|
|
} else {
|
|
int i = 0;
|
|
#ifdef NODE_ID
|
|
uint8_t sender_id = raw[i++];
|
|
if (sender_id == NODE_ID - 1 || sender_id == NODE_ID + 1) { // simulate that NODE_ID can only hear NODE_ID-1 or NODE_ID+1, eg. 3 can't hear 1
|
|
} else {
|
|
_mgr->free(pkt); // put back into pool
|
|
return;
|
|
}
|
|
#endif
|
|
|
|
pkt->header = raw[i++];
|
|
if (pkt->hasTransportCodes()) {
|
|
memcpy(&pkt->transport_codes[0], &raw[i], 2); i += 2;
|
|
memcpy(&pkt->transport_codes[1], &raw[i], 2); i += 2;
|
|
} else {
|
|
pkt->transport_codes[0] = pkt->transport_codes[1] = 0;
|
|
}
|
|
pkt->path_len = raw[i++];
|
|
|
|
uint16_t path_byte_len = pkt->getPathByteLen();
|
|
if (path_byte_len > MAX_PATH_SIZE || i + path_byte_len > len) {
|
|
MESH_DEBUG_PRINTLN("%s Dispatcher::checkRecv(): partial or corrupt packet received, len=%d", getLogDateTime(), len);
|
|
_mgr->free(pkt); // put back into pool
|
|
pkt = NULL;
|
|
} else {
|
|
memcpy(pkt->path, &raw[i], path_byte_len); i += path_byte_len;
|
|
|
|
pkt->payload_len = len - i; // payload is remainder
|
|
if (pkt->payload_len > sizeof(pkt->payload)) {
|
|
MESH_DEBUG_PRINTLN("%s Dispatcher::checkRecv(): packet payload too big, payload_len=%d", getLogDateTime(), (uint32_t)pkt->payload_len);
|
|
_mgr->free(pkt); // put back into pool
|
|
pkt = NULL;
|
|
} else {
|
|
memcpy(pkt->payload, &raw[i], pkt->payload_len);
|
|
|
|
pkt->_snr = _radio->getLastSNR() * 4.0f;
|
|
score = _radio->packetScore(_radio->getLastSNR(), len);
|
|
air_time = _radio->getEstAirtimeFor(len);
|
|
rx_air_time += air_time;
|
|
}
|
|
}
|
|
}
|
|
} else {
|
|
pkt = NULL;
|
|
}
|
|
}
|
|
if (pkt) {
|
|
#if MESH_PACKET_LOGGING
|
|
Serial.print(getLogDateTime());
|
|
Serial.printf(": RX, len=%d (type=%d, route=%s, payload_len=%d) SNR=%d RSSI=%d score=%d time=%d",
|
|
pkt->getRawLength(), pkt->getPayloadType(), pkt->isRouteDirect() ? "D" : "F", pkt->payload_len,
|
|
(int)pkt->getSNR(), (int)_radio->getLastRSSI(), (int)(score*1000), air_time);
|
|
|
|
static uint8_t packet_hash[MAX_HASH_SIZE];
|
|
pkt->calculatePacketHash(packet_hash);
|
|
Serial.print(" hash=");
|
|
mesh::Utils::printHex(Serial, packet_hash, MAX_HASH_SIZE);
|
|
|
|
if (pkt->getPayloadType() == PAYLOAD_TYPE_PATH || pkt->getPayloadType() == PAYLOAD_TYPE_REQ
|
|
|| pkt->getPayloadType() == PAYLOAD_TYPE_RESPONSE || pkt->getPayloadType() == PAYLOAD_TYPE_TXT_MSG) {
|
|
Serial.printf(" [%02X -> %02X]\n", (uint32_t)pkt->payload[1], (uint32_t)pkt->payload[0]);
|
|
} else {
|
|
Serial.printf("\n");
|
|
}
|
|
#endif
|
|
logRx(pkt, pkt->getRawLength(), score); // hook for custom logging
|
|
|
|
if (pkt->isRouteFlood()) {
|
|
n_recv_flood++;
|
|
|
|
int _delay = calcRxDelay(score, air_time);
|
|
if (_delay < 50) {
|
|
MESH_DEBUG_PRINTLN("%s Dispatcher::checkRecv(), score delay below threshold (%d)", getLogDateTime(), _delay);
|
|
processRecvPacket(pkt); // is below the score delay threshold, so process immediately
|
|
} else {
|
|
MESH_DEBUG_PRINTLN("%s Dispatcher::checkRecv(), score delay is: %d millis", getLogDateTime(), _delay);
|
|
if (_delay > MAX_RX_DELAY_MILLIS) {
|
|
_delay = MAX_RX_DELAY_MILLIS;
|
|
}
|
|
_mgr->queueInbound(pkt, futureMillis(_delay)); // add to delayed inbound queue
|
|
}
|
|
} else {
|
|
n_recv_direct++;
|
|
processRecvPacket(pkt);
|
|
}
|
|
}
|
|
}
|
|
|
|
void Dispatcher::processRecvPacket(Packet* pkt) {
|
|
DispatcherAction action = onRecvPacket(pkt);
|
|
if (action == ACTION_RELEASE) {
|
|
_mgr->free(pkt);
|
|
} else if (action == ACTION_MANUAL_HOLD) {
|
|
// sub-class is wanting to manually hold Packet instance, and call releasePacket() at appropriate time
|
|
} else { // ACTION_RETRANSMIT*
|
|
uint8_t priority = (action >> 24) - 1;
|
|
uint32_t _delay = action & 0xFFFFFF;
|
|
|
|
_mgr->queueOutbound(pkt, priority, futureMillis(_delay));
|
|
}
|
|
}
|
|
|
|
void Dispatcher::checkSend() {
|
|
if (_mgr->getOutboundCount(_ms->getMillis()) == 0) return; // nothing waiting to send
|
|
|
|
// Rolling-window duty-cycle gate: refill, then require enough budget for a TX.
|
|
updateTxBudget();
|
|
uint32_t est_airtime = _radio->getEstAirtimeFor(MAX_TRANS_UNIT);
|
|
if (tx_budget_ms < est_airtime / MIN_TX_BUDGET_AIRTIME_DIV) {
|
|
float duty_cycle = 1.0f / (1.0f + getAirtimeBudgetFactor());
|
|
unsigned long needed = est_airtime / MIN_TX_BUDGET_AIRTIME_DIV - tx_budget_ms;
|
|
next_tx_time = futureMillis((unsigned long)(needed / duty_cycle));
|
|
return;
|
|
}
|
|
|
|
if (!millisHasNowPassed(next_tx_time)) return; // CAD/retry backoff still pending
|
|
if (_radio->isReceiving()) { // LBT - check if radio is currently mid-receive, or if channel activity
|
|
if (cad_busy_start == 0) {
|
|
cad_busy_start = _ms->getMillis(); // record when CAD busy state started
|
|
}
|
|
|
|
if (_ms->getMillis() - cad_busy_start > getCADFailMaxDuration()) {
|
|
_err_flags |= ERR_EVENT_CAD_TIMEOUT;
|
|
|
|
MESH_DEBUG_PRINTLN("%s Dispatcher::checkSend(): CAD busy max duration reached!", getLogDateTime());
|
|
// channel activity has gone on too long... (Radio might be in a bad state)
|
|
// force the pending transmit below...
|
|
} else {
|
|
next_tx_time = futureMillis(getCADFailRetryDelay());
|
|
return;
|
|
}
|
|
}
|
|
cad_busy_start = 0; // reset busy state
|
|
|
|
outbound = _mgr->getNextOutbound(_ms->getMillis());
|
|
if (outbound) {
|
|
int len = 0;
|
|
uint8_t raw[MAX_TRANS_UNIT];
|
|
|
|
#ifdef NODE_ID
|
|
raw[len++] = NODE_ID;
|
|
#endif
|
|
raw[len++] = outbound->header;
|
|
if (outbound->hasTransportCodes()) {
|
|
memcpy(&raw[len], &outbound->transport_codes[0], 2); len += 2;
|
|
memcpy(&raw[len], &outbound->transport_codes[1], 2); len += 2;
|
|
}
|
|
raw[len++] = outbound->path_len;
|
|
uint16_t out_pbl = outbound->getPathByteLen();
|
|
memcpy(&raw[len], outbound->path, out_pbl); len += out_pbl;
|
|
|
|
if (len + outbound->payload_len > MAX_TRANS_UNIT) {
|
|
MESH_DEBUG_PRINTLN("%s Dispatcher::checkSend(): FATAL: Invalid packet queued... too long, len=%d", getLogDateTime(), len + outbound->payload_len);
|
|
_mgr->free(outbound);
|
|
outbound = NULL;
|
|
} else {
|
|
memcpy(&raw[len], outbound->payload, outbound->payload_len); len += outbound->payload_len;
|
|
|
|
uint32_t max_airtime = _radio->getEstAirtimeFor(len)*3/2;
|
|
outbound_start = _ms->getMillis();
|
|
bool success = _radio->startSendRaw(raw, len);
|
|
if (!success) {
|
|
MESH_DEBUG_PRINTLN("%s Dispatcher::checkSend(): ERROR: send start failed!", getLogDateTime());
|
|
|
|
logTxFail(outbound, outbound->getRawLength());
|
|
|
|
// re-queue packet for retry instead of dropping it
|
|
int retry_delay = getCADFailRetryDelay();
|
|
unsigned long retry_time = futureMillis(retry_delay);
|
|
_mgr->queueOutbound(outbound, 0, retry_time);
|
|
outbound = NULL;
|
|
next_tx_time = retry_time;
|
|
|
|
// count consecutive failures and reset radio if stuck
|
|
uint8_t threshold = getTxFailResetThreshold();
|
|
if (threshold > 0) {
|
|
tx_fail_count++;
|
|
if (tx_fail_count >= threshold) {
|
|
MESH_DEBUG_PRINTLN("%s Dispatcher::checkSend(): TX stuck (%d failures), resetting radio", getLogDateTime(), tx_fail_count);
|
|
onTxStuck();
|
|
tx_fail_count = 0;
|
|
next_tx_time = futureMillis(2000);
|
|
}
|
|
}
|
|
return;
|
|
}
|
|
tx_fail_count = 0; // clear counter on successful TX start
|
|
outbound_expiry = futureMillis(max_airtime);
|
|
|
|
#if MESH_PACKET_LOGGING
|
|
Serial.print(getLogDateTime());
|
|
Serial.printf(": TX, len=%d (type=%d, route=%s, payload_len=%d)",
|
|
len, outbound->getPayloadType(), outbound->isRouteDirect() ? "D" : "F", outbound->payload_len);
|
|
if (outbound->getPayloadType() == PAYLOAD_TYPE_PATH || outbound->getPayloadType() == PAYLOAD_TYPE_REQ
|
|
|| outbound->getPayloadType() == PAYLOAD_TYPE_RESPONSE || outbound->getPayloadType() == PAYLOAD_TYPE_TXT_MSG) {
|
|
Serial.printf(" [%02X -> %02X]\n", (uint32_t)outbound->payload[1], (uint32_t)outbound->payload[0]);
|
|
} else {
|
|
Serial.printf("\n");
|
|
}
|
|
#endif
|
|
}
|
|
}
|
|
}
|
|
|
|
Packet* Dispatcher::obtainNewPacket() {
|
|
auto pkt = _mgr->allocNew(); // TODO: zero out all fields
|
|
if (pkt == NULL) {
|
|
_err_flags |= ERR_EVENT_FULL;
|
|
} else {
|
|
pkt->payload_len = pkt->path_len = 0;
|
|
pkt->_snr = 0;
|
|
}
|
|
return pkt;
|
|
}
|
|
|
|
void Dispatcher::releasePacket(Packet* packet) {
|
|
_mgr->free(packet);
|
|
}
|
|
|
|
void Dispatcher::sendPacket(Packet* packet, uint8_t priority, uint32_t delay_millis) {
|
|
if (packet->getPathByteLen() > MAX_PATH_SIZE || packet->payload_len > MAX_PACKET_PAYLOAD) {
|
|
MESH_DEBUG_PRINTLN("%s Dispatcher::sendPacket(): ERROR: invalid packet... path_len=%d (byte_len=%d), payload_len=%d", getLogDateTime(), (uint32_t) packet->path_len, (uint32_t) packet->getPathByteLen(), (uint32_t) packet->payload_len);
|
|
_mgr->free(packet);
|
|
} else {
|
|
_mgr->queueOutbound(packet, priority, futureMillis(delay_millis));
|
|
}
|
|
}
|
|
|
|
// Utility function -- handles the case where millis() wraps around back to zero
|
|
// 2's complement arithmetic will handle any unsigned subtraction up to HALF the word size (32-bits in this case)
|
|
bool Dispatcher::millisHasNowPassed(unsigned long timestamp) const {
|
|
return (long)(_ms->getMillis() - timestamp) > 0;
|
|
}
|
|
|
|
unsigned long Dispatcher::futureMillis(int millis_from_now) const {
|
|
return _ms->getMillis() + millis_from_now;
|
|
}
|
|
|
|
} |