From 27e2963c9710fd01523a1dce11f511b1cd368eca Mon Sep 17 00:00:00 2001
From: torntrousers <torntrousers@gmail.com>
Date: Mon, 19 Mar 2018 00:02:47 +0000
Subject: [PATCH] Add getting the frequency error of a packet (#111)

---
 API.md       |  8 ++++++++
 keywords.txt |  1 +
 src/LoRa.cpp | 39 +++++++++++++++++++++++++++++++++++++++
 src/LoRa.h   |  3 +++
 4 files changed, 51 insertions(+)

diff --git a/API.md b/API.md
index 07c808e..0e4a8e9 100644
--- a/API.md
+++ b/API.md
@@ -166,6 +166,14 @@ float snr = LoRa.packetSnr();
 
 Returns the estimated SNR of the received packet in dB.
 
+### Packet Frequency Error
+
+```arduino
+long freqErr = LoRa.packetFrequencyError();
+```
+
+Returns the frequency error of the received packet in Hz. The frequency error is the frequency offset between the receiver centre frequency and that of an incoming LoRa signal.
+
 ### Available
 
 ```arduino
diff --git a/keywords.txt b/keywords.txt
index 6a9ba54..759bb0a 100644
--- a/keywords.txt
+++ b/keywords.txt
@@ -21,6 +21,7 @@ endPacket	KEYWORD2
 parsePacket	KEYWORD2
 packetRssi	KEYWORD2
 packetSnr	KEYWORD2
+packetFrequencyError	KEYWORD2
 
 write	KEYWORD2
 
diff --git a/src/LoRa.cpp b/src/LoRa.cpp
index 96e50dc..78eb172 100644
--- a/src/LoRa.cpp
+++ b/src/LoRa.cpp
@@ -25,6 +25,9 @@
 #define REG_PREAMBLE_LSB         0x21
 #define REG_PAYLOAD_LENGTH       0x22
 #define REG_MODEM_CONFIG_3       0x26
+#define REG_FREQ_ERROR_MSB       0x28
+#define REG_FREQ_ERROR_MID       0x29
+#define REG_FREQ_ERROR_LSB       0x2a
 #define REG_RSSI_WIDEBAND        0x2c
 #define REG_DETECTION_OPTIMIZE   0x31
 #define REG_DETECTION_THRESHOLD  0x37
@@ -211,6 +214,25 @@ float LoRaClass::packetSnr()
   return ((int8_t)readRegister(REG_PKT_SNR_VALUE)) * 0.25;
 }
 
+long LoRaClass::packetFrequencyError()
+{
+  int32_t freqError = 0;
+  freqError = static_cast<int32_t>(readRegister(REG_FREQ_ERROR_MSB) & B111);
+  freqError <<= 8L;
+  freqError += static_cast<int32_t>(readRegister(REG_FREQ_ERROR_MID));
+  freqError <<= 8L;
+  freqError += static_cast<int32_t>(readRegister(REG_FREQ_ERROR_LSB));
+
+  if (readRegister(REG_FREQ_ERROR_MSB) & B1000) { // Sign bit is on
+     freqError -= 524288; // B1000'0000'0000'0000'0000
+  }
+
+  const float fXtal = 32E6; // FXOSC: crystal oscillator (XTAL) frequency (2.5. Chip Specification, p. 14)
+  const float fError = ((static_cast<float>(freqError) * (1L << 24)) / fXtal) * (getSignalBandwidth() / 500000.0f); // p. 37
+
+  return static_cast<long>(fError);
+}
+
 size_t LoRaClass::write(uint8_t byte)
 {
   return write(&byte, sizeof(byte));
@@ -370,6 +392,23 @@ void LoRaClass::setSpreadingFactor(int sf)
   writeRegister(REG_MODEM_CONFIG_2, (readRegister(REG_MODEM_CONFIG_2) & 0x0f) | ((sf << 4) & 0xf0));
 }
 
+long LoRaClass::getSignalBandwidth()
+{
+  byte bw = (readRegister(REG_MODEM_CONFIG_1) >> 4);
+  switch (bw) {
+    case 0: return 7.8E3;
+    case 1: return 10.4E3; 
+    case 2: return 15.6E3; 
+    case 3: return 20.8E3; 
+    case 4: return 31.25E3; 
+    case 5: return 41.7E3; 
+    case 6: return 62.5E3; 
+    case 7: return 125E3; 
+    case 8: return 250E3; 
+    case 9: return 500E3; 
+  }
+}
+
 void LoRaClass::setSignalBandwidth(long sbw)
 {
   int bw;
diff --git a/src/LoRa.h b/src/LoRa.h
index 7616ebd..bf5fb06 100644
--- a/src/LoRa.h
+++ b/src/LoRa.h
@@ -27,6 +27,7 @@ public:
   int parsePacket(int size = 0);
   int packetRssi();
   float packetSnr();
+  long packetFrequencyError();
 
   // from Print
   virtual size_t write(uint8_t byte);
@@ -71,6 +72,8 @@ private:
 
   void handleDio0Rise();
 
+  long getSignalBandwidth();
+
   uint8_t readRegister(uint8_t address);
   void writeRegister(uint8_t address, uint8_t value);
   uint8_t singleTransfer(uint8_t address, uint8_t value);