From ef200ce2628bd9acef48099cf86aba6e6470ddf7 Mon Sep 17 00:00:00 2001
From: Fabrizio Monti <fabrizio.monti@skywarder.eu>
Date: Tue, 27 Feb 2024 14:23:08 +0100
Subject: [PATCH] [VN300] Begun implementation of VNCommon class for VN300.
---
cmake/boardcore.cmake | 1 +
src/shared/sensors/Vectornav/VN300/VN300.cpp | 50 ++---------
src/shared/sensors/Vectornav/VN300/VN300.h | 41 +--------
.../sensors/Vectornav/VNCommonSerial.cpp | 80 +++++++++++++++++
src/shared/sensors/Vectornav/VNCommonSerial.h | 86 +++++++++++++++++++
5 files changed, 176 insertions(+), 82 deletions(-)
create mode 100644 src/shared/sensors/Vectornav/VNCommonSerial.cpp
create mode 100644 src/shared/sensors/Vectornav/VNCommonSerial.h
diff --git a/cmake/boardcore.cmake b/cmake/boardcore.cmake
index c5586af5f..2446c80d2 100644
--- a/cmake/boardcore.cmake
+++ b/cmake/boardcore.cmake
@@ -108,6 +108,7 @@ 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/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
diff --git a/src/shared/sensors/Vectornav/VN300/VN300.cpp b/src/shared/sensors/Vectornav/VN300/VN300.cpp
index 500ae4c3c..f43780283 100755
--- a/src/shared/sensors/Vectornav/VN300/VN300.cpp
+++ b/src/shared/sensors/Vectornav/VN300/VN300.cpp
@@ -34,8 +34,9 @@ VN300::VN300(USART &usart, int userBaudRate,
const VN300Defs::AntennaPosition antPosA,
const VN300Defs::AntennaPosition antPosB,
const Eigen::Matrix3f rotMat)
- : usart(usart), userBaudRate(userBaudRate), samplingMethod(samplingMethod),
- crc(crc), antPosA(antPosA), antPosB(antPosB), rotMat(rotMat)
+ : VNCommonSerial(usart, userBaudRate, "VN300"),
+ samplingMethod(samplingMethod), crc(crc), antPosA(antPosA),
+ antPosB(antPosB), rotMat(rotMat)
{
}
@@ -136,7 +137,7 @@ bool VN300::init()
}
miosix::Thread::sleep(2000); // TODO: needed? for so long?
- if (!configBaudRate(userBaudRate))
+ if (!configBaudRate(baudRate))
{
LOG_ERR(logger, "Unable to config the user VN300 serial port");
return false;
@@ -531,6 +532,8 @@ bool VN300::configBaudRate(int baudRate)
bool VN300::setCrc(bool waitResponse)
{
+ // TODO: refactor this function
+
// Command for the crc change
std::string command;
CRCOptions backup = crc;
@@ -1238,45 +1241,4 @@ bool VN300::verifyChecksum(char *command, int length)
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;
-}
-
-void VN300::clearBuffer()
-{
- char c;
- while (usart.read(&c, 1))
- {
- }
-}
-
} // namespace Boardcore
diff --git a/src/shared/sensors/Vectornav/VN300/VN300.h b/src/shared/sensors/Vectornav/VN300/VN300.h
index 2ed526e1c..7dc702039 100755
--- a/src/shared/sensors/Vectornav/VN300/VN300.h
+++ b/src/shared/sensors/Vectornav/VN300/VN300.h
@@ -55,6 +55,7 @@
#include <diagnostic/PrintLogger.h>
#include <fmt/format.h>
#include <sensors/Sensor.h>
+#include <sensors/Vectornav/VNCommonSerial.h>
#include <string.h>
#include <utils/Debug.h>
@@ -70,7 +71,7 @@ namespace Boardcore
/**
* @brief Driver class for VN300 IMU.
*/
-class VN300 : public Sensor<VN300Data>
+class VN300 : public Sensor<VN300Data>, public VNCommonSerial
{
public:
enum class CRCOptions : uint8_t
@@ -311,42 +312,8 @@ private:
// TODO: put in common files
/**
- * @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.
+ * @brief Default baudrate value for the usart communication.
*/
- uint8_t calculateChecksum8(uint8_t *message, int length);
- // TODO: put in common files
-
- /**
- * @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);
- // TODO: put in common files
-
- /**
- * @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 usart
-
- /**
- * @brief Serial interface that is needed to communicate
- * with the sensor via ASCII codes.
- */
- USART &usart;
- int userBaudRate;
static const int defaultBaudRate = 115200;
VN300Defs::SamplingMethod samplingMethod;
@@ -387,7 +354,5 @@ private:
* @brief Actual strlen() of the recvString.
*/
uint8_t recvStringLength = 0;
-
- PrintLogger logger = Logging::getLogger("VN300");
};
} // namespace Boardcore
diff --git a/src/shared/sensors/Vectornav/VNCommonSerial.cpp b/src/shared/sensors/Vectornav/VNCommonSerial.cpp
new file mode 100644
index 000000000..528e5e2ce
--- /dev/null
+++ b/src/shared/sensors/Vectornav/VNCommonSerial.cpp
@@ -0,0 +1,80 @@
+/* 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 <utils/Debug.h>
+
+namespace Boardcore
+{
+
+VNCommonSerial::VNCommonSerial(USART &usart, int baudrate,
+ const std::string &sensorName)
+ : sensorName(sensorName), usart(usart), baudRate(baudrate),
+ 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;
+}
+
+void VNCommonSerial::clearBuffer()
+{
+ char c;
+ while (usart.read(&c, 1))
+ {
+ }
+}
+
+} // namespace Boardcore
diff --git a/src/shared/sensors/Vectornav/VNCommonSerial.h b/src/shared/sensors/Vectornav/VNCommonSerial.h
new file mode 100644
index 000000000..77278bae4
--- /dev/null
+++ b/src/shared/sensors/Vectornav/VNCommonSerial.h
@@ -0,0 +1,86 @@
+/* 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 <string>
+
+namespace Boardcore
+{
+
+class VNCommonSerial
+{
+public:
+ VNCommonSerial(USART &usart, int baudrate, const std::string &sensorName);
+
+ ~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 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 Serial interface that is needed to communicate
+ * with the sensor via ASCII codes.
+ */
+ USART &usart;
+ int baudRate;
+
+ PrintLogger logger;
+};
+
+} // namespace Boardcore
--
GitLab