diff --git a/src/shared/sensors/Vectornav/VN300/VN300.cpp b/src/shared/sensors/Vectornav/VN300/VN300.cpp
index 430760dbdbab738743307a816d11842ee46b41bc..60a0499cba59f81f7ca785477008e84b68ff94b9 100755
--- a/src/shared/sensors/Vectornav/VN300/VN300.cpp
+++ b/src/shared/sensors/Vectornav/VN300/VN300.cpp
@@ -499,29 +499,6 @@ bool VN300::configBaudRate(int baudRate)
return true;
}
-[[maybe_unused]] bool VN300::resetFactorySettings()
-{
- // Command string
- std::string command =
- "VNRFS"; // Put 0 in register number 0 (Factory Settings)
-
- // Send the command
- if (!sendStringCommand(command))
- {
- return false;
- }
-
- if (!recvStringCommand(recvString.data(), recvStringMaxDimension))
- {
- LOG_WARN(logger, "Unable to sample due to serial communication error");
- return false;
- }
-
- miosix::Thread::sleep(500);
-
- return true;
-}
-
bool VN300::setCrc(bool waitResponse)
{
// TODO: refactor this function
@@ -962,105 +939,4 @@ bool VN300::recvStringCommand(char *command, int maxLength)
return false;
}
-[[maybe_unused]] int VN300::recvBinaryCommand(uint8_t *command)
-{
- uint8_t initByte;
- int i = 0;
- int j = 1;
- bool read = false;
- memset(command, '-', sizeof(uint8_t));
-
- miosix::Thread::sleep(2);
- while (read == false && i < 10000)
- {
- if (usart.read(&initByte, 1))
- {
- command[0] = initByte;
-
- while (usart.read(&initByte, 1) && j < 200)
- {
- command[j] = initByte;
- j++;
- read = true;
- }
-
- break;
- }
-
- i++;
- }
-
- if (read)
- {
- return --j;
- }
- else
- {
- return 0;
- }
-}
-
-uint8_t VN300::checkErrorVN(const char *message)
-{
- if (strncmp(message, "$VNERR,", 7) == 0)
- {
- // Extract the error code
- int errorCode = atoi(&message[7]);
- string error;
- // Handle the error based on the error code
-
- // TODO: the error string is set but never used
-
- switch (errorCode)
- {
- case 1:
- error = "VN300 Hard Fault";
- break;
- case 2:
- error = "VN300 Serial Buffer Overflow";
- break;
- case 3:
- error = "VN300 Invalid Checksum";
- break;
- case 4:
- error = "VN300 Invalid Command";
- break;
- case 5:
- error = "VN300 Not Enough Parameters";
- break;
- case 6:
- error = "VN300 Too Many Parameters";
- break;
- case 7:
- error = "VN300 Invalid Parameter";
- break;
- case 8:
- error = "VN300 Invalid Register";
- break;
- case 9:
- error = "VN300 Unauthorized Access";
- break;
- case 10:
- error = "VN300 Watchdog Reset";
- break;
- case 11:
- error = "VN300 Output Buffer Overflow";
- break;
- case 12:
- error = "VN300 Insufficient Baud Rate";
- break;
- case 255:
- error = "VN300 Error Buffer Overflow";
- break;
- default:
- error = "VN300 Unknown error";
- break;
- }
-
- return errorCode; // Error detected
- }
-
- return 0; // No error detected
-}
-
} // namespace Boardcore
diff --git a/src/shared/sensors/Vectornav/VN300/VN300.h b/src/shared/sensors/Vectornav/VN300/VN300.h
index 9e88c7072ae65785bab996e3aa8cb129c86b4d34..c82675f75cdc16c53bf6725972412adcd1c00ae2 100755
--- a/src/shared/sensors/Vectornav/VN300/VN300.h
+++ b/src/shared/sensors/Vectornav/VN300/VN300.h
@@ -138,14 +138,6 @@ private:
bool configBaudRate(int baud);
// TODO: move to common files
- /**
- * @brief reset to factory settings.
- *
- * @return True if operation succeeded.
- */
- bool resetFactorySettings();
- // TODO: [[maybe_unused]] ... remove
-
/**
* @brief Sets the user selected crc method.
*
@@ -252,30 +244,6 @@ private:
bool sampleBin(VN300Defs::BinaryData &bindata);
// TODO: can be removed and placed inside sampleBinary()
- /**
- * @brief Receives binary data and it parses it in a generic buffer
- *
- * @param command which will be filled with the message
- *
- * @return The length of the message in bytes
- */
- int recvBinaryCommand(uint8_t *command);
- // TODO: [[maybe_unused]] ... remove
-
- /**
- * @brief check if the VN-300 returned an error and differentiate between
- * them. The error is formatted as $VNERR,xx*XX
- *
- * xx can go from 01 to 12 and 255 and XX is the checksum.
- *
- * @param message to be checked.
- *
- * @return error code.
- */
- uint8_t checkErrorVN(const char *message);
- // TODO: used only 1 time to infer the type of error, all other times is
- // "check if ==VNERR" ... can be replaced with a function in common files?
-
/**
* @brief Default baudrate value for the usart communication.
*/
diff --git a/src/shared/sensors/Vectornav/VNCommonSerial.cpp b/src/shared/sensors/Vectornav/VNCommonSerial.cpp
index 21fb61f5cdb2c43dc60741554089d49f277c73e2..3f0b9d3e7fdb58ed3260c5810a3dc21db174fb8f 100644
--- a/src/shared/sensors/Vectornav/VNCommonSerial.cpp
+++ b/src/shared/sensors/Vectornav/VNCommonSerial.cpp
@@ -254,4 +254,16 @@ GyroscopeData VNCommonSerial::sampleGyroscope()
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
+}
+
} // namespace Boardcore
diff --git a/src/shared/sensors/Vectornav/VNCommonSerial.h b/src/shared/sensors/Vectornav/VNCommonSerial.h
index 1f2e4c7f0cf8fc257dbdf0f7d7633f1f7bce7b47..550289d35e5a2d1f4fde57fe7753d00315d5e9fd 100644
--- a/src/shared/sensors/Vectornav/VNCommonSerial.h
+++ b/src/shared/sensors/Vectornav/VNCommonSerial.h
@@ -123,6 +123,16 @@ protected:
*/
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 Serial interface that is needed to communicate
* with the sensor via ASCII codes.