diff --git a/src/core.cpp b/src/core.cpp
index a1c6330..1eca711 100644
--- a/src/core.cpp
+++ b/src/core.cpp
@@ -18,6 +18,9 @@ uint16_t Scan::rssiMethod(uint16_t *result)
     for (int r = 0; r < SAMPLES_RSSI; r++)
     {
         float rssi = getRSSI();
+        if (rssi < -65535)
+            rssi = -65535;
+
         uint16_t abs_rssi = abs(rssi);
         if (abs_rssi < max_signal)
         {
@@ -34,8 +37,16 @@ uint16_t Scan::rssiMethod(uint16_t *result)
         }
         else if (RSSI_OUTPUT_FORMULA == 2)
         {
-            // I like this formula better
-            result_index = uint8_t(abs(rssi) / 2) - 22;
+            if (rssi > HI_RSSI_THRESHOLD)
+            {
+                rssi = HI_RSSI_THRESHOLD;
+            }
+            else if (rssi < LO_RSSI_THRESHOLD)
+            {
+                rssi = LO_RSSI_THRESHOLD;
+            }
+
+            result_index = uint8_t((HI_RSSI_THRESHOLD - rssi) * scale);
         }
 
         if (result_index >= res_size)
diff --git a/src/core.h b/src/core.h
index bdcaa51..36f449c 100644
--- a/src/core.h
+++ b/src/core.h
@@ -31,7 +31,7 @@ constexpr float LO_RSSI_THRESHOLD = HI_RSSI_THRESHOLD - 66;
 struct Scan
 {
     Scan(int sz)
-        : res_size(sz)
+        : res_size(sz), scale((float)sz / (HI_RSSI_THRESHOLD - LO_RSSI_THRESHOLD + 0.1))
     {
     }
 
@@ -40,6 +40,7 @@ struct Scan
     uint16_t rssiMethod(uint16_t *result);
 
     int res_size;
+    float scale;
 };
 
 #endif