[VN100] Fixed self test failing for reading wrong data

src/shared/sensors/VN100/VN100.cpp

@@ -27,8 +27,7 @@
 namespace Boardcore
 {
 
-VN100::VN100(USART &usart, int baudRate, CRCOptions crc,
-             uint16_t samplePeriod)
+VN100::VN100(USART &usart, int baudRate, CRCOptions crc, uint16_t samplePeriod)
     : usart(usart), baudRate(baudRate), samplePeriod(samplePeriod), crc(crc)
 {
 }
@@ -108,12 +107,6 @@ bool VN100::init()
         return false;
     }
 
-    if (!this->start())
-    {
-        LOG_ERR(logger, "Unable to start the sampling thread");
-        return false;
-    }
-
     // Set the isInit flag true
     isInit = true;
 
@@ -128,11 +121,11 @@ void VN100::run()
     while (!shouldStop())
     {
         long long initialTime = miosix::getTick();
-
+        {
             // Sample the data locking the mutex
             miosix::Lock<FastMutex> l(mutex);
             threadSample = sampleData();
-
+        }
         // Sleep for the sampling period
         miosix::Thread::sleepUntil(initialTime + samplePeriod);
     }
@@ -418,20 +411,18 @@ bool VN100::selfTestImpl()
         return false;
     }
 
-    // I check the model number (I perform the procedure twice to delete junk
-    // problems)
-    sendStringCommand("VNRRG,01");
-    miosix::Thread::sleep(
-        100);  // These sleep are important at very high baud rates
-    recvStringCommand(recvString, recvStringMaxDimension);
-    miosix::Thread::sleep(100);
+    // removing junk
+    usart.clearQueue();
 
+    // I check the model number
     if (!sendStringCommand("VNRRG,01"))
     {
         LOG_WARN(logger, "Unable to send string command");
         return false;
     }
 
+    miosix::Thread::sleep(100);
+
     if (!recvStringCommand(recvString, recvStringMaxDimension))
     {
         LOG_WARN(logger, "Unable to receive string command");

src/shared/sensors/VN100/VN100.h

@@ -86,8 +86,7 @@ public:
      * @param Redundancy check option.
      * @param samplePeriod Sampling period in ms
      */
-    VN100(USART &usart, int baudrate,
-          CRCOptions crc        = CRCOptions::CRC_ENABLE_8,
+    VN100(USART &usart, int baudrate, CRCOptions crc = CRCOptions::CRC_ENABLE_8,
           uint16_t samplePeriod = 20);
 
     bool init() override;

src/tests/sensors/test-vn100.cpp

@@ -31,28 +31,42 @@ int main()
 {
     VN100Data sample;
     string sampleRaw;
+
+    GpioPin u2tx1(GPIOA_BASE, 2);
+    GpioPin u2rx1(GPIOA_BASE, 3);
+
+    u2rx1.alternateFunction(7);
+    u2rx1.mode(Mode::ALTERNATE);
+    u2tx1.alternateFunction(7);
+    u2tx1.mode(Mode::ALTERNATE);
+
     USART usart(USART1, 921600);
-    VN100 sensor{usart, 921600,
-                 VN100::CRCOptions::CRC_ENABLE_16};
+    VN100 sensor{usart, 921600, VN100::CRCOptions::CRC_ENABLE_16};
 
     // Let the sensor start up
     Thread::sleep(1000);
 
+    printf("Initializing sensor\n");
     if (!sensor.init())
     {
         printf("Error initializing the sensor!\n");
         return 0;
     }
 
-    printf("Sensor init successful!\n");
-
+    printf("Running self-test\n");
     if (!sensor.selfTest())
     {
-        printf("Error self test check!\n");
+        printf("Unable to execute self-test\n");
+        return 0;
+    }
+
+    if (!sensor.start())
+    {
+        printf("Unable to start the sampling thread\n");
         return 0;
     }
 
-    printf("Sensor self test successful!\n");
+    printf("Sensor sampling thread started!\n");
 
     // Sample and print 100 samples
     for (int i = 0; i < 100; i++)