diff --git a/CMakeLists.txt b/CMakeLists.txt
index 09bec0a5f3a8d1e6bc1aed9a421b271a121b96f2..ecc81f3cda8b50cc39f6dd72bc454462aebbc9f7 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -463,6 +463,10 @@ 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-vn300 src/tests/sensors/test-vn300.cpp)
+sbs_target(test-vn300 stm32f767zi_compute_unit)
+
+
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 5a53153f43b849cb58d00a2589228ea5ae847241..17c8f18bcfe3fe80f80c6e6e07ed3f828c25f72c 100644
--- a/cmake/boardcore.cmake
+++ b/cmake/boardcore.cmake
@@ -109,8 +109,10 @@ 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/VN100Serial.cpp
- ${BOARDCORE_PATH}/src/shared/sensors/VN100/VN100Spi.cpp
+ ${BOARDCORE_PATH}/src/shared/sensors/Vectornav/VNCommonSerial.cpp
+ ${BOARDCORE_PATH}/src/shared/sensors/Vectornav/VN100/VN100Serial.cpp
+ ${BOARDCORE_PATH}/src/shared/sensors/Vectornav/VN100/VN100Spi.cpp
+ ${BOARDCORE_PATH}/src/shared/sensors/Vectornav/VN300/VN300.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 0bed3aa6880acb8ff33e17047ba6484ad2b87230..98884a8dc501f617aba545be8ae4763b7e7d444c 100644
--- a/src/shared/logger/LogTypes.h
+++ b/src/shared/logger/LogTypes.h
@@ -61,8 +61,9 @@
#include <sensors/RotatedIMU/IMUData.h>
#include <sensors/SensorData.h>
#include <sensors/UBXGPS/UBXGPSData.h>
-#include <sensors/VN100/VN100SerialData.h>
-#include <sensors/VN100/VN100SpiData.h>
+#include <sensors/Vectornav/VN100/VN100SerialData.h>
+#include <sensors/Vectornav/VN100/VN100SpiData.h>
+#include <sensors/Vectornav/VN300/VN300Data.h>
#include <sensors/analog/AnalogLoadCellData.h>
#include <sensors/analog/BatteryVoltageSensorData.h>
#include <sensors/analog/Pitot/PitotData.h>
@@ -173,6 +174,7 @@ void registerTypes(Deserializer& ds)
ds.registerType<UBXGPSData>();
ds.registerType<VN100SerialData>();
ds.registerType<VN100SpiData>();
+ ds.registerType<VN300Data>();
ds.registerType<VoltageData>();
ds.registerType<Xbee::XbeeStatus>();
}
diff --git a/src/shared/sensors/VN100/VN100Serial.cpp b/src/shared/sensors/Vectornav/VN100/VN100Serial.cpp
similarity index 100%
rename from src/shared/sensors/VN100/VN100Serial.cpp
rename to src/shared/sensors/Vectornav/VN100/VN100Serial.cpp
diff --git a/src/shared/sensors/VN100/VN100Serial.h b/src/shared/sensors/Vectornav/VN100/VN100Serial.h
similarity index 100%
rename from src/shared/sensors/VN100/VN100Serial.h
rename to src/shared/sensors/Vectornav/VN100/VN100Serial.h
diff --git a/src/shared/sensors/VN100/VN100SerialData.h b/src/shared/sensors/Vectornav/VN100/VN100SerialData.h
similarity index 100%
rename from src/shared/sensors/VN100/VN100SerialData.h
rename to src/shared/sensors/Vectornav/VN100/VN100SerialData.h
diff --git a/src/shared/sensors/VN100/VN100Spi.cpp b/src/shared/sensors/Vectornav/VN100/VN100Spi.cpp
similarity index 100%
rename from src/shared/sensors/VN100/VN100Spi.cpp
rename to src/shared/sensors/Vectornav/VN100/VN100Spi.cpp
diff --git a/src/shared/sensors/VN100/VN100Spi.h b/src/shared/sensors/Vectornav/VN100/VN100Spi.h
similarity index 100%
rename from src/shared/sensors/VN100/VN100Spi.h
rename to src/shared/sensors/Vectornav/VN100/VN100Spi.h
diff --git a/src/shared/sensors/VN100/VN100SpiData.h b/src/shared/sensors/Vectornav/VN100/VN100SpiData.h
similarity index 100%
rename from src/shared/sensors/VN100/VN100SpiData.h
rename to src/shared/sensors/Vectornav/VN100/VN100SpiData.h
diff --git a/src/shared/sensors/VN100/VN100SpiDefs.h b/src/shared/sensors/Vectornav/VN100/VN100SpiDefs.h
similarity index 100%
rename from src/shared/sensors/VN100/VN100SpiDefs.h
rename to src/shared/sensors/Vectornav/VN100/VN100SpiDefs.h
diff --git a/src/shared/sensors/Vectornav/VN300/VN300.cpp b/src/shared/sensors/Vectornav/VN300/VN300.cpp
new file mode 100755
index 0000000000000000000000000000000000000000..55feeebda4484a9e9dc8d6e77f541bfd8fb64a5b
--- /dev/null
+++ b/src/shared/sensors/Vectornav/VN300/VN300.cpp
@@ -0,0 +1,468 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Lorenzo Cucchi, 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 "VN300.h"
+
+#include <drivers/timer/TimestampTimer.h>
+#include <utils/Debug.h>
+
+namespace Boardcore
+{
+
+VN300::VN300(USART& usart, int userBaudRate,
+ VN300Defs::SampleOptions sampleOption, CRCOptions crc,
+ std::chrono::milliseconds timeout,
+ const VN300Defs::AntennaPosition antPosA,
+ const VN300Defs::AntennaPosition antPosB,
+ const Eigen::Matrix3f rotMat)
+ : VNCommonSerial(usart, userBaudRate, "VN300", crc, timeout),
+ sampleOption(sampleOption), antPosA(antPosA), antPosB(antPosB),
+ rotMat(rotMat)
+{
+}
+
+bool VN300::init()
+{
+ SensorErrors backup = lastError;
+
+ // If already initialized
+ if (isInit)
+ {
+ lastError = SensorErrors::ALREADY_INIT;
+ LOG_WARN(logger, "Sensor VN300 already initialized");
+ return false;
+ }
+
+ // Allocate the pre loaded strings based on the user selected crc
+ if (crc == CRCOptions::CRC_ENABLE_16)
+ {
+ preSampleBin1 = "$VNBOM,1*749D\n";
+ }
+ else
+ {
+ preSampleBin1 = "$VNBOM,1*45\n";
+ }
+
+ // Set the error to init fail and if the init process goes without problem
+ // i restore it to the last error
+ lastError = SensorErrors::INIT_FAIL;
+
+ if (recvString.data() == NULL)
+ {
+ LOG_ERR(logger, "Unable to initialize the receive VN300 string");
+ return false;
+ }
+
+ configDefaultSerialPort();
+
+ if (!setCrc(false))
+ {
+ LOG_ERR(logger, "Unable to set the vn300 user selected CRC");
+ return false;
+ }
+
+ if (!disableAsyncMessages(false))
+ {
+ LOG_ERR(logger, "Unable to disable async messages from vn300");
+ return false;
+ }
+
+ if (!configUserSerialPort())
+ {
+ LOG_ERR(logger, "Unable to config the user vn300 serial port");
+ return false;
+ }
+
+ if (!verifyModelNumber("VN-300T-CR"))
+ {
+ LOG_ERR(logger, "Error, model number not corresponding");
+ lastError = INVALID_WHOAMI;
+ return false;
+ }
+
+ if (!setBinaryOutput())
+ {
+ LOG_ERR(logger, "Unable to set binary output register");
+ return false;
+ }
+
+ if (!setAntennaA(antPosA))
+ {
+ LOG_ERR(logger, "Unable to set antenna A position");
+ return false;
+ }
+
+ if (!setCompassBaseline(antPosB))
+ {
+ LOG_ERR(logger, "Unable to set compass baseline");
+ return false;
+ }
+
+ if (!setReferenceFrame(rotMat))
+ {
+ LOG_ERR(logger, "Unable to set reference frame rotation");
+ return false;
+ }
+
+ // I need to repeat this in case of a non default
+ // serial port communication at the beginning
+ if (!setCrc(true))
+ {
+ LOG_ERR(logger, "Unable to set the vn300 user selected CRC");
+ return false;
+ }
+
+ if (!disableAsyncMessages(true))
+ {
+ LOG_ERR(logger, "Unable to disable async messages from vn300");
+ return false;
+ }
+
+ isInit = true;
+
+ // All good i restore the actual last error
+ lastError = backup;
+
+ return true;
+}
+
+bool VN300::selfTest() { return true; }
+
+VN300Data VN300::sampleImpl()
+{
+ D(assert(isInit && "init() was not called"));
+
+ // Reset any errors
+ lastError = SensorErrors::NO_ERRORS;
+
+ if (sampleOption == VN300Defs::SampleOptions::FULL)
+ {
+ return sampleFull();
+ }
+ else if (sampleOption == VN300Defs::SampleOptions::REDUCED)
+ {
+ return sampleReduced();
+ }
+ else
+ {
+ // Sample option not implemented
+ lastError = SensorErrors::COMMAND_FAILED;
+ return lastSample;
+ }
+}
+
+VN300Data VN300::sampleFull()
+{
+ VN300Data data;
+ VN300Defs::BinaryDataFull bindataFull;
+
+ const uint64_t timestamp = TimestampTimer::getTimestamp();
+
+ bool sampleOutcome =
+ false; // True if a valid sample was retrieved from the sensor
+ bool validChecksum = false;
+
+ sampleOutcome =
+ getBinaryOutput<VN300Defs::BinaryDataFull>(bindataFull, preSampleBin1);
+ if (!sampleOutcome)
+ {
+ lastError = NO_NEW_DATA;
+ }
+
+ validChecksum =
+ crc == CRCOptions::CRC_NO ||
+ calculateChecksum16(reinterpret_cast<uint8_t*>(&bindataFull),
+ sizeof(bindataFull)) == 0;
+ if (sampleOutcome && !validChecksum)
+ {
+ lastError = SensorErrors::BUS_FAULT;
+ }
+
+ // Verify if the sample is valid
+ sampleOutcome = sampleOutcome && validChecksum;
+
+ if (sampleOutcome)
+ {
+ buildBinaryDataFull(bindataFull, data, timestamp);
+ return data;
+ }
+ else
+ {
+ // Last error is already set
+ return lastSample;
+ }
+}
+
+VN300Data VN300::sampleReduced()
+{
+ VN300Data data;
+ VN300Defs::BinaryDataReduced binDataReduced;
+
+ const uint64_t timestamp = TimestampTimer::getTimestamp();
+
+ bool sampleOutcome =
+ false; // True if a valid sample was retrieved from the sensor
+ bool validChecksum = false;
+
+ sampleOutcome = getBinaryOutput<VN300Defs::BinaryDataReduced>(
+ binDataReduced, preSampleBin1);
+ if (!sampleOutcome)
+ {
+ lastError = NO_NEW_DATA;
+ }
+
+ validChecksum =
+ crc == CRCOptions::CRC_NO ||
+ calculateChecksum16(reinterpret_cast<uint8_t*>(&binDataReduced),
+ sizeof(binDataReduced)) == 0;
+ if (sampleOutcome && !validChecksum)
+ {
+ lastError = SensorErrors::BUS_FAULT;
+ }
+
+ // Verify if the sample is valid
+ sampleOutcome = sampleOutcome && validChecksum;
+
+ if (sampleOutcome)
+ {
+ buildBinaryDataReduced(binDataReduced, data, timestamp);
+ return data;
+ }
+ else
+ {
+ // Last error is already set
+ return lastSample;
+ }
+}
+
+void VN300::buildBinaryDataFull(const VN300Defs::BinaryDataFull& rawData,
+ VN300Data& data, const uint64_t timestamp)
+{
+ // Quaternion
+ data.quaternionTimestamp = timestamp;
+ data.quaternionW = rawData.quaternionW;
+ data.quaternionX = rawData.quaternionX;
+ data.quaternionY = rawData.quaternionY;
+ data.quaternionZ = rawData.quaternionZ;
+
+ // Accelerometer
+ data.accelerationTimestamp = timestamp;
+ data.accelerationX = rawData.accelX;
+ data.accelerationY = rawData.accelY;
+ data.accelerationZ = rawData.accelZ;
+
+ // Magnetometer
+ data.magneticFieldTimestamp = timestamp;
+ data.magneticFieldX = rawData.magneticX;
+ data.magneticFieldY = rawData.magneticY;
+ data.magneticFieldZ = rawData.magneticZ;
+
+ // Gyroscope
+ data.angularSpeedTimestamp = timestamp;
+ data.angularSpeedX = rawData.angularX;
+ data.angularSpeedY = rawData.angularY;
+ data.angularSpeedZ = rawData.angularZ;
+
+ // Gps
+ data.insTimestamp = timestamp;
+ data.gpsFix = rawData.gpsFix;
+ data.insStatus = rawData.insStatus;
+ data.yaw = rawData.yaw;
+ data.pitch = rawData.pitch;
+ data.roll = rawData.roll;
+ data.latitude = rawData.latitude;
+ data.longitude = rawData.longitude;
+ data.altitude = rawData.altitude;
+ data.velocityX = rawData.velocityX;
+ data.velocityY = rawData.velocityY;
+ data.velocityZ = rawData.velocityZ;
+}
+
+void VN300::buildBinaryDataReduced(const VN300Defs::BinaryDataReduced& rawData,
+ VN300Data& data, const uint64_t timestamp)
+{
+ data = VN300Data();
+
+ // Gps
+ data.insTimestamp = timestamp;
+ data.latitude = rawData.latitude;
+ data.longitude = rawData.longitude;
+ data.altitude = rawData.altitude;
+ data.gpsFix = rawData.gpsFix;
+
+ // Angular
+ data.angularSpeedTimestamp = timestamp;
+ data.angularSpeedX = rawData.angularX;
+ data.angularSpeedY = rawData.angularY;
+ data.angularSpeedZ = rawData.angularZ;
+
+ // Yaw pith roll
+ data.yaw = rawData.yaw;
+ data.pitch = rawData.pitch;
+ data.roll = rawData.roll;
+
+ // Quaternion
+ data.quaternionTimestamp = timestamp;
+ data.quaternionW = rawData.quaternionW;
+ data.quaternionX = rawData.quaternionX;
+ data.quaternionY = rawData.quaternionY;
+ data.quaternionZ = rawData.quaternionZ;
+}
+
+bool VN300::setAntennaA(VN300Defs::AntennaPosition antPos)
+{
+ std::string command;
+
+ command = fmt::format("{}{},{},{}", "VNWRG,57,", antPos.posX, antPos.posY,
+ antPos.posZ);
+
+ usart.clearQueue();
+ if (!sendStringCommand(command))
+ {
+ return false;
+ }
+
+ if (!recvStringCommand(recvString.data(), recvStringMaxDimension))
+ {
+ return false;
+ }
+
+ if (checkErrorVN(recvString.data()))
+ return false;
+
+ return true;
+}
+
+bool VN300::setCompassBaseline(VN300Defs::AntennaPosition antPos)
+{
+ std::string command;
+
+ command = fmt::format("{}{},{},{},{},{},{}", "VNWRG,93,", antPos.posX,
+ antPos.posY, antPos.posZ, antPos.uncX, antPos.uncY,
+ antPos.uncZ);
+
+ usart.clearQueue();
+ if (!sendStringCommand(command))
+ {
+ return false;
+ }
+
+ if (!recvStringCommand(recvString.data(), recvStringMaxDimension))
+ {
+ return false;
+ }
+
+ if (checkErrorVN(recvString.data()))
+ return false;
+
+ return true;
+}
+
+bool VN300::setReferenceFrame(Eigen::Matrix3f rotMat)
+{
+
+ std::string command =
+ fmt::format("{}{},{},{},{},{},{},{},{},{}", "VNWRG,26,", rotMat(0, 0),
+ rotMat(0, 1), rotMat(0, 2), rotMat(1, 0), rotMat(1, 1),
+ rotMat(1, 2), rotMat(2, 0), rotMat(2, 1), rotMat(2, 2));
+
+ usart.clearQueue();
+ if (!sendStringCommand(command))
+ {
+ return false;
+ }
+
+ if (!recvStringCommand(recvString.data(), recvStringMaxDimension))
+ {
+ return false;
+ }
+
+ if (checkErrorVN(recvString.data()))
+ return false;
+
+ return true;
+}
+
+bool VN300::setBinaryOutput()
+{
+ uint16_t outputGroup = 0, commonGroup = 0, gnssGroup = 0;
+
+ switch (sampleOption)
+ {
+ case VN300Defs::SampleOptions::FULL:
+ outputGroup =
+ VN300Defs::BINARYGROUP_COMMON | VN300Defs::BINARYGROUP_GPS;
+
+ commonGroup = VN300Defs::COMMONGROUP_YAWPITCHROLL |
+ VN300Defs::COMMONGROUP_QUATERNION |
+ VN300Defs::COMMONGROUP_ANGULARRATE |
+ VN300Defs::COMMONGROUP_VELOCITY |
+ VN300Defs::COMMONGROUP_ACCEL |
+ VN300Defs::COMMONGROUP_MAGPRES |
+ VN300Defs::COMMONGROUP_INSSTATUS;
+
+ gnssGroup = VN300Defs::GPSGROUP_NUMSATS | VN300Defs::GPSGROUP_FIX |
+ VN300Defs::GPSGROUP_POSLLA;
+ break;
+ case VN300Defs::SampleOptions::REDUCED:
+ outputGroup =
+ VN300Defs::BINARYGROUP_COMMON | VN300Defs::BINARYGROUP_GPS;
+ commonGroup = VN300Defs::COMMONGROUP_YAWPITCHROLL |
+ VN300Defs::COMMONGROUP_QUATERNION |
+ VN300Defs::COMMONGROUP_ANGULARRATE;
+ gnssGroup = VN300Defs::GPSGROUP_FIX | VN300Defs::GPSGROUP_POSLLA;
+
+ break;
+ }
+
+ // "0" means that no asynchronous message is active
+ // "8" represents the divider at which the asynchronous message is sent with
+ // respect to the max rate
+ const char* const comp = "VNWRG,75,0,8";
+
+ std::string command = fmt::format("{},{},{:x},{:x}", comp, outputGroup,
+ commonGroup, gnssGroup);
+
+ usart.clearQueue();
+
+ if (!sendStringCommand(command))
+ {
+ return false;
+ }
+
+ if (!recvStringCommand(recvString.data(), recvStringMaxDimension))
+ {
+ LOG_WARN(logger, "Unable to sample due to serial communication error");
+ return false;
+ }
+
+ if (checkErrorVN(recvString.data()) != 0)
+ {
+ LOG_WARN(logger, "Error while setting binary output: {}",
+ recvString.data());
+ return false;
+ }
+
+ return true;
+}
+
+} // namespace Boardcore
diff --git a/src/shared/sensors/Vectornav/VN300/VN300.h b/src/shared/sensors/Vectornav/VN300/VN300.h
new file mode 100755
index 0000000000000000000000000000000000000000..9bf642020b9c6990dd3db4ec3303c9e3c0348846
--- /dev/null
+++ b/src/shared/sensors/Vectornav/VN300/VN300.h
@@ -0,0 +1,158 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Lorenzo Cucchi, 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/Sensor.h>
+#include <sensors/Vectornav/VNCommonSerial.h>
+
+#include "VN300Data.h"
+#include "VN300Defs.h"
+
+namespace Boardcore
+{
+
+/**
+ * @brief Driver class for VN300 IMU.
+ */
+class VN300 : public Sensor<VN300Data>, public VNCommonSerial
+{
+public:
+ /**
+ * @brief Constructor.
+ *
+ * @param usart Serial bus used for the sensor.
+ * @param BaudRate different from the sensor's default [9600, 19200, 38400,
+ * 57600, 115200, 128000, 230400, 460800, 921600].
+ * @param sampleOption The data packet we want to sample.
+ * @param crc Checksum option.
+ * @param timeout The maximum time that will be waited when reading from the
+ * sensor.
+ * @param antPosA Antenna position A.
+ * @param antPosB Antenna position B.
+ * @param rotMat Rotation matrix.
+ */
+ VN300(USART& usart, int userBaudRate, VN300Defs::SampleOptions sampleOption,
+ CRCOptions crc, std::chrono::milliseconds timeout,
+ VN300Defs::AntennaPosition antPosA = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
+ VN300Defs::AntennaPosition antPosB = {1.0, 0.0, 0.0, 0.0, 0.0, 0.0},
+ Eigen::Matrix3f rotMat = Eigen::Matrix3f::Identity());
+
+ bool init() override;
+
+ bool selfTest() override;
+
+protected:
+ /**
+ * @brief Sample action implementation.
+ */
+ VN300Data sampleImpl() override;
+
+private:
+ /**
+ * @brief Sets the antenna A offset.
+ *
+ * @param antPos antenna position.
+ *
+ * @return True if operation succeeded.
+ */
+ bool setAntennaA(VN300Defs::AntennaPosition antPos);
+
+ /**
+ * @brief Sets the compass baseline, position offset of antenna B respect to
+ * antenna A. Uncertainty must be higher than actual measurement error,
+ * possibly twice as the error.
+ * All measures are in meters [m].
+ *
+ * @param antPos antenna position.
+ *
+ * @return True if operation succeeded.
+ */
+ bool setCompassBaseline(VN300Defs::AntennaPosition antPos);
+
+ /**
+ * @brief Set the reference frame rotation of the sensor in order to have
+ * all the data on the desired reference frame.
+ *
+ * @param rotMat rotation matrix.
+ *
+ * @return True if operation succeeded.
+ */
+ bool setReferenceFrame(Eigen::Matrix3f rotMat);
+
+ /**
+ * @brief Set the binary output register.
+ *
+ * @return True if operation succeeded.
+ */
+ bool setBinaryOutput();
+
+ /**
+ * @brief Utility function for sampling data from the sensor (FULL packet).
+ *
+ * @return The sampled data.
+ */
+ VN300Data sampleFull();
+
+ /**
+ * @brief Utility function for sampling data from the sensor (Reduced
+ * packet).
+ *
+ * @return The sampled data.
+ */
+ VN300Data sampleReduced();
+
+ /**
+ * @brief Build output data packet starting from raw binary data received
+ * from the sensor.
+ *
+ * @param rawData The raw data received from the sensor.
+ * @param data The structure that will contain the output.
+ * @param timestamp The timestamp of the extracted data.
+ */
+ void buildBinaryDataFull(const VN300Defs::BinaryDataFull& rawData,
+ VN300Data& data, const uint64_t timestamp);
+
+ /**
+ * @brief Build output data packet starting from raw binary data received
+ * from the sensor.
+ *
+ * @param rawData The raw data received from the sensor.
+ * @param data The structure that will contain the output.
+ * @param timestamp The timestamp of the extracted data.
+ */
+ void buildBinaryDataReduced(const VN300Defs::BinaryDataReduced& rawData,
+ VN300Data& data, const uint64_t timestamp);
+
+ VN300Defs::SampleOptions sampleOption;
+
+ VN300Defs::AntennaPosition antPosA;
+ VN300Defs::AntennaPosition antPosB;
+ Eigen::Matrix3f rotMat;
+
+ /**
+ * @brief Pre-elaborated binary output polling command.
+ */
+ const char* preSampleBin1 = "";
+};
+
+} // namespace Boardcore
diff --git a/src/shared/sensors/Vectornav/VN300/VN300Data.h b/src/shared/sensors/Vectornav/VN300/VN300Data.h
new file mode 100644
index 0000000000000000000000000000000000000000..538fecbfc5310079dee9c7ab4e7e83b002777b36
--- /dev/null
+++ b/src/shared/sensors/Vectornav/VN300/VN300Data.h
@@ -0,0 +1,99 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Lorenzo Cucchi, 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>
+
+#include "VN300Defs.h"
+
+namespace Boardcore
+{
+
+/**
+ * @brief Data class for VN300.
+ *
+ * Units of measurement:
+ * - Magnetometer [Gauss]
+ * - Accelerometer [m/s^2]
+ * - Gyroscope [rad/s]
+ * - Velocity (NED) [m/s]
+ * - Temperature [°C]
+ * - Pressure [kPa]
+ * - Latitude [deg]
+ * - Longitude [deg]
+ * - Altitude [m]
+ */
+struct VN300Data : public QuaternionData,
+ public MagnetometerData,
+ public AccelerometerData,
+ public GyroscopeData,
+ public VN300Defs::INSData
+{
+
+ // cppcheck-suppress uninitDerivedMemberVar
+ VN300Data()
+ : 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},
+ INSData{0, 0, 0, 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}
+ {
+ }
+
+ // cppcheck-suppress uninitDerivedMemberVar
+ VN300Data(const QuaternionData& quat, const MagnetometerData& magData,
+ const AccelerometerData& accData, const GyroscopeData& gyro,
+ const INSData& ins)
+ : QuaternionData(quat), MagnetometerData(magData),
+ AccelerometerData(accData), GyroscopeData(gyro), INSData(ins)
+ {
+ }
+
+ static std::string header()
+ {
+ return "quaternionTimestamp,quaternionX,quaternionY,quaternionZ,"
+ "quaternionW,magneticFieldTimestamp,"
+ "magneticFieldX,magneticFieldY,magneticFieldZ,"
+ "accelerationTimestamp,accelerationX,accelerationY,"
+ "accelerationZ,angularSpeedTimestamp,angularSpeedX,"
+ "angularSpeedY,angularSpeedZ,insTimeStamp,"
+ "insStatus,yaw,pitch,roll,latitude,"
+ "longitude,altitude,nedVelX,nedVelY,nedVelZ\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 << "," << insTimestamp
+ << "," << insStatus << "," << yaw << "," << pitch << "," << roll
+ << "," << latitude << "," << longitude << "," << altitude << ","
+ << velocityX << "," << velocityY << "," << velocityZ << "\n";
+ }
+};
+
+} // namespace Boardcore
diff --git a/src/shared/sensors/Vectornav/VN300/VN300Defs.h b/src/shared/sensors/Vectornav/VN300/VN300Defs.h
new file mode 100755
index 0000000000000000000000000000000000000000..b7037a26b36a03aabd3c6b51bd527b10b62b0cff
--- /dev/null
+++ b/src/shared/sensors/Vectornav/VN300/VN300Defs.h
@@ -0,0 +1,289 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Lorenzo Cucchi, 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 VN300Defs
+{
+
+/**
+ * @brief The groups of binary data available from the sensor.
+ */
+enum BinaryGroup
+{
+ BINARYGROUP_COMMON = 0x01, ///< Common group.
+ BINARYGROUP_TIME = 0x02, ///< Time group.
+ BINARYGROUP_IMU = 0x04, ///< IMU group.
+ BINARYGROUP_GPS = 0x08, ///< GPS group.
+ BINARYGROUP_ATTITUDE = 0x10, ///< Attitude group.
+ BINARYGROUP_INS = 0x20, ///< INS group.
+ BINARYGROUP_GPS2 = 0x40 ///< GPS2 group.
+};
+
+/**
+ * @brief Values used to select data for the binary output from group 1 (common
+ * group).
+ */
+enum CommonGroup
+{
+ COMMONGROUP_NONE = 0x0000, ///< None.
+ COMMONGROUP_TIMESTARTUP = 0x0001, ///< Time since startup (nanoseconds).
+ COMMONGROUP_TIMEGPS = 0x0002, ///< Gps time.
+ COMMONGROUP_TIMESYNCIN = 0x0004, ///< Time since last sync in trigger.
+ COMMONGROUP_YAWPITCHROLL = 0x0008, ///< Yaw pitch roll.
+ COMMONGROUP_QUATERNION = 0x0010, ///< Quaternion.
+ COMMONGROUP_ANGULARRATE = 0x0020, ///< Angular Rate.
+ COMMONGROUP_POSITION = 0x0040, ///< Position.
+ COMMONGROUP_VELOCITY = 0x0080, ///< Velocity.
+ COMMONGROUP_ACCEL = 0x0100, ///< Acceleration compensated (body).
+ COMMONGROUP_IMU = 0x0200, ///< Uncompensated gyro and accelerometer.
+ COMMONGROUP_MAGPRES =
+ 0x0400, ///< Compensated magnetic, temperature and pressure.
+ COMMONGROUP_DELTATHETA = 0x0800, ///< Delta time, theta and velocity.
+ COMMONGROUP_INSSTATUS = 0x1000, ///< Ins status.
+ COMMONGROUP_SYNCINCNT = 0x2000, ///< SyncIn count.
+ COMMONGROUP_TIMEGPSPPS = 0x4000 ///< Time since last GpsPps trigger.
+};
+
+/**
+ * @brief Values used to select data for the binary output from group 2 (time
+ * group).
+ */
+enum TimeGroup
+{
+ TIMEGROUP_NONE = 0x0000, ///< None.
+ TIMEGROUP_TIMESTARTUP = 0x0001, ///< Time since startup (nanoseconds).
+ TIMEGROUP_TIMEGPS = 0x0002, ///< Gps time.
+ TIMEGROUP_GPSTOW = 0x0004, ///< Time since start of gps week.
+ TIMEGROUP_GPSWEEK = 0x0008, ///< Gps week.
+ TIMEGROUP_TIMESYNCIN = 0x0010, ///< Time since last sync in trigger.
+ TIMEGROUP_TIMEGPSPPS = 0x0020, ///< Time since last GpsPps trigger.
+ TIMEGROUP_TIMEUTC = 0x0040, ///< UTC time.
+ TIMEGROUP_SYNCINCNT = 0x0080, ///< Sync in trigger count.
+ TIMEGROUP_SYNCOUTCNT = 0x0100, ///< Sync out trigger count.
+ TIMEGROUP_TIMESTATUS = 0x0200 ///< Time valid status flag.
+};
+
+/**
+ * @brief Values used to select data for the binary output from group 3 (imu
+ * group).
+ */
+enum ImuGroup
+{
+ IMUGROUP_NONE = 0x0000, ///< None.
+ IMUGROUP_IMUSTATUS = 0x0001, ///< Imu status.
+ IMUGROUP_UNCOMPMAG = 0x0002, ///< Uncompensated magnetic.
+ IMUGROUP_UNCOMPACCEL = 0x0004, ///< Uncompensated accelerometer.
+ IMUGROUP_UNCOMPGYRO = 0x0008, ///< Uncompensated gyroscope.
+ IMUGROUP_TEMP = 0x0010, ///< Temperature.
+ IMUGROUP_PRES = 0x0020, ///< Pressure.
+ IMUGROUP_DELTATHETA = 0x0040, ///< Delta theta angles.
+ IMUGROUP_DELTAVEL = 0x0080, ///< Delta velocity.
+ IMUGROUP_MAG = 0x0100, ///< Compensated magnetometer.
+ IMUGROUP_ACCEL = 0x0200, ///< Compensated accelerometer.
+ IMUGROUP_ANGULARRATE = 0x0400, ///< Compensated gyroscope.
+};
+
+/**
+ * @brief Values used to select data for the binary output from group 4
+ * and 7 (gps and gps2 group).
+ */
+enum GpsGroup
+{
+ GPSGROUP_NONE = 0x0000, ///< None.
+ GPSGROUP_UTC = 0x0001, ///< Gps UTC time.
+ GPSGROUP_TOW = 0x0002, ///< Gps time of week.
+ GPSGROUP_WEEK = 0x0004, ///< Gps week.
+ GPSGROUP_NUMSATS = 0x0008, ///< Number of tracked satellites.
+ GPSGROUP_FIX = 0x0010, ///< Gps fix.
+ GPSGROUP_POSLLA =
+ 0x0020, ///< Gps position (latitude, longitude, altitude).
+ GPSGROUP_POSECEF = 0x0040, ///< Gps position (ECEF).
+ GPSGROUP_VELNED = 0x0080, ///< Gps velocity (NED).
+ GPSGROUP_VELECEF = 0x0100, ///< Gps velocity (ECEF).
+ GPSGROUP_POSU = 0x0200, ///< Position uncertainty (NED).
+ GPSGROUP_VELU = 0x0400, ///< Velocity uncertainty.
+ GPSGROUP_TIMEU = 0x0800, ///< Time uncertainty.
+ GPSGROUP_TIMEINFO = 0x1000, ///< Gps time status and leap seconds.
+ GPSGROUP_DOP = 0x2000, ///< Dilution of precision values.
+};
+
+/**
+ * @brief Values used to select data for the binary output from group 5
+ * (attitude group).
+ */
+enum AttitudeGroup
+{
+ ATTITUDEGROUP_NONE = 0x0000, ///< None.
+ ATTITUDEGROUP_VPESTATUS = 0x0001, ///< VpeStatus.
+ ATTITUDEGROUP_YAWPITCHROLL = 0x0002, ///< Yaw pitch roll.
+ ATTITUDEGROUP_QUATERNION = 0x0004, ///< Quaternion.
+ ATTITUDEGROUP_DCM = 0x0008, ///< Directional cosine matrix.
+ ATTITUDEGROUP_MAGNED = 0x0010, ///< Compensated magnetic (NED).
+ ATTITUDEGROUP_ACCELNED = 0x0020, ///< Compensated acceleration (NED).
+ ATTITUDEGROUP_LINEARACCELBODY =
+ 0x0040, ///< Compensated linear acceleration (no gravity).
+ ATTITUDEGROUP_LINEARACCELNED =
+ 0x0080, ///< Compensated linear acceleration (no gravity) (NED).
+ ATTITUDEGROUP_YPRU = 0x0100, ///< Yaw Pitch Roll uncertainty.
+};
+
+/**
+ * @brief Values used to select data for the binary output from group 6 (ins
+ * group).
+ */
+enum InsGroup
+{
+ INSGROUP_NONE = 0x0000, ///< None.
+ INSGROUP_INSSTATUS = 0x0001, ///< Status.
+ INSGROUP_POSLLA = 0x0002, ///< Position (latitude, longitude, altitude).
+ INSGROUP_POSECEF = 0x0004, ///< Position (ECEF).
+ INSGROUP_VELBODY = 0x0008, ///< Velocity (body).
+ INSGROUP_VELNED = 0x0010, ///< Velocity (NED).
+ INSGROUP_VELECEF = 0x0020, ///< Velocity (ECEF).
+ INSGROUP_MAGECEF = 0x0040, ///< Compensated magnetic (ECEF).
+ INSGROUP_ACCELECEF = 0x0080, ///< Compensated acceleration (ECEF).
+ INSGROUP_LINEARACCELECEF =
+ 0x0100, ///< Compensated linear acceleration (ECEF).
+ INSGROUP_POSU = 0x0200, ///< Position uncertainty.
+ INSGROUP_VELU = 0x0400, ///< Velocity uncertainty.
+};
+
+/**
+ * @brief Structure to handle antenna A position units [m]
+ */
+struct AntennaPosition
+{
+ float posX; // Relative position of GPS antenna (X-axis)
+ float posY;
+ float posZ;
+ float uncX; // Uncertainty in the X-axis position measurement
+ float uncY;
+ float uncZ;
+};
+
+/**
+ * @brief Structure to handle INS (inertial navigation system) data.
+ */
+struct INSData
+{
+ uint64_t insTimestamp;
+ uint16_t gpsFix;
+ uint16_t insStatus;
+ float yaw;
+ float pitch;
+ float roll;
+ float latitude;
+ float longitude;
+ float altitude;
+ float velocityX; // Estimated velocity (NED) in m/s
+ float velocityY;
+ float velocityZ;
+};
+
+/**
+ * @brief Sample options (data output packets) available.
+ */
+enum class SampleOptions : uint8_t
+{
+ FULL, ///< All data is sampled
+ REDUCED, ///< Only data needed for the reduced data packet
+};
+
+/**
+ * @brief Structure to handle binary message in case of full sampling (all
+ * available measurements are taken).
+ */
+struct __attribute__((packed)) BinaryDataFull
+{
+ uint8_t selectedGroups; // Indicates which groups are selected
+ uint16_t selectedFieldsCommonGroup; // Indicates which fields are selected
+ uint16_t selectedFieldsGnssGroup; // Indicates which fields are selected
+ float yaw;
+ float pitch;
+ float roll;
+ float quaternionX;
+ float quaternionY;
+ float quaternionZ;
+ float quaternionW;
+ float angularX;
+ float angularY;
+ float angularZ;
+ float velocityX; // Estimated velocity (NED) in m/s
+ float velocityY;
+ float velocityZ;
+ float accelX;
+ float accelY;
+ float accelZ;
+ float magneticX;
+ float magneticY;
+ float magneticZ;
+ float temperature;
+ float pressure;
+ uint16_t insStatus; // Status of the INS
+ uint8_t numsats; // Number of tracked gps satellites
+ uint8_t gpsFix;
+ double latitude;
+ double longitude;
+ double altitude;
+ uint16_t checksum;
+};
+
+/**
+ * @brief Structure to handle the reduced data packet.
+ * Selected data:
+ * - Common group:
+ * - Yaw pitch and roll
+ * - Quaternion
+ * - Angular rate
+ * - Gps1:
+ * - Fix
+ * - PosLla
+ */
+struct __attribute__((packed)) BinaryDataReduced
+{
+ uint8_t selectedGroups; // Indicates which groups are selected
+ uint16_t selectedFieldsCommonGroup; // Indicates which fields are selected
+ uint16_t selectedFieldsGnssGroup; // Indicates which fields are selected
+ float yaw;
+ float pitch;
+ float roll;
+ float quaternionX;
+ float quaternionY;
+ float quaternionZ;
+ float quaternionW;
+ float angularX;
+ float angularY;
+ float angularZ;
+ uint8_t gpsFix;
+ double latitude;
+ double longitude;
+ double altitude;
+ uint16_t checksum;
+};
+
+} // namespace VN300Defs
+
+} // namespace Boardcore
diff --git a/src/shared/sensors/Vectornav/VNCommonSerial.cpp b/src/shared/sensors/Vectornav/VNCommonSerial.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c4859c0b6cead5e27d26d70ce0b0b18d190fffc1
--- /dev/null
+++ b/src/shared/sensors/Vectornav/VNCommonSerial.cpp
@@ -0,0 +1,503 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro, Lorenzo Cucchi, 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 "VNCommonSerial.h"
+
+#include <drivers/timer/TimestampTimer.h>
+#include <utils/Debug.h>
+
+namespace Boardcore
+{
+
+VNCommonSerial::VNCommonSerial(USART &usart, int baudrate,
+ const char *sensorName, CRCOptions crc,
+ const std::chrono::milliseconds timeout)
+ : usart(usart), baudRate(baudrate), crc(crc),
+ logger(Logging::getLogger(sensorName)), maxTimeout(timeout)
+{
+}
+
+VNCommonSerial::~VNCommonSerial() {}
+
+uint8_t VNCommonSerial::calculateChecksum8(const uint8_t *message, int length)
+{
+ int i;
+ uint8_t result = 0x00;
+
+ // Iterate and XOR all of the elements
+ for (i = 0; i < length; i++)
+ {
+ //^ = XOR Operation
+ result ^= message[i];
+ }
+
+ return result;
+}
+
+uint16_t VNCommonSerial::calculateChecksum16(const uint8_t *message, int length)
+{
+ int i;
+ uint16_t result = 0x0000;
+
+ // Apply the datasheet definition of CRC16-CCITT
+ for (i = 0; i < length; i++)
+ {
+ result = (uint8_t)(result >> 8) | (result << 8);
+ result ^= message[i];
+ result ^= (uint8_t)(result & 0xff) >> 4;
+ result ^= result << 12;
+ result ^= (result & 0x00ff) << 5;
+ }
+
+ return result;
+}
+
+bool VNCommonSerial::verifyChecksum(char *command, int length)
+{
+ int checksumOffset = 0;
+
+ // I look for the checksum position
+ while (checksumOffset < length && command[checksumOffset] != '*')
+ {
+ checksumOffset++;
+ }
+
+ if (checksumOffset == length)
+ {
+ // The command doesn't have any checksum
+ TRACE("No checksum in the command!\n");
+ return false;
+ }
+
+ // Check based on the user selected crc type
+ if (crc == CRCOptions::CRC_ENABLE_16)
+ {
+ if (length != checksumOffset + 5) // 4 hex chars + 1 of position
+ {
+ TRACE("16 bit Checksum wrong length: %d != %d --> %s\n", length,
+ checksumOffset + 5, command);
+ return false;
+ }
+
+ // Calculate the checksum and verify (comparison between numerical
+ // checksum to avoid string bugs e.g 0856 != 865)
+ if (strtol(command + checksumOffset + 1, NULL, 16) !=
+ calculateChecksum16((uint8_t *)(command + 1), checksumOffset - 1))
+ {
+ TRACE("Different checksum: %s\n", command);
+ return false;
+ }
+ }
+ else if (crc == CRCOptions::CRC_ENABLE_8)
+ {
+ if (length != checksumOffset + 3) // 2 hex chars + 1 of position
+ {
+ TRACE("8 bit Checksum wrong length: %d != %d --> %s\n", length,
+ checksumOffset + 3, command);
+ return false;
+ }
+
+ // Calculate the checksum and verify (comparison between numerical
+ // checksum to avoid string bugs e.g 0856 != 865)
+ if (strtol(command + checksumOffset + 1, NULL, 16) !=
+ calculateChecksum8((uint8_t *)(command + 1), checksumOffset - 1))
+ {
+ TRACE("Different checksum: %s\n", command);
+ return false;
+ }
+ }
+
+ return true;
+}
+
+bool VNCommonSerial::disableAsyncMessages(bool waitResponse)
+{
+ // Command string
+ std::string command = "VNWRG,06,00";
+
+ // Clear the receiving queue
+ usart.clearQueue();
+
+ if (!sendStringCommand(command))
+ {
+ return false;
+ }
+
+ if (waitResponse)
+ {
+ if (!recvStringCommand(recvString.data(), recvStringMaxDimension))
+ {
+ return false;
+ }
+
+ if (checkErrorVN(recvString.data()))
+ {
+ return false;
+ }
+ }
+
+ return true;
+}
+
+bool VNCommonSerial::setCrc(bool waitResponse)
+{
+ // Command for the crc change
+ std::string command;
+ CRCOptions backup = crc;
+
+ // Check what type of crc is selected
+ if (crc == CRCOptions::CRC_ENABLE_16)
+ {
+ // The 3 inside the command is the 16bit select. The others are default
+ // values
+ command = "VNWRG,30,0,0,0,0,3,0,1";
+ }
+ else
+ {
+ // Even if the CRC is not enabled i put the 8 bit
+ // checksum because i need to know how many 'X' add at the end
+ // of every command sent
+ command = "VNWRG,30,0,0,0,0,1,0,1";
+ }
+
+ // I need to send the command in both crc because i don't know what type
+ // of crc is previously selected. So in order to get the command accepted
+ // i need to do it two times with different crc.
+ crc = CRCOptions::CRC_ENABLE_8;
+
+ // Send the command
+ if (!sendStringCommand(command))
+ {
+ return false;
+ }
+
+ // Read the answer
+ if (waitResponse)
+ {
+ if (!recvStringCommand(recvString.data(), recvStringMaxDimension))
+ {
+ return false;
+ }
+ }
+
+ crc = CRCOptions::CRC_ENABLE_16;
+
+ // Send the command
+ if (!sendStringCommand(command))
+ {
+ return false;
+ }
+
+ // Read the answer
+ if (waitResponse)
+ {
+ if (!recvStringCommand(recvString.data(), recvStringMaxDimension))
+ {
+ return false;
+ }
+ }
+
+ // Restore the crc
+ crc = backup;
+
+ return true;
+}
+
+void VNCommonSerial::configDefaultSerialPort()
+{
+ // Initial default settings
+ usart.setBaudrate(DEFAULT_BAUDRATE);
+}
+
+bool VNCommonSerial::configUserSerialPort()
+{
+ std::string command;
+
+ // I format the command to change baud rate
+ command = fmt::format("{}{}", "VNWRG,5,", baudRate);
+
+ // I can send the command
+ if (!sendStringCommand(command))
+ {
+ return false;
+ }
+
+ // I can open the serial with user's baud rate
+ usart.setBaudrate(baudRate);
+
+ // Check correct serial init
+ return true;
+}
+
+bool VNCommonSerial::verifyModelNumber(const char *expectedModelNumber)
+{
+ // The model number starts from the 10th position
+ const int modelNumberOffset = 10;
+
+ // Clear the receiving queue
+ usart.clearQueue();
+
+ if (!sendStringCommand("VNRRG,01"))
+ {
+ LOG_WARN(logger, "Unable to send string command");
+ return false;
+ }
+
+ miosix::Thread::sleep(100);
+
+ if (!recvStringCommand(recvString.data(), recvStringMaxDimension))
+ {
+ LOG_WARN(logger, "Unable to receive string command");
+ return false;
+ }
+
+ if (strncmp(expectedModelNumber, recvString.data() + modelNumberOffset,
+ strlen(expectedModelNumber)) != 0)
+ {
+ LOG_ERR(logger, "Model number not corresponding: {} != {}",
+ recvString.data(), expectedModelNumber);
+ return false;
+ }
+
+ if (!verifyChecksum(recvString.data(), recvStringLength))
+ {
+ LOG_ERR(logger, "Checksum verification failed: {}", recvString.data());
+ return false;
+ }
+
+ return true;
+}
+
+QuaternionData VNCommonSerial::sampleQuaternion()
+{
+ unsigned int indexStart = 0;
+ char *nextNumber;
+ QuaternionData data;
+
+ // Look for the second ',' in the string
+ // I can avoid the string control because it has already been done in
+ // sampleImpl
+ for (int i = 0; i < 2; i++)
+ {
+ while (indexStart < recvStringLength && recvString[indexStart] != ',')
+ {
+ indexStart++;
+ }
+ indexStart++;
+ }
+
+ // Parse the data
+ data.quaternionTimestamp = TimestampTimer::getTimestamp();
+ data.quaternionX = strtod(recvString.data() + indexStart + 1, &nextNumber);
+ data.quaternionY = strtod(nextNumber + 1, &nextNumber);
+ data.quaternionZ = strtod(nextNumber + 1, &nextNumber);
+ data.quaternionW = strtod(nextNumber + 1, NULL);
+
+ return data;
+}
+
+MagnetometerData VNCommonSerial::sampleMagnetometer()
+{
+ unsigned int indexStart = 0;
+ char *nextNumber;
+ MagnetometerData data;
+
+ // Look for the sixth ',' in the string
+ // I can avoid the string control because it has already been done in
+ // sampleImpl
+ for (int i = 0; i < 6; i++)
+ {
+ while (indexStart < recvStringLength && recvString[indexStart] != ',')
+ {
+ indexStart++;
+ }
+ indexStart++;
+ }
+
+ // Parse the data
+ data.magneticFieldTimestamp = TimestampTimer::getTimestamp();
+ data.magneticFieldX =
+ strtod(recvString.data() + indexStart + 1, &nextNumber);
+ data.magneticFieldY = strtod(nextNumber + 1, &nextNumber);
+ data.magneticFieldZ = strtod(nextNumber + 1, NULL);
+
+ return data;
+}
+
+AccelerometerData VNCommonSerial::sampleAccelerometer()
+{
+ unsigned int indexStart = 0;
+ char *nextNumber;
+ AccelerometerData data;
+
+ // Look for the ninth ',' in the string
+ // I can avoid the string control because it has already been done in
+ // sampleImpl
+ for (int i = 0; i < 9; i++)
+ {
+ while (indexStart < recvStringLength && recvString[indexStart] != ',')
+ {
+ indexStart++;
+ }
+ indexStart++;
+ }
+
+ // Parse the data
+ data.accelerationTimestamp = TimestampTimer::getTimestamp();
+ data.accelerationX =
+ strtod(recvString.data() + indexStart + 1, &nextNumber);
+ data.accelerationY = strtod(nextNumber + 1, &nextNumber);
+ data.accelerationZ = strtod(nextNumber + 1, NULL);
+
+ return data;
+}
+
+GyroscopeData VNCommonSerial::sampleGyroscope()
+{
+ unsigned int indexStart = 0;
+ char *nextNumber;
+ GyroscopeData data;
+
+ // Look for the twelfth ',' in the string
+ // I can avoid the string control because it has already been done in
+ // sampleImpl
+ for (int i = 0; i < 12; i++)
+ {
+ while (indexStart < recvStringLength && recvString[indexStart] != ',')
+ {
+ indexStart++;
+ }
+ indexStart++;
+ }
+
+ // Parse the data
+ data.angularSpeedTimestamp = TimestampTimer::getTimestamp();
+ data.angularSpeedX =
+ strtod(recvString.data() + indexStart + 1, &nextNumber);
+ data.angularSpeedY = strtod(nextNumber + 1, &nextNumber);
+ data.angularSpeedZ = strtod(nextNumber + 1, NULL);
+
+ return data;
+}
+
+uint8_t VNCommonSerial::checkErrorVN(const char *message)
+{
+ if (strncmp(message, "$VNERR,", 7) == 0)
+ {
+ // Extract the error code
+ int errorCode = atoi(&message[7]);
+ return errorCode; // Error detected
+ }
+
+ return 0; // No error detected
+}
+
+bool VNCommonSerial::closeAndReset()
+{
+ // Send the reset command to the VN300
+ if (!sendStringCommand("VNRST"))
+ {
+ LOG_WARN(logger, "Impossible to reset the VN300");
+ return false;
+ }
+
+ isInit = false;
+
+ return true;
+}
+
+bool VNCommonSerial::sendStringCommand(std::string command)
+{
+ if (crc == CRCOptions::CRC_ENABLE_8)
+ {
+ char checksum[4]; // 2 hex + \n + \0
+ // I convert the calculated checksum in hex using itoa
+ itoa(calculateChecksum8((uint8_t *)command.c_str(), command.length()),
+ checksum, 16);
+ checksum[2] = '\n';
+ checksum[3] = '\0';
+ // I concatenate
+ command = fmt::format("{}{}{}{}", "$", command, "*", checksum);
+ }
+ else if (crc == CRCOptions::CRC_ENABLE_16)
+ {
+ char checksum[6]; // 4 hex + \n + \0
+ // I convert the calculated checksum in hex using itoa
+ itoa(calculateChecksum16((uint8_t *)command.c_str(), command.length()),
+ checksum, 16);
+ checksum[4] = '\n';
+ checksum[5] = '\0';
+ // I concatenate
+ command = fmt::format("{}{}{}{}", "$", command, "*", checksum);
+ }
+ else
+ {
+ // No checksum, i add only 'XX' at the end and not 'XXXX' because
+ // in cas of CRC_NO the enabled crc is 8 bit
+ command = fmt::format("{}{}{}", "$", command, "*XX\n");
+ }
+
+ // I send the final command
+ usart.writeString(command.c_str());
+
+ /**
+ * Wait enough to let the message reach the sensor. The wait time is
+ * expressed in milliseconds. The baudrate is expressed in bps.
+ * As a safety measure 10ms are added to the wait time.
+ *
+ * Thus the command size is multiplied by 1000 to obtain milliseconds and by
+ * 8 because the baudrate is expressed in bit per second.
+ */
+ float waitTime = command.size() * 8000;
+ waitTime /= baudRate;
+ waitTime = ceil(waitTime);
+ waitTime += 10;
+ miosix::Thread::sleep(waitTime);
+
+ return true;
+}
+
+bool VNCommonSerial::recvStringCommand(char *command, int maxLength)
+{
+ int i = 0;
+ // Read the buffer
+ if (!usart.readBlocking(command, maxLength, maxTimeout))
+ {
+ return false;
+ }
+
+ // Iterate until i reach the end or i find \n then i substitute it with a \0
+ while (i < maxLength && command[i] != '\n')
+ {
+ i++;
+ }
+
+ // Terminate the string
+ command[i] = '\0';
+
+ // Assing the length
+ recvStringLength = i - 1;
+
+ return true;
+}
+
+} // namespace Boardcore
diff --git a/src/shared/sensors/Vectornav/VNCommonSerial.h b/src/shared/sensors/Vectornav/VNCommonSerial.h
new file mode 100644
index 0000000000000000000000000000000000000000..08c64c3d50238b225928915d5034901942d44e7d
--- /dev/null
+++ b/src/shared/sensors/Vectornav/VNCommonSerial.h
@@ -0,0 +1,271 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro, Lorenzo Cucchi, 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 <diagnostic/PrintLogger.h>
+#include <drivers/usart/USART.h>
+#include <sensors/SensorData.h>
+
+namespace Boardcore
+{
+
+class VNCommonSerial
+{
+public:
+ enum class CRCOptions : uint8_t
+ {
+ CRC_NO = 0x00,
+ CRC_ENABLE_8 = 0x08,
+ CRC_ENABLE_16 = 0x10
+ };
+
+ /**
+ * @brief Constructor.
+ *
+ * @param usart Serial bus used for the sensor.
+ * @param BaudRate Selectd baudrate.
+ * @param sensorName The name of the sensor (VN100/VN300/...).
+ * @param crc Checksum option.
+ * @param timeout The maximum time that will be waited when reading from the
+ * sensor.
+ */
+ VNCommonSerial(USART &usart, int baudrate, const char *sensorName,
+ CRCOptions crc, const std::chrono::milliseconds timeout);
+
+ ~VNCommonSerial();
+
+ /**
+ * @brief Method to reset the sensor to default values and to close
+ * the connection. Used if you need to close and re initialize the sensor.
+ *
+ * @return True if operation succeeded.
+ */
+ bool closeAndReset();
+
+protected:
+ /**
+ * @brief Calculate the 8bit checksum on the given array.
+ *
+ * @param command Command on which compute the crc.
+ * @param length Array length.
+ *
+ * @return The 8 bit checksum.
+ */
+ uint8_t calculateChecksum8(const uint8_t *message, int length);
+
+ /**
+ * @brief Calculate the 16bit array on the given array.
+ *
+ * @param command Command on which compute the crc.
+ * @param length Array length.
+ *
+ * @return The 16 bit CRC16-CCITT error check.
+ */
+ uint16_t calculateChecksum16(const uint8_t *message, int length);
+
+ /**
+ * @brief Method to verify the crc validity of a command.
+ *
+ * @param command The char array which contains the command.
+ * @param maxLength Maximum length for the command array.
+ *
+ * @return True if operation succeeded.
+ */
+ bool verifyChecksum(char *command, int maxLength);
+
+ /**
+ * @brief Disables the async messages that the sensor is default configured
+ * to send at 40Hz on startup.
+ *
+ * @param waitResponse If true wait for a serial response.
+ *
+ * @return True if operation succeeded.
+ */
+ bool disableAsyncMessages(bool waitResponse);
+
+ /**
+ * @brief Sets the user selected crc method.
+ *
+ * @param waitResponse If true wait for a serial response.
+ *
+ * @return True if operation succeeded.
+ */
+ bool setCrc(bool waitResponse = true);
+
+ /**
+ * @brief Configures the default serial communication.
+ */
+ void configDefaultSerialPort();
+
+ /**
+ * @brief Configures the user defined serial communication.
+ *
+ * @return True if operation succeeded.
+ */
+ bool configUserSerialPort();
+
+ /**
+ * @brief Verify the model number of the sensor.
+ *
+ * @param expectedModelNumber The expected model number.
+ *
+ * @return True if the model number received from the sensor corresponds
+ * with the expected one.
+ */
+ bool verifyModelNumber(const char *expectedModelNumber);
+
+ /**
+ * @brief Utility function used to retrieve the binary output from the
+ * sensor.
+ *
+ * @param binaryData The variable that will hold the data.
+ * @param sampleCommand The command to be sent to sample data.
+ * @return True if operation successful, false otherwise.
+ */
+ template <typename T>
+ bool getBinaryOutput(T &binaryData, const char *const sampleCommand);
+
+ /**
+ * @brief Utility function used to extract quaternion data from the
+ * receiving string.
+ */
+ QuaternionData sampleQuaternion();
+
+ /**
+ * @brief Utility function used to extract magnetometer data from the
+ * receiving string.
+ */
+ MagnetometerData sampleMagnetometer();
+
+ /**
+ * @brief Utility function used to extract accelerometer data from the
+ * receiving string.
+ */
+ AccelerometerData sampleAccelerometer();
+
+ /**
+ * @brief Utility function used to extract gyroscope data from the receiving
+ * string.
+ */
+ GyroscopeData sampleGyroscope();
+
+ /**
+ * @brief Check if the message received from the sensor contains an error.
+ *
+ * @param message The message to be checked.
+ *
+ * @return Returns 0 if no error was found, else returns the actual error
+ * code.
+ */
+ uint8_t checkErrorVN(const char *message);
+
+ /**
+ * @brief Sends the command to the sensor with the correct checksum added
+ * so '*' symbol is not needed at the end of the string as well as the '$'
+ * at the beginning of the command. This function takes into account the
+ * time needed for the command to reach the sensor. DO NOT USE THIS FUNCTION
+ * IF LOW EXECUTION TIME IS NEEDED (for example when sending the sample
+ * command).
+ *
+ * @param command Command to send.
+ *
+ * @return True if operation succeeded.
+ */
+ bool sendStringCommand(std::string command);
+
+ /**
+ * @brief Receives a command from the sensor but
+ * swaps the first \n with a \0 to close the message.
+ *
+ * @param command The char array which will be filled with the command.
+ * @param maxLength Maximum length for the command array.
+ *
+ * @return True if operation succeeded.
+ */
+ bool recvStringCommand(char *command, int maxLength);
+
+ /**
+ * @brief Serial interface that is needed to communicate
+ * with the sensor via ASCII codes.
+ */
+ USART &usart;
+ int baudRate;
+
+ CRCOptions crc;
+
+ PrintLogger logger;
+
+ /**
+ * @brief Default baudrate value for the usart communication.
+ */
+ static const int DEFAULT_BAUDRATE = 115200;
+
+ /**
+ * @brief Maximum size of the receiving string.
+ */
+ static const uint8_t recvStringMaxDimension = 200;
+
+ /**
+ * @brief Buffer used to store the string received from the sensor.
+ */
+ std::array<char, recvStringMaxDimension> recvString;
+
+ /**
+ * @brief Actual strlen() of the recvString.
+ */
+ uint8_t recvStringLength = 0;
+
+ bool isInit = false;
+
+private:
+ /**
+ * @brief The maximum time that will be waited during
+ * a read from the serial channel.
+ */
+ const std::chrono::milliseconds maxTimeout;
+};
+
+template <typename T>
+bool VNCommonSerial::getBinaryOutput(T &binaryData,
+ const char *const sampleCommand)
+{
+ uint8_t initByte = 0;
+
+ // Remove any junk
+ usart.clearQueue();
+
+ usart.writeString(sampleCommand);
+
+ // Check the read of the 0xFA byte to find the start of the message
+ if (usart.readBlocking(&initByte, 1, maxTimeout) && initByte == 0xFA)
+ {
+ if (usart.readBlocking(&binaryData, sizeof(T), maxTimeout))
+ {
+ return true;
+ }
+ }
+
+ return false;
+}
+
+} // namespace Boardcore
diff --git a/src/tests/sensors/test-vn100-serial.cpp b/src/tests/sensors/test-vn100-serial.cpp
index c4aa36e8728d16ae0033b1ea56b26519a6943fef..9d11d0f125ad99680e0adb1b6713cb6ba3e96e8c 100644
--- a/src/tests/sensors/test-vn100-serial.cpp
+++ b/src/tests/sensors/test-vn100-serial.cpp
@@ -22,7 +22,7 @@
#include <drivers/timer/TimestampTimer.h>
#include <inttypes.h>
-#include <sensors/VN100/VN100Serial.h>
+#include <sensors/Vectornav/VN100/VN100Serial.h>
using namespace miosix;
using namespace Boardcore;
diff --git a/src/tests/sensors/test-vn100-spi.cpp b/src/tests/sensors/test-vn100-spi.cpp
index dd0a7e091cb5b55e3d05da3ad47da15b6c92fca9..3750babb55488a881a7bfbfbd18fad2abe719983 100644
--- a/src/tests/sensors/test-vn100-spi.cpp
+++ b/src/tests/sensors/test-vn100-spi.cpp
@@ -21,7 +21,7 @@
*/
#include <drivers/spi/SPIDriver.h>
-#include <sensors/VN100/VN100Spi.h>
+#include <sensors/Vectornav/VN100/VN100Spi.h>
using namespace miosix;
using namespace Boardcore;
diff --git a/src/tests/sensors/test-vn300.cpp b/src/tests/sensors/test-vn300.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a9d5cbaaffb9ffd257d6cf0f425cded50708e891
--- /dev/null
+++ b/src/tests/sensors/test-vn300.cpp
@@ -0,0 +1,90 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Lorenzo Cucchi, 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 <sensors/Vectornav/VN300/VN300.h>
+
+using namespace miosix;
+using namespace Boardcore;
+
+int main()
+{
+ VN300Data sample;
+
+ GpioPin u6tx1(GPIOA_BASE, 2);
+ GpioPin u6rx1(GPIOA_BASE, 3);
+
+ u6rx1.alternateFunction(7);
+ u6rx1.mode(Mode::ALTERNATE);
+ u6tx1.alternateFunction(7);
+ u6tx1.mode(Mode::ALTERNATE);
+
+ const int baud = 921600;
+ USART usart(USART2, baud);
+ VN300 sensor(usart, baud, VN300Defs::SampleOptions::FULL,
+ VN300::CRCOptions::CRC_ENABLE_8,
+ std::chrono::milliseconds(10));
+
+ // Let the sensor start up
+ Thread::sleep(1000);
+
+ printf("Initializing sensor\n");
+ if (!sensor.init())
+ {
+ printf("Error initializing the sensor!\n");
+ return 0;
+ }
+
+ printf("Running self-test\n");
+ if (!sensor.selfTest())
+ {
+ printf("Unable to execute self-test\n");
+ return 0;
+ }
+
+ // Sample and print 10 samples
+ for (int i = 0; i < 100; i++)
+ {
+ sensor.sample();
+ sample = sensor.getLastSample();
+
+ printf("sample %d:\n", i + 1);
+ printf("acc: %f, %f, %f\n", sample.accelerationX, sample.accelerationY,
+ sample.accelerationZ);
+ printf("gyr: %f, %f, %f\n", sample.angularSpeedX, sample.angularSpeedY,
+ sample.angularSpeedZ);
+ printf("magn: %f, %f, %f\n", sample.magneticFieldX,
+ sample.magneticFieldY, sample.magneticFieldZ);
+ printf("attitude: %f, %f, %f\n", sample.yaw, sample.pitch, sample.roll);
+ printf("quaternion: %f, %f, %f, %f\n", sample.quaternionW,
+ sample.quaternionX, sample.quaternionY, sample.quaternionZ);
+ printf("latitude: %f\n", sample.latitude);
+ printf("longitude: %f\n", sample.longitude);
+ printf("gps-fix: %u\n\n", sample.gpsFix);
+
+ Thread::sleep(500);
+ }
+
+ sensor.closeAndReset();
+ printf("Sensor communication closed!\n");
+
+ return 0;
+}