diff --git a/examples/SX128x/SX128x_Receive/SX128x_Receive.ino b/examples/SX128x/SX128x_Receive/SX128x_Receive.ino index 47c6c63b..28dc8047 100644 --- a/examples/SX128x/SX128x_Receive/SX128x_Receive.ino +++ b/examples/SX128x/SX128x_Receive/SX128x_Receive.ino @@ -85,6 +85,12 @@ void loop() { Serial.print(radio.getSNR()); Serial.println(F(" dB")); + // print the Frequency Error + // of the last received packet + Serial.print(F("[SX1280] Frequency Error:\t")); + Serial.print(radio.getFrequencyError()); + Serial.println(F(" Hz")); + } else if (state == RADIOLIB_ERR_RX_TIMEOUT) { // timeout occurred while waiting for a packet Serial.println(F("timeout!")); diff --git a/examples/SX128x/SX128x_Receive_Interrupt/SX128x_Receive_Interrupt.ino b/examples/SX128x/SX128x_Receive_Interrupt/SX128x_Receive_Interrupt.ino index 274121df..77d452c6 100644 --- a/examples/SX128x/SX128x_Receive_Interrupt/SX128x_Receive_Interrupt.ino +++ b/examples/SX128x/SX128x_Receive_Interrupt/SX128x_Receive_Interrupt.ino @@ -136,6 +136,12 @@ void loop() { Serial.print(radio.getSNR()); Serial.println(F(" dB")); + // print the Frequency Error + // of the last received packet + Serial.print(F("[SX1280] Frequency Error:\t")); + Serial.print(radio.getFrequencyError()); + Serial.println(F(" Hz")); + } else if (state == RADIOLIB_ERR_CRC_MISMATCH) { // packet was received, but is malformed Serial.println(F("CRC error!")); diff --git a/src/modules/SX128x/SX128x.cpp b/src/modules/SX128x/SX128x.cpp index 972e6ffc..a59572e4 100644 --- a/src/modules/SX128x/SX128x.cpp +++ b/src/modules/SX128x/SX128x.cpp @@ -1064,6 +1064,39 @@ float SX128x::getSNR() { } } +double SX128x::getFrequencyError() { + // check active modem + uint8_t modem = getPacketType(); + if (!((modem == RADIOLIB_SX128X_PACKET_TYPE_LORA) || (modem == RADIOLIB_SX128X_PACKET_TYPE_RANGING))) { + return (0.0); + } + + uint8_t efeRaw[3] = {0}; + // read the raw frequency error register values + int16_t state = readRegister(RADIOLIB_SX128X_REG_FEI_MSB, &efeRaw[0], 1); + RADIOLIB_ASSERT(state); + state = readRegister(RADIOLIB_SX128X_REG_FEI_MID, &efeRaw[1], 1); + RADIOLIB_ASSERT(state); + state = readRegister(RADIOLIB_SX128X_REG_FEI_LSB, &efeRaw[2], 1); + RADIOLIB_ASSERT(state); + uint32_t efe = ((uint32_t) efeRaw[0] << 16) | ((uint32_t) efeRaw[1] << 8) | efeRaw[2]; + efe &= 0x0FFFFF; + + double error; + + // check the first bit + if (efe & 0x80000) { + // frequency error is negative + efe |= (uint32_t) 0xFFF00000; + efe = ~efe + 1; + error = 1.55 * (double) efe / (1600.0 / (double) _bwKhz) * -1.0; + } else { + error = 1.55 * (double) efe / (1600.0 / (double) _bwKhz); + } + + return (error); +} + size_t SX128x::getPacketLength(bool update) { (void)update; uint8_t rxBufStatus[2] = {0, 0}; diff --git a/src/modules/SX128x/SX128x.h b/src/modules/SX128x/SX128x.h index 9f258bb1..cf358c74 100644 --- a/src/modules/SX128x/SX128x.h +++ b/src/modules/SX128x/SX128x.h @@ -727,6 +727,13 @@ class SX128x: public PhysicalLayer { */ float getSNR(); + /*! + \brief Gets frequency error of the latest received packet. + + \returns Frequency error in Hz. + */ + double getFrequencyError(); + /*! \brief Query modem for the packet length of received payload.