CMakeLists.txt

@@ -445,6 +445,10 @@ sbs_target(test-ubxgps-spi stm32f429zi_skyward_death_stack_x)
 add_executable(test-vn100 src/tests/sensors/test-vn100.cpp)
 sbs_target(test-vn100 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)
 

cmake/boardcore.cmake

@@ -108,7 +108,9 @@ foreach(OPT_BOARD ${BOARDS})
         ${SBS_BASE}/src/shared/sensors/SensorSampler.cpp
         ${SBS_BASE}/src/shared/sensors/UBXGPS/UBXGPSSerial.cpp
         ${SBS_BASE}/src/shared/sensors/UBXGPS/UBXGPSSpi.cpp
-        ${SBS_BASE}/src/shared/sensors/VN100/VN100.cpp
+        ${SBS_BASE}/src/shared/sensors/Vectornav/VNCommonSerial.cpp
+        ${SBS_BASE}/src/shared/sensors/Vectornav/VN100/VN100.cpp
+        ${SBS_BASE}/src/shared/sensors/Vectornav/VN300/VN300.cpp
         ${SBS_BASE}/src/shared/sensors/LIS2MDL/LIS2MDL.cpp
         ${SBS_BASE}/src/shared/sensors/LPS28DFW/LPS28DFW.cpp
         ${SBS_BASE}/src/shared/sensors/LPS22DF/LPS22DF.cpp

mavlink-skyward-lib @ 7ddef5fa

-Subproject commit 59841d5e36c086a66fca49869f420668ebac7b96
+Subproject commit 7ddef5fa31c852bb9474a30b7092a5a5ecc6838b

src/shared/logger/LogTypes.h

@@ -57,7 +57,7 @@
 #include <sensors/MS5803/MS5803Data.h>
 #include <sensors/SensorData.h>
 #include <sensors/UBXGPS/UBXGPSData.h>
-#include <sensors/VN100/VN100Data.h>
+#include <sensors/Vectornav/VN100/VN100Data.h>
 #include <sensors/analog/AnalogLoadCellData.h>
 #include <sensors/analog/BatteryVoltageSensorData.h>
 #include <sensors/analog/Pitot/PitotData.h>

src/shared/sensors/Vectornav/VN300/VN300.h

+/* 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
+
+/**
+ * @brief Driver for the VN300S IMU.
+ *
+ * The VN300S sensor is a calibrated IMU which includes accelerometer,
+ * magnetometer, gyroscope, barometer and temperature sensor. The device
+ * provides also a calibration matrix and an anti-drift matrix for the gyroscope
+ * values. The goal of this driver though is to interface the sensor in its
+ * basic use. Things like asynchronous data and anti-drift techniques haven't
+ * been implemented yet. The driver is intended to be used with the "Rugged
+ * sensor" version (aka only UART communication) although the actual VN300S chip
+ * is capable also of SPI communication.
+ *
+ * The VN300S supports both binary and ASCII encoding for communication but via
+ * serial and with the asynchronous mode disabled only ASCII is available. The
+ * protocol also provides two algorithms to verify the integrity of the messages
+ * (8 bit checksum and 16 bit CRC-CCITT) both selectable by the user using the
+ * constructor method. The serial communication also can be established with
+ * various baud rates:
+ * - 9600
+ * - 19200
+ * - 38400
+ * - 57600
+ * - 115200
+ * - 128000
+ * - 230400
+ * - 460800
+ * - 921600
+ */
+
+#include <ActiveObject.h>
+#include <diagnostic/PrintLogger.h>
+#include <fmt/format.h>
+#include <sensors/Sensor.h>
+#include <sensors/Vectornav/VNCommonSerial.h>
+#include <string.h>
+#include <utils/Debug.h>
+
+#include <Eigen/Core>
+
+#include "VN300Data.h"
+#include "VN300Defs.h"
+#include "drivers/usart/USART.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 Redundancy check option.
+     * @param samplePeriod Sampling period in ms
+     * @param antPos antenna A position
+     */
+    VN300(USART& usart, int userBaudRate,
+          VN300Defs::SamplingMethod samplingMethod =
+              VN300Defs::SamplingMethod::BINARY,
+          CRCOptions crc                     = CRCOptions::CRC_ENABLE_8,
+          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;
+
+    /**
+     * @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();
+    // TODO: move to common files
+
+    bool selfTest() override;
+
+private:
+    /**
+     * @brief Sample action implementation.
+     */
+    VN300Data sampleImpl() override;
+
+    /**
+     * @brief Method to find the baudrate of the sensor at startup
+     *
+     * @return True if operation succeeded.
+     */
+    bool findBaudrate();
+    // TODO: should we keep it? maybe in common files?
+
+    /**
+     * @brief Disables the async messages that the VN300 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 = true);
+    // TODO: move to common files
+
+    /**
+     * @brief Configures the default serial communication.
+     */
+    void configDefaultSerialPort();
+    // TODO: move to common files
+
+    /**
+     * @brief Configures the user defined serial communication.
+     *
+     * @return True if operation succeeded.
+     */
+    bool configUserSerialPort();
+    // TODO: move to common files
+
+    /**
+     * @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);
+    // TODO: move to common files
+
+    /**
+     * @brief Write the settings on the non volatile-memory.
+     *
+     * @return True if operation succeeded.
+     */
+    bool writeSettingsCommand();
+    // TODO: is it used? maybe can be placed in common files?
+
+    /**
+     * @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();
+    // TODO: refactor
+
+    /**
+     * @brief Method implementation of self test.
+     *
+     * @return True if operation succeeded.
+     */
+    bool selfTestImpl();
+
+    VN300Defs::Ins_Lla sampleIns();
+
+    VN300Data sampleBinary();
+
+    VN300Data sampleASCII();
+
+    /**
+     * @brief Receives binary data and parse directly into BinaryData struct
+     * which has the __attribute__(packed)
+     *
+     * @param bindata passed as reference
+     *
+     * @return true if operation succeeded
+     *
+     */
+    bool sampleBin(VN300Defs::BinaryData& bindata);
+    // TODO: can be removed and placed inside sampleBinary()
+
+    /**
+     * @brief Default baudrate value for the usart communication.
+     */
+    static const int defaultBaudRate = 115200;
+
+    VN300Defs::SamplingMethod samplingMethod;
+    bool isInit = false;
+
+    VN300Defs::AntennaPosition antPosA;
+    VN300Defs::AntennaPosition antPosB;
+    Eigen::Matrix3f rotMat;
+
+    /**
+     * @brief IMU pre-elaborated sample string for efficiency reasons.
+     */
+    const char* preSampleImuString = "";
+
+    /**
+     * @brief Temperature and pressure pre-elaborated sample string for
+     * efficiency reasons.
+     */
+    const char* preSampleINSlla = "";
+
+    /**
+     * @brief Pre-elaborated binary output polling command.
+     */
+    const char* preSampleBin1 = "";
+};
+}  // namespace Boardcore

src/shared/sensors/Vectornav/VN300/VN300Data.h

+/* 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 type class
+ */
+struct VN300Data : public QuaternionData,
+                   public MagnetometerData,
+                   public AccelerometerData,
+                   public GyroscopeData,
+                   public VN300Defs::Ins_Lla
+{
+
+    /**
+     * @brief Void parameters constructor
+     */
+    // 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},
+          Ins_Lla{0, 0, 0, 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}
+    {
+    }
+
+    /**
+     * @brief Constructor with parameters
+     *
+     * @param single data structures for all the data
+     */
+    // cppcheck-suppress uninitDerivedMemberVar
+    VN300Data(const QuaternionData& quat, const MagnetometerData& magData,
+              const AccelerometerData& accData, const GyroscopeData& gyro,
+              const Ins_Lla& ins)
+        : QuaternionData(quat), MagnetometerData(magData),
+          AccelerometerData(accData), GyroscopeData(gyro), Ins_Lla(ins)
+    {
+    }
+
+    static std::string header()
+    {
+        return "quaternionTimestamp,quaternionX,quaternionY,quaternionZ,"
+               "quaternionW,magneticFieldTimestamp,"
+               "magneticFieldX,magneticFieldY,magneticFieldZ,"
+               "accelerationTimestamp,accelerationX,accelerationY,"
+               "accelerationZ,angularSpeedTimestamp,angularSpeedX,"
+               "angularSpeedY,angularSpeedZ,insTimeStamp,"
+               "status,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
+           << "," << status << "," << yaw << "," << pitch << "," << roll << ","
+           << latitude << "," << longitude << "," << altitude << "," << nedVelX
+           << "," << nedVelY << "," << nedVelZ << "\n";
+    }
+};
+
+}  // namespace Boardcore

src/shared/sensors/Vectornav/VN300/VN300Defs.h

+/* 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 available binary output groups.
+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 Defines how the samples are extracted from the sensor.
+ */
+enum class SamplingMethod
+{
+    BINARY,   ///< Binary messages are used.
+    TEXTUAL,  ///< Textual messages are used.
+};
+
+/// \brief Flags for the binary group 1 'Common' in the binary output registers.
+enum CommonGroup
+{
+    COMMONGROUP_NONE         = 0x0000,  ///< None.
+    COMMONGROUP_TIMESTARTUP  = 0x0001,  ///< TimeStartup.
+    COMMONGROUP_TIMEGPS      = 0x0002,  ///< TimeGps.
+    COMMONGROUP_TIMESYNCIN   = 0x0004,  ///< TimeSyncIn.
+    COMMONGROUP_YAWPITCHROLL = 0x0008,  ///< YawPitchRoll.
+    COMMONGROUP_QUATERNION   = 0x0010,  ///< Quaternion.
+    COMMONGROUP_ANGULARRATE  = 0x0020,  ///< AngularRate.
+    COMMONGROUP_POSITION     = 0x0040,  ///< Position.
+    COMMONGROUP_VELOCITY     = 0x0080,  ///< Velocity.
+    COMMONGROUP_ACCEL        = 0x0100,  ///< Accel.
+    COMMONGROUP_IMU          = 0x0200,  ///< Imu.
+    COMMONGROUP_MAGPRES      = 0x0400,  ///< MagPres.
+    COMMONGROUP_DELTATHETA   = 0x0800,  ///< DeltaTheta.
+    COMMONGROUP_INSSTATUS    = 0x1000,  ///< InsStatus.
+    COMMONGROUP_SYNCINCNT    = 0x2000,  ///< SyncInCnt.
+    COMMONGROUP_TIMEGPSPPS   = 0x4000   ///< TimeGpsPps.
+};
+
+/// \brief Flags for the binary group 2 'Time' in the binary output registers.
+enum TimeGroup
+{
+    TIMEGROUP_NONE        = 0x0000,  ///< None.
+    TIMEGROUP_TIMESTARTUP = 0x0001,  ///< TimeStartup.
+    TIMEGROUP_TIMEGPS     = 0x0002,  ///< TimeGps.
+    TIMEGROUP_GPSTOW      = 0x0004,  ///< GpsTow.
+    TIMEGROUP_GPSWEEK     = 0x0008,  ///< GpsWeek.
+    TIMEGROUP_TIMESYNCIN  = 0x0010,  ///< TimeSyncIn.
+    TIMEGROUP_TIMEGPSPPS  = 0x0020,  ///< TimeGpsPps.
+    TIMEGROUP_TIMEUTC     = 0x0040,  ///< TimeUTC.
+    TIMEGROUP_SYNCINCNT   = 0x0080,  ///< SyncInCnt.
+    TIMEGROUP_SYNCOUTCNT  = 0x0100,  ///< SyncOutCnt.
+    TIMEGROUP_TIMESTATUS  = 0x0200   ///< TimeStatus.
+};
+
+/// \brief Flags for the binary group 3 'IMU' in the binary output registers.
+enum ImuGroup
+{
+    IMUGROUP_NONE        = 0x0000,  ///< None.
+    IMUGROUP_IMUSTATUS   = 0x0001,  ///< ImuStatus.
+    IMUGROUP_UNCOMPMAG   = 0x0002,  ///< UncompMag.
+    IMUGROUP_UNCOMPACCEL = 0x0004,  ///< UncompAccel.
+    IMUGROUP_UNCOMPGYRO  = 0x0008,  ///< UncompGyro.
+    IMUGROUP_TEMP        = 0x0010,  ///< Temp.
+    IMUGROUP_PRES        = 0x0020,  ///< Pres.
+    IMUGROUP_DELTATHETA  = 0x0040,  ///< DeltaTheta.
+    IMUGROUP_DELTAVEL    = 0x0080,  ///< DeltaVel.
+    IMUGROUP_MAG         = 0x0100,  ///< Mag.
+    IMUGROUP_ACCEL       = 0x0200,  ///< Accel.
+    IMUGROUP_ANGULARRATE = 0x0400,  ///< AngularRate.
+    IMUGROUP_SENSSAT     = 0x0800,  ///< SensSat.
+};
+
+/// \brief Flags for the binary group 4 'GPS' and group 7 'GPS2' in the binary
+/// output registers.
+enum GpsGroup
+{
+    GPSGROUP_NONE     = 0x0000,  ///< None.
+    GPSGROUP_UTC      = 0x0001,  ///< UTC.
+    GPSGROUP_TOW      = 0x0002,  ///< Tow.
+    GPSGROUP_WEEK     = 0x0004,  ///< Week.
+    GPSGROUP_NUMSATS  = 0x0008,  ///< NumSats.
+    GPSGROUP_FIX      = 0x0010,  ///< Fix.
+    GPSGROUP_POSLLA   = 0x0020,  ///< PosLla.
+    GPSGROUP_POSECEF  = 0x0040,  ///< PosEcef.
+    GPSGROUP_VELNED   = 0x0080,  ///< VelNed.
+    GPSGROUP_VELECEF  = 0x0100,  ///< VelEcef.
+    GPSGROUP_POSU     = 0x0200,  ///< PosU.
+    GPSGROUP_VELU     = 0x0400,  ///< VelU.
+    GPSGROUP_TIMEU    = 0x0800,  ///< TimeU.
+    GPSGROUP_TIMEINFO = 0x1000,  ///< TimeInfo.
+    GPSGROUP_DOP      = 0x2000,  ///< Dop.
+};
+
+/// \brief Flags for the binary group 5 'Attitude' in the binary output
+/// registers.
+enum AttitudeGroup
+{
+    ATTITUDEGROUP_NONE            = 0x0000,  ///< None.
+    ATTITUDEGROUP_VPESTATUS       = 0x0001,  ///< VpeStatus.
+    ATTITUDEGROUP_YAWPITCHROLL    = 0x0002,  ///< YawPitchRoll.
+    ATTITUDEGROUP_QUATERNION      = 0x0004,  ///< Quaternion.
+    ATTITUDEGROUP_DCM             = 0x0008,  ///< DCM.
+    ATTITUDEGROUP_MAGNED          = 0x0010,  ///< MagNed.
+    ATTITUDEGROUP_ACCELNED        = 0x0020,  ///< AccelNed.
+    ATTITUDEGROUP_LINEARACCELBODY = 0x0040,  ///< LinearAccelBody.
+    ATTITUDEGROUP_LINEARACCELNED  = 0x0080,  ///< LinearAccelNed.
+    ATTITUDEGROUP_YPRU            = 0x0100,  ///< YprU.
+    ATTITUDEGROUP_HEAVE           = 0x1000,  ///< Heave.
+};
+
+/// \brief Flags for the binary group 6 'INS' in the binary output registers.
+enum InsGroup
+{
+    INSGROUP_NONE            = 0x0000,  ///< None.
+    INSGROUP_INSSTATUS       = 0x0001,  ///< InsStatus.
+    INSGROUP_POSLLA          = 0x0002,  ///< PosLla.
+    INSGROUP_POSECEF         = 0x0004,  ///< PosEcef.
+    INSGROUP_VELBODY         = 0x0008,  ///< VelBody.
+    INSGROUP_VELNED          = 0x0010,  ///< VelNed.
+    INSGROUP_VELECEF         = 0x0020,  ///< VelEcef.
+    INSGROUP_MAGECEF         = 0x0040,  ///< MagEcef.
+    INSGROUP_ACCELECEF       = 0x0080,  ///< AccelEcef.
+    INSGROUP_LINEARACCELECEF = 0x0100,  ///< LinearAccelEcef.
+    INSGROUP_POSU            = 0x0200,  ///< PosU.
+    INSGROUP_VELU            = 0x0400,  ///< VelU.
+};
+
+const unsigned char
+    BinaryGroupLengths[sizeof(uint8_t) * 8][sizeof(uint16_t) * 15] = {
+        {8, 8, 8, 12, 16, 12, 24, 12, 12, 24, 20, 28, 2, 4, 8},     // Group 1
+        {8, 8, 8, 2, 8, 8, 8, 4, 4, 1, 0, 0, 0, 0, 0},              // Group 2
+        {2, 12, 12, 12, 4, 4, 16, 12, 12, 12, 12, 2, 40, 0, 0},     // Group 3
+        {8, 8, 2, 1, 1, 24, 24, 12, 12, 12, 4, 4, 2, 28, 0},        // Group 4
+        {2, 12, 16, 36, 12, 12, 12, 12, 12, 12, 28, 24, 12, 0, 0},  // Group 5
+        {2, 24, 24, 12, 12, 12, 12, 12, 12, 4, 4, 68, 64, 0, 0},    // Group 6
+        {8, 8, 2, 1, 1, 24, 24, 12, 12, 12, 4, 4, 2, 28, 0},        // Group 7
+        {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}  // Invalid group
+};
+
+/**
+ * @brief Structure to handle antenna A position units [m]
+ */
+struct AntennaPosition
+{
+    float posX;
+    float posY;
+    float posZ;
+    float uncX;
+    float uncY;
+    float uncZ;
+};
+
+/**
+ * @brief Structure to handle INS data
+ */
+struct Ins_Lla
+{
+    uint64_t insTimestamp;
+    uint16_t fix_gps;
+    uint16_t fix_ins;
+    uint16_t status;
+    float yaw;
+    float pitch;
+    float roll;
+    float latitude;
+    float longitude;
+    float altitude;
+    float nedVelX;
+    float nedVelY;
+    float nedVelZ;
+};
+
+/**
+ * @brief Structure to handle binary message
+ *
+ * @property The struct needs the packed attribute in order to have contiguous
+ * memory allocation in order to be able to parse directly the received message
+ */
+struct __attribute__((packed)) BinaryData
+{
+    uint8_t group;
+    uint16_t common;
+    uint16_t gnss;
+    float yaw_bin;
+    float pitch_bin;
+    float roll_bin;
+    float quatX_bin;
+    float quatY_bin;
+    float quatZ_bin;
+    float quatW_bin;
+    float angx;
+    float angy;
+    float angz;
+    float velx;
+    float vely;
+    float velz;
+    float accx;
+    float accy;
+    float accz;
+    float magx;
+    float magy;
+    float magz;
+    float temp;
+    float pres;
+    uint16_t ins_status;
+    uint8_t numsats;
+    uint8_t fix;
+    double latitude_bin;
+    double longitude_bin;
+    double altitude_bin;
+    uint16_t checksum;
+};
+
+}  // namespace VN300Defs
+
+}  // namespace Boardcore

src/shared/sensors/Vectornav/VNCommonSerial.cpp

+/* 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 std::string &sensorName, CRCOptions crc)
+    : sensorName(sensorName), usart(usart), baudRate(baudrate), crc(crc),
+      logger(Logging::getLogger(sensorName))
+{
+}
+
+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;
+}
+
+void VNCommonSerial::clearBuffer()
+{
+    char c;
+    while (usart.read(&c, 1))
+    {
+    }
+}
+
+bool VNCommonSerial::asyncPause()
+{
+    usart.writeString("$VNASY,0*XX\n");
+    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::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 some time
+    // TODO dimension the time
+    miosix::Thread::sleep(500);
+
+    return true;
+}
+
+bool VNCommonSerial::recvStringCommand(char *command, int maxLength)
+{
+    // TODO: REMOVE READ-BLOCKING
+    int i = 0;
+    // Read the buffer
+    if (!usart.readBlocking(command, maxLength))
+    {
+        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

src/shared/sensors/Vectornav/VNCommonSerial.h

+/* 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>
+
+#include <string>
+
+namespace Boardcore
+{
+
+class VNCommonSerial
+{
+public:
+    enum class CRCOptions : uint8_t
+    {
+        CRC_NO        = 0x00,
+        CRC_ENABLE_8  = 0x08,
+        CRC_ENABLE_16 = 0x10
+    };
+
+    VNCommonSerial(USART &usart, int baudrate, const std::string &sensorName,
+                   CRCOptions crc);
+
+    ~VNCommonSerial();
+
+private:
+    /**
+     * @brief The name of the sensor, to be displayed inside the log.
+     */
+    const std::string sensorName;
+
+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 Clear the buffer of the serial interface.
+     *
+     * This is a placeholder function for the serial interface.
+     * When the usart driver is corrected this must be changed.
+     */
+    void clearBuffer();
+    // TODO: remove and use the one in the usart driver
+
+    /**
+     * @brief Pause asynchronous messages
+     *
+     * @return True if operation succeeded.
+     */
+    bool asyncPause();
+
+    /**
+     * @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.
+     *
+     * @param command Command to send.
+     *
+     * @return True if operation succeeded.
+     */
+    bool sendStringCommand(std::string command);
+
+    /**
+     * @brief Receives a command from the VN100 serialInterface->recv() 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 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;
+};
+
+}  // namespace Boardcore

src/tests/sensors/test-vn100.cpp

@@ -22,7 +22,7 @@
 
 #include <drivers/timer/TimestampTimer.h>
 #include <inttypes.h>
-#include <sensors/VN100/VN100.h>
+#include <sensors/Vectornav/VN100/VN100.h>
 
 using namespace miosix;
 using namespace Boardcore;

src/tests/sensors/test-vn300.cpp

+/* 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 <drivers/timer/TimestampTimer.h>
+#include <inttypes.h>
+#include <sensors/Vectornav/VN300/VN300.h>
+#include <utils/Stats/Stats.h>
+
+#include <iostream>
+
+#include "diagnostic/CpuMeter/CpuMeter.h"
+#define ENABLE_CPU_METER
+
+using namespace miosix;
+using namespace Boardcore;
+
+int main()
+{
+    VN300Data sample;
+    string sampleRaw;
+
+    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::SamplingMethod::BINARY,
+                 VN300::CRCOptions::CRC_ENABLE_8);
+
+    // 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 < 10; i++)
+    {
+        sensor.sample();
+        sample = sensor.getLastSample();
+
+        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);
+
+        Thread::sleep(500);
+    }
+
+    sensor.closeAndReset();
+    printf("Sensor communication closed!\n");
+
+    return 0;
+}