diff --git a/src/shared/sensors/VN100/VN100.cpp b/src/shared/sensors/VN100/VN100.cpp
index f4fee6778509858a9a4c61b23ca8dfcd102180af..7a808633d782a3b9aa948348c87dbadedf602837 100644
--- a/src/shared/sensors/VN100/VN100.cpp
+++ b/src/shared/sensors/VN100/VN100.cpp
@@ -27,11 +27,9 @@
namespace Boardcore
{
-VN100::VN100(USARTType *portNumber, int baudRate, CRCOptions crc,
- uint16_t samplePeriod)
- : portNumber(portNumber), baudRate(baudRate), crc(crc)
+VN100::VN100(USART &usart, int baudRate, CRCOptions crc, uint16_t samplePeriod)
+ : usart(usart), baudRate(baudRate), samplePeriod(samplePeriod), crc(crc)
{
- this->samplePeriod = samplePeriod;
}
bool VN100::init()
@@ -109,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;
@@ -129,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();
-
+ {
+ // Sample the data locking the mutex
+ miosix::Lock<FastMutex> l(mutex);
+ threadSample = sampleData();
+ }
// Sleep for the sampling period
miosix::Thread::sleepUntil(initialTime + samplePeriod);
}
@@ -151,7 +143,7 @@ bool VN100::sampleRaw()
}
// Send the IMU sampling command
- serialInterface->writeString(preSampleImuString->c_str());
+ usart.writeString(preSampleImuString->c_str());
// Wait some time
// TODO dimension the time
@@ -200,9 +192,6 @@ bool VN100::closeAndReset()
// Free the recvString memory
delete recvString;
- // Free the serialInterface memory
- delete serialInterface;
-
return true;
}
@@ -241,7 +230,7 @@ VN100Data VN100::sampleData()
}
// Returns Quaternion, Magnetometer, Accelerometer and Gyro
- serialInterface->writeString(preSampleImuString->c_str());
+ usart.writeString(preSampleImuString->c_str());
// Wait some time
// TODO dimension the time
@@ -269,7 +258,7 @@ VN100Data VN100::sampleData()
// Returns Magnetometer, Accelerometer, Gyroscope, Temperature and Pressure
// (UNCOMPENSATED) DO NOT USE THESE MAGNETOMETER, ACCELEROMETER AND
// GYROSCOPE VALUES
- serialInterface->writeString(preSampleTempPressString->c_str());
+ usart.writeString(preSampleTempPressString->c_str());
// Wait some time
// TODO dimension the time
@@ -319,7 +308,7 @@ bool VN100::disableAsyncMessages(bool waitResponse)
bool VN100::configDefaultSerialPort()
{
// Initial default settings
- serialInterface = new USART(portNumber, 115200);
+ usart.setBaudrate(115200);
// Check correct serial init
return true;
@@ -342,11 +331,8 @@ bool VN100::configUserSerialPort()
return false;
}
- // Destroy the serial object
- delete serialInterface;
-
// I can open the serial with user's baud rate
- serialInterface = new USART(portNumber, baudRate);
+ usart.setBaudrate(baudRate);
// Check correct serial init
return true;
@@ -425,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");
@@ -654,7 +638,7 @@ bool VN100::sendStringCommand(std::string command)
}
// I send the final command
- serialInterface->writeString(command.c_str());
+ usart.writeString(command.c_str());
// Wait some time
// TODO dimension the time
@@ -667,7 +651,7 @@ bool VN100::recvStringCommand(char *command, int maxLength)
{
int i = 0;
// Read the buffer
- if (!(serialInterface->readBlocking(command, maxLength)))
+ if (!usart.readBlocking(command, maxLength))
{
return false;
}
diff --git a/src/shared/sensors/VN100/VN100.h b/src/shared/sensors/VN100/VN100.h
index d2d5c6c943c088c09a9eae55417276fa07a228ff..64886b774eca270e46dba86435bd1019c308050f 100644
--- a/src/shared/sensors/VN100/VN100.h
+++ b/src/shared/sensors/VN100/VN100.h
@@ -80,14 +80,13 @@ public:
/**
* @brief Constructor.
*
- * @param USART port number.
+ * @param usart Serial bus used for the sensor.
* @param BaudRate different from the sensor's default [9600, 19200, 38400,
* 57600, 115200, 128000, 230400, 460800, 921600].
* @param Redundancy check option.
* @param samplePeriod Sampling period in ms
*/
- VN100(USARTType *portNumber = USART2, int baudRate = 921600,
- CRCOptions crc = CRCOptions::CRC_ENABLE_8,
+ VN100(USART &usart, int baudrate, CRCOptions crc = CRCOptions::CRC_ENABLE_8,
uint16_t samplePeriod = 20);
bool init() override;
@@ -238,8 +237,13 @@ private:
*/
uint16_t calculateChecksum16(uint8_t *message, int length);
- USARTType *portNumber;
+ /**
+ * @brief Serial interface that is needed to communicate
+ * with the sensor via ASCII codes.
+ */
+ USART &usart;
int baudRate;
+
uint16_t samplePeriod;
CRCOptions crc;
bool isInit = false;
@@ -266,12 +270,6 @@ private:
*/
unsigned int recvStringLength = 0;
- /**
- * @brief Serial interface that is needed to communicate
- * with the sensor via ASCII codes.
- */
- USARTInterface *serialInterface = nullptr;
-
/**
* @brief Mutex to synchronize the reading and writing of the threadSample
*/
diff --git a/src/tests/sensors/test-vn100.cpp b/src/tests/sensors/test-vn100.cpp
index 2e93b1eff2ce610def3f0bfe76feed3f7418efc2..5f5fd14ada39d8be9b93487304a685e9819db712 100644
--- a/src/tests/sensors/test-vn100.cpp
+++ b/src/tests/sensors/test-vn100.cpp
@@ -31,26 +31,42 @@ int main()
{
VN100Data sample;
string sampleRaw;
- VN100 sensor{USART1, 921600, VN100::CRCOptions::CRC_ENABLE_16};
+
+ 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(USART2, 115200);
+ VN100 sensor{usart, 115200, 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++)