diff --git a/src/shared/sensors/VN300/VN300.cpp b/src/shared/sensors/VN300/VN300.cpp
index 7f2a73d4e27a35563c0b106cff56a3436e0b2c09..500ae4c3cf1cb08819a1cee1564be5cf862a5003 100755
--- a/src/shared/sensors/VN300/VN300.cpp
+++ b/src/shared/sensors/VN300/VN300.cpp
@@ -1,5 +1,5 @@
/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Author: Lorenzo Cucchi
+ * Author: Lorenzo Cucchi, 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
@@ -29,12 +29,13 @@
namespace Boardcore
{
-VN300::VN300(USART &usart, int userBaudRate, bool isBinary, CRCOptions crc,
- uint16_t samplePeriod, const AntennaPosition antPosA,
- const AntennaPosition antPosB, const Eigen::Matrix3f rotMat)
- : usart(usart), userBaudRate(userBaudRate), isBinary(isBinary),
- samplePeriod(samplePeriod), crc(crc), antPosA(antPosA), antPosB(antPosB),
- rotMat(rotMat)
+VN300::VN300(USART &usart, int userBaudRate,
+ VN300Defs::SamplingMethod samplingMethod, CRCOptions crc,
+ const VN300Defs::AntennaPosition antPosA,
+ const VN300Defs::AntennaPosition antPosB,
+ const Eigen::Matrix3f rotMat)
+ : usart(usart), userBaudRate(userBaudRate), samplingMethod(samplingMethod),
+ crc(crc), antPosA(antPosA), antPosB(antPosB), rotMat(rotMat)
{
}
@@ -53,15 +54,15 @@ 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");
- preSampleINSlla = new string("$VNRRG,63*6BBB\n");
- preSampleBin1 = new string("$VNBOM,1*749D\n");
+ preSampleImuString = "$VNRRG,15*92EA\n";
+ preSampleINSlla = "$VNRRG,63*6BBB\n";
+ preSampleBin1 = "$VNBOM,1*749D\n";
}
else
{
- preSampleImuString = new string("$VNRRG,15*77\n");
- preSampleINSlla = new string("$VNRRG,63*76\n");
- preSampleBin1 = new string("$VNBOM,1*45\n");
+ preSampleImuString = "$VNRRG,15*77\n";
+ preSampleINSlla = "$VNRRG,63*76\n";
+ preSampleBin1 = "$VNBOM,1*45\n";
}
// Set the error to init fail and if the init process goes without problem
@@ -261,7 +262,7 @@ VN300Data VN300::sampleImpl()
// This condition is needed to discern between ASCII and Binary sampling.
// If true the binary sampling is used increasing sampling speed.
// else the ascii setup is used, both return VN300Data
- if (isBinary == true)
+ if (samplingMethod == VN300Defs::SamplingMethod::BINARY)
{
data = sampleBinary();
}
@@ -284,11 +285,11 @@ VN300Data VN300::sampleBinary()
// The sample command is sent to the VN300
// TODO: this or sendStringCommand()?
- usart.writeString(preSampleBin1->c_str());
+ usart.writeString(preSampleBin1.c_str());
// A BinaryData variable is created and it will be passed to the sampleBin()
// function as a reference
- BinaryData bindata;
+ VN300Defs::BinaryData bindata;
// This sleep of 2 ms is used to wait for the reply of the VN300 taking into
// account standard reply times, this free the thread waiting the message
@@ -318,7 +319,7 @@ VN300Data VN300::sampleBinary()
// The static_cast is necessary to cast the double variables into a
// float, this cause less problem with floating point precision errors
// and it's lighter on memory11
- Ins_Lla ins{
+ VN300Defs::Ins_Lla ins{
TimestampTimer::getTimestamp(),
bindata.fix,
bindata.fix, // add function to extract ins_fix from ins_status
@@ -333,11 +334,12 @@ VN300Data VN300::sampleBinary()
bindata.vely,
bindata.velz};
+ lastError = NO_ERRORS;
return VN300Data(quat, mag, acc, gyro, ins);
}
else
{
- // TODO: set lastError to no new data
+ lastError = NO_NEW_DATA;
return lastSample;
}
}
@@ -346,7 +348,7 @@ VN300Data VN300::sampleASCII()
{
clearBuffer();
// Returns Quaternion, Magnetometer, Accelerometer and Gyro
- usart.writeString(preSampleImuString->c_str());
+ usart.writeString(preSampleImuString.c_str());
// Wait some time
@@ -372,7 +374,7 @@ VN300Data VN300::sampleASCII()
clearBuffer();
// Returns INS LLA message
- usart.writeString(preSampleINSlla->c_str());
+ usart.writeString(preSampleINSlla.c_str());
// Wait some time
@@ -389,7 +391,7 @@ VN300Data VN300::sampleASCII()
return lastSample;
}
- Ins_Lla ins = sampleIns();
+ VN300Defs::Ins_Lla ins = sampleIns();
return VN300Data(quat, mag, acc, gyro, ins);
}
@@ -428,17 +430,19 @@ bool VN300::disableAsyncMessages(bool waitResponse)
bool VN300::findBaudrate()
{
- char check[] = "VN";
- const int checkOffset = 1;
+ char check[] = "VN";
+ const int checkOffset = 1;
+ std::array<uint32_t, 9> baudrateList = {
+ 9600, 19200, 38400, 57600, 115200, 128000, 230400, 460800, 921600};
// The for loop change at every iteration the baudrate of the usart
// and sends a message to the VN300, then if the baudrate is correct the VN
// reply and the loop terminates. In this way at the end of the loop the
// correct baudrate is set.
- for (uint32_t i = 0; i < BaudrateList.size(); i++)
+ for (uint32_t i = 0; i < baudrateList.size(); i++)
{
- usart.setBaudrate(BaudrateList[i]);
+ usart.setBaudrate(baudrateList[i]);
miosix::Thread::sleep(50);
// I pause the async messages, we don't know if they are present.
@@ -623,7 +627,7 @@ bool VN300::setCrc(bool waitResponse)
return true;
}
-bool VN300::setAntennaA(AntennaPosition antPos)
+bool VN300::setAntennaA(VN300Defs::AntennaPosition antPos)
{
std::string command;
@@ -647,7 +651,7 @@ bool VN300::setAntennaA(AntennaPosition antPos)
return true;
}
-bool VN300::setCompassBaseline(AntennaPosition antPos)
+bool VN300::setCompassBaseline(VN300Defs::AntennaPosition antPos)
{
std::string command;
@@ -933,11 +937,11 @@ GyroscopeData VN300::sampleGyroscope()
return data;
}
-Ins_Lla VN300::sampleIns()
+VN300Defs::Ins_Lla VN300::sampleIns()
{
unsigned int indexStart = 0;
char *nextNumber;
- Ins_Lla data;
+ VN300Defs::Ins_Lla data;
// Look for the second ',' in the string
// I can avoid the string control because it has already been done in
@@ -973,7 +977,7 @@ Ins_Lla VN300::sampleIns()
return data;
}
-bool VN300::sampleBin(BinaryData &bindata)
+bool VN300::sampleBin(VN300Defs::BinaryData &bindata)
{
// This variable is used as an initial time reference for the while loop
uint64_t initTime = TimestampTimer::getTimestamp();
@@ -989,7 +993,7 @@ bool VN300::sampleBin(BinaryData &bindata)
{
// Reading all the message directly into the struct, this need to be
// packed in order to have contiguous memory addresses
- if (usart.read(&bindata, sizeof(BinaryData)))
+ if (usart.read(&bindata, sizeof(VN300Defs::BinaryData)))
{
return true;
}
diff --git a/src/shared/sensors/VN300/VN300.h b/src/shared/sensors/VN300/VN300.h
index bb31fe46d2500f876adae446ceb8d9b34f171240..2ed526e1cf260c269eebd7ea5123591c865a1052 100755
--- a/src/shared/sensors/VN300/VN300.h
+++ b/src/shared/sensors/VN300/VN300.h
@@ -1,5 +1,5 @@
/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Author: Lorenzo Cucchi
+ * Author: Lorenzo Cucchi, 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
@@ -81,10 +81,6 @@ public:
// sendStringCommand())
};
- // TODO: can be moved inside findBaudrate()? why is it public?
- std::array<uint32_t, 9> BaudrateList = {
- 9600, 19200, 38400, 57600, 115200, 128000, 230400, 460800, 921600};
-
/**
* @brief Constructor.
*
@@ -96,12 +92,12 @@ public:
* @param antPos antenna A position
*/
VN300(USART &usart, int userBaudRate,
- bool isBinary = true, // TODO: are textual readings needed?
- CRCOptions crc = CRCOptions::CRC_ENABLE_8,
- uint16_t samplePeriod = 15, // TODO: why 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());
+ VN300Defs::SamplingMethod samplingMethod =
+ VN300Defs::SamplingMethod::BINARY,
+ CRCOptions crc = CRCOptions::CRC_ENABLE_8,
+ VN300Defs::AntennaPosition antPosA = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
+ VN300Defs::AntennaPosition antPosB = {1.0, 0.0, 0.0, 0.0, 0.0, 0.0},
+ Eigen::Matrix3f rotMat = Eigen::Matrix3f::Identity());
bool init() override;
@@ -190,7 +186,7 @@ private:
*
* @return True if operation succeeded.
*/
- bool setAntennaA(AntennaPosition antPos);
+ bool setAntennaA(VN300Defs::AntennaPosition antPos);
/**
* @brief Sets the compass baseline, position offset of antenna B respect to
@@ -202,7 +198,7 @@ private:
*
* @return True if operation succeeded.
*/
- bool setCompassBaseline(AntennaPosition antPos);
+ bool setCompassBaseline(VN300Defs::AntennaPosition antPos);
/**
* @brief Set the reference frame rotation of the sensor in order to have
@@ -229,24 +225,19 @@ private:
*/
bool selfTestImpl();
- QuaternionData
- sampleQuaternion(); // TODO: can be removed, not needed with binary output
+ QuaternionData sampleQuaternion();
- MagnetometerData sampleMagnetometer(); // TODO: can be removed, not needed
- // with binary output
+ MagnetometerData sampleMagnetometer();
- AccelerometerData sampleAccelerometer(); // TODO: can be removed, not
- // needed with binary output
+ AccelerometerData sampleAccelerometer();
- GyroscopeData
- sampleGyroscope(); // TODO: can be removed, not needed with binary output
+ GyroscopeData sampleGyroscope();
- Ins_Lla sampleIns(); // TODO: can be removed, not needed with binary output
+ VN300Defs::Ins_Lla sampleIns();
VN300Data sampleBinary();
- VN300Data
- sampleASCII(); // TODO: can be removed, not needed with binary output
+ VN300Data sampleASCII();
/**
* @brief Sends the command to the sensor with the correct checksum added
@@ -281,7 +272,7 @@ private:
* @return true if operation succeeded
*
*/
- bool sampleBin(BinaryData &bindata);
+ bool sampleBin(VN300Defs::BinaryData &bindata);
// TODO: can be removed and placed inside sampleBinary()
/**
@@ -356,48 +347,46 @@ private:
*/
USART &usart;
int userBaudRate;
- const int defaultBaudRate = 115200;
+ static const int defaultBaudRate = 115200;
- bool isBinary; // TODO: are textual readings needed?
- uint16_t samplePeriod; // TODO: is this actually used? seems not...
+ VN300Defs::SamplingMethod samplingMethod;
CRCOptions crc;
bool isInit = false;
- AntennaPosition antPosA;
- AntennaPosition antPosB;
+ VN300Defs::AntennaPosition antPosA;
+ VN300Defs::AntennaPosition antPosB;
Eigen::Matrix3f rotMat;
/**
* @brief IMU pre-elaborated sample string for efficiency reasons.
*/
- string *preSampleImuString = nullptr;
- // TODO: this should not be a pointer
+ string preSampleImuString = "";
/**
* @brief Temperature and pressure pre-elaborated sample string for
* efficiency reasons.
*/
- string *preSampleINSlla = nullptr;
- // TODO: this should not be a pointer
+ string preSampleINSlla = "";
/**
* @brief Binary output polling command
*/
- string *preSampleBin1 = nullptr;
- // TODO: this should not be a pointer
+ string preSampleBin1 = "";
- static const unsigned int recvStringMaxDimension = 200;
+ /**
+ * @brief Maximum size of the receiving string.
+ */
+ static const uint8_t recvStringMaxDimension = 200;
/**
- * @brief Pointer to the received string by the sensor. Allocated 1 time
- * only (200 bytes).
+ * @brief Buffer used to store the string received from the sensor.
*/
std::array<char, recvStringMaxDimension> recvString;
/**
* @brief Actual strlen() of the recvString.
*/
- unsigned int recvStringLength = 0;
+ uint8_t recvStringLength = 0;
PrintLogger logger = Logging::getLogger("VN300");
};
diff --git a/src/shared/sensors/VN300/VN300Data.h b/src/shared/sensors/VN300/VN300Data.h
index 13f6934def5619dc40eb1468e3ce808a653853f6..5326cde528a24d867795556ea7bbf7eb2bc7bb3e 100644
--- a/src/shared/sensors/VN300/VN300Data.h
+++ b/src/shared/sensors/VN300/VN300Data.h
@@ -1,5 +1,5 @@
/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Author: Lorenzo Cucchi
+ * Author: Lorenzo Cucchi, 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,84 +23,11 @@
#pragma once
#include <sensors/SensorData.h>
+#include <sensors/VN300/VN300Defs.h>
namespace Boardcore
{
-/**
- * @brief Structure to handle antenna A position units [m]
- */
-struct AntennaPosition // TODO: move inside defs
-{
- float posX;
- float posY;
- float posZ;
- float uncX;
- float uncY;
- float uncZ;
-};
-
-/**
- * @brief Structure to handle INS data
- */
-struct Ins_Lla // TODO: move inside defs, should this go inside SensorData.h?
-{
- uint64_t insTimestamp;
- uint16_t fix_gps;
- uint16_t fix_ins;
- uint16_t status;
- float yaw;
- float pitch;
- float roll;
- float latitude;
- float longitude;
- float altitude;
- float nedVelX;
- float nedVelY;
- float nedVelZ;
-};
-
-/**
- * @brief Structure to handle binary message
- *
- * @property The struct needs the packed attribute in order to have contiguous
- * memory allocation in order to be able to parse directly the received message
- */
-struct __attribute__((packed)) BinaryData // TODO: move inside defs
-{
- uint8_t group;
- uint16_t common;
- uint16_t gnss;
- float yaw_bin;
- float pitch_bin;
- float roll_bin;
- float quatX_bin;
- float quatY_bin;
- float quatZ_bin;
- float quatW_bin;
- float angx;
- float angy;
- float angz;
- float velx;
- float vely;
- float velz;
- float accx;
- float accy;
- float accz;
- float magx;
- float magy;
- float magz;
- float temp;
- float pres;
- uint16_t ins_status;
- uint8_t numsats;
- uint8_t fix;
- double latitude_bin;
- double longitude_bin;
- double altitude_bin;
- uint16_t checksum;
-};
-
/**
* @brief data type class
*/
@@ -108,7 +35,7 @@ struct VN300Data : public QuaternionData,
public MagnetometerData,
public AccelerometerData,
public GyroscopeData,
- public Ins_Lla
+ public VN300Defs::Ins_Lla
{
/**
diff --git a/src/shared/sensors/VN300/VN300Defs.h b/src/shared/sensors/VN300/VN300Defs.h
index 2b173e83c9c2b4ebead9f3180ab838444647fbf0..4c0721790d6c2a716b98d758171ed33bf8d2dc27 100755
--- a/src/shared/sensors/VN300/VN300Defs.h
+++ b/src/shared/sensors/VN300/VN300Defs.h
@@ -1,5 +1,5 @@
/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Author: Lorenzo Cucchi
+ * Author: Lorenzo Cucchi, 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
@@ -40,6 +40,15 @@ enum BinaryGroup
BINARYGROUP_GPS2 = 0x40 ///< GPS2 group.
};
+/**
+ * @brief Defines how the samples are extracted from the sensor.
+ */
+enum class SamplingMethod
+{
+ BINARY, ///< Binary messages are used.
+ TEXTUAL, ///< Textual messages are used.
+};
+
/// \brief Flags for the binary group 1 'Common' in the binary output registers.
enum CommonGroup
{
@@ -162,6 +171,80 @@ const unsigned char
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} // Invalid group
};
+/**
+ * @brief Structure to handle antenna A position units [m]
+ */
+struct AntennaPosition
+{
+ float posX;
+ float posY;
+ float posZ;
+ float uncX;
+ float uncY;
+ float uncZ;
+};
+
+/**
+ * @brief Structure to handle INS data
+ */
+struct Ins_Lla
+{
+ uint64_t insTimestamp;
+ uint16_t fix_gps;
+ uint16_t fix_ins;
+ uint16_t status;
+ float yaw;
+ float pitch;
+ float roll;
+ float latitude;
+ float longitude;
+ float altitude;
+ float nedVelX;
+ float nedVelY;
+ float nedVelZ;
+};
+
+/**
+ * @brief Structure to handle binary message
+ *
+ * @property The struct needs the packed attribute in order to have contiguous
+ * memory allocation in order to be able to parse directly the received message
+ */
+struct __attribute__((packed)) BinaryData
+{
+ uint8_t group;
+ uint16_t common;
+ uint16_t gnss;
+ float yaw_bin;
+ float pitch_bin;
+ float roll_bin;
+ float quatX_bin;
+ float quatY_bin;
+ float quatZ_bin;
+ float quatW_bin;
+ float angx;
+ float angy;
+ float angz;
+ float velx;
+ float vely;
+ float velz;
+ float accx;
+ float accy;
+ float accz;
+ float magx;
+ float magy;
+ float magz;
+ float temp;
+ float pres;
+ uint16_t ins_status;
+ uint8_t numsats;
+ uint8_t fix;
+ double latitude_bin;
+ double longitude_bin;
+ double altitude_bin;
+ uint16_t checksum;
+};
+
} // namespace VN300Defs
} // namespace Boardcore
diff --git a/src/tests/sensors/test-vn300.cpp b/src/tests/sensors/test-vn300.cpp
index d3e4015c025b1f0da131682af13ac29dc42d1fc2..b9a072e4c948d241f8e0133dfe75217e680aae46 100644
--- a/src/tests/sensors/test-vn300.cpp
+++ b/src/tests/sensors/test-vn300.cpp
@@ -1,5 +1,5 @@
/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Author: Lorenzo Cucchi
+ * Author: Lorenzo Cucchi, 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
@@ -49,7 +49,8 @@ int main()
dbg.mode(Mode::OUTPUT);
USART usart(USART6, 115200);
- VN300 sensor(usart, 115200, true, VN300::CRCOptions::CRC_ENABLE_8);
+ VN300 sensor(usart, 115200, VN300Defs::SamplingMethod::BINARY,
+ VN300::CRCOptions::CRC_ENABLE_8);
// Let the sensor start up
Thread::sleep(1000);