From 1356f736a3f630e4621fbc8fc563351236f1e6c4 Mon Sep 17 00:00:00 2001
From: Anthony Elder <ant.elder@gmail.com>
Date: Fri, 2 Mar 2018 11:24:32 +0000
Subject: [PATCH] Update for review comments

---
 keywords.txt |  1 -
 src/LoRa.cpp | 30 ++++++++++++++++--------------
 src/LoRa.h   |  6 +++---
 3 files changed, 19 insertions(+), 18 deletions(-)

diff --git a/keywords.txt b/keywords.txt
index f4f82c7..759bb0a 100644
--- a/keywords.txt
+++ b/keywords.txt
@@ -49,7 +49,6 @@ random	KEYWORD2
 setPins	KEYWORD2
 setSPIFrequency	KEYWORD2
 dumpRegisters	KEYWORD2
-getSignalBandwidth	KEYWORD2
 
 #######################################
 # Constants (LITERAL1)
diff --git a/src/LoRa.cpp b/src/LoRa.cpp
index 58a10eb..df2bea0 100644
--- a/src/LoRa.cpp
+++ b/src/LoRa.cpp
@@ -25,7 +25,9 @@
 #define REG_PREAMBLE_LSB         0x21
 #define REG_PAYLOAD_LENGTH       0x22
 #define REG_MODEM_CONFIG_3       0x26
-#define REG_FREQ_ERROR           0x28
+#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
@@ -212,23 +214,23 @@ float LoRaClass::packetSnr()
   return ((int8_t)readRegister(REG_PKT_SNR_VALUE)) * 0.25;
 }
 
-double LoRaClass::packetFrequencyError()
+long LoRaClass::packetFrequencyError()
 {
-  int32_t fe = (int32_t)readRegister(REG_FREQ_ERROR) & 7;
-  fe <<= 8L;
-  fe += (int32_t)readRegister(REG_FREQ_ERROR+1);
-  fe <<= 8L;
-  fe += (int32_t)readRegister(REG_FREQ_ERROR+2);
-  
-  if (readRegister(REG_FREQ_ERROR) & 8) {
-    fe = fe - 524288;
+  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
   }
 
-  double freqError = (double)fe;
-  freqError *=  (16777216.0 / 32000000.0);
-  freqError *= (getSignalBandwidth() / 500000.0);
+  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 freqError;
+  return static_cast<long>(fError);
 }
 
 size_t LoRaClass::write(uint8_t byte)
diff --git a/src/LoRa.h b/src/LoRa.h
index d7335f1..bf5fb06 100644
--- a/src/LoRa.h
+++ b/src/LoRa.h
@@ -27,7 +27,7 @@ public:
   int parsePacket(int size = 0);
   int packetRssi();
   float packetSnr();
-  double packetFrequencyError();
+  long packetFrequencyError();
 
   // from Print
   virtual size_t write(uint8_t byte);
@@ -66,14 +66,14 @@ public:
 
   void dumpRegisters(Stream& out);
 
-  long getSignalBandwidth();
-
 private:
   void explicitHeaderMode();
   void implicitHeaderMode();
 
   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);