From 95ceeb9041a3ba62f703de275ed8f3352bb5b4f4 Mon Sep 17 00:00:00 2001
From: Lorenzo Cucchi <lorenzo.cucchi@skywarder.eu>
Date: Thu, 22 Jun 2023 21:07:15 +0200
Subject: [PATCH] [VN300] New INIT sequence and new INS sampling
---
src/shared/sensors/VN300/VN300.cpp | 589 +++++++++++++++++++++++----
src/shared/sensors/VN300/VN300.h | 347 ++++++----------
src/shared/sensors/VN300/VN300Data.h | 65 ++-
3 files changed, 683 insertions(+), 318 deletions(-)
diff --git a/src/shared/sensors/VN300/VN300.cpp b/src/shared/sensors/VN300/VN300.cpp
index 16a4946c8..881aad08a 100644
--- a/src/shared/sensors/VN300/VN300.cpp
+++ b/src/shared/sensors/VN300/VN300.cpp
@@ -26,11 +26,13 @@
namespace Boardcore
{
-VN300::VN300(USARTType *portNumber, USARTInterface::Baudrate baudRate,
- CRCOptions crc, uint16_t samplePeriod)
- : portNumber(portNumber), baudRate(baudRate), crc(crc)
+
+VN300::VN300(USART &usart, int baudRate, CRCOptions crc, uint16_t samplePeriod,
+ AntennaPosition antPosA, AntennaPosition antPosB,
+ Eigen::Matrix3f rotMat)
+ : usart(usart), baudRate(baudRate), samplePeriod(samplePeriod), crc(crc),
+ antPosA(antPosA), antPosB(antPosB), rotMat(rotMat)
{
- this->samplePeriod = samplePeriod;
}
bool VN300::init()
@@ -41,7 +43,7 @@ bool VN300::init()
if (isInit)
{
lastError = SensorErrors::ALREADY_INIT;
- LOG_WARN(logger, "Sensor vn300 already initilized");
+ LOG_WARN(logger, "Sensor VN300 already initilized");
return true;
}
@@ -51,13 +53,13 @@ bool VN300::init()
// Allocate the pre loaded strings based on the user selected crc
if (crc == CRCOptions::CRC_ENABLE_16)
{
- preSampleImuString = new string("$VNRRG,15*92EA\n");
- preSampleTempPressString = new string("$VNRRG,54*4E0F\n");
+ preSampleImuString = new string("$VNRRG,15*92EA\n");
+ preSampleINSlla = new string("$VNRRG,63*6BBB\n");
}
else
{
- preSampleImuString = new string("$VNRRG,15*77\n");
- preSampleTempPressString = new string("$VNRRG,54*72\n");
+ preSampleImuString = new string("$VNRRG,15*77\n");
+ preSampleINSlla = new string("$VNRRG,63*76\n");
}
// Set the error to init fail and if the init process goes without problem
@@ -66,31 +68,43 @@ bool VN300::init()
if (recvString == NULL)
{
- LOG_ERR(logger, "Unable to initialize the receive vn300 string");
+ LOG_ERR(logger, "Unable to initialize the receive VN300 string");
return false;
}
if (!configDefaultSerialPort())
{
- LOG_ERR(logger, "Unable to config the default vn100 serial port");
+ LOG_ERR(logger, "Unable to config the default VN300 serial port");
return false;
}
if (!setCrc(false))
{
- LOG_ERR(logger, "Unable to set the vn300 user selected CRC");
+ LOG_ERR(logger, "Unable to set the VN300 user selected CRC");
return false;
}
if (!disableAsyncMessages(false))
{
- LOG_ERR(logger, "Unable to disable async messages from vn100");
+ LOG_ERR(logger, "Unable to disable async messages from VN300");
+ return false;
+ }
+
+ if (!setReferenceFrame(rotMat))
+ {
+ LOG_ERR(logger, "Unable to set reference frame rotation");
+ return false;
+ }
+
+ if (!writeSettingsCommand())
+ {
+ LOG_ERR(logger, "Unable to save settings to non-volatile memory");
return false;
}
if (!configUserSerialPort())
{
- LOG_ERR(logger, "Unable to config the user vn100 serial port");
+ LOG_ERR(logger, "Unable to config the user VN300 serial port");
return false;
}
@@ -98,19 +112,25 @@ bool VN300::init()
// serial port communication at the beginning
if (!setCrc(true))
{
- LOG_ERR(logger, "Unable to set the vn100 user selected CRC");
+ LOG_ERR(logger, "Unable to set the VN300 user selected CRC");
return false;
}
if (!disableAsyncMessages(true))
{
- LOG_ERR(logger, "Unable to disable async messages from vn100");
+ LOG_ERR(logger, "Unable to disable async messages from VN300");
return false;
}
- if (!this->start())
+ if (!setAntennaA(antPosA))
{
- LOG_ERR(logger, "Unable to start the sampling thread");
+ LOG_ERR(logger, "Unable to set antenna A position");
+ return false;
+ }
+
+ if (!setCompassBaseline(antPosB))
+ {
+ LOG_ERR(logger, "Unable to set compass baseline");
return false;
}
@@ -128,23 +148,128 @@ void VN300::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);
}
}
+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
+ if (!isInit)
+ {
+ lastError = SensorErrors::NOT_INIT;
+ LOG_WARN(logger, "Sensor VN300 already not initilized");
+ return true;
+ }
+
+ // Send the reset command to the VN300
+ if (!sendStringCommand("VNRST"))
+ {
+ LOG_WARN(logger, "Impossible to reset the VN300");
+ return false;
+ }
+
+ isInit = false;
+
+ // Free the recvString memory
+ delete recvString;
+
+ return true;
+}
+
+bool VN300::writeSettingsCommand()
+{
+ if (!sendStringCommand("VNWNV"))
+ {
+ 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"))
+ {
+ LOG_WARN(logger, "Impossible to reset the VN300");
+
+ return false;
+ }
+
+ miosix::Thread::sleep(1000);
+
+ return true;
+}
+
+bool VN300::selfTest()
+{
+ if (!selfTestImpl())
+ {
+ lastError = SensorErrors::SELF_TEST_FAIL;
+ LOG_WARN(logger, "Unable to perform a successful VN300 self test");
+ return false;
+ }
+
+ return true;
+}
+
+VN300Data VN300::sampleImpl()
+{
+ miosix::Lock<FastMutex> l(mutex);
+ return threadSample;
+}
+
VN300Data VN300::sampleData()
{
if (!isInit)
{
lastError = SensorErrors::NOT_INIT;
LOG_WARN(logger,
- "Unable to sample due to not initialized vn100 sensor");
+ "Unable to sample due to not initialized VN300 sensor");
return lastSample;
}
@@ -155,11 +280,7 @@ VN300Data VN300::sampleData()
}
// Returns Quaternion, Magnetometer, Accelerometer and Gyro
- if (!(serialInterface->writeString(preSampleImuString->c_str())))
- {
- // If something goes wrong i return the last sampled data
- return lastSample;
- }
+ usart.writeString(preSampleImuString->c_str());
// Wait some time
// TODO dimension the time
@@ -173,25 +294,18 @@ VN300Data VN300::sampleData()
if (!verifyChecksum(recvString, recvStringLength))
{
- LOG_WARN(logger, "Vn100 sampling message invalid checksum");
+ LOG_WARN(logger, "VN300 sampling message invalid checksum");
// If something goes wrong i return the last sampled data
return lastSample;
}
- // Now i have to parse the data
QuaternionData quat = sampleQuaternion();
MagnetometerData mag = sampleMagnetometer();
AccelerometerData acc = sampleAccelerometer();
GyroscopeData gyro = sampleGyroscope();
- // Returns Magnetometer, Accelerometer, Gyroscope, Temperature and Pressure
- // (UNCOMPENSATED) DO NOT USE THESE MAGNETOMETER, ACCELEROMETER AND
- // GYROSCOPE VALUES
- if (!(serialInterface->writeString(preSampleTempPressString->c_str())))
- {
- // If something goes wrong i return the last sampled data
- return lastSample;
- }
+ // Returns INS LLA message
+ usart.writeString(preSampleINSlla->c_str());
// Wait some time
// TODO dimension the time
@@ -199,22 +313,19 @@ VN300Data VN300::sampleData()
if (!recvStringCommand(recvString, recvStringMaxDimension))
{
- // If something goes wrong i return the last sampled data
return lastSample;
}
if (!verifyChecksum(recvString, recvStringLength))
{
- LOG_WARN(logger, "Vn100 sampling message invalid checksum");
- // If something goes wrong i return the last sampled data
+ LOG_WARN(logger, "VN300 sampling message invalid checksum");
+
return lastSample;
}
- // Parse the data
- TemperatureData temp = sampleTemperature();
- PressureData press = samplePressure();
+ Ins_Lla ins = sampleIns();
- return VN100Data(quat, mag, acc, gyro, temp, press);
+ return VN300Data(quat, mag, acc, gyro, ins);
}
bool VN300::disableAsyncMessages(bool waitResponse)
@@ -233,32 +344,9 @@ bool VN300::disableAsyncMessages(bool waitResponse)
if (waitResponse)
{
recvStringCommand(recvString, recvStringMaxDimension);
- }
- return true;
-}
-
-bool VN300::AsyncPauseCommand(bool waitResponse, bool selection)
-{
- if (selection)
- {
- if (!sendStringCommand(ASYNC_PAUSE_COMMAND))
- {
+ if (checkErrorVN(recvString))
return false;
- }
- }
- else
- {
- if (!sendStringCommand(ASYNC_RESUME_COMMAND))
- {
- return false;
- }
- }
-
- // Read the answer
- if (waitResponse)
- {
- recvStringCommand(recvString, recvStringMaxDimension);
}
return true;
@@ -267,10 +355,10 @@ bool VN300::AsyncPauseCommand(bool waitResponse, bool selection)
bool VN300::configDefaultSerialPort()
{
// Initial default settings
- serialInterface = new USART(portNumber, USARTInterface::Baudrate::B115200);
+ usart.setBaudrate(115200);
// Check correct serial init
- return serialInterface->init();
+ return true;
}
/**
@@ -282,7 +370,7 @@ bool VN300::configUserSerialPort()
std::string command;
// I format the command to change baud rate
- command = fmt::format("{}{}", "VNWRG,5,", static_cast<int>(baudRate));
+ command = fmt::format("{}{}", "VNWRG,5,2", baudRate);
// I can send the command
if (!sendStringCommand(command))
@@ -290,14 +378,11 @@ bool VN300::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 serialInterface->init();
+ return true;
}
bool VN300::setCrc(bool waitResponse)
@@ -311,14 +396,14 @@ bool VN300::setCrc(bool waitResponse)
{
// The 3 inside the command is the 16bit select. The others are default
// values
- command = "VNRRG,30,0,0,0,0,3,0,1";
+ command = "VNWRG,30,0,0,0,0,3,0,1";
}
else
{
// Even if the CRC is not enabled i put the 8 bit
// checksum because i need to know how many 'X' add at the end
// of every command sent
- command = "VNRRG,30,0,0,0,0,1,0,1";
+ command = "VNWRG,30,0,0,0,0,1,0,1";
}
// I need to send the command in both crc because i don't know what type
@@ -336,6 +421,7 @@ bool VN300::setCrc(bool waitResponse)
if (waitResponse)
{
recvStringCommand(recvString, recvStringMaxDimension);
+ checkErrorVN(recvString);
}
crc = CRCOptions::CRC_ENABLE_16;
@@ -350,6 +436,7 @@ bool VN300::setCrc(bool waitResponse)
if (waitResponse)
{
recvStringCommand(recvString, recvStringMaxDimension);
+ checkErrorVN(recvString);
}
// Restore the crc
@@ -358,6 +445,253 @@ bool VN300::setCrc(bool waitResponse)
return true;
}
+bool VN300::setAntennaA(AntennaPosition antPos)
+{
+ std::string command;
+
+ command = fmt::format("{}{},{},{}", "VNWRG,57,", antPos.posX, antPos.posY,
+ antPos.posZ);
+
+ if (!sendStringCommand(command))
+ {
+ return false;
+ }
+
+ return true;
+}
+
+bool VN300::setCompassBaseline(AntennaPosition antPos)
+{
+ std::string command;
+
+ command = fmt::format("{}{},{},{},{},{},{}", "VNWRG,93,", antPos.posX,
+ antPos.posY, antPos.posZ, antPos.uncX, antPos.uncY,
+ antPos.uncZ);
+
+ if (!sendStringCommand(command))
+ {
+ return false;
+ }
+
+ return true;
+}
+
+bool VN300::setReferenceFrame(Eigen::Matrix3f rotMat)
+{
+ std::string command;
+
+ command =
+ fmt::format("{}{},{},{},{},{},{},{},{},{}", "VNWRG, 26", rotMat(0, 0),
+ rotMat(0, 1), rotMat(0, 2), rotMat(1, 0), rotMat(1, 1),
+ rotMat(1, 2), rotMat(2, 0), rotMat(2, 1), rotMat(2, 2));
+
+ // I can send the command
+ if (!sendStringCommand(command))
+ {
+ return false;
+ }
+
+ return true;
+}
+
+bool VN300::selfTestImpl()
+{
+ char modelNumber[] = "VN-300T-CR";
+ const int modelNumberOffset = 10;
+
+ // Check the init status
+ if (!isInit)
+ {
+ lastError = SensorErrors::NOT_INIT;
+ LOG_WARN(
+ logger,
+ "Unable to perform VN300 self test due to not initialized sensor");
+ return false;
+ }
+
+ // 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");
+ return false;
+ }
+
+ // Now i check that the model number is VN-100 starting from the 10th
+ // position because of the message structure
+ if (strncmp(modelNumber, recvString + modelNumberOffset,
+ strlen(modelNumber)) != 0)
+ {
+ LOG_ERR(logger, "VN-300 not corresponding: {} != {}", recvString,
+ modelNumber);
+ return false;
+ }
+
+ // I check the checksum
+ if (!verifyChecksum(recvString, recvStringLength))
+ {
+ LOG_ERR(logger, "Checksum verification failed: {}", recvString);
+ return false;
+ }
+
+ 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.quatTimestamp = TimestampTimer::getTimestamp();
+ data.quatX = strtod(recvString + indexStart + 1, &nextNumber);
+ data.quatY = strtod(nextNumber + 1, &nextNumber);
+ data.quatZ = strtod(nextNumber + 1, &nextNumber);
+ data.quatW = 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 + 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 + 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 + indexStart + 1, &nextNumber);
+ data.angularSpeedY = strtod(nextNumber + 1, &nextNumber);
+ data.angularSpeedZ = strtod(nextNumber + 1, NULL);
+
+ return data;
+}
+
+Ins_Lla VN300::sampleIns()
+{
+ unsigned int indexStart = 0;
+ char *nextNumber;
+ Ins_Lla data;
+
+ // Look for the eleventh ',' 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.insTimestamp = TimestampTimer::getTimestamp();
+ data.time_gps = strtod(recvString + indexStart + 1, &nextNumber);
+ data.week = strtod(nextNumber + 1, &nextNumber);
+ data.status = strtod(nextNumber + 1, &nextNumber);
+ data.yaw = strtof(nextNumber + 1, &nextNumber);
+ data.pitch = strtof(nextNumber + 1, &nextNumber);
+ data.roll = strtof(nextNumber + 1, &nextNumber);
+ data.latitude = strtof(nextNumber + 1, &nextNumber);
+ data.longitude = strtof(nextNumber + 1, &nextNumber);
+ data.altitude = strtof(nextNumber + 1, &nextNumber);
+ data.nedVelX = strtof(nextNumber + 1, &nextNumber);
+ data.nedVelY = strtof(nextNumber + 1, &nextNumber);
+ data.nedVelZ = strtof(nextNumber + 1, NULL);
+
+ return data;
+}
+
bool VN300::sendStringCommand(std::string command)
{
if (crc == CRCOptions::CRC_ENABLE_8)
@@ -388,20 +722,103 @@ 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
- if (!serialInterface->writeString(command.c_str()))
+ usart.writeString(command.c_str());
+
+ // Wait some time
+ // TODO dimension the time
+ // miosix::Thread::sleep(1);
+
+ return true;
+}
+
+bool VN300::recvStringCommand(char *command, int maxLength)
+{
+ int i = 0;
+ // Read the buffer
+ if (!usart.readBlocking(command, maxLength))
{
return false;
}
- // Wait some time
- // TODO dimension the time
- miosix::Thread::sleep(500);
+ // Iterate until i reach the end or i find \n then i substitute it with a \0
+ while (i < maxLength && command[i] != '\n')
+ {
+ i++;
+ }
+
+ // Terminate the string
+ command[i] = '\0';
+
+ // Assing the length
+ recvStringLength = i - 1;
return true;
}
+bool VN300::checkErrorVN(const char *message)
+{
+ if (strncmp(message, "$VNERR,", 7) == 0)
+ {
+ // Extract the error code
+ int errorCode = atoi(&message[7]);
+ string error;
+ // Handle the error based on the error code
+ switch (errorCode)
+ {
+ case 1:
+ error = "VN300 Hard Fault";
+ break;
+ case 2:
+ error = "VN300 Serial Buffer Overflow";
+ break;
+ case 3:
+ error = "VN300 Invalid Checksum";
+ break;
+ case 4:
+ error = "VN300 Invalid Command";
+ break;
+ case 5:
+ error = "VN300 Not Enough Parameters";
+ break;
+ case 6:
+ error = "VN300 Too Many Parameters";
+ break;
+ case 7:
+ error = "VN300 Invalid Parameter";
+ break;
+ case 8:
+ error = "VN300 Invalid Register";
+ break;
+ case 9:
+ error = "VN300 Unauthorized Access";
+ break;
+ case 10:
+ error = "VN300 Watchdog Reset";
+ break;
+ case 11:
+ error = "VN300 Output Buffer Overflow";
+ break;
+ case 12:
+ error = "VN300 Insufficient Baud Rate";
+ break;
+ case 255:
+ error = "VN300 Error Buffer Overflow";
+ break;
+ default:
+ error = "VN300 Unknown error";
+ break;
+ }
+
+ printf("%s\n", error.c_str());
+
+ return true; // Error detected
+ }
+
+ return false; // No error detected
+}
+
bool VN300::verifyChecksum(char *command, int length)
{
int checksumOffset = 0;
@@ -493,4 +910,4 @@ uint16_t VN300::calculateChecksum16(uint8_t *message, int length)
return result;
}
-} // namespace Boardcore
\ No newline at end of file
+} // namespace Boardcore
diff --git a/src/shared/sensors/VN300/VN300.h b/src/shared/sensors/VN300/VN300.h
index b503615de..af43c56f7 100644
--- a/src/shared/sensors/VN300/VN300.h
+++ b/src/shared/sensors/VN300/VN300.h
@@ -23,15 +23,18 @@
#pragma once
/**
- * @brief Driver for the VN300 IMU.
+ * @brief Driver for the VN300S IMU.
*
- * The VN-300 is a miniature, surface-mount, high-performance GPS-Aided Inertial
- * Navigation System (GPS/INS). Incorporating the latest solid-state MEMS sensor
- * technology, the VN-300 combines a set of 3-axis accelerometers, 3-axis gyros,
- * 3-axis magnetometer, a barometric pressure sensor, two separate 50-channel L1
- * GPS receivers, as well as a 32-bit processor into a miniature aluminum
- * enclosure.
- * The VN300 supports both binary and ASCII encoding for communication but via
+ * The VN300S sensor is a calibrated IMU which includes accelerometer,
+ * magnetometer, gyroscope, barometer and temperature sensor. The device
+ * provides also a calibration matrix and an anti-drift matrix for the gyroscope
+ * values. The goal of this driver though is to interface the sensor in its
+ * basic use. Things like asynchronous data and anti-drift techniques haven't
+ * been implemented yet. The driver is intended to be used with the "Rugged
+ * sensor" version (aka only UART communication) although the actual VN300S chip
+ * is capable also of SPI communication.
+ *
+ * The VN300S supports both binary and ASCII encoding for communication but via
* serial and with the asynchronous mode disabled only ASCII is available. The
* protocol also provides two algorithms to verify the integrity of the messages
* (8 bit checksum and 16 bit CRC-CCITT) both selectable by the user using the
@@ -55,218 +58,75 @@
#include <string.h>
#include <utils/Debug.h>
+#include <Eigen/Core>
+
#include "VN300Data.h"
#include "drivers/usart/USART.h"
namespace Boardcore
{
+
/**
- * @brief Driver class for VN300
+ * @brief Driver class for VN300 IMU.
*/
class VN300 : public Sensor<VN300Data>, public ActiveObject
{
public:
- /**
- * @brief Checksum Options
- */
enum class CRCOptions : uint8_t
{
CRC_NO = 0x00,
CRC_ENABLE_8 = 0x08,
CRC_ENABLE_16 = 0x10
};
- /**
- * @brief Async options
- * ASYNC_NO corresponds to no Async communication
- * ASYNC_P1 corresponds to Async comm only on serial port 1
- * ASYNC_P2 corresponds to Async comm only on serial port 2
- * ASYNC_BOTH both serial port are used
- */
- enum class AsyncOptions : uint16_t
- {
- ASYNC_NO = 0x00,
- ASYNC_P1 = 0x01,
- ASYNC_P2 = 0x02,
- ASYNC_BOTH = 0x03
- };
-
- enum class RateDivisor : uint16_t
- {
- RDIV_400_HZ = 0x01,
- RDIV_200_HZ = 0x02,
- RDIV_100_HZ = 0x04,
- RDIV_50_HZ = 0x08,
- RDIV_40_HZ = 0x0A
- };
/**
- * @brief Group's bit for binary register setup
- */
- enum OutputGroup : uint16_t
- {
- GROUP_1 = 1 << 0,
- GROUP_2 = 1 << 1,
- GROUP_3 = 1 << 2,
- GROUP_4 = 1 << 3,
- GROUP_5 = 1 << 4,
- GROUP_6 = 1 << 5,
- GROUP_7 = 1 << 6
- };
-
- /**
- * @brief Common Group
- */
- enum GroupField_1 : uint16_t
- {
- T_START = 1 << 0,
- T_GPS = 1 << 1,
- T_SYNC = 1 << 2,
- Y_P_R = 1 << 3,
- QUAT = 1 << 4,
- ANG_RATE = 1 << 5,
- POS = 1 << 6,
- VEL = 1 << 7,
- ACCEL = 1 << 8,
- IMU = 1 << 9,
- MAGPRES = 1 << 10,
- DELT_TH = 1 << 11,
- INS_STAT = 1 << 12,
- SYNC_IN_CNT = 1 << 13,
- T_GPS_PPS = 1 << 14
- };
-
- /**
- * @brief TIME Group
+ * @brief Constructor.
+ *
+ * @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
+ * @param antPos antenna A position
*/
- enum GroupField_2 : uint16_t
- {
- T_START = 1 << 0,
- T_GPS = 1 << 1,
- GPS_TOW = 1 << 2,
- GPS_WEEK = 1 << 3,
- T_SYNC = 1 << 4,
- T_GPS_PPS = 1 << 5,
- T_UTC = 1 << 6,
- SYNC_IN_CNT = 1 << 7,
- SYNC_OUT_CNT = 1 << 8,
- T_STATUS = 1 << 9,
- };
+ VN300(USART &usart, int baudrate, CRCOptions crc = CRCOptions::CRC_ENABLE_8,
+ uint16_t samplePeriod = 1,
+ 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());
- /**
- * @brief IMU Group
- */
- enum GroupField_3 : uint16_t
- {
- IMU_STAT = 1 << 0,
- UNC_MAG = 1 << 1,
- UNC_ACC = 1 << 2,
- UNC_GYRO = 1 << 3,
- TEMP = 1 << 4,
- PRES = 1 << 5,
- DELTA_TH = 1 << 6,
- DELTA_VEL = 1 << 7,
- MAG = 1 << 8,
- ACCEL = 1 << 9,
- ANG_RATE = 1 << 10,
- };
-
- /**
- * @brief GNSS1 Group
- */
- enum GroupField_4 : uint16_t
- {
- UTC = 1 << 0,
- TOW = 1 << 1,
- WEEK = 1 << 2,
- N_SATS = 1 << 3,
- FIX = 1 << 4,
- POS_LLA = 1 << 5,
- POS_ECEF = 1 << 6,
- VEL_NED = 1 << 7,
- VEL_ECEF = 1 << 8,
- POS_U = 1 << 9,
- VEL_U = 1 << 10,
- TIME_U = 1 << 11,
- TIME_INFO = 1 << 12,
- DOP = 1 << 13,
- SAT_INFO = 1 << 14,
- RAW_MEAS = 1 << 15
- };
+ bool init() override;
/**
- * @brief Attitude Group
+ * @brief Method to sample the raw data without parsing.
+ *
+ * @return True if operation succeeded.
*/
- enum GroupField_5 : uint16_t
- {
- RESERVED = 1 << 0,
- Y_P_R = 1 << 1,
- QUAT = 1 << 2,
- DCM = 1 << 3,
- MAG_NED = 1 << 4,
- ACC_NED = 1 << 5,
- LIN_ACC_BOD = 1 << 6,
- LIN_ACC_NED = 1 << 7,
- YprU = 1 << 8,
- RESERVED = 1 << 9,
- RESERVED = 1 << 10,
- RESERVED = 1 << 11
- };
+ bool sampleRaw();
/**
- * @brief INS Group
+ * @brief Method to get the raw sample.
+ *
+ * @return String that represents the sample.
*/
- enum GroupField_6 : uint16_t
- {
- INS_STATUS = 1 << 0,
- POS_LLA = 1 << 1,
- POS_ECEF = 1 << 2,
- VEL_BODY = 1 << 3,
- VEL_NED = 1 << 4,
- VEL_ECEF = 1 << 5,
- MAG_ECEF = 1 << 6,
- ACC_ECEF = 1 << 7,
- LIN_ACC_ECEF = 1 << 8,
- POS_U = 1 << 9,
- VEL_U = 1 << 10
- };
+ string getLastRawSample();
/**
- * @brief GNSS2 Group
+ * @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.
+ *
+ * @return True if operation succeeded.
*/
- enum GroupField_7 : uint16_t
- {
- UTC = 1 << 0,
- TOW = 1 << 1,
- WEEK = 1 << 2,
- N_SATS = 1 << 3,
- FIX = 1 << 4,
- POS_LLA = 1 << 5,
- POS_ECEF = 1 << 6,
- VEL_NED = 1 << 7,
- VEL_ECEF = 1 << 8,
- POS_U = 1 << 9,
- VEL_U = 1 << 10,
- TIME_U = 1 << 11,
- TIME_INFO = 1 << 12,
- DOP = 1 << 13,
- SAT_INFO = 1 << 14,
- RAW_MEAS = 1 << 15
- };
-
- VN300(USARTType *portNumber = USART1,
- USART::Baudrate baudRate = USART::Baudrate::B921600,
- CRCOptions crc = CRCOptions::CRC_ENABLE_8,
- uint16_t samplePeriod = 50);
-
- bool init() override;
-
- bool sampleRaw();
-
bool closeAndReset();
bool selfTest() override;
private:
+ /**
+ * @brief Sample action implementation.
+ */
+ VN300Data sampleImpl() override;
+
/**
* @brief Active object method, about the thread execution
*/
@@ -275,12 +135,12 @@ private:
/**
* @brief Sampling method used by the thread
*
- * @return VN100Data The sampled data
+ * @return VN300Data The sampled data
*/
VN300Data sampleData();
/**
- * @brief Disables the async messages that the vn100 is default configured
+ * @brief Disables the async messages that the VN300 is default configured
* to send at 40Hz on startup.
*
* @param waitResponse If true wait for a serial response.
@@ -289,20 +149,6 @@ private:
*/
bool disableAsyncMessages(bool waitResponse = true);
- /**
- * @brief Pause async messages in order to write command without receiving
- * continuous data transmission
- *
- * @param waitResponse If true wait for a serial response.
- *
- * @param selection if true pause the async output, if false restart the
- * output
- *
- * @return True if operation succeeded.
- *
- */
- bool VN300::AsyncPauseCommand(bool waitResponse = false, bool selection);
-
/**
* @brief Configures the default serial communication.
*
@@ -326,6 +172,44 @@ private:
*/
bool setCrc(bool waitResponse = true);
+ /**
+ * @brief Write the settings on the non volatile-memory.
+ *
+ * @return True if operation succeeded.
+ */
+ bool writeSettingsCommand();
+
+ /**
+ * @brief Sets the antenna A offset.
+ *
+ * @param antPos antenna position.
+ *
+ * @return True if operation succeeded.
+ */
+ bool setAntennaA(AntennaPosition antPos);
+
+ /**
+ * @brief Sets the compass baseline, position offset of antenna B respect to
+ * antenna A. Uncertainty must be higher than actual measurement error,
+ * possibly twice as the error.
+ * All measures are in meters [m].
+ *
+ * @param antPos antenna position.
+ *
+ * @return True if operation succeeded.
+ */
+ bool setCompassBaseline(AntennaPosition antPos);
+
+ /**
+ * @brief set the reference frame rotation of the sensor in order to have
+ * all the data on the desired reference frame.
+ *
+ * @param rotMat rotation matrix.
+ *
+ * @return if operation succeeded.
+ */
+ bool setReferenceFrame(Eigen::Matrix3f rotMat);
+
/**
* @brief Method implementation of self test.
*
@@ -333,6 +217,15 @@ private:
*/
bool selfTestImpl();
+ QuaternionData sampleQuaternion();
+
+ MagnetometerData sampleMagnetometer();
+
+ AccelerometerData sampleAccelerometer();
+
+ GyroscopeData sampleGyroscope();
+
+ Ins_Lla sampleIns();
/**
* @brief Sends the command to the sensor with the correct checksum added
* so '*' symbol is not needed at the end of the string as well as the '$'
@@ -345,7 +238,7 @@ private:
bool sendStringCommand(std::string command);
/**
- * @brief Receives a command from the VN100 serialInterface->recv() but
+ * @brief Receives a command from the VN300 serialInterface->recv() but
* swaps the first \n with a \0 to close the message.
*
* @param command The char array which will be filled with the command.
@@ -355,6 +248,18 @@ private:
*/
bool recvStringCommand(char *command, int maxLength);
+ /**
+ * @brief check if the VN-300 returned an error and differentiate between
+ * them. The error is formatted as $VNERR,xx*XX
+ *
+ * xx can go from 01 to 12 and 255 and XX is the checksum.
+ *
+ * @param message to be checked.
+ *
+ * @return True if error are present.
+ */
+ bool checkErrorVN(const char *message);
+
/**
* @brief Method to verify the crc validity of a command.
*
@@ -385,14 +290,32 @@ private:
*/
uint16_t calculateChecksum16(uint8_t *message, int length);
- USARTType *portNumber;
- USART::Baudrate baudRate;
+ /**
+ * @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;
- std::string ASYNC_PAUSE_COMMAND = "VNASY,0";
- std::string ASYNC_RESUME_COMMAND = "VNASY,1";
+ AntennaPosition antPosA;
+ AntennaPosition antPosB;
+ Eigen::Matrix3f rotMat;
+
+ /**
+ * @brief IMU pre-elaborated sample string for efficiency reasons.
+ */
+ string *preSampleImuString = nullptr;
+
+ /**
+ * @brief Temperature and pressure pre-elaborated sample string for
+ * efficiency reasons.
+ */
+ string *preSampleINSlla = nullptr;
+
/**
* @brief Pointer to the received string by the sensor. Allocated 1 time
* only (200 bytes).
@@ -404,20 +327,14 @@ 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
*/
mutable miosix::FastMutex mutex;
VN300Data threadSample;
- PrintLogger logger = Logging::getLogger("vn300");
+ PrintLogger logger = Logging::getLogger("VN300");
static const unsigned int recvStringMaxDimension = 200;
};
-} // namespace Boardcore
\ No newline at end of file
+} // namespace Boardcore
diff --git a/src/shared/sensors/VN300/VN300Data.h b/src/shared/sensors/VN300/VN300Data.h
index 3df6e3968..106ee67c6 100644
--- a/src/shared/sensors/VN300/VN300Data.h
+++ b/src/shared/sensors/VN300/VN300Data.h
@@ -40,15 +40,43 @@ struct QuaternionData
};
/**
- * @brief Structure to handle quaternion data
+ * @brief Structure to handle antenna A position units [m]
*/
+struct AntennaPosition
+{
+ float posX;
+ float posY;
+ float posZ;
+ float uncX;
+ float uncY;
+ float uncZ;
+};
+struct Ins_Lla
+{
+ uint64_t insTimestamp;
+ double time_gps;
+ uint16_t week;
+ uint16_t status;
+ float yaw;
+ float pitch;
+ float roll;
+ float latitude;
+ float longitude;
+ float altitude;
+ float nedVelX;
+ float nedVelY;
+ float nedVelZ;
+};
+
+/**
+ * @brief data type class
+ */
struct VN300Data : public QuaternionData,
public MagnetometerData,
public AccelerometerData,
public GyroscopeData,
- public TemperatureData,
- public PressureData
+ public Ins_Lla
{
/**
@@ -56,10 +84,12 @@ 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},
- TemperatureData{0, 0.0}, PressureData{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}
{
}
@@ -71,11 +101,9 @@ struct VN300Data : public QuaternionData,
// cppcheck-suppress passedByValue
// cppcheck-suppress uninitDerivedMemberVar
VN300Data(QuaternionData quat, MagnetometerData magData,
- AccelerometerData accData, GyroscopeData gyro,
- TemperatureData temp, PressureData pres)
+ AccelerometerData accData, GyroscopeData gyro, Ins_Lla ins)
: QuaternionData(quat), MagnetometerData(magData),
- AccelerometerData(accData), GyroscopeData(gyro),
- TemperatureData(temp), PressureData(pres)
+ AccelerometerData(accData), GyroscopeData(gyro), Ins_Lla(ins)
{
}
@@ -85,9 +113,11 @@ struct VN300Data : public QuaternionData,
"magneticFieldX,magneticFieldY,magneticFieldZ,"
"accelerationTimestamp,accelerationX,accelerationY,"
"accelerationZ,angularSpeedTimestamp,angularSpeedX,"
- "angularSpeedY,angularSpeedZ,temperatureTimestamp,"
- "temperature,pressureTimestamp,pressure\n";
+ "angularSpeedY,angularSpeedZ,insTimeStamp,"
+ "time_gps,week,status,yaw,pitch,roll,latitude,"
+ "longitude,altitude,nedVelX,nedVelY,nedVelZ\n";
}
+
void print(std::ostream& os) const
{
os << quatTimestamp << "," << quatX << "," << quatY << "," << quatZ
@@ -96,10 +126,11 @@ struct VN300Data : public QuaternionData,
<< "," << accelerationTimestamp << "," << accelerationX << ","
<< accelerationY << "," << accelerationZ << ","
<< angularSpeedTimestamp << "," << angularSpeedX << ","
- << angularSpeedY << "," << angularSpeedZ << ","
- << temperatureTimestamp << "," << temperature << ","
- << pressureTimestamp << "," << pressure << "\n";
+ << angularSpeedY << "," << angularSpeedZ << "," << insTimestamp
+ << "," << time_gps << "," << week << "," << status << yaw << pitch
+ << roll << latitude << longitude << altitude << nedVelX << nedVelY
+ << nedVelZ << "\n";
}
};
-} // namespace Boardcore
\ No newline at end of file
+} // namespace Boardcore
--
GitLab