diff --git a/src/shared/sensors/Vectornav/VN300/VN300.cpp b/src/shared/sensors/Vectornav/VN300/VN300.cpp
index fe1d007b6cec4e6a2292d6fb19200bfda607b7f1..d5243bced4c1d5de2f9fa2daf0c5d03c7b969272 100644
--- a/src/shared/sensors/Vectornav/VN300/VN300.cpp
+++ b/src/shared/sensors/Vectornav/VN300/VN300.cpp
@@ -318,10 +318,8 @@ void VN300::buildBinaryDataReduced(const VN300Defs::BinaryDataReduced& rawData,
bool VN300::setAntennaA(VN300Defs::AntennaPosition antPos)
{
- std::string command;
-
- command = fmt::format("{}{},{},{}", "VNWRG,57,", antPos.posX, antPos.posY,
- antPos.posZ);
+ std::string command = fmt::format("{}{},{},{}", "VNWRG,57,", antPos.posX,
+ antPos.posY, antPos.posZ);
usart.clearQueue();
if (!sendStringCommand(command))
@@ -338,11 +336,9 @@ bool VN300::setAntennaA(VN300Defs::AntennaPosition antPos)
bool VN300::setCompassBaseline(VN300Defs::AntennaPosition antPos)
{
- std::string command;
-
- command = fmt::format("{}{},{},{},{},{},{}", "VNWRG,93,", antPos.posX,
- antPos.posY, antPos.posZ, antPos.uncX, antPos.uncY,
- antPos.uncZ);
+ std::string command = fmt::format("{}{},{},{},{},{},{}", "VNWRG,93,",
+ antPos.posX, antPos.posY, antPos.posZ,
+ antPos.uncX, antPos.uncY, antPos.uncZ);
usart.clearQueue();
if (!sendStringCommand(command))
diff --git a/src/shared/sensors/Vectornav/VNCommonSerial.cpp b/src/shared/sensors/Vectornav/VNCommonSerial.cpp
index ad1a8cdd2ab70b9ca63c06ba988351b8e8bdb18b..d271510142d11d5d16d9f167e8343c9814d7ab79 100644
--- a/src/shared/sensors/Vectornav/VNCommonSerial.cpp
+++ b/src/shared/sensors/Vectornav/VNCommonSerial.cpp
@@ -214,10 +214,8 @@ void VNCommonSerial::configDefaultSerialPort()
bool VNCommonSerial::configUserSerialPort()
{
- std::string command;
-
// I format the command to change baud rate
- command = fmt::format("{}{}", "VNWRG,5,", baudRate);
+ std::string command = fmt::format("{}{}", "VNWRG,5,", baudRate);
// I can send the command
if (!sendStringCommand(command))
@@ -399,6 +397,173 @@ bool VNCommonSerial::closeAndReset()
return true;
}
+bool VNCommonSerial::startHSIEstimator(uint8_t convergeRate)
+{
+ D(assert((convergeRate >= 1 && convergeRate <= 5) &&
+ "convergeRate must be between 1 and 5"));
+
+ // Select HSI_RUN, runs the real-time hard/soft
+ // iron calibration.
+ const uint8_t hsiMode = 1;
+
+ // Select USE_ONBOARD, Onboard HSI is applied to
+ // the magnetic measurements.
+ const uint8_t hsiOutput = 3;
+
+ std::string command =
+ fmt::format("VNWRG,44,{},{},{}", hsiMode, hsiOutput, convergeRate);
+
+ return writeRegister(command);
+}
+
+bool VNCommonSerial::stopHSIEstimator()
+{
+ // Select HSI_OFF, real-time hard/soft iron calibration
+ // algorithm is turned off.
+ const uint8_t hsiMode = 0;
+
+ // Keep USE_ONBOARD, Onboard HSI is applied to
+ // the magnetic measurements.
+ const uint8_t hsiOutput = 3;
+
+ // Not influent, we are stopping the algorithm
+ const uint8_t convergeRate = 5;
+
+ std::string command =
+ fmt::format("VNWRG,44,{},{},{}", hsiMode, hsiOutput, convergeRate);
+
+ return writeRegister(command);
+}
+
+std::string VNCommonSerial::getHSIEstimatorValues()
+{
+ // Clear the receiving queue
+ usart.clearQueue();
+
+ if (!sendStringCommand("VNRRG,47"))
+ {
+ LOG_ERR(logger, "getHSIEstimatorValues: unable to send string command");
+ return "";
+ }
+
+ miosix::Thread::sleep(100);
+
+ if (!recvStringCommand(recvString.data(), recvStringMaxDimension))
+ {
+ LOG_WARN(logger,
+ "getHSIEstimatorValues: unable to receive string command");
+ return "";
+ }
+
+ if (!verifyChecksum(recvString.data(), recvStringLength))
+ {
+ LOG_ERR(logger,
+ "getHSIEstimatorValues: checksum verification failed: {}",
+ recvString.data());
+ return "";
+ }
+
+ return recvString.data();
+}
+
+bool VNCommonSerial::setMagnetometerCompensation(const Eigen::Matrix3f& c,
+ const Eigen::Vector3f& b)
+{
+ std::string command =
+ fmt::format("VNWRG,23,{},{},{},{},{},{},{},{},{},{},{},{}", c(0, 0),
+ c(0, 1), c(0, 2), c(1, 0), c(1, 1), c(1, 2), c(2, 0),
+ c(2, 1), c(2, 2), b(0), b(1), b(2));
+
+ return writeRegister(command);
+}
+
+bool VNCommonSerial::setAccelerometerCompensation(const Eigen::Matrix3f& c,
+ const Eigen::Vector3f& b)
+{
+ std::string command =
+ fmt::format("VNWRG,25,{},{},{},{},{},{},{},{},{},{},{},{}", c(0, 0),
+ c(0, 1), c(0, 2), c(1, 0), c(1, 1), c(1, 2), c(2, 0),
+ c(2, 1), c(2, 2), b(0), b(1), b(2));
+
+ return writeRegister(command);
+}
+
+bool VNCommonSerial::setGyroscopeCompensation(const Eigen::Matrix3f& c,
+ const Eigen::Vector3f& b)
+{
+ std::string command =
+ fmt::format("VNWRG,84,{},{},{},{},{},{},{},{},{},{},{},{}", c(0, 0),
+ c(0, 1), c(0, 2), c(1, 0), c(1, 1), c(1, 2), c(2, 0),
+ c(2, 1), c(2, 2), b(0), b(1), b(2));
+
+ return writeRegister(command);
+}
+
+bool VNCommonSerial::saveConfiguration()
+{
+ // Clear the receiving queue
+ usart.clearQueue();
+
+ if (!sendStringCommand("VNWNV"))
+ {
+ LOG_ERR(logger, "saveConfiguration: unable to send string command");
+ return false;
+ }
+
+ // The write settings command takes ~500ms to complete
+ miosix::Thread::sleep(500);
+
+ if (!recvStringCommand(recvString.data(), recvStringMaxDimension))
+ {
+ LOG_WARN(logger, "saveConfiguration: unable to receive string command");
+ return false;
+ }
+
+ if (!verifyChecksum(recvString.data(), recvStringLength))
+ {
+ LOG_ERR(logger, "saveConfiguration: checksum verification failed: {}",
+ recvString.data());
+ return false;
+ }
+
+ return true;
+}
+
+bool VNCommonSerial::restoreFactorySettings()
+{
+ // Clear the receiving queue
+ usart.clearQueue();
+
+ if (!sendStringCommand("VNRFS"))
+ {
+ LOG_ERR(logger,
+ "restoreFactorySettings: unable to send string command");
+ return false;
+ }
+
+ miosix::Thread::sleep(100);
+
+ if (!recvStringCommand(recvString.data(), recvStringMaxDimension))
+ {
+ LOG_WARN(logger,
+ "restoreFactorySettings: unable to receive string command");
+ return false;
+ }
+
+ if (!verifyChecksum(recvString.data(), recvStringLength))
+ {
+ LOG_ERR(logger,
+ "restoreFactorySettings: checksum verification failed: {}",
+ recvString.data());
+ return false;
+ }
+
+ // Everything is fine, let the sensor restart
+ miosix::Thread::sleep(2000);
+
+ return true;
+}
+
bool VNCommonSerial::sendStringCommand(std::string command)
{
if (crc == CRCOptions::CRC_ENABLE_8)
@@ -470,4 +635,28 @@ bool VNCommonSerial::recvStringCommand(char* command, int maxLength)
return true;
}
+bool VNCommonSerial::writeRegister(const std::string& command)
+{
+ usart.clearQueue();
+ if (!sendStringCommand(command))
+ {
+ LOG_ERR(logger, "Write register failed: could not send the command");
+ return false;
+ }
+
+ if (!recvStringCommand(recvString.data(), recvStringMaxDimension))
+ {
+ LOG_ERR(logger, "Write register failed: recvStringCommand() failed");
+ return false;
+ }
+
+ if (checkErrorVN(recvString.data()))
+ {
+ LOG_ERR(logger, "Write register failed: {}", recvString.data());
+ return false;
+ }
+
+ return true;
+}
+
} // namespace Boardcore
diff --git a/src/shared/sensors/Vectornav/VNCommonSerial.h b/src/shared/sensors/Vectornav/VNCommonSerial.h
index 66ab3a6ac685818df5256ff7536f35e57f5421b8..6fa6c14f2f1a46956b0470560e1073938eabf0ba 100644
--- a/src/shared/sensors/Vectornav/VNCommonSerial.h
+++ b/src/shared/sensors/Vectornav/VNCommonSerial.h
@@ -62,6 +62,83 @@ public:
*/
bool closeAndReset();
+ /**
+ * @brief Start the real-time hard/soft iron calibration. The algorithm
+ * will continue until stopHSIEstimator() is called.
+ *
+ * @param convergeRate Controls how quickly the hard/soft iron solution
+ * is allowed to converge onto a new solution.
+ * Only values between 1 and 5 are allowed.
+ * 1 = Solution converges slowly over approximately 60-90 seconds.
+ * 5 = Solution converges rapidly over approximately 15-20 seconds.
+ *
+ * @return True if the algorithm started successfully.
+ */
+ bool startHSIEstimator(uint8_t convergeRate);
+
+ /**
+ * @brief Real-time hard/soft iron calibration algorithm is turned off.
+ *
+ * @return True if the command is accepted by the sensor.
+ */
+ bool stopHSIEstimator();
+
+ /**
+ * @return The raw string containing the estimated hard and soft
+ * iron compensation parameters.
+ */
+ std::string getHSIEstimatorValues();
+
+ /**
+ * @brief Set custom compensation parameters for the magnetometer.
+ *
+ * @param c The hard and soft iron compensation parameters. See the
+ * datasheet for details.
+ * @param b The hard and soft iron compensation parameters. See the
+ * datasheet for details. Unit of measurement [Gauss]
+ * @return True if operation succeeded.
+ */
+ bool setMagnetometerCompensation(const Eigen::Matrix3f& c,
+ const Eigen::Vector3f& b);
+
+ /**
+ * @brief Set custom compensation parameters for the accelerometer.
+ *
+ * @param c The accelerometer compensation parameters. See the
+ * datasheet for details.
+ * @param b The accelerometer compensation parameters. See the
+ * datasheet for details. Unit of measurement [m/s^2]
+ * @return True if operation succeeded.
+ */
+ bool setAccelerometerCompensation(const Eigen::Matrix3f& c,
+ const Eigen::Vector3f& b);
+
+ /**
+ * @brief Set custom compensation parameters for the gyroscope.
+ *
+ * @param c The gyroscope compensation parameters. See the
+ * datasheet for details.
+ * @param b The gyroscope compensation parameters. See the
+ * datasheet for details. Unit of measurement [rad/s]
+ * @return True if operation succeeded.
+ */
+ bool setGyroscopeCompensation(const Eigen::Matrix3f& c,
+ const Eigen::Vector3f& b);
+
+ /**
+ * @brief Write the current register settings into non-volatile
+ * memory. Once the settings are stored in non-volatile (Flash)
+ * memory, the VN module can be power cycled or reset, and
+ * the register will be reloaded from non-volatile memory.
+ */
+ bool saveConfiguration();
+
+ /**
+ * @brief Restore the VN module’s factory default settings and
+ * reset the module.
+ */
+ bool restoreFactorySettings();
+
protected:
/**
* @brief Calculate the 8bit checksum on the given array.
@@ -204,6 +281,17 @@ protected:
*/
bool recvStringCommand(char* command, int maxLength);
+ /**
+ * @brief Utility function used to set a register on the sensor.
+ * This function relies on sendStringCommand: DO NOT USE THIS FUNCTION
+ * IF LOW EXECUTION TIME IS NEEDED.
+ *
+ * @param command Write command to be sent to the sensor.
+ *
+ * @return True if operation succeeded.
+ */
+ bool writeRegister(const std::string& command);
+
/**
* @brief Serial interface that is needed to communicate
* with the sensor via ASCII codes.