[VN300] modified read control, timed wait

Added the clearBuffer function, fixed CRC function

src/shared/sensors/VN300/VN300.cpp

@@ -27,11 +27,11 @@
 namespace Boardcore
 {
 
-VN300::VN300(USART &usart, int baudRate, CRCOptions crc, uint16_t samplePeriod,
-             const AntennaPosition antPosA, const AntennaPosition antPosB,
-             const Eigen::Matrix3f rotMat)
-    : usart(usart), baudRate(baudRate), samplePeriod(samplePeriod), crc(crc),
-      antPosA(antPosA), antPosB(antPosB), rotMat(rotMat)
+VN300::VN300(USART &usart, int userBaudRate, CRCOptions crc,
+             uint16_t samplePeriod, const AntennaPosition antPosA,
+             const AntennaPosition antPosB, const Eigen::Matrix3f rotMat)
+    : usart(usart), userBaudRate(userBaudRate), samplePeriod(samplePeriod),
+      crc(crc), antPosA(antPosA), antPosB(antPosB), rotMat(rotMat)
 {
 }
 
@@ -84,15 +84,15 @@ bool VN300::init()
         return false;
     }
 
-    if (!configDefaultSerialPort())
+    if (!setCrc(true))
     {
-        LOG_ERR(logger, "Unable to config the default VN300 serial port");
+        LOG_ERR(logger, "Unable to set the VN300 user selected CRC");
         return false;
     }
 
-    if (!setCrc(false))
+    if (!configBaudRate(defaultBaudRate))
     {
-        LOG_ERR(logger, "Unable to set the VN300 user selected CRC");
+        LOG_ERR(logger, "Unable to config the default VN300 serial port");
         return false;
     }
 
@@ -125,14 +125,14 @@ bool VN300::init()
         LOG_ERR(logger, "Unable to save settings to non-volatile memory");
         return false;
     }
+    miosix::Thread::sleep(2000);
 
-    if (!configUserSerialPort())
+    if (!configBaudRate(userBaudRate))
     {
         LOG_ERR(logger, "Unable to config the user VN300 serial port");
         return false;
     }
 
-    // I need to repeat this in case of a non default
     // serial port communication at the beginning
     if (!setCrc(true))
     {
@@ -183,14 +183,18 @@ bool VN300::closeAndReset()
 bool VN300::writeSettingsCommand()
 {
     miosix::Thread::sleep(50);
+    clearBuffer();
     if (!sendStringCommand("VNWNV"))
     {
         LOG_WARN(logger, "Impossible to save settings");
     }
 
-    miosix::Thread::sleep(500);
+    miosix::Thread::sleep(1000);
     // Read the answer
-    recvStringCommand(recvString, recvStringMaxDimension);
+    if (!recvStringCommand(recvString, recvStringMaxDimension))
+    {
+        return false;
+    }
 
     if (checkErrorVN(recvString))
         return false;
@@ -205,7 +209,11 @@ bool VN300::writeSettingsCommand()
 
     miosix::Thread::sleep(500);
     // Read the answer
-    recvStringCommand(recvString, recvStringMaxDimension);
+    if (!recvStringCommand(recvString, recvStringMaxDimension))
+    {
+        LOG_WARN(logger, "Impossible to reset the VN300");
+        return false;
+    }
 
     if (checkErrorVN(recvString))
         return false;
@@ -305,6 +313,7 @@ bool VN300::disableAsyncMessages(bool waitResponse)
 
     miosix::Thread::sleep(50);
     // Send the command
+    clearBuffer();
     if (!sendStringCommand(command))
     {
         return false;
@@ -322,37 +331,10 @@ bool VN300::disableAsyncMessages(bool waitResponse)
     return true;
 }
 
-bool VN300::configDefaultSerialPort()
-{
-    miosix::Thread::sleep(50);
-    // 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);
-
-    return true;
-}
-
 bool VN300::findBaudrate()
 {
-    char modelNumber[]          = "VN-300T-CR";
-    const int modelNumberOffset = 10;
+    char modelNumber[]          = "VN";
+    const int modelNumberOffset = 1;
 
     for (uint32_t i = 0; i < BaudrateList.size(); i++)
     {
@@ -363,9 +345,6 @@ bool VN300::findBaudrate()
         // 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"))
         //{
@@ -373,12 +352,13 @@ bool VN300::findBaudrate()
         //    return false;
         //}
         miosix::Thread::sleep(50);
+
         usart.writeString("$VNRRG,01*XX\n");
 
         if (recvStringCommand(recvString, recvStringMaxDimension))
         {
             if (strncmp(modelNumber, recvString + modelNumberOffset,
-                        strlen(modelNumber)) != 0)
+                        strlen(modelNumber)) == 0)
             {
                 return true;
             }
@@ -393,15 +373,17 @@ bool VN300::findBaudrate()
  * Even if the user configured baudrate is the default, I want to reset the
  * buffer to clean the junk.
  */
-bool VN300::configUserSerialPort()
+bool VN300::configBaudRate(int baudRate)
 {
 
     std::string command;
 
     // I format the command to change baud rate
-    command = fmt::format("{}{}", "VNWRG,5,", baudRate);
+    command                     = fmt::format("{}{}", "VNWRG,05,", baudRate);
+    const int modelNumberOffset = 1;
 
     miosix::Thread::sleep(50);
+    clearBuffer();
     // I can send the command
     if (!sendStringCommand(command))
     {
@@ -413,10 +395,10 @@ bool VN300::configUserSerialPort()
         LOG_WARN(logger, "Unable to sample due to serial communication error");
         return false;
     }
-
-    if (checkErrorVN(recvString))
+    if (strncmp(command.c_str(), recvString + modelNumberOffset,
+                strlen(command.c_str())) != 0)
     {
-        LOG_WARN(logger, "Unable to change serial port baudrate");
+        LOG_WARN(logger, "The message is wrong {}", recvString);
         return false;
     }
 
@@ -426,7 +408,7 @@ bool VN300::configUserSerialPort()
     return true;
 }
 
-bool VN300::resetFactorySettings()
+[[maybe_unused]] bool VN300::resetFactorySettings()
 {
     // Command string
     std::string command =
@@ -475,6 +457,7 @@ bool VN300::setCrc(bool waitResponse)
     crc = CRCOptions::CRC_ENABLE_8;
 
     miosix::Thread::sleep(50);
+    clearBuffer();
     // Send the command
     if (!sendStringCommand(command))
     {
@@ -484,19 +467,32 @@ bool VN300::setCrc(bool waitResponse)
     // Read the answer
     if (waitResponse)
     {
-        recvStringCommand(recvString, recvStringMaxDimension);
+        if (recvStringCommand(recvString, recvStringMaxDimension))
+        {
             uint8_t error = checkErrorVN(recvString);
+
             if (error == 3)
             {
                 crc = CRCOptions::CRC_ENABLE_16;
 
+                miosix::Thread::sleep(50);
+                clearBuffer();
                 // Send the command
                 if (!sendStringCommand(command))
                 {
                     return false;
                 }
-            recvStringCommand(recvString, recvStringMaxDimension);
-            checkErrorVN(recvString);
+                if (!recvStringCommand(recvString, recvStringMaxDimension))
+                {
+                    return false;
+                }
+
+                uint8_t error2 = checkErrorVN(recvString);
+
+                if (error2 != 0)
+                {
+                    return false;
+                }
             }
             else if (error != 0)
             {
@@ -507,6 +503,7 @@ bool VN300::setCrc(bool waitResponse)
                 return true;
             }
         }
+    }
     else
     {
         crc = CRCOptions::CRC_ENABLE_16;
@@ -539,6 +536,7 @@ bool VN300::setAntennaA(AntennaPosition antPos)
                           antPos.posZ);
 
     miosix::Thread::sleep(50);
+    clearBuffer();
     if (!sendStringCommand(command))
     {
         return false;
@@ -563,6 +561,7 @@ bool VN300::setCompassBaseline(AntennaPosition antPos)
                           antPos.uncZ);
 
     miosix::Thread::sleep(50);
+    clearBuffer();
     if (!sendStringCommand(command))
     {
         return false;
@@ -588,7 +587,7 @@ bool VN300::setReferenceFrame(Eigen::Matrix3f rotMat)
                     rotMat(1, 2), rotMat(2, 0), rotMat(2, 1), rotMat(2, 2));
 
     miosix::Thread::sleep(50);
-    // I can send the command
+    clearBuffer();
     if (!sendStringCommand(command))
     {
         return false;
@@ -844,8 +843,10 @@ bool VN300::recvStringCommand(char *command, int maxLength)
     char initChar;
     int j      = 1;
     bool read  = false;
+    command[0] = '\0';
 
-    for (int i = 0; i < 10000; i++)
+    int i = 0;
+    while (read == false && i < 10000)
     {
         // Read the first char
         if (usart.read(&initChar, 1) && initChar == '$')
@@ -857,17 +858,21 @@ bool VN300::recvStringCommand(char *command, int maxLength)
             {
                 command[j] = initChar;
                 j++;
-            }
-
                 read = true;
+            }
             break;
         }
+        i++;
     }
 
     if (read)
     {
         command[j] = '\0';
     }
+    else
+    {
+        return false;
+    }
 
     recvStringLength = j - 1;
 
@@ -1025,4 +1030,12 @@ uint16_t VN300::calculateChecksum16(uint8_t *message, int length)
     return result;
 }
 
+void VN300::clearBuffer()
+{
+    char c;
+    while (usart.read(&c, 1))
+    {
+    }
+}
+
 }  // namespace Boardcore

src/shared/sensors/VN300/VN300.h

@@ -74,7 +74,6 @@ class VN300 : public Sensor<VN300Data>
 public:
     enum class CRCOptions : uint8_t
     {
-        CRC_NO        = 0x00,
         CRC_ENABLE_8  = 0x08,
         CRC_ENABLE_16 = 0x10
     };
@@ -92,8 +91,8 @@ public:
      * @param samplePeriod Sampling period in ms
      * @param antPos antenna A position
      */
-    VN300(USART &usart, int baudrate, CRCOptions crc = CRCOptions::CRC_ENABLE_8,
-          uint16_t samplePeriod   = 15,
+    VN300(USART &usart, int userBaudRate,
+          CRCOptions crc = CRCOptions::CRC_ENABLE_8, 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());
@@ -140,19 +139,12 @@ private:
      */
     bool disableAsyncMessages(bool waitResponse = true);
 
-    /**
-     * @brief Configures the default serial communication.
-     *
-     * @return True if operation succeeded.
-     */
-    bool configDefaultSerialPort();
-
     /**
      * @brief Configures the user defined serial communication.
      *
      * @return True if operation succeeded.
      */
-    bool configUserSerialPort();
+    bool configBaudRate(int baud);
 
     /**
      * @brief reset to factory settings.
@@ -288,12 +280,21 @@ private:
      */
     uint16_t calculateChecksum16(uint8_t *message, int length);
 
+    /**
+     * @brief Clear the buffer of the serial interface.
+     *
+     * This is a placeholder function for the serial interface.
+     * When the usart driver is corrected this must be changed.
+     */
+    void clearBuffer();
+
     /**
      * @brief Serial interface that is needed to communicate
      * with the sensor via ASCII codes.
      */
     USART &usart;
-    int baudRate;
+    int userBaudRate;
+    const int defaultBaudRate = 115200;
 
     uint16_t samplePeriod;
     CRCOptions crc;

src/tests/sensors/test-vn300.cpp

@@ -45,7 +45,7 @@ int main()
     u6tx1.mode(Mode::ALTERNATE);
 
     USART usart(USART6, 115200);
-    VN300 sensor(usart, 115200, VN300::CRCOptions::CRC_ENABLE_8);
+    VN300 sensor(usart, 230400, VN300::CRCOptions::CRC_ENABLE_8);
 
     // Let the sensor start up
     Thread::sleep(1000);
@@ -106,7 +106,7 @@ int main()
     CpuMeterData cpuData = Boardcore::CpuMeter::getCpuStats();
     printf("CPU: %f\n", cpuData.mean);
 
-    // sensor.closeAndReset();
+    sensor.closeAndReset();
     printf("Sensor communication closed!\n");
 
     return 0;