From 2ae0188e50399739720c8cfa75ab849170d379b7 Mon Sep 17 00:00:00 2001
From: Libor Tomsik <libor@tomsik.eu>
Date: Thu, 15 Apr 2021 08:05:05 +0200
Subject: [PATCH] Created method for setting AFC bandwidth

---
 src/modules/SX127x/SX127x.cpp | 58 +++++++++++++++++++++++------------
 src/modules/SX127x/SX127x.h   | 20 ++++++++++--
 2 files changed, 56 insertions(+), 22 deletions(-)

diff --git a/src/modules/SX127x/SX127x.cpp b/src/modules/SX127x/SX127x.cpp
index b508cd7c..ae9f3995 100644
--- a/src/modules/SX127x/SX127x.cpp
+++ b/src/modules/SX127x/SX127x.cpp
@@ -84,8 +84,15 @@ int16_t SX127x::beginFSK(uint8_t chipVersion, float br, float freqDev, float rxB
   state = SX127x::setBitRate(br);
   RADIOLIB_ASSERT(state);
 
-  // set frequency deviation
-  state = SX127x::setFrequencyDeviation(freqDev);
+  // set receiver bandwidth
+  state = SX127x::setRxBandwidth(rxBw);
+  RADIOLIB_ASSERT(state);
+
+  //set AFC bandwidth
+  state = SX127x::setAFCBandwidth(rxBw);
+  RADIOLIB_ASSERT(state);
+
+  state = _mod->SPIsetRegValue(SX127X_REG_RX_CONFIG, 0x1E);//TODO
   RADIOLIB_ASSERT(state);
 
   // set receiver bandwidth
@@ -731,6 +738,19 @@ int16_t SX127x::setFrequencyDeviation(float freqDev) {
   return(state);
 }
 
+uint8_t SX127x::calculateBWManExp(float bandwidth) const
+{
+  for(uint8_t e = 7; e >= 1; e--) {
+    for(int8_t m = 2; m >= 0; m--) {
+      float point = (SX127X_CRYSTAL_FREQ * 1000000.0)/(((4 * m) + 16) * ((uint32_t)1 << (e + 2)));
+      if(abs(bandwidth - ((point / 1000.0) + 0.05)) <= 0.5) {
+        return((m << 3) | e);
+      }
+    }
+  }
+  return 0;
+}
+
 int16_t SX127x::setRxBandwidth(float rxBw) {
   // check active modem
   if(getActiveModem() != SX127X_FSK_OOK) {
@@ -743,26 +763,24 @@ int16_t SX127x::setRxBandwidth(float rxBw) {
   int16_t state = setMode(SX127X_STANDBY);
   RADIOLIB_ASSERT(state);
 
-  // calculate exponent and mantissa values
-  for(uint8_t e = 7; e >= 1; e--) {
-    for(int8_t m = 2; m >= 0; m--) {
-      float point = (SX127X_CRYSTAL_FREQ * 1000000.0)/(((4 * m) + 16) * ((uint32_t)1 << (e + 2)));
-      if(abs(rxBw - ((point / 1000.0) + 0.05)) <= 0.5) {
-        // set Rx bandwidth during AFC
-        state = _mod->SPIsetRegValue(SX127X_REG_AFC_BW, (m << 3) | e, 4, 0);
-        RADIOLIB_ASSERT(state);
+  // set Rx bandwidth
+  return(_mod->SPIsetRegValue(SX127X_REG_RX_BW, calculateBWManExp(rxBw), 4, 0));
+}
 
-        // set Rx bandwidth
-        state = _mod->SPIsetRegValue(SX127X_REG_RX_BW, (m << 3) | e, 4, 0);
-        if(state == ERR_NONE) {
-          SX127x::_rxBw = rxBw;
-        }
-
-        return(state);
-      }
+int16_t SX127x::setAFCBandwidth(float rxBw){
+    // check active modem
+    if(getActiveModem() != SX127X_FSK_OOK){
+        return(ERR_WRONG_MODEM);
     }
-  }
-  return(ERR_UNKNOWN);
+
+    RADIOLIB_CHECK_RANGE(rxBw, 2.6, 250.0, ERR_INVALID_RX_BANDWIDTH);
+
+    // set mode to STANDBY
+    int16_t state = setMode(SX127X_STANDBY);
+    RADIOLIB_ASSERT(state);
+
+    // set AFC bandwidth
+    return(_mod->SPIsetRegValue(SX127X_REG_AFC_BW, calculateBWManExp(rxBw), 4, 0));
 }
 
 int16_t SX127x::setSyncWord(uint8_t* syncWord, size_t len) {
diff --git a/src/modules/SX127x/SX127x.h b/src/modules/SX127x/SX127x.h
index 5ed17ddb..7508f6c6 100644
--- a/src/modules/SX127x/SX127x.h
+++ b/src/modules/SX127x/SX127x.h
@@ -814,6 +814,15 @@ class SX127x: public PhysicalLayer {
     */
     int16_t setRxBandwidth(float rxBw);
 
+    /*!
+      \brief Sets FSK automatic frequency correction bandwidth. Allowed values range from 2.6 to 250 kHz. Only available in FSK mode.
+
+      \param rxBw Receiver AFC bandwidth to be set (in kHz).
+
+      \returns \ref status_codes
+    */
+    int16_t setAFCBandwidth(float afcBw);
+
     /*!
       \brief Sets FSK sync word. Allowed sync words are up to 8 bytes long and can not contain null bytes. Only available in FSK mode.
 
@@ -993,7 +1002,7 @@ class SX127x: public PhysicalLayer {
     int16_t invertIQ(bool invertIQ);
 
 #if !defined(RADIOLIB_GODMODE) && !defined(RADIOLIB_LOW_LEVEL)
-  protected:
+protected:
 #endif
     Module* _mod;
 
@@ -1006,7 +1015,6 @@ class SX127x: public PhysicalLayer {
     uint8_t _sf = 0;
     uint8_t _cr = 0;
     float _br = 0;
-    float _rxBw = 0;
     bool _ook = false;
     bool _crcEnabled = false;
     size_t _packetLength = 0;
@@ -1030,6 +1038,14 @@ class SX127x: public PhysicalLayer {
     int16_t setActiveModem(uint8_t modem);
     void clearIRQFlags();
     void clearFIFO(size_t count); // used mostly to clear remaining bytes in FIFO after a packet read
+    /**
+     * @brief Calculate exponent and mantissa values for receiver bandwidth and AFC
+     *
+     * \param bandwidth bandwidth to be set (in kHz).
+     *
+     * \returns bandwidth in manitsa and exponent format
+     */
+    uint8_t calculateBWManExp(float bandwidth) const;
 };
 
 #endif