From 00604b52d570fefe8cf7f2b0e0174cb293597e60 Mon Sep 17 00:00:00 2001
From: Fabrizio Monti <fabrizio.monti@skywarder.eu>
Date: Tue, 5 Mar 2024 16:03:04 +0100
Subject: [PATCH] [VN300] Moved sendStringCommand and recvStringCommand inside
VNCommonSerial.
---
src/shared/sensors/Vectornav/VN300/VN300.cpp | 79 +------------------
src/shared/sensors/Vectornav/VN300/VN300.h | 24 ------
.../sensors/Vectornav/VNCommonSerial.cpp | 66 ++++++++++++++++
src/shared/sensors/Vectornav/VNCommonSerial.h | 22 ++++++
src/tests/sensors/test-vn300.cpp | 4 +-
5 files changed, 90 insertions(+), 105 deletions(-)
diff --git a/src/shared/sensors/Vectornav/VN300/VN300.cpp b/src/shared/sensors/Vectornav/VN300/VN300.cpp
index 40ca7b2bb..3782a4871 100755
--- a/src/shared/sensors/Vectornav/VN300/VN300.cpp
+++ b/src/shared/sensors/Vectornav/VN300/VN300.cpp
@@ -723,6 +723,7 @@ VN300Defs::Ins_Lla VN300::sampleIns()
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
@@ -737,84 +738,6 @@ bool VN300::sampleBin(VN300Defs::BinaryData &bindata)
return false;
}
-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
- usart.writeString(command.c_str());
-
- miosix::Thread::sleep(500);
-
- return true;
-}
-
-bool VN300::recvStringCommand(char *command, int maxLength)
-{
- command[0] = '\0';
-
- // This sleep of 2 ms is used to wait for the reply of the VN300 taking into
- // account standard reply times, this free the thread waiting the message
- miosix::Thread::sleep(2);
-
- // This variable is used as an initial time reference for the while loop
- uint64_t initTime = TimestampTimer::getTimestamp();
-
- // The time condition is used to take into account time variation on the
- // reply of the vn300, this takes into account the start of the reply
- while (TimestampTimer::getTimestamp() - initTime <= 3)
- {
- char initChar;
- // Read the first char
- // TODO try to remove the if statement and test it with only the while
- // loop
- if (usart.read(&initChar, 1) && initChar == '$')
- {
- command[0] = '$';
- int j = 1;
-
- while (usart.read(&initChar, 1) && initChar != '\n' &&
- j < maxLength)
- {
- command[j] = initChar;
- j++;
- }
- command[j] = '\0';
- recvStringLength = j - 1;
- return true;
- }
- }
-
- return false;
-}
-
void VN300::configDefaultSerialPort()
{
// Initial default settings
diff --git a/src/shared/sensors/Vectornav/VN300/VN300.h b/src/shared/sensors/Vectornav/VN300/VN300.h
index ea9fb1b87..1cdb7d11b 100755
--- a/src/shared/sensors/Vectornav/VN300/VN300.h
+++ b/src/shared/sensors/Vectornav/VN300/VN300.h
@@ -214,30 +214,6 @@ private:
VN300Data sampleASCII();
- /**
- * @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);
- // TODO: put in common files
-
- /**
- * @brief Receives a command from the VN300 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);
- // TODO: put in common files
-
/**
* @brief Receives binary data and parse directly into BinaryData struct
* which has the __attribute__(packed)
diff --git a/src/shared/sensors/Vectornav/VNCommonSerial.cpp b/src/shared/sensors/Vectornav/VNCommonSerial.cpp
index 3f0b9d3e7..b2f332083 100644
--- a/src/shared/sensors/Vectornav/VNCommonSerial.cpp
+++ b/src/shared/sensors/Vectornav/VNCommonSerial.cpp
@@ -266,4 +266,70 @@ uint8_t VNCommonSerial::checkErrorVN(const char *message)
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
diff --git a/src/shared/sensors/Vectornav/VNCommonSerial.h b/src/shared/sensors/Vectornav/VNCommonSerial.h
index 550289d35..ff479e15f 100644
--- a/src/shared/sensors/Vectornav/VNCommonSerial.h
+++ b/src/shared/sensors/Vectornav/VNCommonSerial.h
@@ -133,6 +133,28 @@ protected:
*/
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.
diff --git a/src/tests/sensors/test-vn300.cpp b/src/tests/sensors/test-vn300.cpp
index 65dc9a6c1..44cf99da6 100644
--- a/src/tests/sensors/test-vn300.cpp
+++ b/src/tests/sensors/test-vn300.cpp
@@ -80,9 +80,7 @@ int main()
sample.angularSpeedZ);
printf("magn: %f, %f, %f\n", sample.magneticFieldX,
sample.magneticFieldY, sample.magneticFieldZ);
- printf("latitude: %f\n", sample.latitude);
- printf("longitude: %f\n", sample.longitude);
- printf("altitude: %f\n", sample.altitude);
+ printf("attitude: %f, %f, %f\n", sample.yaw, sample.pitch, sample.roll);
Thread::sleep(500);
}
--
GitLab