diff --git a/src/shared/sensors/VN300/VN300.cpp b/src/shared/sensors/VN300/VN300.cpp
index b92d3e621f784b283369c8f791d06801948f76f0..db66364670e46c1cd1cd2b22a3ff9698cdc09129 100644
--- a/src/shared/sensors/VN300/VN300.cpp
+++ b/src/shared/sensors/VN300/VN300.cpp
@@ -28,8 +28,8 @@ namespace Boardcore
{
VN300::VN300(USART &usart, int baudRate, CRCOptions crc, uint16_t samplePeriod,
- AntennaPosition antPosA, AntennaPosition antPosB,
- Eigen::Matrix3f rotMat)
+ const AntennaPosition antPosA, const AntennaPosition antPosB,
+ const Eigen::Matrix3f rotMat)
: usart(usart), baudRate(baudRate), samplePeriod(samplePeriod), crc(crc),
antPosA(antPosA), antPosB(antPosB), rotMat(rotMat)
{
@@ -84,17 +84,11 @@ bool VN300::init()
return false;
}
- // if (!configDefaultSerialPort())
- //{
- // LOG_ERR(logger, "Unable to config the default VN300 serial port");
- // return false;
- // }
-
- // if (!resetFactorySettings())
- //{
- // LOG_ERR(logger, "Unable to reset the VN300 to factory settings");
- // return false;
- // }
+ if (!configDefaultSerialPort())
+ {
+ LOG_ERR(logger, "Unable to config the default VN300 serial port");
+ return false;
+ }
if (!setCrc(false))
{
@@ -161,63 +155,6 @@ bool VN300::init()
return true;
}
-void VN300::run()
-{
- while (!shouldStop())
- {
- long long initialTime = miosix::getTick();
-
- VN300Data data = sampleData();
- {
- // Sample the data locking the mutex
- miosix::Lock<FastMutex> l(mutex);
- threadSample = data;
- }
- // Sleep for the sampling period
- miosix::Thread::sleepUntil(initialTime + samplePeriod);
- printf("Sample time: %lld\n", miosix::getTick() - initialTime);
- }
-}
-
-bool VN300::sampleRaw()
-{
- // Sensor not init
- if (!isInit)
- {
- lastError = SensorErrors::NOT_INIT;
- LOG_WARN(logger,
- "Unable to sample due to not initialized VN300 sensor");
- return false;
- }
-
- // Send the IMU sampling command
- usart.writeString(preSampleImuString->c_str());
-
- // Wait some time
- // TODO dimension the time
- miosix::Thread::sleep(1);
-
- // Receive the string
- if (!recvStringCommand(recvString, recvStringMaxDimension))
- {
- LOG_WARN(logger, "Unable to sample due to serial communication error");
- return false;
- }
-
- return true;
-}
-
-string VN300::getLastRawSample()
-{
- // If not init i return the void string
- if (!isInit)
- {
- return string("");
- }
-
- return string(recvString, recvStringLength);
-}
-
bool VN300::closeAndReset()
{
// Sensor not init
@@ -250,9 +187,6 @@ bool VN300::writeSettingsCommand()
LOG_WARN(logger, "Impossible to save settings");
}
- // Write settings command takes approximately 500ms
- miosix::Thread::sleep(500);
-
// Send the reset command to the VN300 in order to restart the Kalman filter
if (!sendStringCommand("VNRST"))
{
@@ -261,8 +195,6 @@ bool VN300::writeSettingsCommand()
return false;
}
- miosix::Thread::sleep(500);
-
return true;
}
@@ -279,12 +211,6 @@ bool VN300::selfTest()
}
VN300Data VN300::sampleImpl()
-{
- miosix::Lock<FastMutex> l(mutex);
- return threadSample;
-}
-
-VN300Data VN300::sampleData()
{
if (!isInit)
{
@@ -350,11 +276,8 @@ VN300Data VN300::sampleData()
bool VN300::asyncPause()
{
- std::string command = "$VNASY,0*XX\n\0";
- usart.writeString(command.c_str());
-
- miosix::Thread::sleep(1000);
+ usart.writeString("$VNASY,0*XX\n");
return true;
}
@@ -385,33 +308,58 @@ bool VN300::disableAsyncMessages(bool waitResponse)
bool VN300::configDefaultSerialPort()
{
- // Initial default settings
+ // I can send the command
+ if (!sendStringCommand("VNWRG,5,115200"))
+ {
+ return false;
+ }
+
+ if (!recvStringCommand(recvString, recvStringMaxDimension))
+ {
+ LOG_WARN(logger, "Unable to sample due to serial communication error");
+ return false;
+ }
+
+ if (checkErrorVN(recvString))
+ {
+ LOG_WARN(logger, "Unable to change serial port baudrate");
+ return false;
+ }
+
+ // I can open the serial with user's baud rate
usart.setBaudrate(115200);
- // Check correct serial init
return true;
}
bool VN300::findBaudrate()
{
+ char modelNumber[] = "VN-300T-CR";
+ const int modelNumberOffset = 10;
+
for (uint32_t i = 0; i < BaudrateList.size(); i++)
{
- // I set the baudrate
usart.setBaudrate(BaudrateList[i]);
- printf("Baudrate: %d\n", BaudrateList[i]);
- char initChar;
- uint64_t in_time = miosix::getTick();
- while (miosix::getTick() - in_time < 30)
+
+ // I pause the async messages, we don't know if they are present.
+ asyncPause();
+
+ // removing junk
+ usart.clearQueue();
+
+ // I check the model number
+ if (!sendStringCommand("VNRRG,01"))
+ {
+ LOG_WARN(logger, "Unable to send string command");
+ return false;
+ }
+
+ if (recvStringCommand(recvString, recvStringMaxDimension))
{
- usart.writeString("$VNRRG,01*XX\n");
- // Read the first char
- if (usart.read(&initChar, 1))
+ if (strncmp(modelNumber, recvString + modelNumberOffset,
+ strlen(modelNumber)) != 0)
{
- // If it is a '$' i break
- if (initChar == '$')
- {
- return true;
- }
+ return true;
}
}
}
@@ -426,6 +374,7 @@ bool VN300::findBaudrate()
*/
bool VN300::configUserSerialPort()
{
+
std::string command;
// I format the command to change baud rate
@@ -448,11 +397,9 @@ bool VN300::configUserSerialPort()
LOG_WARN(logger, "Unable to change serial port baudrate");
return false;
}
- else
- {
- // I can open the serial with user's baud rate
- usart.setBaudrate(baudRate);
- }
+
+ // I can open the serial with user's baud rate
+ usart.setBaudrate(baudRate);
return true;
}
@@ -469,11 +416,13 @@ bool VN300::resetFactorySettings()
return false;
}
- // if (!recvStringCommand(recvString, recvStringMaxDimension))
- //{
- // LOG_WARN(logger, "Unable to sample due to serial communication
- // error"); return false;
- // }
+ if (!recvStringCommand(recvString, recvStringMaxDimension))
+ {
+ LOG_WARN(logger, "Unable to sample due to serial communication error");
+ return false;
+ }
+
+ miosix::Thread::sleep(500);
return true;
}
@@ -602,6 +551,8 @@ bool VN300::selfTestImpl()
return false;
}
+ miosix::Thread::sleep(100);
+
// removing junk
usart.clearQueue();
@@ -612,8 +563,6 @@ bool VN300::selfTestImpl()
return false;
}
- miosix::Thread::sleep(100);
-
if (!recvStringCommand(recvString, recvStringMaxDimension))
{
LOG_WARN(logger, "Unable to receive string command");
@@ -816,7 +765,7 @@ bool VN300::sendStringCommand(std::string command)
// in cas of CRC_NO the enabled crc is 8 bit
command = fmt::format("{}{}{}", "$", command, "*XX\n");
}
- printf("%s\n", command.c_str());
+
// I send the final command
usart.writeString(command.c_str());
@@ -825,49 +774,36 @@ bool VN300::sendStringCommand(std::string command)
bool VN300::recvStringCommand(char *command, int maxLength)
{
- uint64_t end_time = 0;
- uint64_t in_time = TimestampTimer::getTimestamp();
char initChar;
- for (;;)
+ int j = 1;
+ bool read = false;
+
+ for (int i = 0; i < 10000; i++)
{
// Read the first char
- if (!usart.readBlocking(&initChar, 1))
+ if (usart.read(&initChar, 1) && initChar == '$')
{
- return false;
- }
- // If it is a '$' i break
- if (initChar == '$')
- {
- break;
- }
- }
- command[0] = '$';
+ command[0] = '$';
- int i = 0;
- // Read the buffer
+ while (usart.read(&initChar, 1) && initChar != '\n' &&
+ j < maxLength)
+ {
+ command[j] = initChar;
+ j++;
+ }
- if (!usart.readBlocking(command + 1, maxLength - 1))
- {
- return false;
+ read = true;
+ break;
+ }
}
- // Iterate until i reach the end or i find \n then i substitute it with a \0
- while (i < maxLength && command[i] != '\n')
+ if (read)
{
- i++;
- // assert for testing purposes
- assert(i < maxLength);
+ command[j] = '\0';
}
- // Terminate the string
- command[i] = '\0';
-
- // Assing the length
- recvStringLength = i - 1;
+ recvStringLength = j - 1;
- end_time = TimestampTimer::getTimestamp() - in_time;
- printf("Time to read: %lld\n", end_time);
-
return true;
}
@@ -925,8 +861,6 @@ bool VN300::checkErrorVN(const char *message)
break;
}
- printf("%s\n", error.c_str());
-
return true; // Error detected
}
diff --git a/src/shared/sensors/VN300/VN300.h b/src/shared/sensors/VN300/VN300.h
index 3ba621dfd778c2dbba5c3e867ae9d9119e10b6a9..7b40eb0b452e0d8bb7d4aa1e5ce037fd9c09a781 100644
--- a/src/shared/sensors/VN300/VN300.h
+++ b/src/shared/sensors/VN300/VN300.h
@@ -69,7 +69,7 @@ namespace Boardcore
/**
* @brief Driver class for VN300 IMU.
*/
-class VN300 : public Sensor<VN300Data>, public ActiveObject
+class VN300 : public Sensor<VN300Data>
{
public:
enum class CRCOptions : uint8_t
@@ -79,8 +79,9 @@ public:
CRC_ENABLE_16 = 0x10
};
- std::array<uint32_t, 9> BaudrateList = {9600, 19200, 38400, 57600, 115200, 128000, 230400, 460800, 921600};
-
+ std::array<uint32_t, 9> BaudrateList = {
+ 9600, 19200, 38400, 57600, 115200, 128000, 230400, 460800, 921600};
+
/**
* @brief Constructor.
*
@@ -92,27 +93,13 @@ public:
* @param antPos antenna A position
*/
VN300(USART &usart, int baudrate, CRCOptions crc = CRCOptions::CRC_ENABLE_8,
- uint16_t samplePeriod = 20,
+ uint16_t samplePeriod = 15,
AntennaPosition antPosA = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
AntennaPosition antPosB = {1.0, 0.0, 0.0, 0.0, 0.0, 0.0},
Eigen::Matrix3f rotMat = Eigen::Matrix3f::Identity());
bool init() override;
- /**
- * @brief Method to sample the raw data without parsing.
- *
- * @return True if operation succeeded.
- */
- bool sampleRaw();
-
- /**
- * @brief Method to get the raw sample.
- *
- * @return String that represents the sample.
- */
- string getLastRawSample();
-
/**
* @brief Method to reset the sensor to default values and to close
* the connection. Used if you need to close and re initialize the sensor.
@@ -129,18 +116,6 @@ private:
*/
VN300Data sampleImpl() override;
- /**
- * @brief Active object method, about the thread execution
- */
- void run() override;
-
- /**
- * @brief Sampling method used by the thread
- *
- * @return VN300Data The sampled data
- */
- VN300Data sampleData();
-
/**
* @brief Method to find the baudrate of the sensor at startup
*
@@ -181,9 +156,9 @@ private:
/**
* @brief reset to factory settings.
- *
+ *
* @return True if operation succeeded.
- */
+ */
bool resetFactorySettings();
/**
diff --git a/src/shared/sensors/VN300/VN300Data.h b/src/shared/sensors/VN300/VN300Data.h
index 106ee67c672693c2293323add7c6b73c404f81f5..26f552e3c6fdb8d9bea3a4c75c34c004367e1095 100644
--- a/src/shared/sensors/VN300/VN300Data.h
+++ b/src/shared/sensors/VN300/VN300Data.h
@@ -84,12 +84,10 @@ struct VN300Data : public QuaternionData,
*/
// cppcheck-suppress uninitDerivedMemberVar
VN300Data()
- : QuaternionData{0, 0.0, 0.0, 0.0, 0.0},
- MagnetometerData{0, 0.0, 0.0, 0.0}, AccelerometerData{0, 0.0, 0.0,
- 0.0},
- GyroscopeData{0, 0.0, 0.0, 0.0}, Ins_Lla{0, 0.0, 0, 0, 0.0, 0.0,
- 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0}
+ : QuaternionData{0, 0.0, 0.0, 0.0, 0.0}, MagnetometerData{0, 0.0, 0.0,
+ 0.0},
+ AccelerometerData{0, 0.0, 0.0, 0.0}, GyroscopeData{0, 0.0, 0.0, 0.0},
+ Ins_Lla{0, 0.0, 0, 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}
{
}
@@ -98,10 +96,10 @@ struct VN300Data : public QuaternionData,
*
* @param single data structures for all the data
*/
- // cppcheck-suppress passedByValue
// cppcheck-suppress uninitDerivedMemberVar
- VN300Data(QuaternionData quat, MagnetometerData magData,
- AccelerometerData accData, GyroscopeData gyro, Ins_Lla ins)
+ VN300Data(const QuaternionData& quat, const MagnetometerData& magData,
+ const AccelerometerData& accData, const GyroscopeData& gyro,
+ const Ins_Lla& ins)
: QuaternionData(quat), MagnetometerData(magData),
AccelerometerData(accData), GyroscopeData(gyro), Ins_Lla(ins)
{