Merge pull request #282 from yeckel/feature/implement_AFC_control

Feature/implement afc control

keywords.txt

@@ -104,10 +104,14 @@ setPreambleLength	KEYWORD2
 setGain	KEYWORD2
 getFrequencyError	KEYWORD2
 getRSSI	KEYWORD2
+getAFCError	KEYWORD2
 getSNR	KEYWORD2
 getDataRate	KEYWORD2
 setBitRate	KEYWORD2
 setRxBandwidth	KEYWORD2
+setAFCBandwidth	KEYWORD2
+setAFC		KEYWORD2
+setAFCAGCTrigger	KEYWORD2
 setFrequencyDeviation	KEYWORD2
 setNodeAddress	KEYWORD2
 setBroadcastAddress	KEYWORD2

src/modules/SX127x/SX127x.cpp

@@ -88,6 +88,17 @@ int16_t SX127x::beginFSK(uint8_t chipVersion, float br, float freqDev, float rxB
   state = SX127x::setFrequencyDeviation(freqDev);
   RADIOLIB_ASSERT(state);
 
+  //set AFC bandwidth
+  state = SX127x::setAFCBandwidth(rxBw);
+  RADIOLIB_ASSERT(state);
+
+  //sets AFC&AGC trigger to RSSI and preamble detect
+  state = SX127x::setAFCAGCTrigger(SX127X_RX_TRIGGER_BOTH);
+  RADIOLIB_ASSERT(state);
+
+  state = SX127x::setAFC(true);
+  RADIOLIB_ASSERT(state);
+
   // set receiver bandwidth
   state = SX127x::setRxBandwidth(rxBw);
   RADIOLIB_ASSERT(state);
@@ -658,6 +669,22 @@ float SX127x::getFrequencyError(bool autoCorrect) {
   return(ERR_UNKNOWN);
 }
 
+float SX127x::getAFCError()
+{
+  // check active modem
+  int16_t modem = getActiveModem();
+  if(modem != SX127X_FSK_OOK) {
+    return 0;
+  }
+
+  // get raw frequency error
+  int16_t raw = (uint16_t)_mod->SPIreadRegister(SX127X_REG_AFC_MSB) << 8;
+  raw |= _mod->SPIreadRegister(SX127X_REG_AFC_LSB);
+
+  uint32_t base = 1;
+  return raw * (32000000.0 / (float)(base << 19));
+}
+
 float SX127x::getSNR() {
   // check active modem
   if(getActiveModem() != SX127X_LORA) {
@@ -731,6 +758,19 @@ int16_t SX127x::setFrequencyDeviation(float freqDev) {
   return(state);
 }
 
+uint8_t SX127x::calculateBWManExp(float bandwidth)
+{
+  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 +783,43 @@ int16_t SX127x::setRxBandwidth(float rxBw) {
   int16_t state = setMode(SX127X_STANDBY);
   RADIOLIB_ASSERT(state);
 
-  // calculate exponent and mantissa values
+  // set Rx bandwidth
-  for(uint8_t e = 7; e >= 1; e--) {
+  return(_mod->SPIsetRegValue(SX127X_REG_RX_BW, calculateBWManExp(rxBw), 4, 0));
-    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) {
+int16_t SX127x::setAFCBandwidth(float rxBw){
-        // set Rx bandwidth during AFC
+  // check active modem
-        state = _mod->SPIsetRegValue(SX127X_REG_AFC_BW, (m << 3) | e, 4, 0);
+  if(getActiveModem() != SX127X_FSK_OOK){
+      return(ERR_WRONG_MODEM);
+  }
+
+  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 Rx bandwidth
+  // set AFC bandwidth
-        state = _mod->SPIsetRegValue(SX127X_REG_RX_BW, (m << 3) | e, 4, 0);
+  return(_mod->SPIsetRegValue(SX127X_REG_AFC_BW, calculateBWManExp(rxBw), 4, 0));
-        if(state == ERR_NONE) {
-          SX127x::_rxBw = rxBw;
 }
 
-        return(state);
+int16_t SX127x::setAFC(bool isEnabled){
+  // check active modem
+  if(getActiveModem() != SX127X_FSK_OOK) {
+    return(ERR_WRONG_MODEM);
   }
+
+  //set AFC auto on/off
+  return(_mod->SPIsetRegValue(SX127X_REG_RX_CONFIG, isEnabled ? SX127X_AFC_AUTO_ON : SX127X_AFC_AUTO_OFF, 4, 4));
 }
+
+int16_t SX127x::setAFCAGCTrigger(uint8_t trigger){
+  if(getActiveModem() != SX127X_FSK_OOK) {
+    return(ERR_WRONG_MODEM);
   }
-  return(ERR_UNKNOWN);
+
+  //set AFC&AGC trigger
+  return(_mod->SPIsetRegValue(SX127X_REG_RX_CONFIG, trigger, 2, 0));
 }
 
 int16_t SX127x::setSyncWord(uint8_t* syncWord, size_t len) {

src/modules/SX127x/SX127x.h

@@ -773,6 +773,13 @@ class SX127x: public PhysicalLayer {
     */
     float getFrequencyError(bool autoCorrect = false);
 
+    /*!
+      \brief Gets current AFC error.
+
+      \returns Frequency offset from RF in Hz if AFC is enabled and triggered, zero otherwise.
+    */
+    float getAFCError();
+
     /*!
       \brief Gets signal-to-noise ratio of the latest received packet.
 
@@ -814,6 +821,33 @@ 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 Enables or disables FSK automatic frequency correction(AFC)
+
+      \param isEnabled AFC enabled or disabled
+
+      \return \ref status_codes
+    */
+    int16_t setAFC(bool isEnabled);
+
+    /*!
+      \brief Controls trigger of AFC and AGC
+
+      \param trigger one from SX127X_RX_TRIGGER_NONE, SX127X_RX_TRIGGER_RSSI_INTERRUPT, SX127X_RX_TRIGGER_PREAMBLE_DETECT, SX127X_RX_TRIGGER_BOTH
+
+      \return \ref status_codes
+    */
+    int16_t setAFCAGCTrigger(uint8_t trigger);
+
     /*!
       \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.
 
@@ -1006,7 +1040,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 +1063,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
+     */
+    static uint8_t calculateBWManExp(float bandwidth);
 };
 
 #endif