diff --git a/src/shared/sensors/Vectornav/VN300/VN300.cpp b/src/shared/sensors/Vectornav/VN300/VN300.cpp
index 032184fc7e27295aef8bf87731e6dcf9e253de87..430760dbdbab738743307a816d11842ee46b41bc 100755
--- a/src/shared/sensors/Vectornav/VN300/VN300.cpp
+++ b/src/shared/sensors/Vectornav/VN300/VN300.cpp
@@ -277,8 +277,6 @@ VN300Data VN300::sampleImpl()
VN300Data VN300::sampleBinary()
{
- // TODO: rewrite completely
-
// This function is used to clear the usart buffer, it needs to be replaced
// with the function from usart class
// TODO
@@ -822,118 +820,6 @@ bool VN300::selfTestImpl()
return true;
}
-QuaternionData VN300::sampleQuaternion()
-{
- unsigned int indexStart = 0;
- char *nextNumber;
- QuaternionData data;
-
- // Look for the second ',' in the string
- // I can avoid the string control because it has already been done in
- // sampleImpl
- for (int i = 0; i < 2; i++)
- {
- while (indexStart < recvStringLength && recvString[indexStart] != ',')
- {
- indexStart++;
- }
- indexStart++;
- }
-
- // Parse the data
- data.quaternionTimestamp = TimestampTimer::getTimestamp();
- data.quaternionX = strtod(recvString.data() + indexStart + 1, &nextNumber);
- data.quaternionY = strtod(nextNumber + 1, &nextNumber);
- data.quaternionZ = strtod(nextNumber + 1, &nextNumber);
- data.quaternionW = strtod(nextNumber + 1, NULL);
-
- return data;
-}
-
-MagnetometerData VN300::sampleMagnetometer()
-{
- unsigned int indexStart = 0;
- char *nextNumber;
- MagnetometerData data;
-
- // Look for the sixth ',' in the string
- // I can avoid the string control because it has already been done in
- // sampleImpl
- for (int i = 0; i < 6; i++)
- {
- while (indexStart < recvStringLength && recvString[indexStart] != ',')
- {
- indexStart++;
- }
- indexStart++;
- }
-
- // Parse the data
- data.magneticFieldTimestamp = TimestampTimer::getTimestamp();
- data.magneticFieldX =
- strtod(recvString.data() + indexStart + 1, &nextNumber);
- data.magneticFieldY = strtod(nextNumber + 1, &nextNumber);
- data.magneticFieldZ = strtod(nextNumber + 1, NULL);
-
- return data;
-}
-
-AccelerometerData VN300::sampleAccelerometer()
-{
- unsigned int indexStart = 0;
- char *nextNumber;
- AccelerometerData data;
-
- // Look for the ninth ',' in the string
- // I can avoid the string control because it has already been done in
- // sampleImpl
- for (int i = 0; i < 9; i++)
- {
- while (indexStart < recvStringLength && recvString[indexStart] != ',')
- {
- indexStart++;
- }
- indexStart++;
- }
-
- // Parse the data
- data.accelerationTimestamp = TimestampTimer::getTimestamp();
- data.accelerationX =
- strtod(recvString.data() + indexStart + 1, &nextNumber);
- data.accelerationY = strtod(nextNumber + 1, &nextNumber);
- data.accelerationZ = strtod(nextNumber + 1, NULL);
-
- return data;
-}
-
-GyroscopeData VN300::sampleGyroscope()
-{
- unsigned int indexStart = 0;
- char *nextNumber;
- GyroscopeData data;
-
- // Look for the twelfth ',' in the string
- // I can avoid the string control because it has already been done in
- // sampleImpl
- for (int i = 0; i < 12; i++)
- {
- while (indexStart < recvStringLength && recvString[indexStart] != ',')
- {
- indexStart++;
- }
- indexStart++;
- }
-
- // Parse the data
- data.angularSpeedTimestamp = TimestampTimer::getTimestamp();
- data.angularSpeedX =
- strtod(recvString.data() + indexStart + 1, &nextNumber);
- data.angularSpeedY = strtod(nextNumber + 1, &nextNumber);
- data.angularSpeedZ = strtod(nextNumber + 1, NULL);
-
- return data;
-}
-
VN300Defs::Ins_Lla VN300::sampleIns()
{
unsigned int indexStart = 0;
diff --git a/src/shared/sensors/Vectornav/VN300/VN300.h b/src/shared/sensors/Vectornav/VN300/VN300.h
index 3d5abd288f3e1d7fe72442ba47f22a1fa2246f87..9e88c7072ae65785bab996e3aa8cb129c86b4d34 100755
--- a/src/shared/sensors/Vectornav/VN300/VN300.h
+++ b/src/shared/sensors/Vectornav/VN300/VN300.h
@@ -210,14 +210,6 @@ private:
*/
bool selfTestImpl();
- QuaternionData sampleQuaternion();
-
- MagnetometerData sampleMagnetometer();
-
- AccelerometerData sampleAccelerometer();
-
- GyroscopeData sampleGyroscope();
-
VN300Defs::Ins_Lla sampleIns();
VN300Data sampleBinary();
@@ -311,20 +303,5 @@ private:
* @brief Binary output polling command
*/
string preSampleBin1 = "";
-
- /**
- * @brief Maximum size of the receiving string.
- */
- static const uint8_t recvStringMaxDimension = 200;
-
- /**
- * @brief Buffer used to store the string received from the sensor.
- */
- std::array<char, recvStringMaxDimension> recvString;
-
- /**
- * @brief Actual strlen() of the recvString.
- */
- uint8_t recvStringLength = 0;
};
} // namespace Boardcore
diff --git a/src/shared/sensors/Vectornav/VNCommonSerial.cpp b/src/shared/sensors/Vectornav/VNCommonSerial.cpp
index 17ca4dcdc372fc483c3888b2d431fce1b274b399..21fb61f5cdb2c43dc60741554089d49f277c73e2 100644
--- a/src/shared/sensors/Vectornav/VNCommonSerial.cpp
+++ b/src/shared/sensors/Vectornav/VNCommonSerial.cpp
@@ -22,6 +22,7 @@
#include "VNCommonSerial.h"
+#include <drivers/timer/TimestampTimer.h>
#include <utils/Debug.h>
namespace Boardcore
@@ -141,4 +142,116 @@ bool VNCommonSerial::asyncPause()
return true;
}
+QuaternionData VNCommonSerial::sampleQuaternion()
+{
+ unsigned int indexStart = 0;
+ char *nextNumber;
+ QuaternionData data;
+
+ // Look for the second ',' in the string
+ // I can avoid the string control because it has already been done in
+ // sampleImpl
+ for (int i = 0; i < 2; i++)
+ {
+ while (indexStart < recvStringLength && recvString[indexStart] != ',')
+ {
+ indexStart++;
+ }
+ indexStart++;
+ }
+
+ // Parse the data
+ data.quaternionTimestamp = TimestampTimer::getTimestamp();
+ data.quaternionX = strtod(recvString.data() + indexStart + 1, &nextNumber);
+ data.quaternionY = strtod(nextNumber + 1, &nextNumber);
+ data.quaternionZ = strtod(nextNumber + 1, &nextNumber);
+ data.quaternionW = strtod(nextNumber + 1, NULL);
+
+ return data;
+}
+
+MagnetometerData VNCommonSerial::sampleMagnetometer()
+{
+ unsigned int indexStart = 0;
+ char *nextNumber;
+ MagnetometerData data;
+
+ // Look for the sixth ',' in the string
+ // I can avoid the string control because it has already been done in
+ // sampleImpl
+ for (int i = 0; i < 6; i++)
+ {
+ while (indexStart < recvStringLength && recvString[indexStart] != ',')
+ {
+ indexStart++;
+ }
+ indexStart++;
+ }
+
+ // Parse the data
+ data.magneticFieldTimestamp = TimestampTimer::getTimestamp();
+ data.magneticFieldX =
+ strtod(recvString.data() + indexStart + 1, &nextNumber);
+ data.magneticFieldY = strtod(nextNumber + 1, &nextNumber);
+ data.magneticFieldZ = strtod(nextNumber + 1, NULL);
+
+ return data;
+}
+
+AccelerometerData VNCommonSerial::sampleAccelerometer()
+{
+ unsigned int indexStart = 0;
+ char *nextNumber;
+ AccelerometerData data;
+
+ // Look for the ninth ',' in the string
+ // I can avoid the string control because it has already been done in
+ // sampleImpl
+ for (int i = 0; i < 9; i++)
+ {
+ while (indexStart < recvStringLength && recvString[indexStart] != ',')
+ {
+ indexStart++;
+ }
+ indexStart++;
+ }
+
+ // Parse the data
+ data.accelerationTimestamp = TimestampTimer::getTimestamp();
+ data.accelerationX =
+ strtod(recvString.data() + indexStart + 1, &nextNumber);
+ data.accelerationY = strtod(nextNumber + 1, &nextNumber);
+ data.accelerationZ = strtod(nextNumber + 1, NULL);
+
+ return data;
+}
+
+GyroscopeData VNCommonSerial::sampleGyroscope()
+{
+ unsigned int indexStart = 0;
+ char *nextNumber;
+ GyroscopeData data;
+
+ // Look for the twelfth ',' in the string
+ // I can avoid the string control because it has already been done in
+ // sampleImpl
+ for (int i = 0; i < 12; i++)
+ {
+ while (indexStart < recvStringLength && recvString[indexStart] != ',')
+ {
+ indexStart++;
+ }
+ indexStart++;
+ }
+
+ // Parse the data
+ data.angularSpeedTimestamp = TimestampTimer::getTimestamp();
+ data.angularSpeedX =
+ strtod(recvString.data() + indexStart + 1, &nextNumber);
+ data.angularSpeedY = strtod(nextNumber + 1, &nextNumber);
+ data.angularSpeedZ = strtod(nextNumber + 1, NULL);
+
+ return data;
+}
+
} // namespace Boardcore
diff --git a/src/shared/sensors/Vectornav/VNCommonSerial.h b/src/shared/sensors/Vectornav/VNCommonSerial.h
index db983fa077b106a3b478d1b42fec386dc6666c8d..1f2e4c7f0cf8fc257dbdf0f7d7633f1f7bce7b47 100644
--- a/src/shared/sensors/Vectornav/VNCommonSerial.h
+++ b/src/shared/sensors/Vectornav/VNCommonSerial.h
@@ -24,6 +24,7 @@
#include <diagnostic/PrintLogger.h>
#include <drivers/usart/USART.h>
+#include <sensors/SensorData.h>
#include <string>
@@ -98,6 +99,30 @@ protected:
*/
bool asyncPause();
+ /**
+ * @brief Utility function used to extract quaternion data from the
+ * receiving string.
+ */
+ QuaternionData sampleQuaternion();
+
+ /**
+ * @brief Utility function used to extract magnetometer data from the
+ * receiving string.
+ */
+ MagnetometerData sampleMagnetometer();
+
+ /**
+ * @brief Utility function used to extract accelerometer data from the
+ * receiving string.
+ */
+ AccelerometerData sampleAccelerometer();
+
+ /**
+ * @brief Utility function used to extract gyroscope data from the receiving
+ * string.
+ */
+ GyroscopeData sampleGyroscope();
+
/**
* @brief Serial interface that is needed to communicate
* with the sensor via ASCII codes.
@@ -108,6 +133,21 @@ protected:
CRCOptions crc;
PrintLogger logger;
+
+ /**
+ * @brief Maximum size of the receiving string.
+ */
+ static const uint8_t recvStringMaxDimension = 200;
+
+ /**
+ * @brief Buffer used to store the string received from the sensor.
+ */
+ std::array<char, recvStringMaxDimension> recvString;
+
+ /**
+ * @brief Actual strlen() of the recvString.
+ */
+ uint8_t recvStringLength = 0;
};
} // namespace Boardcore