From ef4e55b6575914cc021c4eb36173db9339d3a7c8 Mon Sep 17 00:00:00 2001
From: Federico Mandelli <federico.mandelli@skywarder.eu>
Date: Sat, 25 Jan 2025 11:57:56 +0000
Subject: [PATCH] [UBXGPSSpi] Removed 1ms thread sleeps after sampling
---
src/shared/sensors/UBXGPS/UBXGPSSpi.cpp | 28 ++++++++++++++++++++-----
src/shared/sensors/UBXGPS/UBXGPSSpi.h | 4 +++-
2 files changed, 26 insertions(+), 6 deletions(-)
diff --git a/src/shared/sensors/UBXGPS/UBXGPSSpi.cpp b/src/shared/sensors/UBXGPS/UBXGPSSpi.cpp
index 72bc7ad12..03fed0fb0 100644
--- a/src/shared/sensors/UBXGPS/UBXGPSSpi.cpp
+++ b/src/shared/sensors/UBXGPS/UBXGPSSpi.cpp
@@ -49,7 +49,8 @@ UBXGPSSpi::UBXGPSSpi(SPIBusInterface& spiBus, miosix::GpioPin spiCs,
SPIBusConfig UBXGPSSpi::getDefaultSPIConfig()
{
SPIBusConfig config;
- config.clockDivider = SPI::ClockDivider::DIV_16;
+ config.clockDivider = SPI::ClockDivider::DIV_16;
+ config.csSetupTimeUs = 10;
return config;
}
@@ -109,9 +110,22 @@ bool UBXGPSSpi::selfTest() { return true; }
UBXGPSData UBXGPSSpi::sampleImpl()
{
+ int64_t currentTimestamp = Kernel::getOldTick();
+
+ // Returns the last sample if the time difference is less than 1ms
+ // From sensor specification, the time between SPI transactions must be at
+ // least 1ms. If sampling is requested before that time has passed, return
+ // the last sample.
+
+ if (currentTimestamp - lastSampleTimestamp < 1)
+ {
+ lastError = SensorErrors::NO_NEW_DATA;
+ return lastSample;
+ }
+
UBXPvtFrame pvt;
- if (!readUBXFrame(pvt))
+ if (!readUBXFrame(pvt, false))
return lastSample;
UBXPvtFrame::Payload& pvtP = pvt.getPayload();
@@ -138,6 +152,8 @@ UBXGPSData UBXGPSSpi::sampleImpl()
.nanosecond = pvtP.nano,
.accuracy = pvtP.tAcc};
+ lastSampleTimestamp = Kernel::getOldTick();
+ lastError = SensorErrors::NO_ERRORS;
return sample;
}
@@ -238,7 +254,7 @@ bool UBXGPSSpi::setPVTMessageRate()
return safeWriteUBXFrame(frame);
}
-bool UBXGPSSpi::readUBXFrame(UBXFrame& frame)
+bool UBXGPSSpi::readUBXFrame(UBXFrame& frame, bool wait)
{
long long start = Kernel::getOldTick();
long long end = start + READ_TIMEOUT;
@@ -256,7 +272,8 @@ bool UBXGPSSpi::readUBXFrame(UBXFrame& frame)
{
// LOG_ERR(logger, "Timeout for read expired");
spiSlave.bus.deselect(spiSlave.cs);
- Thread::sleep(1); // GPS minimum time after deselect
+ if (wait)
+ Thread::sleep(1); // GPS minimum time after deselect
return false;
}
@@ -297,7 +314,8 @@ bool UBXGPSSpi::readUBXFrame(UBXFrame& frame)
spiSlave.bus.read(frame.checksum, 2);
spiSlave.bus.deselect(spiSlave.cs);
- Thread::sleep(1); // GPS minimum time after deselect
+ if (wait)
+ Thread::sleep(1); // GPS minimum time after deselect
}
if (!frame.isValid())
diff --git a/src/shared/sensors/UBXGPS/UBXGPSSpi.h b/src/shared/sensors/UBXGPS/UBXGPSSpi.h
index eb14fb4e2..6164696bf 100644
--- a/src/shared/sensors/UBXGPS/UBXGPSSpi.h
+++ b/src/shared/sensors/UBXGPS/UBXGPSSpi.h
@@ -117,9 +117,10 @@ private:
* @brief Reads a UBX frame.
*
* @param frame The received frame.
+ * @param wait Whether to wait 1ms after slave deselection or not
* @return True if a valid frame was read.
*/
- bool readUBXFrame(UBXFrame& frame);
+ bool readUBXFrame(UBXFrame& frame, bool wait = true);
/**
* @brief Writes a UBX frame.
@@ -139,6 +140,7 @@ private:
SPISlave spiSlave;
uint8_t sampleRate;
+ int64_t lastSampleTimestamp = 0;
PrintLogger logger = Logging::getLogger("ubxgps");
--
GitLab