From e9b6e27739007acdc890d48fef210277216feb36 Mon Sep 17 00:00:00 2001
From: jgromes <jan.gromes@gmail.com>
Date: Sat, 12 Oct 2024 14:15:05 +0100
Subject: [PATCH] [SX126x] Fix FSK addresses on transmission (#1268)

---
 src/modules/SX126x/SX126x.cpp | 30 ++++++++++++++++++++++--------
 src/modules/SX126x/SX126x.h   |  5 +++--
 2 files changed, 25 insertions(+), 10 deletions(-)

diff --git a/src/modules/SX126x/SX126x.cpp b/src/modules/SX126x/SX126x.cpp
index 7109d6d9..63e37939 100644
--- a/src/modules/SX126x/SX126x.cpp
+++ b/src/modules/SX126x/SX126x.cpp
@@ -509,9 +509,6 @@ void SX126x::clearChannelScanAction() {
 }
 
 int16_t SX126x::startTransmit(const uint8_t* data, size_t len, uint8_t addr) {
-  // suppress unused variable warning
-  (void)addr;
-
   // check packet length
   if(len > RADIOLIB_SX126X_MAX_PACKET_LENGTH) {
     return(RADIOLIB_ERR_PACKET_TOO_LONG);
@@ -527,10 +524,19 @@ int16_t SX126x::startTransmit(const uint8_t* data, size_t len, uint8_t addr) {
   uint8_t modem = getPacketType();
   if(modem == RADIOLIB_SX126X_PACKET_TYPE_LORA) {
     state = setPacketParams(this->preambleLengthLoRa, this->crcTypeLoRa, len, this->headerType, this->invertIQEnabled);
+  
   } else if(modem == RADIOLIB_SX126X_PACKET_TYPE_GFSK) {
     state = setPacketParamsFSK(this->preambleLengthFSK, this->crcTypeFSK, this->syncWordLength, this->addrComp, this->whitening, this->packetType, len);
+    
+    // address is taken from the register
+    if(this->addrComp != RADIOLIB_SX126X_GFSK_ADDRESS_FILT_OFF) {
+      RADIOLIB_ASSERT(state);
+      state = writeRegister(RADIOLIB_SX126X_REG_NODE_ADDRESS, &addr, 1);
+    }
+
   } else if(modem != RADIOLIB_SX126X_PACKET_TYPE_LR_FHSS) {
     return(RADIOLIB_ERR_UNKNOWN);
+  
   }
   RADIOLIB_ASSERT(state);
 
@@ -617,7 +623,14 @@ int16_t SX126x::startTransmit(const uint8_t* data, size_t len, uint8_t addr) {
 
 int16_t SX126x::finishTransmit() {
   // clear interrupt flags
-  clearIrqStatus();
+  int16_t state = clearIrqStatus();
+  RADIOLIB_ASSERT(state);
+
+  // restore the original node address
+  if(getPacketType() == RADIOLIB_SX126X_PACKET_TYPE_GFSK) {
+    state = writeRegister(RADIOLIB_SX126X_REG_NODE_ADDRESS, &this->nodeAddr, 1);
+    RADIOLIB_ASSERT(state);
+  }
 
   // set mode to standby to disable transmitter/RF switch
   return(standby());
@@ -739,14 +752,14 @@ int16_t SX126x::readData(uint8_t* data, size_t len) {
   // if that's the case, the first call will return "SPI command timeout error"
   // check the IRQ to be sure this really originated from timeout event
   int16_t state = this->mod->SPIcheckStream();
-  if((state == RADIOLIB_ERR_SPI_CMD_TIMEOUT) && (getIrqFlags() & RADIOLIB_SX126X_IRQ_TIMEOUT)) {
+  uint16_t irq = getIrqFlags();
+  if((state == RADIOLIB_ERR_SPI_CMD_TIMEOUT) && (irq & RADIOLIB_SX126X_IRQ_TIMEOUT)) {
     // this is definitely Rx timeout
     return(RADIOLIB_ERR_RX_TIMEOUT);
   }
   RADIOLIB_ASSERT(state);
 
   // check integrity CRC
-  uint16_t irq = getIrqFlags();
   int16_t crcState = RADIOLIB_ERR_NONE;
   // Report CRC mismatch when there's a payload CRC error, or a header error and no valid header (to avoid false alarm from previous packet)
   if((irq & RADIOLIB_SX126X_IRQ_CRC_ERR) || ((irq & RADIOLIB_SX126X_IRQ_HEADER_ERR) && !(irq & RADIOLIB_SX126X_IRQ_HEADER_VALID))) {
@@ -1220,7 +1233,7 @@ int16_t SX126x::setSyncBits(uint8_t *syncWord, uint8_t bitsLen) {
   return(setSyncWord(syncWord, bytesLen));
 }
 
-int16_t SX126x::setNodeAddress(uint8_t nodeAddr) {
+int16_t SX126x::setNodeAddress(uint8_t addr) {
   // check active modem
   if(getPacketType() != RADIOLIB_SX126X_PACKET_TYPE_GFSK) {
     return(RADIOLIB_ERR_WRONG_MODEM);
@@ -1232,7 +1245,8 @@ int16_t SX126x::setNodeAddress(uint8_t nodeAddr) {
   RADIOLIB_ASSERT(state);
 
   // set node address
-  state = writeRegister(RADIOLIB_SX126X_REG_NODE_ADDRESS, &nodeAddr, 1);
+  this->nodeAddr = addr;
+  state = writeRegister(RADIOLIB_SX126X_REG_NODE_ADDRESS, &addr, 1);
 
   return(state);
 }
diff --git a/src/modules/SX126x/SX126x.h b/src/modules/SX126x/SX126x.h
index a64356aa..766e0208 100644
--- a/src/modules/SX126x/SX126x.h
+++ b/src/modules/SX126x/SX126x.h
@@ -901,10 +901,10 @@ class SX126x: public PhysicalLayer {
 
     /*!
       \brief Sets node address. Calling this method will also enable address filtering for node address only.
-      \param nodeAddr Node address to be set.
+      \param addr Node address to be set.
       \returns \ref status_codes
     */
-    int16_t setNodeAddress(uint8_t nodeAddr);
+    int16_t setNodeAddress(uint8_t addr);
 
     /*!
       \brief Sets broadcast address. Calling this method will also enable address
@@ -1263,6 +1263,7 @@ class SX126x: public PhysicalLayer {
     uint8_t rxBandwidth = 0, pulseShape = 0, crcTypeFSK = 0, syncWordLength = 0, addrComp = 0, whitening = 0, packetType = 0;
     uint16_t preambleLengthFSK = 0;
     float rxBandwidthKhz = 0;
+    uint8_t nodeAddr = 0;
 
     float dataRateMeasured = 0;