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.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 "VN300.h"
+
+#include <drivers/timer/TimestampTimer.h>
+
+#include "diagnostic/CpuMeter/CpuMeter.h"
+
+namespace Boardcore
+{
+
+VN300::VN300(USART &usart, int userBaudRate,
+             VN300Defs::SamplingMethod samplingMethod, CRCOptions crc,
+             const VN300Defs::AntennaPosition antPosA,
+             const VN300Defs::AntennaPosition antPosB,
+             const Eigen::Matrix3f rotMat)
+    : VNCommonSerial(usart, userBaudRate, "VN300", crc),
+      samplingMethod(samplingMethod), 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 true;
+    }
+
+    // Allocate the pre loaded strings based on the user selected crc
+    if (crc == CRCOptions::CRC_ENABLE_16)
+    {
+        preSampleImuString = "$VNRRG,15*92EA\n";
+        preSampleINSlla    = "$VNRRG,63*6BBB\n";
+        preSampleBin1      = "$VNBOM,1*749D\n";
+    }
+    else
+    {
+        preSampleImuString = "$VNRRG,15*77\n";
+        preSampleINSlla    = "$VNRRG,63*76\n";
+        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 (!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;
+    }
+
+    // Set the isInit flag true
+    isInit = true;
+
+    // All good i restore the actual last error
+    lastError = backup;
+
+    return true;
+}
+
+bool VN300::closeAndReset()
+{
+    // Sensor not init
+    if (!isInit)
+    {
+        lastError = SensorErrors::NOT_INIT;
+        LOG_WARN(logger, "Sensor VN300 already not initialized");
+        return true;
+    }
+
+    // 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 VN300::writeSettingsCommand()
+{
+    miosix::Thread::sleep(50);
+    clearBuffer();
+    if (!sendStringCommand("VNWNV"))
+    {
+        LOG_WARN(logger, "Impossible to save settings");
+    }
+
+    miosix::Thread::sleep(1000);
+    // Read the answer
+    if (!recvStringCommand(recvString.data(), recvStringMaxDimension))
+    {
+        return false;
+    }
+
+    if (checkErrorVN(recvString.data()))
+        return false;
+
+    // Send the reset command to the VN300 in order to restart the Kalman filter
+    if (!sendStringCommand("VNRST"))
+    {
+        LOG_WARN(logger, "Impossible to reset the VN300");
+
+        return false;
+    }
+
+    // A long wait is needed in order to let the sensor startup
+    miosix::Thread::sleep(500);
+
+    // Read the answer
+    if (!recvStringCommand(recvString.data(), recvStringMaxDimension))
+    {
+        LOG_WARN(logger, "Impossible to reset the VN300");
+        return false;
+    }
+
+    if (checkErrorVN(recvString.data()))
+        return false;
+
+    return true;
+}
+
+bool VN300::selfTest()
+{
+    if (!selfTestImpl())
+    {
+        lastError = SensorErrors::SELF_TEST_FAIL;
+        LOG_WARN(logger, "Unable to perform a successful VN300 self test");
+        return false;
+    }
+
+    return true;
+}
+
+VN300Data VN300::sampleImpl()
+{
+    D(assert(isInit && "init() was not called"));
+    // Reset any errors
+    lastError = SensorErrors::NO_ERRORS;
+
+    if (samplingMethod == VN300Defs::SamplingMethod::BINARY)
+    {
+        return sampleBinary();
+    }
+    else
+    {
+        return sampleASCII();
+    }
+}
+
+VN300Data VN300::sampleBinary()
+{
+    // This function is used to clear the usart buffer, it needs to be replaced
+    // with the function from usart class
+    // TODO: needed?
+    clearBuffer();
+
+    // The sample command is sent to the VN300
+    // TODO: this or sendStringCommand()?
+    usart.writeString(preSampleBin1);
+
+    // A BinaryData variable is created and it will be passed to the sampleBin()
+    // function as a reference
+    VN300Defs::BinaryData bindata;
+
+    const uint64_t timestamp = TimestampTimer::getTimestamp();
+
+    // The @if is necessary to check the result of the sampleBin function,
+    // the sampleBin will return true and the modified bindata variable from
+    // which it's necessary to parse data into the VN300Data struct.
+    if (sampleBin(bindata))  // TODO: this call is not needed, can be done
+                             // directly here (or at least change func names)
+    {
+
+        QuaternionData quat{timestamp, bindata.quatW_bin, bindata.quatX_bin,
+                            bindata.quatY_bin, bindata.quatZ_bin};
+
+        AccelerometerData acc{timestamp, bindata.accx, bindata.accy,
+                              bindata.accz};
+
+        MagnetometerData mag{timestamp, bindata.magx, bindata.magy,
+                             bindata.magz};
+
+        GyroscopeData gyro{timestamp, bindata.angx, bindata.angy, bindata.angz};
+
+        // The static_cast is necessary to cast the double variables into a
+        // float, this cause less problem with floating point precision errors
+        // and it's lighter on memory11
+        VN300Defs::Ins_Lla ins{
+            timestamp,
+            bindata.fix,
+            bindata.fix,  // add function to extract ins_fix from ins_status
+            bindata.ins_status,
+            bindata.yaw_bin,
+            bindata.pitch_bin,
+            bindata.roll_bin,
+            static_cast<float>(bindata.latitude_bin),  // TODO: is it safe?
+            static_cast<float>(bindata.longitude_bin),
+            static_cast<float>(bindata.altitude_bin),
+            bindata.velx,
+            bindata.vely,
+            bindata.velz};
+
+        return VN300Data(quat, mag, acc, gyro, ins);
+    }
+    else
+    {
+        lastError = NO_NEW_DATA;
+        return lastSample;
+    }
+}
+
+VN300Data VN300::sampleASCII()
+{
+    clearBuffer();
+    // Returns Quaternion, Magnetometer, Accelerometer and Gyro
+    usart.writeString(preSampleImuString);
+
+    if (!recvStringCommand(recvString.data(), recvStringMaxDimension))
+    {
+        LOG_WARN(logger, "Unable to sample due to serial communication error");
+        lastError = BUS_FAULT;
+        return lastSample;
+    }
+
+    if (!verifyChecksum(recvString.data(), recvStringLength))
+    {
+        LOG_WARN(logger, "VN300 sampling message invalid checksum");
+        // If something goes wrong i return the last sampled data
+        lastError = BUS_FAULT;
+        return lastSample;
+    }
+
+    QuaternionData quat   = sampleQuaternion();
+    MagnetometerData mag  = sampleMagnetometer();
+    AccelerometerData acc = sampleAccelerometer();
+    GyroscopeData gyro    = sampleGyroscope();
+
+    clearBuffer();
+    // Returns INS LLA message
+    usart.writeString(preSampleINSlla);
+
+    if (!recvStringCommand(recvString.data(), recvStringMaxDimension))
+    {
+        LOG_WARN(logger, "Unable to sample due to serial communication error");
+        lastError = BUS_FAULT;
+        return lastSample;
+    }
+
+    if (!verifyChecksum(recvString.data(), recvStringLength))
+    {
+        LOG_WARN(logger, "VN300 sampling message invalid checksum");
+        lastError = BUS_FAULT;
+        return lastSample;
+    }
+
+    VN300Defs::Ins_Lla ins = sampleIns();
+
+    return VN300Data(quat, mag, acc, gyro, ins);
+}
+
+bool VN300::disableAsyncMessages(bool waitResponse)
+{
+    // Command string
+    std::string command =
+        "VNWRG,06,00";  // Put 0 in register number 6 (ASYNC Config)
+
+    miosix::Thread::sleep(
+        50);  // TODO: needed? I don't think so, but has to be checked
+    // Send the command
+    clearBuffer();
+    if (!sendStringCommand(command))
+    {
+        return false;
+    }
+
+    // Read the answer
+    if (waitResponse)
+    {
+        recvStringCommand(recvString.data(), recvStringMaxDimension);
+
+        if (checkErrorVN(recvString.data()))
+            return false;
+    }
+
+    return true;
+}
+
+bool VN300::findBaudrate()
+{
+    char check[]                         = "VN";
+    const int checkOffset                = 1;
+    std::array<uint32_t, 9> baudrateList = {
+        9600, 19200, 38400, 57600, 115200, 128000, 230400, 460800, 921600};
+
+    // The for loop change at every iteration the baudrate of the usart
+    // and sends a message to the VN300, then if the baudrate is correct the VN
+    // reply and the loop terminates. In this way at the end of the loop the
+    // correct baudrate is set.
+    for (uint32_t i = 0; i < baudrateList.size(); i++)
+    {
+
+        usart.setBaudrate(baudrateList[i]);
+
+        miosix::Thread::sleep(
+            50);  // TODO: needed? I don't think so, but has to be checked
+        // I pause the async messages, we don't know if they are present.
+        asyncPause();
+
+        miosix::Thread::sleep(
+            50);  // TODO: needed? I don't think so, but has to be checked
+
+        usart.writeString("$VNRRG,01*XX\n");
+
+        if (recvStringCommand(recvString.data(), recvStringMaxDimension))
+        {
+            if (strncmp(check, recvString.data() + checkOffset,
+                        strlen(check)) == 0)
+            {
+                return true;
+            }
+        }
+    }
+
+    // If I don't find the correct baudrate I set the default Baudrate
+    usart.setBaudrate(defaultBaudRate);
+    // If i'm here, i didn't find the correct baudrate
+    return false;
+}
+
+bool VN300::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)
+    {
+        recvStringCommand(recvString.data(), recvStringMaxDimension);
+    }
+
+    crc = CRCOptions::CRC_ENABLE_16;
+
+    // Send the command
+    if (!sendStringCommand(command))
+    {
+        return false;
+    }
+
+    // Read the answer
+    if (waitResponse)
+    {
+        recvStringCommand(recvString.data(), recvStringMaxDimension);
+    }
+
+    // Restore the crc
+    crc = backup;
+
+    return true;
+}
+
+bool VN300::setAntennaA(VN300Defs::AntennaPosition antPos)
+{
+    std::string command;
+
+    command = fmt::format("{}{},{},{}", "VNWRG,57,", antPos.posX, antPos.posY,
+                          antPos.posZ);
+
+    miosix::Thread::sleep(
+        50);  // TODO: needed? I don't think so, but has to be checked
+    clearBuffer();
+    if (!sendStringCommand(command))
+    {
+        return false;
+    }
+
+    // Read the answer
+
+    recvStringCommand(recvString.data(), recvStringMaxDimension);
+
+    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);
+
+    miosix::Thread::sleep(
+        50);  // TODO: needed? I don't think so, but has to be checked
+    clearBuffer();
+    if (!sendStringCommand(command))
+    {
+        return false;
+    }
+
+    // Read the answer
+
+    recvStringCommand(recvString.data(), recvStringMaxDimension);
+
+    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));
+
+    miosix::Thread::sleep(
+        50);  // TODO: needed? I don't think so, but has to be checked
+    clearBuffer();
+    if (!sendStringCommand(command))
+    {
+        return false;
+    }
+
+    // Read the answer
+    recvStringCommand(recvString.data(), recvStringMaxDimension);
+
+    if (checkErrorVN(recvString.data()))
+        return false;
+
+    return true;
+}
+
+bool VN300::setBinaryOutput()
+{
+    // Here the output groups and the elements of each group are declared.
+    // Reference to VN300Defs for all the possible groups and elements.
+    // In order to change this elements it's mandatory to modify the struct
+    // BinaryData, doing this is also necessary to modify the VN300Data struct,
+    // this cause also that the sampleBinary function needs to be modified.
+    // Another side effect it's that if a changed is done here it's highly
+    // probable that the ascii version can't sample all the same data.
+    uint16_t outputGroup =
+        VN300Defs::BINARYGROUP_COMMON | VN300Defs::BINARYGROUP_GPS;
+
+    uint16_t commonGroup =
+        VN300Defs::COMMONGROUP_YAWPITCHROLL |
+        VN300Defs::COMMONGROUP_QUATERNION | VN300Defs::COMMONGROUP_ANGULARRATE |
+        VN300Defs::COMMONGROUP_VELOCITY | VN300Defs::COMMONGROUP_ACCEL |
+        VN300Defs::COMMONGROUP_MAGPRES | VN300Defs::COMMONGROUP_INSSTATUS;
+
+    uint16_t gnssGroup = VN300Defs::GPSGROUP_NUMSATS | VN300Defs::GPSGROUP_FIX |
+                         VN300Defs::GPSGROUP_POSLLA;
+
+    // The "comp" string it's created in order format the message and also
+    // compare the reply. The fields represents the binary register "75", the
+    // type of communication "0" means that no asynchronous message is active
+    // and "8" represents the divider at which the asynchronous message is sent
+    // respect to the max rate
+    const char *comp = "VNWRG,75,0,8";
+
+    // Using fmt::format it's possible to format the string also adding the
+    // groups and their respective fields
+    std::string command = fmt::format("{},{},{:x},{:x}", comp, outputGroup,
+                                      commonGroup, gnssGroup);
+
+    // This sleep is used to wait for the VN300, in this phase there are
+    // problems with the deterministic times of the VN300 replies and receiving
+    // capabilities vary randomly. 50ms were found with various test, this wait
+    // can be reduced but sporadic problems could arise.
+    miosix::Thread::sleep(
+        50);  // TODO: needed? I don't think so, but has to be checked
+
+    // This function is used to clear the usart buffer, it needs to be replaced
+    // with the function from usart class
+    // TODO
+    clearBuffer();
+
+    // Send the command
+    if (!sendStringCommand(command))
+    {
+        return false;
+    }
+
+    miosix::Thread::sleep(
+        20);  // TODO: needed? I don't think so, but has to be checked
+
+    if (!recvStringCommand(recvString.data(), recvStringMaxDimension))
+    {
+        LOG_WARN(logger, "Unable to sample due to serial communication error");
+        return false;
+    }
+
+    // The reply is compared with the comp variable and in case of an error the
+    // received message is passed to the logger
+    // TODO: why not using checkErrorVN()?
+    if (strncmp(comp, recvString.data() + 1, strlen(comp)) != 0)
+    {
+        LOG_WARN(logger, "The reply is wrong {}", recvString.data());
+        return false;
+    }
+
+    return true;
+}
+
+bool VN300::selfTestImpl()
+{
+    char modelNumber[]          = "VN-300T-CR";
+    const int modelNumberOffset = 10;
+
+    // Check the init status
+    if (!isInit)
+    {
+        lastError = SensorErrors::NOT_INIT;
+        LOG_WARN(
+            logger,
+            "Unable to perform VN300 self test due to not initialized sensor");
+        return false;
+    }
+
+    miosix::Thread::sleep(100);
+
+    // removing junk
+    // TODO: change to usart.clear()
+    clearBuffer();
+
+    // I check the model number
+    if (!sendStringCommand("VNRRG,01"))
+    {
+        LOG_WARN(logger, "Unable to send string command");
+        lastError = SELF_TEST_FAIL;
+        return false;
+    }
+    miosix::Thread::sleep(20);  // TODO: TO BE REMOVED ONLY FOR EMULATOR
+
+    if (!recvStringCommand(recvString.data(), recvStringMaxDimension))
+    {
+        LOG_WARN(logger, "Unable to receive string command");
+        lastError = SELF_TEST_FAIL;
+        return false;
+    }
+
+    // Now i check that the model number is VN-100 starting from the 10th
+    // position because of the message structure
+    if (strncmp(modelNumber, recvString.data() + modelNumberOffset,
+                strlen(modelNumber)) != 0)
+    {
+        LOG_ERR(logger, "VN-300 not corresponding: {} != {}", recvString.data(),
+                modelNumber);
+        lastError = SELF_TEST_FAIL;
+        return false;
+    }
+
+    // I check the checksum
+    if (!verifyChecksum(recvString.data(), recvStringLength))
+    {
+        LOG_ERR(logger, "Checksum verification failed: {}", recvString.data());
+        lastError = SELF_TEST_FAIL;
+        return false;
+    }
+
+    return true;
+}
+
+VN300Defs::Ins_Lla VN300::sampleIns()
+{
+    unsigned int indexStart = 0;
+    char *nextNumber;
+    VN300Defs::Ins_Lla 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++;
+    }
+
+    // Add the timestamp to the parsed data
+    data.insTimestamp = TimestampTimer::getTimestamp();
+    // Parse the data skipping time and week of the gps
+    strtod(recvString.data() + indexStart, &nextNumber);
+    strtol(nextNumber + 1, &nextNumber, 16);
+    data.status =
+        static_cast<uint16_t>(strtol(nextNumber + 1, &nextNumber, 16));
+    data.fix_ins   = data.status & 0x03;
+    data.fix_gps   = (data.status >> 2) & 0x01;
+    data.yaw       = strtof(nextNumber + 1, &nextNumber);
+    data.pitch     = strtof(nextNumber + 1, &nextNumber);
+    data.roll      = strtof(nextNumber + 1, &nextNumber);
+    data.latitude  = strtod(nextNumber + 1, &nextNumber);
+    data.longitude = strtod(nextNumber + 1, &nextNumber);
+    data.altitude  = strtod(nextNumber + 1, &nextNumber);
+    data.nedVelX   = strtof(nextNumber + 1, &nextNumber);
+    data.nedVelY   = strtof(nextNumber + 1, &nextNumber);
+    data.nedVelZ   = strtof(nextNumber + 1, NULL);
+
+    return data;
+}
+
+bool VN300::sampleBin(VN300Defs::BinaryData &bindata)
+{
+    // TODO: REMOVE READ-BLOCKING
+    unsigned char initByte = 0;
+
+    // Check the read of the 0xFA byte to find the start of the message
+    if (usart.readBlocking(&initByte, 1) && initByte == 0xFA)
+    {
+        if (usart.readBlocking(&bindata, sizeof(VN300Defs::BinaryData)))
+        {
+            return true;
+        }
+    }
+
+    return false;
+}
+
+void VN300::configDefaultSerialPort()
+{
+    // Initial default settings
+    usart.setBaudrate(defaultBaudRate);
+}
+
+/**
+ * Even if the user configured baudrate is the default, I want to reset the
+ * buffer to clean the junk.
+ */
+bool VN300::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;
+}
+
+}  // namespace Boardcore

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;
+}