From a93412216a4213a6a21e0529efb402c87cd52278 Mon Sep 17 00:00:00 2001
From: Scott Powell <sqij@protonmail.com>
Date: Tue, 4 Feb 2025 15:00:28 +1100
Subject: [PATCH] * new packet score function

---
 src/helpers/CustomLLCC68Wrapper.h       |  7 +------
 src/helpers/CustomSX1262Wrapper.h       |  7 +------
 src/helpers/CustomSX1268Wrapper.h       |  7 +------
 src/helpers/RadioLibWrappers.cpp        | 21 +++++++++++++++++++++
 src/helpers/RadioLibWrappers.h          |  3 ++-
 src/helpers/StaticPoolPacketManager.cpp |  4 ++--
 6 files changed, 28 insertions(+), 21 deletions(-)

diff --git a/src/helpers/CustomLLCC68Wrapper.h b/src/helpers/CustomLLCC68Wrapper.h
index b971fe34..4777e8df 100644
--- a/src/helpers/CustomLLCC68Wrapper.h
+++ b/src/helpers/CustomLLCC68Wrapper.h
@@ -21,11 +21,6 @@ public:
 
   float packetScore(float snr, int packet_len) override {
     int sf = ((CustomLLCC68 *)_radio)->spreadingFactor;
-    const float A = 0.7;
-    const float B = 0.4;
-  
-    float ber = exp(-pow(10, (snr / 10)) / (A * pow(10, (snr / 10)) + B * (1 << sf)));
-
-    return pow(1 - ber, packet_len * 8);
+    return packetScoreInt(snr, sf, packet_len);
   }
 };
diff --git a/src/helpers/CustomSX1262Wrapper.h b/src/helpers/CustomSX1262Wrapper.h
index 24676b4c..9bfb57cd 100644
--- a/src/helpers/CustomSX1262Wrapper.h
+++ b/src/helpers/CustomSX1262Wrapper.h
@@ -22,11 +22,6 @@ public:
 
   float packetScore(float snr, int packet_len) override {
     int sf = ((CustomSX1262 *)_radio)->spreadingFactor;
-    const float A = 0.7;
-    const float B = 0.4;
-  
-    float ber = exp(-pow(10, (snr / 10)) / (A * pow(10, (snr / 10)) + B * (1 << sf)));
-
-    return pow(1 - ber, packet_len * 8);
+    return packetScoreInt(snr, sf, packet_len);
   }
 };
diff --git a/src/helpers/CustomSX1268Wrapper.h b/src/helpers/CustomSX1268Wrapper.h
index 6120c4a1..bd6ddfaf 100644
--- a/src/helpers/CustomSX1268Wrapper.h
+++ b/src/helpers/CustomSX1268Wrapper.h
@@ -21,11 +21,6 @@ public:
 
   float packetScore(float snr, int packet_len) override {
     int sf = ((CustomSX1268 *)_radio)->spreadingFactor;
-    const float A = 0.7;
-    const float B = 0.4;
-  
-    float ber = exp(-pow(10, (snr / 10)) / (A * pow(10, (snr / 10)) + B * (1 << sf)));
-
-    return pow(1 - ber, packet_len * 8);
+    return packetScoreInt(snr, sf, packet_len);
   }
 };
diff --git a/src/helpers/RadioLibWrappers.cpp b/src/helpers/RadioLibWrappers.cpp
index ef035638..41abb382 100644
--- a/src/helpers/RadioLibWrappers.cpp
+++ b/src/helpers/RadioLibWrappers.cpp
@@ -96,3 +96,24 @@ float RadioLibWrapper::getLastRSSI() const {
 float RadioLibWrapper::getLastSNR() const {
   return _radio->getSNR();
 }
+
+// Approximate SNR threshold per SF for successful reception (based on Semtech datasheets)
+static float snr_threshold[] = {
+    -7.5,  // SF7 needs at least -7.5 dB SNR
+    -10,   // SF8 needs at least -10 dB SNR
+    -12.5, // SF9 needs at least -12.5 dB SNR
+    -15,  // SF10 needs at least -15 dB SNR
+    -17.5,// SF11 needs at least -17.5 dB SNR
+    -20   // SF12 needs at least -20 dB SNR
+};
+  
+float RadioLibWrapper::packetScoreInt(float snr, int sf, int packet_len) {
+  if (sf < 7) return 0.0f;
+  
+  if (snr < snr_threshold[sf - 7]) return 0.0f;    // Below threshold, no chance of success
+
+  auto success_rate_based_on_snr = (snr - snr_threshold[sf - 7]) / 10.0;
+  auto collision_penalty = 1 - (packet_len / 256.0);   // Assuming max packet of 256 bytes
+
+  return max(0.0, min(1.0, success_rate_based_on_snr * collision_penalty));
+}
diff --git a/src/helpers/RadioLibWrappers.h b/src/helpers/RadioLibWrappers.h
index 448af5a2..a4ec52cf 100644
--- a/src/helpers/RadioLibWrappers.h
+++ b/src/helpers/RadioLibWrappers.h
@@ -10,6 +10,7 @@ protected:
   uint32_t n_recv, n_sent;
 
   void idle();
+  float packetScoreInt(float snr, int sf, int packet_len);
 
 public:
   RadioLibWrapper(PhysicalLayer& radio, mesh::MainBoard& board) : _radio(&radio), _board(&board) { n_recv = n_sent = 0; }
@@ -26,7 +27,7 @@ public:
   virtual float getLastRSSI() const override;
   virtual float getLastSNR() const override;
 
-  float packetScore(float snr, int packet_len) override { return 0.85f; }  // stub impl
+  float packetScore(float snr, int packet_len) override { return packetScoreInt(snr, 10, packet_len); }  // assume sf=10
 };
 
 /**
diff --git a/src/helpers/StaticPoolPacketManager.cpp b/src/helpers/StaticPoolPacketManager.cpp
index 3cd0906e..07bc8f2e 100644
--- a/src/helpers/StaticPoolPacketManager.cpp
+++ b/src/helpers/StaticPoolPacketManager.cpp
@@ -97,8 +97,8 @@ mesh::Packet* StaticPoolPacketManager::removeOutboundByIdx(int i) {
 }
 
 void StaticPoolPacketManager::queueInbound(mesh::Packet* packet, uint32_t scheduled_for) {
-  // TODO
+  rx_queue.add(packet, 0, scheduled_for);
 }
 mesh::Packet* StaticPoolPacketManager::getNextInbound(uint32_t now) {
-  return NULL;  // TODO
+  return rx_queue.get(now);
 }