From 7087c543f391a8b9dbe646359d212b14876484f4 Mon Sep 17 00:00:00 2001
From: Lorenzo Cucchi <lorenzo.cucchi@skywarder.eu>
Date: Thu, 18 May 2023 22:49:38 +0200
Subject: [PATCH] [VN300] Initial commit files definition
---
src/shared/sensors/VN300/VN300.cpp | 496 +++++++++++++++++++++++++++
src/shared/sensors/VN300/VN300.h | 423 +++++++++++++++++++++++
src/shared/sensors/VN300/VN300Data.h | 105 ++++++
3 files changed, 1024 insertions(+)
create mode 100644 src/shared/sensors/VN300/VN300.cpp
create mode 100644 src/shared/sensors/VN300/VN300.h
create mode 100644 src/shared/sensors/VN300/VN300Data.h
diff --git a/src/shared/sensors/VN300/VN300.cpp b/src/shared/sensors/VN300/VN300.cpp
new file mode 100644
index 000000000..16a4946c8
--- /dev/null
+++ b/src/shared/sensors/VN300/VN300.cpp
@@ -0,0 +1,496 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Lorenzo Cucchi
+ *
+ * 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>
+
+namespace Boardcore
+{
+VN300::VN300(USARTType *portNumber, USARTInterface::Baudrate baudRate,
+ CRCOptions crc, uint16_t samplePeriod)
+ : portNumber(portNumber), baudRate(baudRate), crc(crc)
+{
+ this->samplePeriod = samplePeriod;
+}
+
+bool VN300::init()
+{
+ SensorErrors backup = lastError;
+
+ // If already initialized
+ if (isInit)
+ {
+ lastError = SensorErrors::ALREADY_INIT;
+ LOG_WARN(logger, "Sensor vn300 already initilized");
+ return true;
+ }
+
+ // Allocate the receive vector
+ recvString = new char[recvStringMaxDimension];
+
+ // Allocate the pre loaded strings based on the user selected crc
+ if (crc == CRCOptions::CRC_ENABLE_16)
+ {
+ preSampleImuString = new string("$VNRRG,15*92EA\n");
+ preSampleTempPressString = new string("$VNRRG,54*4E0F\n");
+ }
+ else
+ {
+ preSampleImuString = new string("$VNRRG,15*77\n");
+ preSampleTempPressString = new string("$VNRRG,54*72\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 == NULL)
+ {
+ LOG_ERR(logger, "Unable to initialize the receive vn300 string");
+ return false;
+ }
+
+ if (!configDefaultSerialPort())
+ {
+ LOG_ERR(logger, "Unable to config the default vn100 serial port");
+ return false;
+ }
+
+ 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 vn100");
+ return false;
+ }
+
+ if (!configUserSerialPort())
+ {
+ LOG_ERR(logger, "Unable to config the user vn100 serial port");
+ 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 vn100 user selected CRC");
+ return false;
+ }
+
+ if (!disableAsyncMessages(true))
+ {
+ LOG_ERR(logger, "Unable to disable async messages from vn100");
+ return false;
+ }
+
+ if (!this->start())
+ {
+ LOG_ERR(logger, "Unable to start the sampling thread");
+ return false;
+ }
+
+ // Set the isInit flag true
+ isInit = true;
+
+ // All good i restore the actual last error
+ lastError = backup;
+
+ return true;
+}
+
+void VN300::run()
+{
+ while (!shouldStop())
+ {
+ long long initialTime = miosix::getTick();
+
+ // Sample the data locking the mutex
+ miosix::Lock<FastMutex> l(mutex);
+ threadSample = sampleData();
+
+ // Sleep for the sampling period
+ miosix::Thread::sleepUntil(initialTime + samplePeriod);
+ }
+}
+
+VN300Data VN300::sampleData()
+{
+ if (!isInit)
+ {
+ lastError = SensorErrors::NOT_INIT;
+ LOG_WARN(logger,
+ "Unable to sample due to not initialized vn100 sensor");
+ return lastSample;
+ }
+
+ // Before sampling i check for errors
+ if (lastError != SensorErrors::NO_ERRORS)
+ {
+ return lastSample;
+ }
+
+ // Returns Quaternion, Magnetometer, Accelerometer and Gyro
+ if (!(serialInterface->writeString(preSampleImuString->c_str())))
+ {
+ // If something goes wrong i return the last sampled data
+ return lastSample;
+ }
+
+ // Wait some time
+ // TODO dimension the time
+ miosix::Thread::sleep(1);
+
+ if (!recvStringCommand(recvString, recvStringMaxDimension))
+ {
+ // If something goes wrong i return the last sampled data
+ return lastSample;
+ }
+
+ if (!verifyChecksum(recvString, recvStringLength))
+ {
+ LOG_WARN(logger, "Vn100 sampling message invalid checksum");
+ // If something goes wrong i return the last sampled data
+ return lastSample;
+ }
+
+ // Now i have to parse the data
+ QuaternionData quat = sampleQuaternion();
+ MagnetometerData mag = sampleMagnetometer();
+ AccelerometerData acc = sampleAccelerometer();
+ GyroscopeData gyro = sampleGyroscope();
+
+ // Returns Magnetometer, Accelerometer, Gyroscope, Temperature and Pressure
+ // (UNCOMPENSATED) DO NOT USE THESE MAGNETOMETER, ACCELEROMETER AND
+ // GYROSCOPE VALUES
+ if (!(serialInterface->writeString(preSampleTempPressString->c_str())))
+ {
+ // If something goes wrong i return the last sampled data
+ return lastSample;
+ }
+
+ // Wait some time
+ // TODO dimension the time
+ miosix::Thread::sleep(1);
+
+ if (!recvStringCommand(recvString, recvStringMaxDimension))
+ {
+ // If something goes wrong i return the last sampled data
+ return lastSample;
+ }
+
+ if (!verifyChecksum(recvString, recvStringLength))
+ {
+ LOG_WARN(logger, "Vn100 sampling message invalid checksum");
+ // If something goes wrong i return the last sampled data
+ return lastSample;
+ }
+
+ // Parse the data
+ TemperatureData temp = sampleTemperature();
+ PressureData press = samplePressure();
+
+ return VN100Data(quat, mag, acc, gyro, temp, press);
+}
+
+bool VN300::disableAsyncMessages(bool waitResponse)
+{
+ // Command string
+ std::string command =
+ "VNWRG,06,00"; // Put 0 in register number 6 (ASYNC Config)
+
+ // Send the command
+ if (!sendStringCommand(command))
+ {
+ return false;
+ }
+
+ // Read the answer
+ if (waitResponse)
+ {
+ recvStringCommand(recvString, recvStringMaxDimension);
+ }
+
+ return true;
+}
+
+bool VN300::AsyncPauseCommand(bool waitResponse, bool selection)
+{
+ if (selection)
+ {
+ if (!sendStringCommand(ASYNC_PAUSE_COMMAND))
+ {
+ return false;
+ }
+ }
+ else
+ {
+ if (!sendStringCommand(ASYNC_RESUME_COMMAND))
+ {
+ return false;
+ }
+ }
+
+ // Read the answer
+ if (waitResponse)
+ {
+ recvStringCommand(recvString, recvStringMaxDimension);
+ }
+
+ return true;
+}
+
+bool VN300::configDefaultSerialPort()
+{
+ // Initial default settings
+ serialInterface = new USART(portNumber, USARTInterface::Baudrate::B115200);
+
+ // Check correct serial init
+ return serialInterface->init();
+}
+
+/**
+ * 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,", static_cast<int>(baudRate));
+
+ // I can send the command
+ if (!sendStringCommand(command))
+ {
+ return false;
+ }
+
+ // Destroy the serial object
+ delete serialInterface;
+
+ // I can open the serial with user's baud rate
+ serialInterface = new USART(portNumber, baudRate);
+
+ // Check correct serial init
+ return serialInterface->init();
+}
+
+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 = "VNRRG,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 = "VNRRG,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, recvStringMaxDimension);
+ }
+
+ crc = CRCOptions::CRC_ENABLE_16;
+
+ // Send the command
+ if (!sendStringCommand(command))
+ {
+ return false;
+ }
+
+ // Read the answer
+ if (waitResponse)
+ {
+ recvStringCommand(recvString, recvStringMaxDimension);
+ }
+
+ // Restore the crc
+ crc = backup;
+
+ return true;
+}
+
+bool VN300::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
+ if (!serialInterface->writeString(command.c_str()))
+ {
+ return false;
+ }
+
+ // Wait some time
+ // TODO dimension the time
+ miosix::Thread::sleep(500);
+
+ return true;
+}
+
+bool VN300::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;
+}
+
+uint8_t VN300::calculateChecksum8(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 VN300::calculateChecksum16(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;
+}
+
+} // namespace Boardcore
\ No newline at end of file
diff --git a/src/shared/sensors/VN300/VN300.h b/src/shared/sensors/VN300/VN300.h
new file mode 100644
index 000000000..b503615de
--- /dev/null
+++ b/src/shared/sensors/VN300/VN300.h
@@ -0,0 +1,423 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Lorenzo Cucchi
+ *
+ * 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 VN300 IMU.
+ *
+ * The VN-300 is a miniature, surface-mount, high-performance GPS-Aided Inertial
+ * Navigation System (GPS/INS). Incorporating the latest solid-state MEMS sensor
+ * technology, the VN-300 combines a set of 3-axis accelerometers, 3-axis gyros,
+ * 3-axis magnetometer, a barometric pressure sensor, two separate 50-channel L1
+ * GPS receivers, as well as a 32-bit processor into a miniature aluminum
+ * enclosure.
+ * The VN300 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 <string.h>
+#include <utils/Debug.h>
+
+#include "VN300Data.h"
+#include "drivers/usart/USART.h"
+
+namespace Boardcore
+{
+/**
+ * @brief Driver class for VN300
+ */
+class VN300 : public Sensor<VN300Data>, public ActiveObject
+{
+public:
+ /**
+ * @brief Checksum Options
+ */
+ enum class CRCOptions : uint8_t
+ {
+ CRC_NO = 0x00,
+ CRC_ENABLE_8 = 0x08,
+ CRC_ENABLE_16 = 0x10
+ };
+ /**
+ * @brief Async options
+ * ASYNC_NO corresponds to no Async communication
+ * ASYNC_P1 corresponds to Async comm only on serial port 1
+ * ASYNC_P2 corresponds to Async comm only on serial port 2
+ * ASYNC_BOTH both serial port are used
+ */
+ enum class AsyncOptions : uint16_t
+ {
+ ASYNC_NO = 0x00,
+ ASYNC_P1 = 0x01,
+ ASYNC_P2 = 0x02,
+ ASYNC_BOTH = 0x03
+ };
+
+ enum class RateDivisor : uint16_t
+ {
+ RDIV_400_HZ = 0x01,
+ RDIV_200_HZ = 0x02,
+ RDIV_100_HZ = 0x04,
+ RDIV_50_HZ = 0x08,
+ RDIV_40_HZ = 0x0A
+ };
+
+ /**
+ * @brief Group's bit for binary register setup
+ */
+ enum OutputGroup : uint16_t
+ {
+ GROUP_1 = 1 << 0,
+ GROUP_2 = 1 << 1,
+ GROUP_3 = 1 << 2,
+ GROUP_4 = 1 << 3,
+ GROUP_5 = 1 << 4,
+ GROUP_6 = 1 << 5,
+ GROUP_7 = 1 << 6
+ };
+
+ /**
+ * @brief Common Group
+ */
+ enum GroupField_1 : uint16_t
+ {
+ T_START = 1 << 0,
+ T_GPS = 1 << 1,
+ T_SYNC = 1 << 2,
+ Y_P_R = 1 << 3,
+ QUAT = 1 << 4,
+ ANG_RATE = 1 << 5,
+ POS = 1 << 6,
+ VEL = 1 << 7,
+ ACCEL = 1 << 8,
+ IMU = 1 << 9,
+ MAGPRES = 1 << 10,
+ DELT_TH = 1 << 11,
+ INS_STAT = 1 << 12,
+ SYNC_IN_CNT = 1 << 13,
+ T_GPS_PPS = 1 << 14
+ };
+
+ /**
+ * @brief TIME Group
+ */
+ enum GroupField_2 : uint16_t
+ {
+ T_START = 1 << 0,
+ T_GPS = 1 << 1,
+ GPS_TOW = 1 << 2,
+ GPS_WEEK = 1 << 3,
+ T_SYNC = 1 << 4,
+ T_GPS_PPS = 1 << 5,
+ T_UTC = 1 << 6,
+ SYNC_IN_CNT = 1 << 7,
+ SYNC_OUT_CNT = 1 << 8,
+ T_STATUS = 1 << 9,
+ };
+
+ /**
+ * @brief IMU Group
+ */
+ enum GroupField_3 : uint16_t
+ {
+ IMU_STAT = 1 << 0,
+ UNC_MAG = 1 << 1,
+ UNC_ACC = 1 << 2,
+ UNC_GYRO = 1 << 3,
+ TEMP = 1 << 4,
+ PRES = 1 << 5,
+ DELTA_TH = 1 << 6,
+ DELTA_VEL = 1 << 7,
+ MAG = 1 << 8,
+ ACCEL = 1 << 9,
+ ANG_RATE = 1 << 10,
+ };
+
+ /**
+ * @brief GNSS1 Group
+ */
+ enum GroupField_4 : uint16_t
+ {
+ UTC = 1 << 0,
+ TOW = 1 << 1,
+ WEEK = 1 << 2,
+ N_SATS = 1 << 3,
+ FIX = 1 << 4,
+ POS_LLA = 1 << 5,
+ POS_ECEF = 1 << 6,
+ VEL_NED = 1 << 7,
+ VEL_ECEF = 1 << 8,
+ POS_U = 1 << 9,
+ VEL_U = 1 << 10,
+ TIME_U = 1 << 11,
+ TIME_INFO = 1 << 12,
+ DOP = 1 << 13,
+ SAT_INFO = 1 << 14,
+ RAW_MEAS = 1 << 15
+ };
+
+ /**
+ * @brief Attitude Group
+ */
+ enum GroupField_5 : uint16_t
+ {
+ RESERVED = 1 << 0,
+ Y_P_R = 1 << 1,
+ QUAT = 1 << 2,
+ DCM = 1 << 3,
+ MAG_NED = 1 << 4,
+ ACC_NED = 1 << 5,
+ LIN_ACC_BOD = 1 << 6,
+ LIN_ACC_NED = 1 << 7,
+ YprU = 1 << 8,
+ RESERVED = 1 << 9,
+ RESERVED = 1 << 10,
+ RESERVED = 1 << 11
+ };
+
+ /**
+ * @brief INS Group
+ */
+ enum GroupField_6 : uint16_t
+ {
+ INS_STATUS = 1 << 0,
+ POS_LLA = 1 << 1,
+ POS_ECEF = 1 << 2,
+ VEL_BODY = 1 << 3,
+ VEL_NED = 1 << 4,
+ VEL_ECEF = 1 << 5,
+ MAG_ECEF = 1 << 6,
+ ACC_ECEF = 1 << 7,
+ LIN_ACC_ECEF = 1 << 8,
+ POS_U = 1 << 9,
+ VEL_U = 1 << 10
+ };
+
+ /**
+ * @brief GNSS2 Group
+ */
+ enum GroupField_7 : uint16_t
+ {
+ UTC = 1 << 0,
+ TOW = 1 << 1,
+ WEEK = 1 << 2,
+ N_SATS = 1 << 3,
+ FIX = 1 << 4,
+ POS_LLA = 1 << 5,
+ POS_ECEF = 1 << 6,
+ VEL_NED = 1 << 7,
+ VEL_ECEF = 1 << 8,
+ POS_U = 1 << 9,
+ VEL_U = 1 << 10,
+ TIME_U = 1 << 11,
+ TIME_INFO = 1 << 12,
+ DOP = 1 << 13,
+ SAT_INFO = 1 << 14,
+ RAW_MEAS = 1 << 15
+ };
+
+ VN300(USARTType *portNumber = USART1,
+ USART::Baudrate baudRate = USART::Baudrate::B921600,
+ CRCOptions crc = CRCOptions::CRC_ENABLE_8,
+ uint16_t samplePeriod = 50);
+
+ bool init() override;
+
+ bool sampleRaw();
+
+ bool closeAndReset();
+
+ bool selfTest() override;
+
+private:
+ /**
+ * @brief Active object method, about the thread execution
+ */
+ void run() override;
+
+ /**
+ * @brief Sampling method used by the thread
+ *
+ * @return VN100Data The sampled data
+ */
+ VN300Data sampleData();
+
+ /**
+ * @brief Disables the async messages that the vn100 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);
+
+ /**
+ * @brief Pause async messages in order to write command without receiving
+ * continuous data transmission
+ *
+ * @param waitResponse If true wait for a serial response.
+ *
+ * @param selection if true pause the async output, if false restart the
+ * output
+ *
+ * @return True if operation succeeded.
+ *
+ */
+ bool VN300::AsyncPauseCommand(bool waitResponse = false, bool selection);
+
+ /**
+ * @brief Configures the default serial communication.
+ *
+ * @return True if operation succeeded.
+ */
+ bool configDefaultSerialPort();
+
+ /**
+ * @brief Configures the user defined serial communication.
+ *
+ * @return True if operation succeeded.
+ */
+ bool configUserSerialPort();
+
+ /**
+ * @brief Sets the user selected crc method.
+ *
+ * @param waitResponse If true wait for a serial response.
+ *
+ * @return True if operation succeeded.
+ */
+ bool setCrc(bool waitResponse = true);
+
+ /**
+ * @brief Method implementation of self test.
+ *
+ * @return True if operation succeeded.
+ */
+ bool selfTestImpl();
+
+ /**
+ * @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 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 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(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(uint8_t *message, int length);
+
+ USARTType *portNumber;
+ USART::Baudrate baudRate;
+ uint16_t samplePeriod;
+ CRCOptions crc;
+ bool isInit = false;
+
+ std::string ASYNC_PAUSE_COMMAND = "VNASY,0";
+ std::string ASYNC_RESUME_COMMAND = "VNASY,1";
+ /**
+ * @brief Pointer to the received string by the sensor. Allocated 1 time
+ * only (200 bytes).
+ */
+ char *recvString = nullptr;
+
+ /**
+ * @brief Actual strlen() of the recvString.
+ */
+ unsigned int recvStringLength = 0;
+
+ /**
+ * @brief Serial interface that is needed to communicate
+ * with the sensor via ASCII codes.
+ */
+ USARTInterface *serialInterface = nullptr;
+
+ /**
+ * @brief Mutex to synchronize the reading and writing of the threadSample
+ */
+ mutable miosix::FastMutex mutex;
+ VN300Data threadSample;
+
+ PrintLogger logger = Logging::getLogger("vn300");
+
+ static const unsigned int recvStringMaxDimension = 200;
+};
+} // namespace Boardcore
\ No newline at end of file
diff --git a/src/shared/sensors/VN300/VN300Data.h b/src/shared/sensors/VN300/VN300Data.h
new file mode 100644
index 000000000..3df6e3968
--- /dev/null
+++ b/src/shared/sensors/VN300/VN300Data.h
@@ -0,0 +1,105 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Lorenzo Cucchi
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <sensors/SensorData.h>
+
+namespace Boardcore
+{
+
+/**
+ * @brief Structure to handle quaternion data
+ */
+struct QuaternionData
+{
+ uint64_t quatTimestamp;
+ float quatX;
+ float quatY;
+ float quatZ;
+ float quatW;
+};
+
+/**
+ * @brief Structure to handle quaternion data
+ */
+
+struct VN300Data : public QuaternionData,
+ public MagnetometerData,
+ public AccelerometerData,
+ public GyroscopeData,
+ public TemperatureData,
+ public PressureData
+{
+
+ /**
+ * @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},
+ TemperatureData{0, 0.0}, PressureData{0, 0.0}
+ {
+ }
+
+ /**
+ * @brief Constructor with parameters
+ *
+ * @param single data structures for all the data
+ */
+ // cppcheck-suppress passedByValue
+ // cppcheck-suppress uninitDerivedMemberVar
+ VN300Data(QuaternionData quat, MagnetometerData magData,
+ AccelerometerData accData, GyroscopeData gyro,
+ TemperatureData temp, PressureData pres)
+ : QuaternionData(quat), MagnetometerData(magData),
+ AccelerometerData(accData), GyroscopeData(gyro),
+ TemperatureData(temp), PressureData(pres)
+ {
+ }
+
+ static std::string header()
+ {
+ return "quatTimestamp,quatX,quatY,quatZ,quatW,magneticFieldTimestamp,"
+ "magneticFieldX,magneticFieldY,magneticFieldZ,"
+ "accelerationTimestamp,accelerationX,accelerationY,"
+ "accelerationZ,angularSpeedTimestamp,angularSpeedX,"
+ "angularSpeedY,angularSpeedZ,temperatureTimestamp,"
+ "temperature,pressureTimestamp,pressure\n";
+ }
+ void print(std::ostream& os) const
+ {
+ os << quatTimestamp << "," << quatX << "," << quatY << "," << quatZ
+ << "," << quatW << "," << magneticFieldTimestamp << ","
+ << magneticFieldX << "," << magneticFieldY << "," << magneticFieldZ
+ << "," << accelerationTimestamp << "," << accelerationX << ","
+ << accelerationY << "," << accelerationZ << ","
+ << angularSpeedTimestamp << "," << angularSpeedX << ","
+ << angularSpeedY << "," << angularSpeedZ << ","
+ << temperatureTimestamp << "," << temperature << ","
+ << pressureTimestamp << "," << pressure << "\n";
+ }
+};
+
+} // namespace Boardcore
\ No newline at end of file
--
GitLab