[SX128x] Implemented ranging fixes from #293

jgromes · May 5, 2021 · 333aa43 · 333aa43
1 parent 0810a6c
commit 333aa43
Showing 1 changed file with 10 additions and 7 deletions.
diff --git a/src/modules/SX128x/SX1280.cpp b/src/modules/SX128x/SX1280.cpp
@@ -105,7 +105,7 @@ int16_t SX1280::startRanging(bool master, uint32_t addr) {
       return(ERR_INVALID_BANDWIDTH);
   }
   uint8_t calBuff[] = { (uint8_t)((val >> 8) & 0xFF), (uint8_t)(val & 0xFF) };
-  state = writeRegister(SX128X_REG_RANGING_CALIBRATION_MSB, calBuff, 4);
+  state = writeRegister(SX128X_REG_RANGING_CALIBRATION_MSB, calBuff, 2);
   RADIOLIB_ASSERT(state);
 
   // set role and start ranging
@@ -138,7 +138,7 @@ float SX1280::getRangingResult() {
   state = readRegister(SX128X_REG_RANGING_LORA_CLOCK_ENABLE, data, 1);
   RADIOLIB_ASSERT(state);
 
-  data[0] |= 1;
+  data[0] |= (1 << 1);
   state = writeRegister(SX128X_REG_RANGING_LORA_CLOCK_ENABLE, data, 1);
   RADIOLIB_ASSERT(state);
 
@@ -147,22 +147,25 @@ float SX1280::getRangingResult() {
   RADIOLIB_ASSERT(state);
 
   data[0] &= 0xCF;
-  data[0] |= 1 << 4;
+  data[0] |= (1 << 4);
   state = writeRegister(SX128X_REG_RANGING_TYPE, data, 1);
   RADIOLIB_ASSERT(state);
 
   // read the register values
-  state = readRegister(SX128X_REG_RANGING_RESULT_MSB, data + 1, 3);
+  state = readRegister(SX128X_REG_RANGING_RESULT_MSB, &data[0], 1);
+  RADIOLIB_ASSERT(state);
+  state = readRegister(SX128X_REG_RANGING_RESULT_MID, &data[1], 1);
+  RADIOLIB_ASSERT(state);
+  state = readRegister(SX128X_REG_RANGING_RESULT_LSB, &data[2], 1);
   RADIOLIB_ASSERT(state);
 
   // set mode to standby RC
   state = standby();
   RADIOLIB_ASSERT(state);
 
   // calculate the real result
-  uint32_t raw = 0;
-  memcpy(&raw, data, sizeof(uint32_t));
-  return((float)raw * (150.0/(4.096 * _bwKhz)));
+  uint32_t raw = ((uint32_t)data[0] << 16) | ((uint32_t)data[1] << 8) | data[2];
+  return((float)raw * 150.0 / (4.096 * _bwKhz));
 }
 
 #endif