From 86bd04a3d775158825cbe07b6fe598abc0d3d992 Mon Sep 17 00:00:00 2001
From: Fabrizio Monti <fabrizio.monti@skywarder.eu>
Date: Mon, 25 Mar 2024 23:51:34 +0100
Subject: [PATCH] [VN100-SPI] Added data ready interrupt.
---
src/shared/sensors/VN100/VN100Spi.cpp | 54 +++++++++++++++++++++++--
src/shared/sensors/VN100/VN100Spi.h | 20 ++++++++-
src/shared/sensors/VN100/VN100SpiDefs.h | 30 ++++++++++++--
src/tests/sensors/test-vn100-spi.cpp | 5 ++-
4 files changed, 101 insertions(+), 8 deletions(-)
diff --git a/src/shared/sensors/VN100/VN100Spi.cpp b/src/shared/sensors/VN100/VN100Spi.cpp
index a2089312b..21e1315f7 100644
--- a/src/shared/sensors/VN100/VN100Spi.cpp
+++ b/src/shared/sensors/VN100/VN100Spi.cpp
@@ -32,8 +32,9 @@ namespace Boardcore
{
VN100Spi::VN100Spi(SPIBus& bus, miosix::GpioPin csPin,
- SPIBusConfig busConfiguration)
- : spiSlave(bus, csPin, busConfiguration)
+ SPIBusConfig busConfiguration, uint16_t syncOutSkipFactor)
+ : spiSlave(bus, csPin, busConfiguration),
+ syncOutSkipFactor(syncOutSkipFactor)
{
}
@@ -50,13 +51,20 @@ bool VN100Spi::init()
// Send dummy data to clean up
sendDummyPacket();
- if (checkModelNumber() == false)
+ if (!checkModelNumber())
{
LOG_ERR(logger, "Got bad CHIPID");
lastError = SensorErrors::INVALID_WHOAMI;
return false;
}
+ if (!setInterrupt())
+ {
+ LOG_ERR(logger, "Unable to set data ready interrupt");
+ lastError = SensorErrors::INIT_FAIL;
+ return false;
+ }
+
isInit = true;
lastError = SensorErrors::NO_ERRORS;
return true;
@@ -102,6 +110,46 @@ void VN100Spi::sendDummyPacket()
transaction.write32(0);
}
+bool VN100Spi::setInterrupt()
+{
+ /**
+ * The data ready interrupt is set via the synchronization control register,
+ * by setting the SyncOut mode.
+ *
+ * Imu data is sampled at 800Hz, while Attitude data (quaternion) is sampled
+ * at 400 Hz. Considering that attitude data has a lower rate we set the
+ * data ready to trigger on attitude data.
+ *
+ * We can set the SyncOutSkipFactor, that defines how many times the sync
+ * out event should be skipped before actually triggering the SyncOut pin.
+ * This way we can control the rate at which the data ready interrupt is
+ * triggered.
+ *
+ * The values not needed for the data ready of the register will be set to
+ * default.
+ */
+
+ // Init struct and set default values
+ VN100SpiDefs::SynchronizationData sData;
+ sData.syncInMode = 3; // Set to: count number of trigger events on SYNC_IN.
+ sData.syncInEdge = 0; // Trigger on rising edge
+ sData.syncInSkipFactor = 0; // Don't skip
+
+ // Set needed values
+ sData.syncOutMode = 3; // Trigger when attitude measurements are available
+ sData.syncOutPolarity = 1; // Positive output pulse on the SyncOut pin
+ sData.syncOutSkipFactor = syncOutSkipFactor;
+ sData.syncOutPulseWidth =
+ 1000000; // Width of the SyncOut pulse in nanoseconds. Now is set to 1
+ // millisecond
+ // TODO: is 1ms fine for syncOutPulseWidth? Too long? Too short?
+
+ uint8_t err = writeRegister(VN100SpiDefs::REG_SYNC, (uint8_t*)&sData,
+ sizeof(VN100SpiDefs::SynchronizationData));
+
+ return err == 0;
+}
+
bool VN100Spi::selfTest()
{
D(assert(isInit && "init() was not called"));
diff --git a/src/shared/sensors/VN100/VN100Spi.h b/src/shared/sensors/VN100/VN100Spi.h
index 0e4291085..5c95de594 100644
--- a/src/shared/sensors/VN100/VN100Spi.h
+++ b/src/shared/sensors/VN100/VN100Spi.h
@@ -43,8 +43,12 @@ public:
* @param bus SPI bus.
* @param csPin SPI chip select pin.
* @param busConfiguration SPI bus configuration.
+ * @param syncOutSkipFactor The SyncOutSkipFactor defines how many times the
+ * data ready event should be skipped before actually triggering the
+ * interrupt pin.
*/
- VN100Spi(SPIBus& bus, miosix::GpioPin csPin, SPIBusConfig busConfiguration);
+ VN100Spi(SPIBus& bus, miosix::GpioPin csPin, SPIBusConfig busConfiguration,
+ uint16_t syncOutSkipFactor);
/**
* @brief Initialize the sensor.
@@ -79,6 +83,13 @@ private:
void sendDummyPacket();
// TODO: check if really needed and if there is a better solution
+ /**
+ * @brief Set the data ready interrupt.
+ *
+ * @return True if the operation is successful, false otherwise.
+ */
+ bool setInterrupt();
+
/**
* @brief Get accelerometer, gyroscope, magnetometer, pressure and
* temperature measurements from the sensor.
@@ -139,6 +150,13 @@ private:
SPISlave spiSlave;
+ /**
+ * @brief The SyncOutSkipFactor defines how many times the sync out event
+ * should be skipped before actually triggering the SyncOut pin (data
+ * ready).
+ */
+ const uint16_t syncOutSkipFactor = 0;
+
PrintLogger logger =
Logging::getLogger("vn100-spi"); // TODO: is it fine? Should it be
// changed to the exact model number?
diff --git a/src/shared/sensors/VN100/VN100SpiDefs.h b/src/shared/sensors/VN100/VN100SpiDefs.h
index 64e8c8602..c60936bfd 100644
--- a/src/shared/sensors/VN100/VN100SpiDefs.h
+++ b/src/shared/sensors/VN100/VN100SpiDefs.h
@@ -33,9 +33,10 @@ namespace VN100SpiDefs
*/
enum Registers
{
- REG_MODEL_NUMBER = 1,
- REG_QUATERNION_DATA = 9,
- REG_IMU_DATA = 54,
+ REG_MODEL_NUMBER = 1, ///< WhoAmI register
+ REG_QUATERNION_DATA = 9, ///< Quaternion data register
+ REG_SYNC = 32, ///< Used to set data ready interrupt
+ REG_IMU_DATA = 54, ///< Imu data register
};
/**
@@ -47,6 +48,29 @@ enum Commands
WRITE_REG = 2,
};
+/**
+ * @brief Data format of the synchronization control register, used for reading
+ * and writing operations.
+ */
+struct __attribute__((packed)) SynchronizationData
+{
+ uint8_t syncInMode; ///< Behaviour of the syncIn event
+ uint8_t syncInEdge; ///< Trigger syncIn on rising or falling edge
+ uint16_t syncInSkipFactor; ///< How many times trigger edges defined by
+ ///< SyncInEdge should occur prior to triggering
+ ///< a SyncIn event
+ const uint32_t RESERVED = 0; ///< Reserved, do not use
+ uint8_t syncOutMode; ///< Behavior of the SyncOut event
+ uint8_t syncOutPolarity; ///< The polarity of the output pulse on the
+ ///< SyncOut pin (positive or negative)
+ uint16_t syncOutSkipFactor; ///< how many times the sync out event should
+ ///< be skipped before actually triggering the
+ ///< SyncOut pin
+ uint32_t
+ syncOutPulseWidth; ///< Controls the desired width of the SyncOut pulse
+ const uint32_t RESERVED2 = 0; ///< Reserved, do not use
+};
+
/**
* @brief The expected model number to be red from the sensor.
*/
diff --git a/src/tests/sensors/test-vn100-spi.cpp b/src/tests/sensors/test-vn100-spi.cpp
index c94d91c29..9bdbc30c7 100644
--- a/src/tests/sensors/test-vn100-spi.cpp
+++ b/src/tests/sensors/test-vn100-spi.cpp
@@ -43,12 +43,15 @@ int main()
mosiPin.alternateFunction(6);
mosiPin.mode(Mode::ALTERNATE);
+ GpioPin intPin(GPIOC_BASE, 15); // PC15 interrupt pin
+ intPin.mode(Mode::INPUT);
+
SPIBusConfig busConfiguration; // Bus configuration for the sensor
busConfiguration.clockDivider = SPI::ClockDivider::DIV_256;
busConfiguration.mode =
SPI::Mode::MODE_3; // Set clock polarity to 0 and phase to 1
- VN100Spi sensor(bus, csPin, busConfiguration);
+ VN100Spi sensor(bus, csPin, busConfiguration, 200);
// Let the sensor start up
Thread::sleep(1000);
--
GitLab