diff --git a/CMakeLists.txt b/CMakeLists.txt
index 3695cb6d85a68baced219cc0d63dc3ad4d888c7f..2c858831ed86a80964624b7c2f84fdc3338b0aba 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -447,8 +447,11 @@ sbs_target(test-ubxgps-serial stm32f429zi_death_stack_v2)
add_executable(test-ubxgps-spi src/tests/sensors/test-ubxgps-spi.cpp)
sbs_target(test-ubxgps-spi stm32f429zi_death_stack_v2)
-add_executable(test-vn100 src/tests/sensors/test-vn100.cpp)
-sbs_target(test-vn100 stm32f407vg_stm32f4discovery)
+add_executable(test-vn100-serial src/tests/sensors/test-vn100-serial.cpp)
+sbs_target(test-vn100-serial stm32f407vg_stm32f4discovery)
+
+add_executable(test-vn100-spi src/tests/sensors/test-vn100-spi.cpp)
+sbs_target(test-vn100-spi stm32f407vg_stm32f4discovery)
add_executable(test-lis2mdl src/tests/sensors/test-lis2mdl.cpp)
sbs_target(test-lis2mdl stm32f429zi_stm32f4discovery)
diff --git a/cmake/boardcore.cmake b/cmake/boardcore.cmake
index ae8a0541ac65e343a7de3a3caeafa9c5af91233e..1f725d32bd35de35b3412b66125c39c2b3956e3a 100644
--- a/cmake/boardcore.cmake
+++ b/cmake/boardcore.cmake
@@ -110,7 +110,8 @@ set(BOARDCORE_SRC
${BOARDCORE_PATH}/src/shared/sensors/SensorSampler.cpp
${BOARDCORE_PATH}/src/shared/sensors/UBXGPS/UBXGPSSerial.cpp
${BOARDCORE_PATH}/src/shared/sensors/UBXGPS/UBXGPSSpi.cpp
- ${BOARDCORE_PATH}/src/shared/sensors/VN100/VN100.cpp
+ ${BOARDCORE_PATH}/src/shared/sensors/VN100/VN100Serial.cpp
+ ${BOARDCORE_PATH}/src/shared/sensors/VN100/VN100Spi.cpp
${BOARDCORE_PATH}/src/shared/sensors/LIS2MDL/LIS2MDL.cpp
${BOARDCORE_PATH}/src/shared/sensors/LPS28DFW/LPS28DFW.cpp
${BOARDCORE_PATH}/src/shared/sensors/LPS22DF/LPS22DF.cpp
diff --git a/src/shared/logger/LogTypes.h b/src/shared/logger/LogTypes.h
index c10289c254926df18a1e04eac9a5b5fd1540faa3..70e6426fbbdcab9915279a870c77cce2cf9296b5 100644
--- a/src/shared/logger/LogTypes.h
+++ b/src/shared/logger/LogTypes.h
@@ -59,7 +59,8 @@
#include <sensors/RotatedIMU/IMUData.h>
#include <sensors/SensorData.h>
#include <sensors/UBXGPS/UBXGPSData.h>
-#include <sensors/VN100/VN100Data.h>
+#include <sensors/VN100/VN100SerialData.h>
+#include <sensors/VN100/VN100SpiData.h>
#include <sensors/analog/AnalogLoadCellData.h>
#include <sensors/analog/BatteryVoltageSensorData.h>
#include <sensors/analog/Pitot/PitotData.h>
@@ -165,7 +166,8 @@ void registerTypes(Deserializer& ds)
ds.registerType<TemperatureData>();
ds.registerType<TimestampData>();
ds.registerType<UBXGPSData>();
- ds.registerType<VN100Data>();
+ ds.registerType<VN100SerialData>();
+ ds.registerType<VN100SpiData>();
ds.registerType<VoltageData>();
ds.registerType<Xbee::XbeeStatus>();
}
diff --git a/src/shared/sensors/VN100/VN100.cpp b/src/shared/sensors/VN100/VN100Serial.cpp
similarity index 93%
rename from src/shared/sensors/VN100/VN100.cpp
rename to src/shared/sensors/VN100/VN100Serial.cpp
index 12ebff0ee883897658b221572133527d9ed31f86..e0df012ee2d2ac20a2ceefd91cb0c94c053328ec 100644
--- a/src/shared/sensors/VN100/VN100.cpp
+++ b/src/shared/sensors/VN100/VN100Serial.cpp
@@ -20,7 +20,7 @@
* THE SOFTWARE.
*/
-#include "VN100.h"
+#include "VN100Serial.h"
#include <drivers/timer/TimestampTimer.h>
#include <utils/KernelTime.h>
@@ -28,12 +28,13 @@
namespace Boardcore
{
-VN100::VN100(USART &usart, int baudRate, CRCOptions crc, uint16_t samplePeriod)
+VN100Serial::VN100Serial(USART &usart, int baudRate, CRCOptions crc,
+ uint16_t samplePeriod)
: usart(usart), baudRate(baudRate), samplePeriod(samplePeriod), crc(crc)
{
}
-bool VN100::init()
+bool VN100Serial::init()
{
SensorErrors backup = lastError;
@@ -117,7 +118,7 @@ bool VN100::init()
return true;
}
-void VN100::run()
+void VN100Serial::run()
{
while (!shouldStop())
{
@@ -132,7 +133,7 @@ void VN100::run()
}
}
-bool VN100::sampleRaw()
+bool VN100Serial::sampleRaw()
{
// Sensor not init
if (!isInit)
@@ -160,7 +161,7 @@ bool VN100::sampleRaw()
return true;
}
-string VN100::getLastRawSample()
+string VN100Serial::getLastRawSample()
{
// If not init i return the void string
if (!isInit)
@@ -171,7 +172,7 @@ string VN100::getLastRawSample()
return string(recvString, recvStringLength);
}
-bool VN100::closeAndReset()
+bool VN100Serial::closeAndReset()
{
// Sensor not init
if (!isInit)
@@ -196,7 +197,7 @@ bool VN100::closeAndReset()
return true;
}
-bool VN100::selfTest()
+bool VN100Serial::selfTest()
{
if (!selfTestImpl())
{
@@ -208,13 +209,13 @@ bool VN100::selfTest()
return true;
}
-VN100Data VN100::sampleImpl()
+VN100SerialData VN100Serial::sampleImpl()
{
miosix::Lock<FastMutex> l(mutex);
return threadSample;
}
-VN100Data VN100::sampleData()
+VN100SerialData VN100Serial::sampleData()
{
if (!isInit)
{
@@ -282,10 +283,10 @@ VN100Data VN100::sampleData()
TemperatureData temp = sampleTemperature();
PressureData press = samplePressure();
- return VN100Data(quat, mag, acc, gyro, temp, press);
+ return VN100SerialData(quat, mag, acc, gyro, temp, press);
}
-bool VN100::disableAsyncMessages(bool waitResponse)
+bool VN100Serial::disableAsyncMessages(bool waitResponse)
{
// Command string
std::string command =
@@ -306,7 +307,7 @@ bool VN100::disableAsyncMessages(bool waitResponse)
return true;
}
-bool VN100::configDefaultSerialPort()
+bool VN100Serial::configDefaultSerialPort()
{
// Initial default settings
usart.setBaudrate(115200);
@@ -319,7 +320,7 @@ bool VN100::configDefaultSerialPort()
* Even if the user configured baudrate is the default, I want to reset the
* buffer to clean the junk.
*/
-bool VN100::configUserSerialPort()
+bool VN100Serial::configUserSerialPort()
{
std::string command;
@@ -339,7 +340,7 @@ bool VN100::configUserSerialPort()
return true;
}
-bool VN100::setCrc(bool waitResponse)
+bool VN100Serial::setCrc(bool waitResponse)
{
// Command for the crc change
std::string command;
@@ -397,7 +398,7 @@ bool VN100::setCrc(bool waitResponse)
return true;
}
-bool VN100::selfTestImpl()
+bool VN100Serial::selfTestImpl()
{
char modelNumber[] = "VN-100";
const int modelNumberOffset = 10;
@@ -450,7 +451,7 @@ bool VN100::selfTestImpl()
return true;
}
-QuaternionData VN100::sampleQuaternion()
+QuaternionData VN100Serial::sampleQuaternion()
{
unsigned int indexStart = 0;
char *nextNumber;
@@ -478,7 +479,7 @@ QuaternionData VN100::sampleQuaternion()
return data;
}
-MagnetometerData VN100::sampleMagnetometer()
+MagnetometerData VN100Serial::sampleMagnetometer()
{
unsigned int indexStart = 0;
char *nextNumber;
@@ -505,7 +506,7 @@ MagnetometerData VN100::sampleMagnetometer()
return data;
}
-AccelerometerData VN100::sampleAccelerometer()
+AccelerometerData VN100Serial::sampleAccelerometer()
{
unsigned int indexStart = 0;
char *nextNumber;
@@ -532,7 +533,7 @@ AccelerometerData VN100::sampleAccelerometer()
return data;
}
-GyroscopeData VN100::sampleGyroscope()
+GyroscopeData VN100Serial::sampleGyroscope()
{
unsigned int indexStart = 0;
char *nextNumber;
@@ -559,7 +560,7 @@ GyroscopeData VN100::sampleGyroscope()
return data;
}
-TemperatureData VN100::sampleTemperature()
+TemperatureData VN100Serial::sampleTemperature()
{
unsigned int indexStart = 0;
TemperatureData data;
@@ -583,7 +584,7 @@ TemperatureData VN100::sampleTemperature()
return data;
}
-PressureData VN100::samplePressure()
+PressureData VN100Serial::samplePressure()
{
unsigned int indexStart = 0;
PressureData data;
@@ -607,7 +608,7 @@ PressureData VN100::samplePressure()
return data;
}
-bool VN100::sendStringCommand(std::string command)
+bool VN100Serial::sendStringCommand(std::string command)
{
if (crc == CRCOptions::CRC_ENABLE_8)
{
@@ -648,7 +649,7 @@ bool VN100::sendStringCommand(std::string command)
return true;
}
-bool VN100::recvStringCommand(char *command, int maxLength)
+bool VN100Serial::recvStringCommand(char *command, int maxLength)
{
int i = 0;
// Read the buffer
@@ -672,7 +673,7 @@ bool VN100::recvStringCommand(char *command, int maxLength)
return true;
}
-bool VN100::verifyChecksum(char *command, int length)
+bool VN100Serial::verifyChecksum(char *command, int length)
{
int checksumOffset = 0;
@@ -730,7 +731,7 @@ bool VN100::verifyChecksum(char *command, int length)
return true;
}
-uint8_t VN100::calculateChecksum8(uint8_t *message, int length)
+uint8_t VN100Serial::calculateChecksum8(uint8_t *message, int length)
{
int i;
uint8_t result = 0x00;
@@ -745,7 +746,7 @@ uint8_t VN100::calculateChecksum8(uint8_t *message, int length)
return result;
}
-uint16_t VN100::calculateChecksum16(uint8_t *message, int length)
+uint16_t VN100Serial::calculateChecksum16(uint8_t *message, int length)
{
int i;
uint16_t result = 0x0000;
diff --git a/src/shared/sensors/VN100/VN100.h b/src/shared/sensors/VN100/VN100Serial.h
similarity index 94%
rename from src/shared/sensors/VN100/VN100.h
rename to src/shared/sensors/VN100/VN100Serial.h
index 64886b774eca270e46dba86435bd1019c308050f..cd0959f743c0efef266f30d92165c7de5fc2786f 100644
--- a/src/shared/sensors/VN100/VN100.h
+++ b/src/shared/sensors/VN100/VN100Serial.h
@@ -58,7 +58,7 @@
#include <string.h>
#include <utils/Debug.h>
-#include "VN100Data.h"
+#include "VN100SerialData.h"
#include "drivers/usart/USART.h"
namespace Boardcore
@@ -67,7 +67,7 @@ namespace Boardcore
/**
* @brief Driver class for VN100 IMU.
*/
-class VN100 : public Sensor<VN100Data>, public ActiveObject
+class VN100Serial : public Sensor<VN100SerialData>, public ActiveObject
{
public:
enum class CRCOptions : uint8_t
@@ -86,8 +86,9 @@ public:
* @param Redundancy check option.
* @param samplePeriod Sampling period in ms
*/
- VN100(USART &usart, int baudrate, CRCOptions crc = CRCOptions::CRC_ENABLE_8,
- uint16_t samplePeriod = 20);
+ VN100Serial(USART &usart, int baudrate,
+ CRCOptions crc = CRCOptions::CRC_ENABLE_8,
+ uint16_t samplePeriod = 20);
bool init() override;
@@ -119,7 +120,7 @@ private:
/**
* @brief Sample action implementation.
*/
- VN100Data sampleImpl() override;
+ VN100SerialData sampleImpl() override;
/**
* @brief Active object method, about the thread execution
@@ -131,7 +132,7 @@ private:
*
* @return VN100Data The sampled data
*/
- VN100Data sampleData();
+ VN100SerialData sampleData();
/**
* @brief Disables the async messages that the vn100 is default configured
@@ -274,9 +275,9 @@ private:
* @brief Mutex to synchronize the reading and writing of the threadSample
*/
mutable miosix::FastMutex mutex;
- VN100Data threadSample;
+ VN100SerialData threadSample;
- PrintLogger logger = Logging::getLogger("vn100");
+ PrintLogger logger = Logging::getLogger("vn100-serial");
static const unsigned int recvStringMaxDimension = 200;
};
diff --git a/src/shared/sensors/VN100/VN100Data.h b/src/shared/sensors/VN100/VN100SerialData.h
similarity index 86%
rename from src/shared/sensors/VN100/VN100Data.h
rename to src/shared/sensors/VN100/VN100SerialData.h
index 0b1f190425d5cd0db4b451c2d69aae3d0af8ff60..31e903bdad3955a3caf466bfde899d1edab87573 100644
--- a/src/shared/sensors/VN100/VN100Data.h
+++ b/src/shared/sensors/VN100/VN100SerialData.h
@@ -30,19 +30,19 @@ namespace Boardcore
/**
* @brief data type class
*/
-struct VN100Data : public QuaternionData,
- public MagnetometerData,
- public AccelerometerData,
- public GyroscopeData,
- public TemperatureData,
- public PressureData
+struct VN100SerialData : public QuaternionData,
+ public MagnetometerData,
+ public AccelerometerData,
+ public GyroscopeData,
+ public TemperatureData,
+ public PressureData
{
/**
* @brief Void parameters constructor
*/
// cppcheck-suppress uninitDerivedMemberVar
- VN100Data()
+ VN100SerialData()
: QuaternionData{0, 0.0, 0.0, 0.0, 0.0}, MagnetometerData{0, 0.0, 0.0,
0.0},
AccelerometerData{0, 0.0, 0.0, 0.0}, GyroscopeData{0, 0.0, 0.0, 0.0},
@@ -57,9 +57,9 @@ struct VN100Data : public QuaternionData,
*/
// cppcheck-suppress passedByValue
// cppcheck-suppress uninitDerivedMemberVar
- VN100Data(QuaternionData quat, MagnetometerData magData,
- AccelerometerData accData, GyroscopeData gyro,
- TemperatureData temp, PressureData pres)
+ VN100SerialData(QuaternionData quat, MagnetometerData magData,
+ AccelerometerData accData, GyroscopeData gyro,
+ TemperatureData temp, PressureData pres)
: QuaternionData(quat), MagnetometerData(magData),
AccelerometerData(accData), GyroscopeData(gyro),
TemperatureData(temp), PressureData(pres)
diff --git a/src/shared/sensors/VN100/VN100Spi.cpp b/src/shared/sensors/VN100/VN100Spi.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7caea2a86bb2313020811e6a6c3a10973ff9bed3
--- /dev/null
+++ b/src/shared/sensors/VN100/VN100Spi.cpp
@@ -0,0 +1,375 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Fabrizio Monti
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "VN100Spi.h"
+
+#include <drivers/timer/TimestampTimer.h>
+#include <interfaces/endianness.h>
+#include <utils/Debug.h>
+
+namespace Boardcore
+{
+
+VN100Spi::VN100Spi(SPIBus& bus, miosix::GpioPin csPin,
+ SPIBusConfig busConfiguration, uint16_t syncOutSkipFactor)
+ : spiSlave(bus, csPin, busConfiguration),
+ syncOutSkipFactor(syncOutSkipFactor)
+{
+}
+
+bool VN100Spi::init()
+{
+ if (isInit)
+ {
+ LOG_ERR(logger, "init() should be called once");
+ lastError = SensorErrors::ALREADY_INIT;
+ return false;
+ }
+
+ // First communication after startup might fail
+ // Send dummy data to clean up
+ sendDummyPacket();
+
+ if (!checkModelNumber())
+ {
+ LOG_ERR(logger, "Got bad CHIPID");
+ lastError = SensorErrors::INVALID_WHOAMI;
+ return false;
+ }
+
+ miosix::delayUs(100);
+
+ if (!setInterrupt())
+ {
+ LOG_ERR(logger, "Unable to set data ready interrupt");
+ lastError = SensorErrors::INIT_FAIL;
+ return false;
+ }
+
+ // Wait to ensure that enough time has passed before the next operation
+ miosix::delayUs(100);
+
+ isInit = true;
+ lastError = SensorErrors::NO_ERRORS;
+ return true;
+}
+
+bool VN100Spi::checkModelNumber()
+{
+ int i = 0;
+ char buf[VN100SpiDefs::MODEL_NUMBER_SIZE];
+ for (i = 0; i < VN100SpiDefs::MODEL_NUMBER_SIZE; ++i)
+ {
+ buf[i] = 0;
+ }
+
+ VN100SpiDefs::VNErrors err = readRegister(VN100SpiDefs::REG_MODEL_NUMBER,
+ reinterpret_cast<uint8_t*>(buf),
+ VN100SpiDefs::MODEL_NUMBER_SIZE);
+ if (err != VN100SpiDefs::VNErrors::NO_ERROR)
+ {
+ // An error occurred while attempting to service the request
+ LOG_ERR(logger, "Error while reading CHIPID");
+ TRACE("Error code: %u\n", err);
+ return false;
+ }
+
+ // Check the model number
+ if (strncmp(VN100SpiDefs::MODEL_NUMBER, buf,
+ strlen(VN100SpiDefs::MODEL_NUMBER)) != 0)
+ {
+ LOG_ERR(logger, "Error, invalid CHIPID");
+ TRACE("%s != %s\n", VN100SpiDefs::MODEL_NUMBER, buf);
+ return false;
+ }
+
+ return true;
+}
+
+void VN100Spi::sendDummyPacket()
+{
+ SPITransaction transaction(spiSlave);
+ transaction.write32(0);
+
+ /**
+ * After issuing a command the vn100 needs al least 100 microseconds
+ * before providing a reply. Considering this function is called only
+ * during the initialization phase I wait for a full millisecond, as a
+ * safety measure.
+ */
+ miosix::Thread::sleep(1);
+}
+
+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.
+ *
+ * SyncIn values, which aren't 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 = VN100SpiDefs::SYNC_OUT_PULSE_WIDTH;
+
+ VN100SpiDefs::VNErrors err = writeRegister(
+ VN100SpiDefs::REG_SYNC, reinterpret_cast<uint8_t*>(&sData),
+ sizeof(VN100SpiDefs::SynchronizationData));
+
+ if (err != VN100SpiDefs::VNErrors::NO_ERROR)
+ {
+ TRACE("setInterrupt() failed, error: %u\n", err);
+ return false;
+ }
+
+ return true;
+}
+
+bool VN100Spi::selfTest() { return true; }
+
+VN100SpiData VN100Spi::sampleImpl()
+{
+ D(assert(isInit && "init() was not called"));
+
+ // Reset any errors.
+ lastError = SensorErrors::NO_ERRORS;
+
+ VN100SpiData data;
+ data.accelerationTimestamp = TimestampTimer::getTimestamp();
+ data.angularSpeedTimestamp = data.accelerationTimestamp;
+ data.magneticFieldTimestamp = data.accelerationTimestamp;
+ data.quaternionTimestamp = data.accelerationTimestamp;
+
+ if (!getSample(data))
+ {
+ // An error occurred while gathering data
+ lastError = NO_NEW_DATA;
+ return lastSample;
+ }
+
+ return data;
+}
+
+bool VN100Spi::getSample(VN100SpiData& data)
+{
+ VN100SpiDefs::RawImuQuatData rawData;
+
+ VN100SpiDefs::VNErrors err =
+ readRegister(VN100SpiDefs::REG_QUAT_IMU_DATA,
+ reinterpret_cast<uint8_t*>(&rawData), sizeof(rawData));
+
+ if (err != VN100SpiDefs::VNErrors::NO_ERROR)
+ {
+ // An error occurred while reading data
+ TRACE("getSample() failed, error: %u\n", err);
+ return false;
+ }
+
+ // Get measurements from raw data
+ data.quaternionX = rawData.quatX;
+ data.quaternionY = rawData.quatY;
+ data.quaternionZ = rawData.quatZ;
+ data.quaternionW = rawData.quatW;
+ data.magneticFieldX = rawData.magX;
+ data.magneticFieldY = rawData.magY;
+ data.magneticFieldZ = rawData.magZ;
+ data.accelerationX = rawData.accX;
+ data.accelerationY = rawData.accY;
+ data.accelerationZ = rawData.accZ;
+ data.angularSpeedX = rawData.gyrX;
+ data.angularSpeedY = rawData.gyrY;
+ data.angularSpeedZ = rawData.gyrZ;
+
+ return true;
+}
+
+TemperatureData VN100Spi::getTemperature()
+{
+ TemperatureData data;
+
+ VN100SpiDefs::RawTempPressData rawData;
+
+ // Get timestamp
+ data.temperatureTimestamp = TimestampTimer::getTimestamp();
+
+ VN100SpiDefs::VNErrors err =
+ readRegister(VN100SpiDefs::REG_TEMP_PRESS_DATA,
+ reinterpret_cast<uint8_t*>(&rawData), sizeof(rawData));
+
+ if (err != VN100SpiDefs::VNErrors::NO_ERROR)
+ {
+ // An error occurred while reading data
+ TRACE("getTemperature() failed, error: %u\n", err);
+ return data;
+ }
+
+ // Get measurement from raw data
+ data.temperature = rawData.temp;
+
+ return data;
+}
+
+PressureData VN100Spi::getPressure()
+{
+ PressureData data;
+
+ VN100SpiDefs::RawTempPressData rawData;
+
+ // Get timestamp
+ data.pressureTimestamp = TimestampTimer::getTimestamp();
+
+ VN100SpiDefs::VNErrors err =
+ readRegister(VN100SpiDefs::REG_TEMP_PRESS_DATA,
+ reinterpret_cast<uint8_t*>(&rawData), sizeof(rawData));
+
+ if (err != VN100SpiDefs::VNErrors::NO_ERROR)
+ {
+ // An error occurred while reading data
+ TRACE("getPressure() failed, error: %u\n", err);
+ return data;
+ }
+
+ // Get measurement from raw data
+ data.pressure = rawData.press;
+
+ return data;
+}
+
+VN100SpiDefs::VNErrors VN100Spi::readRegister(const uint8_t regId,
+ uint8_t* payloadBuf,
+ const uint32_t payloadSize)
+{
+ /**
+ * When reading from a sensor's register 2 spi transactions are needed.
+ *
+ * First I have to send the request packet, then wait at least 100
+ * microseconds to let the sensor process the request.
+ *
+ * After this period of time we can proceed with the reading. First
+ * we receive a 4 bytes header, whit the first byte always 0, the second
+ * being the read register command, the third being the register we asked
+ * for and the fourth the error value.
+ *
+ * Finally we receive the content of the register.
+ *
+ * Low level spi is needed in order to issue multiple readings without
+ * raising the chip select.
+ */
+
+ const uint32_t requestPacket =
+ (VN100SpiDefs::READ_REG << 24) | // Read register command
+ (regId << 16); // Id of the register
+
+ // Send request packet
+ spiSlave.bus.select(spiSlave.cs);
+ spiSlave.bus.write32(requestPacket);
+ spiSlave.bus.deselect(spiSlave.cs);
+
+ // Wait at least 100us
+ miosix::delayUs(100);
+
+ // Read response
+ spiSlave.bus.select(spiSlave.cs);
+
+ // Discard the first 3 bytes of the response
+ VN100SpiDefs::VNErrors err =
+ (VN100SpiDefs::VNErrors)(spiSlave.bus.read32() & 255);
+
+ if (err != VN100SpiDefs::VNErrors::NO_ERROR)
+ {
+ // An error occurred while attempting to service the request
+ spiSlave.bus.deselect(spiSlave.cs);
+ return err;
+ }
+
+ spiSlave.bus.read(payloadBuf, payloadSize);
+
+ spiSlave.bus.deselect(spiSlave.cs);
+
+ return VN100SpiDefs::VNErrors::NO_ERROR;
+}
+
+VN100SpiDefs::VNErrors VN100Spi::writeRegister(const uint8_t regId,
+ const uint8_t* payloadBuf,
+ const uint32_t payloadSize)
+{
+ /**
+ * When writing to a sensor's register 2 spi transactions are needed.
+ *
+ * First I have to send the request packet with the value to be written,
+ * then wait at least 100 microseconds to let the sensor process the
+ * request.
+ *
+ * After this period of time we proceed with reading the outcome of the
+ * operation. We receive a 4 bytes header, whit the first byte always 0, the
+ * second being the write register command, the third being the register we
+ * asked for and the fourth the error value. If the error value is 0 no
+ * error occurred and the operation is successful.
+ *
+ * Low level spi is needed in order to issue multiple readings and writings
+ * without raising the chip select.
+ */
+
+ const uint32_t requestPacket =
+ (VN100SpiDefs::WRITE_REG << 24) | // Read register command
+ (regId << 16); // Id of the register
+
+ // Send request packet
+ spiSlave.bus.select(spiSlave.cs);
+ spiSlave.bus.write32(requestPacket);
+ spiSlave.bus.write(payloadBuf, payloadSize);
+ spiSlave.bus.deselect(spiSlave.cs);
+
+ // Wait at least 100us
+ miosix::delayUs(100);
+
+ // Read response
+ spiSlave.bus.select(spiSlave.cs);
+
+ // Discard the first 3 bytes of the response
+ uint8_t err = spiSlave.bus.read32() & 255;
+
+ spiSlave.bus.deselect(spiSlave.cs);
+
+ return (VN100SpiDefs::VNErrors)err;
+}
+
+} // namespace Boardcore
diff --git a/src/shared/sensors/VN100/VN100Spi.h b/src/shared/sensors/VN100/VN100Spi.h
new file mode 100644
index 0000000000000000000000000000000000000000..d90ea7c16ec5dc09f63393d0a5aed0a5e7cf21d0
--- /dev/null
+++ b/src/shared/sensors/VN100/VN100Spi.h
@@ -0,0 +1,171 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Fabrizio Monti
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+/**
+ * Driver for the VN100 SPI IMU.
+ *
+ * The VN100 sensor is a calibrated IMU which includes accelerometer,
+ * magnetometer, gyroscope, barometer and temperature sensor. It also provides
+ * attitude data (yaw, pith, roll, quaternion).
+ * This driver samples imu compensated data (accelerometer, gyroscope and
+ * magnetometer) and quaternion data.
+ *
+ * The sampling rate is 400Hz. The data ready interrupt can be set to a lower
+ * rate by changing the syncOutSkipFactor parameter.
+ *
+ * ATTENTION: at least 100 microseconds has to pass between the read/write
+ * operations with the sensor.
+ */
+
+#include <diagnostic/PrintLogger.h>
+#include <drivers/spi/SPIDriver.h>
+#include <sensors/Sensor.h>
+
+#include "VN100SpiData.h"
+#include "VN100SpiDefs.h"
+
+namespace Boardcore
+{
+
+/**
+ * @brief Driver class for VN100 Spi IMU.
+ */
+class VN100Spi : public Sensor<VN100SpiData>
+{
+public:
+ /**
+ * @brief VN100 constructor.
+ *
+ * @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,
+ uint16_t syncOutSkipFactor);
+
+ /**
+ * @brief Initialize the sensor.
+ */
+ bool init() override;
+
+ /**
+ * @brief Performs self test for the sensor.
+ *
+ * @return Return true if the test was successful.
+ */
+ bool selfTest() override;
+
+ /**
+ * @brief Retrieve temperature data from the sensor [°C].
+ */
+ TemperatureData getTemperature();
+
+ /**
+ * @brief Retrieve pressure data from the sensor [kPa].
+ */
+ PressureData getPressure();
+
+protected:
+ /**
+ * @brief Gather data from the sensor.
+ */
+ VN100SpiData sampleImpl() override;
+
+private:
+ /**
+ * @brief Check the model number register.
+ *
+ * @return Returns false if the red value is not valid.
+ */
+ bool checkModelNumber();
+
+ /**
+ * @brief Utility function used to clean the junk before starting to
+ * communicate with the sensor. It send a 4 bytes packet of zeros to the
+ * sensor.
+ */
+ void sendDummyPacket();
+
+ /**
+ * @brief Set the data ready interrupt.
+ *
+ * @return True if the operation is successful, false otherwise.
+ */
+ bool setInterrupt();
+
+ /**
+ * @brief Get quaternion, accelerometer, gyroscope and magnetometer
+ * measurements from the sensor.
+ *
+ * @param data The variable where measurements will be stored.
+ *
+ * @return True if the operation is successful, false otherwise.
+ */
+ bool getSample(VN100SpiData& data);
+
+ /**
+ * @brief Utility function used to read from a register of the sensor.
+ *
+ * @param regId The id of the register to read from.
+ * @param payloadBuf The buffer where data will be stored.
+ * @param payloadSize The amount of data (in bytes) to be read from the
+ * register.
+ *
+ * @return Zero if the operation is successful, the error code otherwise.
+ */
+ VN100SpiDefs::VNErrors readRegister(const uint8_t regId,
+ uint8_t* payloadBuf,
+ const uint32_t payloadSize);
+
+ /**
+ * @brief Utility function used to write data to a register of the sensor.
+ *
+ * @param regId The id of the register to be written.
+ * @param payloadBuf The buffer containing the data to be written.
+ * @param payloadSize The amount of data (in bytes) to be written.
+ *
+ * @return Zero if the operation is successful, the error code otherwise.
+ */
+ VN100SpiDefs::VNErrors writeRegister(const uint8_t regId,
+ const uint8_t* payloadBuf,
+ const uint32_t payloadSize);
+
+ bool isInit = false;
+
+ 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");
+};
+
+} // namespace Boardcore
diff --git a/src/shared/sensors/VN100/VN100SpiData.h b/src/shared/sensors/VN100/VN100SpiData.h
new file mode 100644
index 0000000000000000000000000000000000000000..40376473b76155ea2dab640716cbe00cdd8aed41
--- /dev/null
+++ b/src/shared/sensors/VN100/VN100SpiData.h
@@ -0,0 +1,81 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Fabrizio Monti
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <sensors/SensorData.h>
+
+namespace Boardcore
+{
+
+/**
+ * @brief Data type class for VN100 Spi.
+ *
+ * Units of measurement:
+ * - Magnetometer [Gauss]
+ * - Accelerometer [m/s^2]
+ * - Gyroscope [rad/s]
+ */
+struct VN100SpiData : public QuaternionData,
+ public MagnetometerData,
+ public AccelerometerData,
+ public GyroscopeData
+{
+
+ VN100SpiData()
+ : QuaternionData{0, 0.0, 0.0, 0.0, 0.0}, MagnetometerData{0, 0.0, 0.0,
+ 0.0},
+ AccelerometerData{0, 0.0, 0.0, 0.0}, GyroscopeData{0, 0.0, 0.0, 0.0}
+ {
+ }
+
+ VN100SpiData(QuaternionData quat, MagnetometerData magData,
+ AccelerometerData accData, GyroscopeData gyro,
+ TemperatureData temp, PressureData pres)
+ : QuaternionData(quat), MagnetometerData(magData),
+ AccelerometerData(accData), GyroscopeData(gyro)
+ {
+ }
+
+ static std::string header()
+ {
+ return "quatTimestamp,quatX,quatY,quatZ,quatW,magneticFieldTimestamp,"
+ "magneticFieldX,magneticFieldY,magneticFieldZ,"
+ "accelerationTimestamp,accelerationX,accelerationY,"
+ "accelerationZ,angularSpeedTimestamp,angularSpeedX,"
+ "angularSpeedY,angularSpeedZ\n";
+ }
+
+ void print(std::ostream& os) const
+ {
+ os << quaternionTimestamp << "," << quaternionX << "," << quaternionY
+ << "," << quaternionZ << "," << quaternionW << ","
+ << magneticFieldTimestamp << "," << magneticFieldX << ","
+ << magneticFieldY << "," << magneticFieldZ << ","
+ << accelerationTimestamp << "," << accelerationX << ","
+ << accelerationY << "," << accelerationZ << ","
+ << angularSpeedTimestamp << "," << angularSpeedX << ","
+ << angularSpeedY << "," << angularSpeedZ << "\n";
+ }
+};
+
+} // namespace Boardcore
diff --git a/src/shared/sensors/VN100/VN100SpiDefs.h b/src/shared/sensors/VN100/VN100SpiDefs.h
new file mode 100644
index 0000000000000000000000000000000000000000..f39d7adc09961b59215a72136a20fbfdc27337c9
--- /dev/null
+++ b/src/shared/sensors/VN100/VN100SpiDefs.h
@@ -0,0 +1,155 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Fabrizio Monti
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+namespace Boardcore
+{
+
+namespace VN100SpiDefs
+{
+
+/**
+ * @brief Internal registers definitions.
+ */
+enum Registers : uint8_t
+{
+ REG_MODEL_NUMBER = 1, ///< WhoAmI register
+ REG_QUAT_IMU_DATA = 15, ///< Quaternion, accelerometer, gyroscope and
+ ///< magnetometer data register
+ REG_SYNC = 32, ///< Used to set data ready interrupt
+ REG_TEMP_PRESS_DATA = 54, ///< Temperature and pressure data register
+};
+
+/**
+ * @brief Commands available for the sensor.
+ */
+enum Commands : uint8_t
+{
+ READ_REG = 1,
+ WRITE_REG = 2,
+};
+
+/**
+ * @brief Error codes of the sensor.
+ */
+enum class VNErrors : uint8_t
+{
+ NO_ERROR = 0,
+ HARD_FAULT = 1,
+ SERIAL_BUFFER_OVERFLOW = 2,
+ INVALID_CHECKSUM = 3,
+ INVALID_COMMAND = 4,
+ NOT_ENOUGH_PARAMETERS = 5,
+ TOO_MANY_PARAMETERS = 6,
+ INVALID_PARAMETER = 7,
+ INVALID_REGISTER = 8,
+ UNAUTHORIZED_ACCESS = 9,
+ WATCHDOG_RESET = 10,
+ OUTPUT_BUFFER_OVERFLOW = 11,
+ INSUFFICIENT_BAUDRATE = 12,
+ ERROR_BUFFER_OVERFLOW = 255,
+};
+
+/**
+ * @brief Data format of the synchronization control register, used for read
+ * and write 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 Data format of the data register (imu and quaternion), used for
+ * sampling operations.
+ */
+struct __attribute__((packed)) RawImuQuatData
+{
+ float quatX;
+ float quatY;
+ float quatZ;
+ float quatW;
+ float magX;
+ float magY;
+ float magZ;
+ float accX;
+ float accY;
+ float accZ;
+ float gyrX;
+ float gyrY;
+ float gyrZ;
+};
+
+/**
+ * @brief Data format of the data register (temperature and pressure), used
+ * for sampling operations.
+ */
+struct __attribute__((packed)) RawTempPressData
+{
+ float magX;
+ float magY;
+ float magZ;
+ float accX;
+ float accY;
+ float accZ;
+ float gyrX;
+ float gyrY;
+ float gyrZ;
+ float temp;
+ float press;
+};
+
+/**
+ * @brief The expected model number to be red from the sensor.
+ */
+const char* const MODEL_NUMBER = "VN-100";
+
+/**
+ * @brief Size of the buffer used to retrieve the model number from the sensor.
+ * It corresponds to the size of the register, see the datasheet for details.
+ */
+const int MODEL_NUMBER_SIZE = 24;
+
+/**
+ * @brief Width of the SyncOut pulse in nanoseconds. Now is set to 1
+ * millisecond.
+ */
+const uint32_t SYNC_OUT_PULSE_WIDTH = 1000000;
+
+} // namespace VN100SpiDefs
+
+} // namespace Boardcore
diff --git a/src/tests/sensors/test-vn100.cpp b/src/tests/sensors/test-vn100-serial.cpp
similarity index 94%
rename from src/tests/sensors/test-vn100.cpp
rename to src/tests/sensors/test-vn100-serial.cpp
index 5f5fd14ada39d8be9b93487304a685e9819db712..c4aa36e8728d16ae0033b1ea56b26519a6943fef 100644
--- a/src/tests/sensors/test-vn100.cpp
+++ b/src/tests/sensors/test-vn100-serial.cpp
@@ -22,14 +22,14 @@
#include <drivers/timer/TimestampTimer.h>
#include <inttypes.h>
-#include <sensors/VN100/VN100.h>
+#include <sensors/VN100/VN100Serial.h>
using namespace miosix;
using namespace Boardcore;
int main()
{
- VN100Data sample;
+ VN100SerialData sample;
string sampleRaw;
GpioPin u2tx1(GPIOA_BASE, 2);
@@ -41,7 +41,7 @@ int main()
u2tx1.mode(Mode::ALTERNATE);
USART usart(USART2, 115200);
- VN100 sensor{usart, 115200, VN100::CRCOptions::CRC_ENABLE_16};
+ VN100Serial sensor{usart, 115200, VN100Serial::CRCOptions::CRC_ENABLE_16};
// Let the sensor start up
Thread::sleep(1000);
diff --git a/src/tests/sensors/test-vn100-spi.cpp b/src/tests/sensors/test-vn100-spi.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..dd0a7e091cb5b55e3d05da3ad47da15b6c92fca9
--- /dev/null
+++ b/src/tests/sensors/test-vn100-spi.cpp
@@ -0,0 +1,92 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Fabrizio Monti
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <drivers/spi/SPIDriver.h>
+#include <sensors/VN100/VN100Spi.h>
+
+using namespace miosix;
+using namespace Boardcore;
+
+int main()
+{
+ // spi setup
+ SPIBus bus(SPI3);
+
+ GpioPin csPin(GPIOE_BASE, 3); // PE3 CS
+ csPin.mode(Mode::OUTPUT);
+ GpioPin clockPin(GPIOB_BASE, 3); // PB3 CK
+ clockPin.mode(Mode::ALTERNATE);
+ clockPin.alternateFunction(6);
+ GpioPin misoPin(GPIOB_BASE, 4); // PB4 MISO
+ misoPin.mode(Mode::ALTERNATE);
+ misoPin.alternateFunction(6);
+ GpioPin mosiPin(GPIOB_BASE, 5); // PB5 MOSI
+ mosiPin.mode(Mode::ALTERNATE);
+ mosiPin.alternateFunction(6);
+
+ GpioPin intPin(GPIOC_BASE, 15); // PC15 interrupt pin
+ intPin.mode(Mode::INPUT);
+
+ SPIBusConfig busConfiguration;
+ busConfiguration.clockDivider = SPI::ClockDivider::DIV_64;
+ busConfiguration.mode = SPI::Mode::MODE_3;
+
+ VN100Spi sensor(bus, csPin, busConfiguration, 200);
+
+ // Let the sensor start up
+ Thread::sleep(1000);
+
+ if (!sensor.init())
+ {
+ printf("Error, cannot initialize the sensor\n\n");
+ return 0;
+ }
+ printf("Sensor initialized\n");
+
+ if (!sensor.selfTest())
+ {
+ printf("Error while performing self test\n\n");
+ return 0;
+ }
+ printf("Self test successful\n");
+
+ for (int i = 0; i < 100; ++i)
+ {
+ sensor.sample();
+ VN100SpiData sample = sensor.getLastSample();
+
+ printf("sample %d:\n", i + 1);
+ printf("acc: %llu, %.3f, %.3f, %.3f\n", sample.accelerationTimestamp,
+ sample.accelerationX, sample.accelerationY,
+ sample.accelerationZ);
+ printf("ang: %.3f, %.3f, %.3f\n", sample.angularSpeedX,
+ sample.angularSpeedY, sample.angularSpeedZ);
+ printf("mag: %.3f, %.3f, %.3f\n", sample.magneticFieldX,
+ sample.magneticFieldY, sample.magneticFieldZ);
+ printf("quat: %.3f, %.3f, %.3f, %.3f\n\n", sample.quaternionX,
+ sample.quaternionY, sample.quaternionZ, sample.quaternionW);
+
+ Thread::sleep(500);
+ }
+
+ return 0;
+}