diff --git a/src/shared/sensors/Vectornav/VN100/VN100Serial.cpp b/src/shared/sensors/Vectornav/VN100/VN100Serial.cpp
index 70f7eaa3dfd73642c744f4cf72459930fd2f3b3c..2b98abb17df9ca7d372ebc3f4fd3d6f9b17c68fe 100644
--- a/src/shared/sensors/Vectornav/VN100/VN100Serial.cpp
+++ b/src/shared/sensors/Vectornav/VN100/VN100Serial.cpp
@@ -1,5 +1,5 @@
/* Copyright (c) 2021 Skyward Experimental Rocketry
- * Author: Matteo Pignataro
+ * Author: Matteo Pignataro, Fabrizio Monti
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -23,14 +23,13 @@
#include "VN100Serial.h"
#include <drivers/timer/TimestampTimer.h>
-#include <utils/KernelTime.h>
namespace Boardcore
{
-VN100Serial::VN100Serial(USART& usart, int baudRate, CRCOptions crc,
- uint16_t samplePeriod)
- : usart(usart), baudRate(baudRate), samplePeriod(samplePeriod), crc(crc)
+VN100Serial::VN100Serial(USART& usart, int baud, CRCOptions crc,
+ std::chrono::milliseconds timeout)
+ : VNCommonSerial(usart, baud, "vn100-serial", crc, timeout)
{
}
@@ -42,56 +41,58 @@ bool VN100Serial::init()
if (isInit)
{
lastError = SensorErrors::ALREADY_INIT;
- LOG_WARN(logger, "Sensor vn100 already initilized");
+ LOG_WARN(logger, "Sensor vn100 already initialized");
return true;
}
- // Allocate the receive vector
- recvString = new char[recvStringMaxDimension];
-
// 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");
- }
- else
+ switch (this->crc)
{
- preSampleImuString = new string("$VNRRG,15*77\n");
- preSampleTempPressString = new string("$VNRRG,54*72\n");
+ case CRCOptions::CRC_ENABLE_8:
+ askSampleCommand = "$VNBOM,1*45\n";
+ break;
+ case CRCOptions::CRC_ENABLE_16:
+ askSampleCommand = "$VNBOM,1*749D\n";
+ break;
+ case CRCOptions::CRC_NO:
+ askSampleCommand = "$VNBOM,1*XX\n";
+ break;
}
// Set the error to init fail and if the init process goes without problem
// i restore it to the last error
lastError = SensorErrors::INIT_FAIL;
- if (recvString == NULL)
+ configDefaultSerialPort();
+
+ if (!setCrc(false))
{
- LOG_ERR(logger, "Unable to initialize the receive vn100 string");
+ LOG_ERR(logger, "Unable to set the vn100 user selected CRC");
return false;
}
- if (!configDefaultSerialPort())
+ if (!disableAsyncMessages(false))
{
- LOG_ERR(logger, "Unable to config the default vn100 serial port");
+ LOG_ERR(logger, "Unable to disable async messages from vn100");
return false;
}
- if (!setCrc(false))
+ if (!configUserSerialPort())
{
- LOG_ERR(logger, "Unable to set the vn100 user selected CRC");
+ LOG_ERR(logger, "Unable to config the user vn100 serial port");
return false;
}
- if (!disableAsyncMessages(false))
+ if (!verifyModelNumber("VN-100"))
{
- LOG_ERR(logger, "Unable to disable async messages from vn100");
+ LOG_ERR(logger, "Error, model number not corresponding");
+ lastError = INVALID_WHOAMI;
return false;
}
- if (!configUserSerialPort())
+ if (!setBinaryOutput())
{
- LOG_ERR(logger, "Unable to config the user vn100 serial port");
+ LOG_ERR(logger, "Unable to set binary output register");
return false;
}
@@ -118,614 +119,134 @@ bool VN100Serial::init()
return true;
}
-void VN100Serial::run()
-{
- while (!shouldStop())
- {
- long long initialTime = Kernel::getOldTick();
- {
- // Sample the data locking the mutex
- miosix::Lock<FastMutex> l(mutex);
- threadSample = sampleData();
- }
- // Sleep for the sampling period
- Kernel::Thread::sleepUntil(initialTime + samplePeriod);
- }
-}
-
-bool VN100Serial::sampleRaw()
-{
- // Sensor not init
- if (!isInit)
- {
- lastError = SensorErrors::NOT_INIT;
- LOG_WARN(logger,
- "Unable to sample due to not initialized vn100 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 VN100Serial::getLastRawSample()
-{
- // If not init i return the void string
- if (!isInit)
- return string("");
-
- return string(recvString, recvStringLength);
-}
-
-bool VN100Serial::closeAndReset()
-{
- // Sensor not init
- if (!isInit)
- {
- lastError = SensorErrors::NOT_INIT;
- LOG_WARN(logger, "Sensor vn100 already not initilized");
- return true;
- }
-
- // Send the reset command to the vn100
- if (!sendStringCommand("VNRST"))
- {
- LOG_WARN(logger, "Impossible to reset the vn100");
- return false;
- }
-
- isInit = false;
-
- // Free the recvString memory
- delete recvString;
-
- return true;
-}
-
-bool VN100Serial::selfTest()
-{
- if (!selfTestImpl())
- {
- lastError = SensorErrors::SELF_TEST_FAIL;
- LOG_WARN(logger, "Unable to perform a successful vn100 self test");
- return false;
- }
-
- return true;
-}
+bool VN100Serial::selfTest() { return true; }
VN100SerialData VN100Serial::sampleImpl()
{
- miosix::Lock<FastMutex> l(mutex);
- return threadSample;
-}
-
-VN100SerialData VN100Serial::sampleData()
-{
- if (!isInit)
- {
- lastError = SensorErrors::NOT_INIT;
- LOG_WARN(logger,
- "Unable to sample due to not initialized vn100 sensor");
- return lastSample;
- }
-
- // Before sampling i check for errors
- if (lastError != SensorErrors::NO_ERRORS)
- return lastSample;
+ VN100SerialData data;
+ BinaryData binData;
- // Returns Quaternion, Magnetometer, Accelerometer and Gyro
- usart.writeString(preSampleImuString->c_str());
+ const uint64_t timestamp = TimestampTimer::getTimestamp();
- // Wait some time
- // TODO dimension the time
- miosix::Thread::sleep(1);
+ bool sampleOutcome =
+ false; // True if a valid sample was retrieved from the sensor
- 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
- return lastSample;
- }
+ sampleOutcome = getBinaryOutput<BinaryData>(binData, askSampleCommand);
+ if (!sampleOutcome)
+ lastError = NO_NEW_DATA;
- // Now i have to parse the data
- QuaternionData quat = sampleQuaternion();
- MagnetometerData mag = sampleMagnetometer();
- AccelerometerData acc = sampleAccelerometer();
- GyroscopeData gyro = sampleGyroscope();
+ // With binary output the checksum is always calculated with checksum16
+ bool validChecksum =
+ (crc == CRCOptions::CRC_NO) ||
+ (calculateChecksum16(reinterpret_cast<uint8_t*>(&binData),
+ sizeof(binData)) == 0);
- // Returns Magnetometer, Accelerometer, Gyroscope, Temperature and Pressure
- // (UNCOMPENSATED) DO NOT USE THESE MAGNETOMETER, ACCELEROMETER AND
- // GYROSCOPE VALUES
- usart.writeString(preSampleTempPressString->c_str());
+ if (sampleOutcome && !validChecksum)
+ lastError = SensorErrors::BUS_FAULT;
- // Wait some time
- // TODO dimension the time
- miosix::Thread::sleep(1);
+ // Verify if the sample is valid
+ sampleOutcome = sampleOutcome && validChecksum;
- if (!recvStringCommand(recvString, recvStringMaxDimension))
+ if (sampleOutcome)
{
- // If something goes wrong i return the last sampled data
- return lastSample;
+ buildBinaryData(binData, data, timestamp);
+ return data;
}
-
- if (!verifyChecksum(recvString, recvStringLength))
+ else
{
- LOG_WARN(logger, "Vn100 sampling message invalid checksum");
- // If something goes wrong i return the last sampled data
+ // Last error is already set
return lastSample;
}
-
- // Parse the data
- TemperatureData temp = sampleTemperature();
- PressureData press = samplePressure();
-
- return VN100SerialData(quat, mag, acc, gyro, temp, press);
}
-bool VN100Serial::disableAsyncMessages(bool waitResponse)
+void VN100Serial::buildBinaryData(const BinaryData& binData,
+ VN100SerialData& data,
+ const uint64_t sampleTimestamp)
{
- // Command string
- std::string command =
- "VNWRG,06,00"; // Put 0 in register number 6 (ASYNC Config)
-
- // Send the command
- if (!sendStringCommand(command))
- return false;
-
- // Read the answer
- if (waitResponse)
- recvStringCommand(recvString, recvStringMaxDimension);
-
- return true;
-}
-
-bool VN100Serial::configDefaultSerialPort()
-{
- // Initial default settings
- usart.setBaudrate(115200);
-
- // Check correct serial init
- return true;
-}
-
-/**
- * Even if the user configured baudrate is the default, I want to reset the
- * buffer to clean the junk.
- */
-bool VN100Serial::configUserSerialPort()
-{
- std::string command;
-
- // I format the command to change baud rate
- command = fmt::format("{}{}", "VNWRG,5,", baudRate);
-
- // I can send the command
- if (!sendStringCommand(command))
- return false;
-
- // I can open the serial with user's baud rate
- usart.setBaudrate(baudRate);
-
- // Check correct serial init
- return true;
-}
-
-bool VN100Serial::setCrc(bool waitResponse)
-{
- // Command for the crc change
- std::string command;
- CRCOptions backup = crc;
-
- // Check what type of crc is selected
- if (crc == CRCOptions::CRC_ENABLE_16)
- {
- // The 3 inside the command is the 16bit select. The others are default
- // values
- 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 = "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
- // of crc is previously selected. So in order to get the command accepted
- // i need to do it two times with different crc.
- crc = CRCOptions::CRC_ENABLE_8;
+ // Acceleration data
+ data.accelerationTimestamp = sampleTimestamp;
+ data.accelerationX = binData.accelerationX;
+ data.accelerationY = binData.accelerationY;
+ data.accelerationZ = binData.accelerationZ;
- // Send the command
- if (!sendStringCommand(command))
- return false;
+ // Angular speed data
+ data.angularSpeedTimestamp = sampleTimestamp;
+ data.angularSpeedX = binData.angularX;
+ data.angularSpeedY = binData.angularY;
+ data.angularSpeedZ = binData.angularZ;
- // Read the answer
- if (waitResponse)
- recvStringCommand(recvString, recvStringMaxDimension);
+ // Magnetometer data
+ data.magneticFieldTimestamp = sampleTimestamp;
+ data.magneticFieldX = binData.magneticFieldX;
+ data.magneticFieldY = binData.magneticFieldY;
+ data.magneticFieldZ = binData.magneticFieldZ;
- crc = CRCOptions::CRC_ENABLE_16;
+ // Quaternion data
+ data.quaternionTimestamp = sampleTimestamp;
+ data.quaternionX = binData.quaternionX;
+ data.quaternionY = binData.quaternionY;
+ data.quaternionZ = binData.quaternionZ;
+ data.quaternionW = binData.quaternionW;
- // Send the command
- if (!sendStringCommand(command))
- return false;
+ // Temperature data
+ data.temperatureTimestamp = sampleTimestamp;
+ data.temperature = binData.temperature;
- // Read the answer
- if (waitResponse)
- recvStringCommand(recvString, recvStringMaxDimension);
-
- // Restore the crc
- crc = backup;
-
- return true;
+ // Pressure data
+ data.pressureTimestamp = sampleTimestamp;
+ data.pressure = binData.pressure;
}
-bool VN100Serial::selfTestImpl()
+bool VN100Serial::setBinaryOutput()
{
- char modelNumber[] = "VN-100";
- const int modelNumberOffset = 10;
+ /**
+ * This command samples quaternion data, accelerometer, angular rate,
+ * magnetometer, temperature, and pressure (with asynchronous messages
+ * disabled).
+ *
+ * Refer to the datasheet to modify the sampled data. Note that
+ * changing the sampled data also requires updating VN100SerialData
+ * and BinaryData.
+ */
- // Check the init status
- if (!isInit)
+ const char* setBinarySampleCommand = "";
+ switch (this->crc)
{
- lastError = SensorErrors::NOT_INIT;
- LOG_WARN(
- logger,
- "Unable to perform vn100 self test due to not initialized sensor");
- return false;
+ case CRCOptions::CRC_ENABLE_8:
+ setBinarySampleCommand = "$VNWRG,75,0,16,01,0530*44\n";
+ break;
+ case CRCOptions::CRC_ENABLE_16:
+ setBinarySampleCommand = "$VNWRG,75,0,16,01,0530*9CFA\n";
+ break;
+ case CRCOptions::CRC_NO:
+ setBinarySampleCommand = "$VNWRG,75,0,16,01,0530*XX\n";
+ break;
}
- // removing junk
usart.clearQueue();
- // I check the model number
- if (!sendStringCommand("VNRRG,01"))
+ if (!sendStringCommand(setBinarySampleCommand))
{
- 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");
+ LOG_WARN(logger,
+ "sendStringCommand() failed, cannot set binary output");
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)
+ if (!recvStringCommand(recvString.data(), recvStringMaxDimension))
{
- LOG_ERR(logger, "VN-100 not corresponding: {} != {}", recvString,
- modelNumber);
+ LOG_WARN(logger,
+ "recvStringCommand() failed, cannot set binary output");
return false;
}
- // I check the checksum
- if (!verifyChecksum(recvString, recvStringLength))
+ if (checkErrorVN(recvString.data()) != 0)
{
- LOG_ERR(logger, "Checksum verification failed: {}", recvString);
+ LOG_WARN(logger, "Error while setting binary output: {}",
+ recvString.data());
return false;
}
return true;
}
-QuaternionData VN100Serial::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.quaternionTimestamp = TimestampTimer::getTimestamp();
- data.quaternionX = strtod(recvString + indexStart + 1, &nextNumber);
- data.quaternionY = strtod(nextNumber + 1, &nextNumber);
- data.quaternionZ = strtod(nextNumber + 1, &nextNumber);
- data.quaternionW = strtod(nextNumber + 1, NULL);
-
- return data;
-}
-
-MagnetometerData VN100Serial::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 VN100Serial::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 VN100Serial::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;
-}
-
-TemperatureData VN100Serial::sampleTemperature()
-{
- unsigned int indexStart = 0;
- TemperatureData 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 < 11; i++)
- {
- while (indexStart < recvStringLength && recvString[indexStart] != ',')
- indexStart++;
- indexStart++;
- }
-
- // Parse the data
- data.temperatureTimestamp = TimestampTimer::getTimestamp();
- data.temperature = strtod(recvString + indexStart + 1, NULL);
-
- return data;
-}
-
-PressureData VN100Serial::samplePressure()
-{
- unsigned int indexStart = 0;
- PressureData 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.pressureTimestamp = TimestampTimer::getTimestamp();
- data.pressure = strtod(recvString + indexStart + 1, NULL);
-
- return data;
-}
-
-bool VN100Serial::sendStringCommand(std::string command)
-{
- if (crc == CRCOptions::CRC_ENABLE_8)
- {
- char checksum[4]; // 2 hex + \n + \0
- // I convert the calculated checksum in hex using itoa
- itoa(calculateChecksum8((uint8_t*)command.c_str(), command.length()),
- checksum, 16);
- checksum[2] = '\n';
- checksum[3] = '\0';
- // I concatenate
- command = fmt::format("{}{}{}{}", "$", command, "*", checksum);
- }
- else if (crc == CRCOptions::CRC_ENABLE_16)
- {
- char checksum[6]; // 4 hex + \n + \0
- // I convert the calculated checksum in hex using itoa
- itoa(calculateChecksum16((uint8_t*)command.c_str(), command.length()),
- checksum, 16);
- checksum[4] = '\n';
- checksum[5] = '\0';
- // I concatenate
- command = fmt::format("{}{}{}{}", "$", command, "*", checksum);
- }
- else
- {
- // No checksum, i add only 'XX' at the end and not 'XXXX' because
- // in cas of CRC_NO the enabled crc is 8 bit
- command = fmt::format("{}{}{}", "$", command, "*XX\n");
- }
-
- // I send the final command
- usart.writeString(command.c_str());
-
- // Wait some time
- // TODO dimension the time
- miosix::Thread::sleep(500);
-
- return true;
-}
-
-bool VN100Serial::recvStringCommand(char* command, int maxLength)
-{
- int i = 0;
- // Read the buffer
- if (!usart.readBlocking(command, maxLength))
- return false;
-
- // 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 VN100Serial::verifyChecksum(char* command, int length)
-{
- int checksumOffset = 0;
-
- // I look for the checksum position
- while (checksumOffset < length && command[checksumOffset] != '*')
- checksumOffset++;
-
- if (checksumOffset == length)
- {
- // The command doesn't have any checksum
- TRACE("No checksum in the command!\n");
- return false;
- }
-
- // Check based on the user selected crc type
- if (crc == CRCOptions::CRC_ENABLE_16)
- {
- if (length != checksumOffset + 5) // 4 hex chars + 1 of position
- {
- TRACE("16 bit Checksum wrong length: %d != %d --> %s\n", length,
- checksumOffset + 5, command);
- return false;
- }
-
- // Calculate the checksum and verify (comparison between numerical
- // checksum to avoid string bugs e.g 0856 != 865)
- if (strtol(command + checksumOffset + 1, NULL, 16) !=
- calculateChecksum16((uint8_t*)(command + 1), checksumOffset - 1))
- {
- TRACE("Different checksum: %s\n", command);
- return false;
- }
- }
- else if (crc == CRCOptions::CRC_ENABLE_8)
- {
- if (length != checksumOffset + 3) // 2 hex chars + 1 of position
- {
- TRACE("8 bit Checksum wrong length: %d != %d --> %s\n", length,
- checksumOffset + 3, command);
- return false;
- }
-
- // Calculate the checksum and verify (comparison between numerical
- // checksum to avoid string bugs e.g 0856 != 865)
- if (strtol(command + checksumOffset + 1, NULL, 16) !=
- calculateChecksum8((uint8_t*)(command + 1), checksumOffset - 1))
- {
- TRACE("Different checksum: %s\n", command);
- return false;
- }
- }
-
- return true;
-}
-
-uint8_t VN100Serial::calculateChecksum8(uint8_t* message, int length)
-{
- int i;
- uint8_t result = 0x00;
-
- // Iterate and XOR all of the elements
- for (i = 0; i < length; i++)
- {
- //^ = XOR Operation
- result ^= message[i];
- }
-
- return result;
-}
-
-uint16_t VN100Serial::calculateChecksum16(uint8_t* message, int length)
-{
- int i;
- uint16_t result = 0x0000;
-
- // Apply the datasheet definition of CRC16-CCITT
- for (i = 0; i < length; i++)
- {
- result = (uint8_t)(result >> 8) | (result << 8);
- result ^= message[i];
- result ^= (uint8_t)(result & 0xff) >> 4;
- result ^= result << 12;
- result ^= (result & 0x00ff) << 5;
- }
-
- return result;
-}
-
} // namespace Boardcore
diff --git a/src/shared/sensors/Vectornav/VN100/VN100Serial.h b/src/shared/sensors/Vectornav/VN100/VN100Serial.h
index 0c1702cf3cff508a9c72c0e1b0478a663b7ceb51..83587ea16c3cd60d73b9fa482a24ad15bd90d183 100644
--- a/src/shared/sensors/Vectornav/VN100/VN100Serial.h
+++ b/src/shared/sensors/Vectornav/VN100/VN100Serial.h
@@ -1,5 +1,5 @@
/* Copyright (c) 2021 Skyward Experimental Rocketry
- * Author: Matteo Pignataro
+ * Author: Matteo Pignataro, Fabrizio Monti
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -25,41 +25,22 @@
/**
* @brief Driver for the VN100S IMU.
*
- * The VN100S 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 VN100S chip
- * is capable also of SPI communication.
+ * The VN100 sensor is a calibrated IMU which includes accelerometer,
+ * magnetometer, gyroscope, barometer and temperature sensor. It also provides
+ * attitude data (yaw, pith, roll, quaternion). The device provides also a
+ * calibration matrix and an anti-drift matrix for the gyroscope values.
*
- * The VN100S 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
- * constructor method. The serial communication also can be established with
- * various baud rates:
- * - 9600
- * - 19200
- * - 38400
- * - 57600
- * - 115200
- * - 128000
- * - 230400
- * - 460800
- * - 921600
+ * This driver samples IMU compensated data (accelerometer, gyroscope and
+ * magnetometer), quaternion data, temperature and pressure data.
+ * The sampling rate is 400Hz.
+ *
+ * This driver only supports binary encoding for communication.
*/
-#include <ActiveObject.h>
-#include <diagnostic/PrintLogger.h>
-#include <fmt/format.h>
#include <sensors/Sensor.h>
-#include <string.h>
-#include <utils/Debug.h>
+#include <sensors/Vectornav/VNCommonSerial.h>
#include "VN100SerialData.h"
-#include "drivers/usart/USART.h"
namespace Boardcore
{
@@ -67,53 +48,24 @@ namespace Boardcore
/**
* @brief Driver class for VN100 IMU.
*/
-class VN100Serial : public Sensor<VN100SerialData>, public ActiveObject
+class VN100Serial : public Sensor<VN100SerialData>, public VNCommonSerial
{
public:
- enum class CRCOptions : uint8_t
- {
- CRC_NO = 0x00,
- CRC_ENABLE_8 = 0x08,
- CRC_ENABLE_16 = 0x10
- };
-
/**
* @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 crc Checksum option.
+ * @param timeout The maximum time that will be waited when reading from the
+ * sensor.
*/
- VN100Serial(USART& usart, int baudrate,
- CRCOptions crc = CRCOptions::CRC_ENABLE_8,
- uint16_t samplePeriod = 20);
+ VN100Serial(USART& usart, int baudrate, CRCOptions crc,
+ std::chrono::milliseconds timeout);
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.
- *
- * @return True if operation succeeded.
- */
- bool closeAndReset();
-
bool selfTest() override;
protected:
@@ -124,162 +76,51 @@ protected:
private:
/**
- * @brief Active object method, about the thread execution
- */
- void run() override;
-
- /**
- * @brief Sampling method used by the thread
- *
- * @return VN100Data The sampled data
- */
- VN100SerialData sampleData();
-
- /**
- * @brief Disables the async messages that the vn100 is default configured
- * to send at 40Hz on startup.
- *
- * @param waitResponse If true wait for a serial response.
- *
- * @return True if operation succeeded.
- */
- 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();
-
- /**
- * @brief Sets the user selected crc method.
- *
- * @param waitResponse If true wait for a serial response.
- *
- * @return True if operation succeeded.
- */
- bool setCrc(bool waitResponse = true);
-
- /**
- * @brief Method implementation of self test.
- *
- * @return True if operation succeeded.
- */
- bool selfTestImpl();
-
- QuaternionData sampleQuaternion();
-
- MagnetometerData sampleMagnetometer();
-
- AccelerometerData sampleAccelerometer();
-
- GyroscopeData sampleGyroscope();
-
- TemperatureData sampleTemperature();
-
- PressureData samplePressure();
-
- /**
- * @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 '$'
- * at the beginning of the command.
- *
- * @param command Command to send.
- *
- * @return True if operation succeeded.
+ * @brief Struct used to store the binary data received from the sensor.
*/
- bool sendStringCommand(std::string command);
+ struct __attribute__((packed)) BinaryData
+ {
+ uint8_t group;
+ uint16_t group1;
+ float quaternionX;
+ float quaternionY;
+ float quaternionZ;
+ float quaternionW;
+ float angularX;
+ float angularY;
+ float angularZ;
+ float accelerationX;
+ float accelerationY;
+ float accelerationZ;
+ float magneticFieldX;
+ float magneticFieldY;
+ float magneticFieldZ;
+ float temperature;
+ float pressure;
+ uint16_t crc;
+ };
/**
- * @brief Receives a command from the VN100 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.
- * @param maxLength Maximum length for the command array.
+ * @brief Build output data packet starting from raw binary data received
+ * from the sensor.
*
- * @return True if operation succeeded.
+ * @param rawData The raw data received from the sensor.
+ * @param data The structure that will contain the output.
+ * @param timestamp The timestamp of the extracted data.
*/
- bool recvStringCommand(char* command, int maxLength);
+ void buildBinaryData(const BinaryData& binData, VN100SerialData& data,
+ const uint64_t sampleTimestamp);
/**
- * @brief Method to verify the crc validity of a command.
- *
- * @param command The char array which contains the command.
- * @param maxLength Maximum length for the command array.
+ * @brief Set the binary output register.
*
* @return True if operation succeeded.
*/
- bool verifyChecksum(char* command, int maxLength);
+ bool setBinaryOutput();
/**
- * @brief Calculate the 8bit checksum on the given array.
- *
- * @param command Command on which compute the crc.
- * @param length Array length.
- *
- * @return The 8 bit checksum.
+ * @brief Pre computed command used to ask a binary sample to the sensor.
*/
- uint8_t calculateChecksum8(uint8_t* message, int length);
-
- /**
- * @brief Calculate the 16bit array on the given array.
- *
- * @param command Command on which compute the crc.
- * @param length Array length.
- *
- * @return The 16 bit CRC16-CCITT error check.
- */
- uint16_t calculateChecksum16(uint8_t* message, int length);
-
- /**
- * @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;
-
- /**
- * @brief IMU pre-elaborated sample string for efficiency reasons.
- */
- string* preSampleImuString = nullptr;
-
- /**
- * @brief Temperature and pressure pre-elaborated sample string for
- * efficiency reasons.
- */
- string* preSampleTempPressString = nullptr;
-
- /**
- * @brief Pointer to the received string by the sensor. Allocated 1 time
- * only (200 bytes).
- */
- char* recvString = nullptr;
-
- /**
- * @brief Actual strlen() of the recvString.
- */
- unsigned int recvStringLength = 0;
-
- /**
- * @brief Mutex to synchronize the reading and writing of the threadSample
- */
- mutable miosix::FastMutex mutex;
- VN100SerialData threadSample;
-
- PrintLogger logger = Logging::getLogger("vn100-serial");
-
- static const unsigned int recvStringMaxDimension = 200;
+ const char* askSampleCommand = "";
};
} // namespace Boardcore
diff --git a/src/shared/sensors/Vectornav/VN100/VN100SerialData.h b/src/shared/sensors/Vectornav/VN100/VN100SerialData.h
index dc1fa4dbf87112c38342fa6af17ba426762dc78a..91050d0d835a46863b45267b2b211fc01508148d 100644
--- a/src/shared/sensors/Vectornav/VN100/VN100SerialData.h
+++ b/src/shared/sensors/Vectornav/VN100/VN100SerialData.h
@@ -1,5 +1,5 @@
/* Copyright (c) 2021 Skyward Experimental Rocketry
- * Author: Matteo Pignataro
+ * Author: Matteo Pignataro, Fabrizio Monti
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
diff --git a/src/shared/sensors/Vectornav/VNCommonSerial.cpp b/src/shared/sensors/Vectornav/VNCommonSerial.cpp
index d271510142d11d5d16d9f167e8343c9814d7ab79..d5696d9c2b4e2d99177fa178f9554e1119ab678c 100644
--- a/src/shared/sensors/Vectornav/VNCommonSerial.cpp
+++ b/src/shared/sensors/Vectornav/VNCommonSerial.cpp
@@ -267,110 +267,6 @@ bool VNCommonSerial::verifyModelNumber(const char* expectedModelNumber)
return true;
}
-QuaternionData VNCommonSerial::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.quaternionTimestamp = TimestampTimer::getTimestamp();
- data.quaternionX = strtod(recvString.data() + indexStart + 1, &nextNumber);
- data.quaternionY = strtod(nextNumber + 1, &nextNumber);
- data.quaternionZ = strtod(nextNumber + 1, &nextNumber);
- data.quaternionW = strtod(nextNumber + 1, NULL);
-
- return data;
-}
-
-MagnetometerData VNCommonSerial::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.data() + indexStart + 1, &nextNumber);
- data.magneticFieldY = strtod(nextNumber + 1, &nextNumber);
- data.magneticFieldZ = strtod(nextNumber + 1, NULL);
-
- return data;
-}
-
-AccelerometerData VNCommonSerial::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.data() + indexStart + 1, &nextNumber);
- data.accelerationY = strtod(nextNumber + 1, &nextNumber);
- data.accelerationZ = strtod(nextNumber + 1, NULL);
-
- return data;
-}
-
-GyroscopeData VNCommonSerial::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.data() + indexStart + 1, &nextNumber);
- data.angularSpeedY = strtod(nextNumber + 1, &nextNumber);
- data.angularSpeedZ = strtod(nextNumber + 1, NULL);
-
- return data;
-}
-
uint8_t VNCommonSerial::checkErrorVN(const char* message)
{
if (strncmp(message, "$VNERR,", 7) == 0)
diff --git a/src/shared/sensors/Vectornav/VNCommonSerial.h b/src/shared/sensors/Vectornav/VNCommonSerial.h
index 6fa6c14f2f1a46956b0470560e1073938eabf0ba..53f807e93246f3c0cb500b6510334070af4e8149 100644
--- a/src/shared/sensors/Vectornav/VNCommonSerial.h
+++ b/src/shared/sensors/Vectornav/VNCommonSerial.h
@@ -222,30 +222,6 @@ protected:
template <typename T>
bool getBinaryOutput(T& binaryData, const char* const sampleCommand);
- /**
- * @brief Utility function used to extract quaternion data from the
- * receiving string.
- */
- QuaternionData sampleQuaternion();
-
- /**
- * @brief Utility function used to extract magnetometer data from the
- * receiving string.
- */
- MagnetometerData sampleMagnetometer();
-
- /**
- * @brief Utility function used to extract accelerometer data from the
- * receiving string.
- */
- AccelerometerData sampleAccelerometer();
-
- /**
- * @brief Utility function used to extract gyroscope data from the receiving
- * string.
- */
- GyroscopeData sampleGyroscope();
-
/**
* @brief Check if the message received from the sensor contains an error.
*
diff --git a/src/tests/sensors/test-vn100-serial.cpp b/src/tests/sensors/test-vn100-serial.cpp
index 9d11d0f125ad99680e0adb1b6713cb6ba3e96e8c..c94541399ca5ba554aba5ac399cec8fbd4bbbf9a 100644
--- a/src/tests/sensors/test-vn100-serial.cpp
+++ b/src/tests/sensors/test-vn100-serial.cpp
@@ -1,5 +1,5 @@
/* Copyright (c) 2021 Skyward Experimental Rocketry
- * Author: Matteo Pignataro
+ * Author: Matteo Pignataro, Fabrizio Monti
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -20,7 +20,6 @@
* THE SOFTWARE.
*/
-#include <drivers/timer/TimestampTimer.h>
#include <inttypes.h>
#include <sensors/Vectornav/VN100/VN100Serial.h>
@@ -30,7 +29,6 @@ using namespace Boardcore;
int main()
{
VN100SerialData sample;
- string sampleRaw;
GpioPin u2tx1(GPIOA_BASE, 2);
GpioPin u2rx1(GPIOA_BASE, 3);
@@ -41,7 +39,8 @@ int main()
u2tx1.mode(Mode::ALTERNATE);
USART usart(USART2, 115200);
- VN100Serial sensor{usart, 115200, VN100Serial::CRCOptions::CRC_ENABLE_16};
+ VN100Serial sensor{usart, 115200, VNCommonSerial::CRCOptions::CRC_ENABLE_16,
+ std::chrono::seconds(5)};
// Let the sensor start up
Thread::sleep(1000);
@@ -60,15 +59,6 @@ int main()
return 0;
}
- if (!sensor.start())
- {
- printf("Unable to start the sampling thread\n");
- return 0;
- }
-
- printf("Sensor sampling thread started!\n");
-
- // Sample and print 100 samples
for (int i = 0; i < 100; i++)
{
sensor.sample();
@@ -78,12 +68,14 @@ int main()
sample.accelerationY, sample.accelerationZ);
printf("ang: %.3f, %.3f, %.3f\n", sample.angularSpeedX,
sample.angularSpeedY, sample.angularSpeedZ);
+ printf("mag: %.3f, %.3f, %.3f\n", sample.magneticFieldX,
+ sample.magneticFieldY, sample.magneticFieldZ);
+ printf("quat: %.3f, %.3f, %.3f, %.3f\n", sample.quaternionX,
+ sample.quaternionY, sample.quaternionZ, sample.quaternionW);
+ printf("temp: %.3f\n", sample.temperature);
+ printf("press: %.3f\n\n", sample.pressure);
- sensor.sampleRaw();
- sampleRaw = sensor.getLastRawSample();
- printf("%s\n", sampleRaw.c_str());
- // Thread::sleep(100);
- printf("\n");
+ Thread::sleep(500);
}
sensor.closeAndReset();