diff --git a/src/shared/sensors/Vectornav/VN300/VN300.cpp b/src/shared/sensors/Vectornav/VN300/VN300.cpp
index f437802832948672181ede378a756c94114b7649..032184fc7e27295aef8bf87731e6dcf9e253de87 100755
--- a/src/shared/sensors/Vectornav/VN300/VN300.cpp
+++ b/src/shared/sensors/Vectornav/VN300/VN300.cpp
@@ -34,9 +34,9 @@ VN300::VN300(USART &usart, int userBaudRate,
const VN300Defs::AntennaPosition antPosA,
const VN300Defs::AntennaPosition antPosB,
const Eigen::Matrix3f rotMat)
- : VNCommonSerial(usart, userBaudRate, "VN300"),
- samplingMethod(samplingMethod), crc(crc), antPosA(antPosA),
- antPosB(antPosB), rotMat(rotMat)
+ : VNCommonSerial(usart, userBaudRate, "VN300", crc),
+ samplingMethod(samplingMethod), antPosA(antPosA), antPosB(antPosB),
+ rotMat(rotMat)
{
}
@@ -397,12 +397,6 @@ VN300Data VN300::sampleASCII()
return VN300Data(quat, mag, acc, gyro, ins);
}
-bool VN300::asyncPause()
-{
- usart.writeString("$VNASY,0*XX\n");
- return true;
-}
-
bool VN300::disableAsyncMessages(bool waitResponse)
{
// Command string
@@ -1183,62 +1177,4 @@ uint8_t VN300::checkErrorVN(const char *message)
return 0; // No error detected
}
-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;
-}
-
} // namespace Boardcore
diff --git a/src/shared/sensors/Vectornav/VN300/VN300.h b/src/shared/sensors/Vectornav/VN300/VN300.h
index 7dc7020396829b05d82cc28ae6d57091c97d1f31..3d5abd288f3e1d7fe72442ba47f22a1fa2246f87 100755
--- a/src/shared/sensors/Vectornav/VN300/VN300.h
+++ b/src/shared/sensors/Vectornav/VN300/VN300.h
@@ -74,14 +74,6 @@ namespace Boardcore
class VN300 : public Sensor<VN300Data>, public VNCommonSerial
{
public:
- enum class CRCOptions : uint8_t
- {
- CRC_ENABLE_8 = 0x08,
- CRC_ENABLE_16 = 0x10
- // TODO: add CRC_NO (handled by adding XX at the end of a command inside
- // sendStringCommand())
- };
-
/**
* @brief Constructor.
*
@@ -127,14 +119,6 @@ private:
bool findBaudrate();
// TODO: should we keep it? maybe in common files?
- /**
- * @brief pause asynchronous messages
- *
- * @return True if operation succeeded.
- */
- bool asyncPause();
- // TODO: should we keep it? not needed with vn100... with vn300?
-
/**
* @brief Disables the async messages that the VN300 is default configured
* to send at 40Hz on startup.
@@ -300,24 +284,12 @@ private:
// 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 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);
- // TODO: put in common files
-
/**
* @brief Default baudrate value for the usart communication.
*/
static const int defaultBaudRate = 115200;
VN300Defs::SamplingMethod samplingMethod;
- CRCOptions crc;
bool isInit = false;
VN300Defs::AntennaPosition antPosA;
diff --git a/src/shared/sensors/Vectornav/VNCommonSerial.cpp b/src/shared/sensors/Vectornav/VNCommonSerial.cpp
index 528e5e2ceee1c191ada260fedd4d4856dc90df3e..17ca4dcdc372fc483c3888b2d431fce1b274b399 100644
--- a/src/shared/sensors/Vectornav/VNCommonSerial.cpp
+++ b/src/shared/sensors/Vectornav/VNCommonSerial.cpp
@@ -28,8 +28,8 @@ namespace Boardcore
{
VNCommonSerial::VNCommonSerial(USART &usart, int baudrate,
- const std::string &sensorName)
- : sensorName(sensorName), usart(usart), baudRate(baudrate),
+ const std::string &sensorName, CRCOptions crc)
+ : sensorName(sensorName), usart(usart), baudRate(baudrate), crc(crc),
logger(Logging::getLogger(sensorName))
{
}
@@ -69,6 +69,64 @@ uint16_t VNCommonSerial::calculateChecksum16(const uint8_t *message, int length)
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;
@@ -77,4 +135,10 @@ void VNCommonSerial::clearBuffer()
}
}
+bool VNCommonSerial::asyncPause()
+{
+ usart.writeString("$VNASY,0*XX\n");
+ return true;
+}
+
} // namespace Boardcore
diff --git a/src/shared/sensors/Vectornav/VNCommonSerial.h b/src/shared/sensors/Vectornav/VNCommonSerial.h
index 77278bae446af3a269c33cb30910d6afa06437fa..db983fa077b106a3b478d1b42fec386dc6666c8d 100644
--- a/src/shared/sensors/Vectornav/VNCommonSerial.h
+++ b/src/shared/sensors/Vectornav/VNCommonSerial.h
@@ -33,7 +33,15 @@ namespace Boardcore
class VNCommonSerial
{
public:
- VNCommonSerial(USART &usart, int baudrate, const std::string &sensorName);
+ 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();
@@ -64,6 +72,16 @@ protected:
*/
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.
*
@@ -73,6 +91,13 @@ protected:
void clearBuffer();
// TODO: remove and use the one in the usart driver
+ /**
+ * @brief Pause asynchronous messages
+ *
+ * @return True if operation succeeded.
+ */
+ bool asyncPause();
+
/**
* @brief Serial interface that is needed to communicate
* with the sensor via ASCII codes.
@@ -80,6 +105,8 @@ protected:
USART &usart;
int baudRate;
+ CRCOptions crc;
+
PrintLogger logger;
};