From 333aa433d94ca28365966ddca60878cdd4cadbd0 Mon Sep 17 00:00:00 2001 From: jgromes Date: Wed, 5 May 2021 20:34:59 +0200 Subject: [PATCH] [SX128x] Implemented ranging fixes from #293 --- src/modules/SX128x/SX1280.cpp | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/src/modules/SX128x/SX1280.cpp b/src/modules/SX128x/SX1280.cpp index 66b728cd..eb9b3e0e 100644 --- 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,12 +147,16 @@ 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 @@ -160,9 +164,8 @@ float SX1280::getRangingResult() { 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