Fix airtime calculation in dispatcher

src/Dispatcher.cpp

@@ -273,11 +273,15 @@ void Dispatcher::processRecvPacket(Packet* pkt) {
 }
 
 void Dispatcher::checkSend() {
-  if (_mgr->getOutboundCount(_ms->getMillis()) == 0) return;
+  unsigned long now = _ms->getMillis();
+  if (_mgr->getOutboundCount(now) == 0) return;
 
   updateTxBudget();
 
-  uint32_t est_airtime = _radio->getEstAirtimeFor(MAX_TRANS_UNIT);
+  Packet* next_outbound = _mgr->peekNextOutbound(now);
+  if (next_outbound == NULL) return;
+
+  uint32_t est_airtime = _radio->getEstAirtimeFor(next_outbound->getRawLength());
   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;
@@ -304,7 +308,7 @@ void Dispatcher::checkSend() {
   }
   cad_busy_start = 0;  // reset busy state
 
-  outbound = _mgr->getNextOutbound(_ms->getMillis());
+  outbound = _mgr->getNextOutbound(now);
   if (outbound) {
     int len = 0;
     uint8_t raw[MAX_TRANS_UNIT];
@@ -387,4 +391,4 @@ unsigned long Dispatcher::futureMillis(int millis_from_now) const {
   return _ms->getMillis() + millis_from_now;
 }
 
-}
+}

src/Dispatcher.h

@@ -91,6 +91,7 @@ class PacketManager {
 
   virtual void queueOutbound(Packet* packet, uint8_t priority, uint32_t scheduled_for) = 0;
   virtual Packet* getNextOutbound(uint32_t now) = 0;    // by priority
+  virtual Packet* peekNextOutbound(uint32_t now) = 0;   // by priority, without removing from queue
   virtual int getOutboundCount(uint32_t now) const = 0;
   virtual int getOutboundTotal() const = 0;
   virtual int getFreeCount() const = 0;

src/helpers/StaticPoolPacketManager.cpp

@@ -19,7 +19,7 @@ int PacketQueue::countBefore(uint32_t now) const {
   return n;
 }
 
-mesh::Packet* PacketQueue::get(uint32_t now) {
+int PacketQueue::findBest(uint32_t now) const {
   uint8_t min_pri = 0xFF;
   int best_idx = -1;
   for (int j = 0; j < _num; j++) {
@@ -29,6 +29,17 @@ mesh::Packet* PacketQueue::get(uint32_t now) {
       best_idx = j;
     }
   }
+  return best_idx;
+}
+
+mesh::Packet* PacketQueue::peek(uint32_t now) const {
+  int best_idx = findBest(now);
+  if (best_idx < 0) return NULL;   // empty, or all items are still in the future
+  return _table[best_idx];
+}
+
+mesh::Packet* PacketQueue::get(uint32_t now) {
+  int best_idx = findBest(now);
   if (best_idx < 0) return NULL;   // empty, or all items are still in the future
 
   mesh::Packet* top = _table[best_idx];
@@ -95,6 +106,10 @@ mesh::Packet* StaticPoolPacketManager::getNextOutbound(uint32_t now) {
   return send_queue.get(now);
 }
 
+mesh::Packet* StaticPoolPacketManager::peekNextOutbound(uint32_t now) {
+  return send_queue.peek(now);
+}
+
 int  StaticPoolPacketManager::getOutboundCount(uint32_t now) const {
   return send_queue.countBefore(now);
 }

src/helpers/StaticPoolPacketManager.h

@@ -7,10 +7,12 @@ class PacketQueue {
   uint8_t* _pri_table;
   uint32_t* _schedule_table;
   int _size, _num;
+  int findBest(uint32_t now) const;
 
 public:
   PacketQueue(int max_entries);
   mesh::Packet* get(uint32_t now);
+  mesh::Packet* peek(uint32_t now) const;
   bool add(mesh::Packet* packet, uint8_t priority, uint32_t scheduled_for);
   int count() const { return _num; }
   int countBefore(uint32_t now) const;
@@ -28,11 +30,12 @@ class StaticPoolPacketManager : public mesh::PacketManager {
   void free(mesh::Packet* packet) override;
   void queueOutbound(mesh::Packet* packet, uint8_t priority, uint32_t scheduled_for) override;
   mesh::Packet* getNextOutbound(uint32_t now) override;
+  mesh::Packet* peekNextOutbound(uint32_t now) override;
   int getOutboundCount(uint32_t now) const override;
   int getOutboundTotal() const override;
   int getFreeCount() const override;
   mesh::Packet* getOutboundByIdx(int i) override;
   mesh::Packet* removeOutboundByIdx(int i) override;
   void queueInbound(mesh::Packet* packet, uint32_t scheduled_for) override;
   mesh::Packet* getNextInbound(uint32_t now) override;
-};
+};