diff --git a/src/shared/sensors/VN300/VN300.cpp b/src/shared/sensors/VN300/VN300.cpp
index 8bb95216c3f54dd6dc4ca35a7e0112059b17174b..8252e94a4630751b5f33efd01fa68640e99254ea 100644
--- a/src/shared/sensors/VN300/VN300.cpp
+++ b/src/shared/sensors/VN300/VN300.cpp
@@ -24,14 +24,17 @@
#include <drivers/timer/TimestampTimer.h>
+#include "diagnostic/CpuMeter/CpuMeter.h"
+
namespace Boardcore
{
-VN300::VN300(USART &usart, int userBaudRate, CRCOptions crc,
+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), samplePeriod(samplePeriod),
- crc(crc), antPosA(antPosA), antPosB(antPosB), rotMat(rotMat)
+ : usart(usart), userBaudRate(userBaudRate), isBinary(isBinary),
+ samplePeriod(samplePeriod), crc(crc), antPosA(antPosA), antPosB(antPosB),
+ rotMat(rotMat)
{
}
@@ -55,11 +58,13 @@ bool VN300::init()
{
preSampleImuString = new string("$VNRRG,15*92EA\n");
preSampleINSlla = new string("$VNRRG,63*6BBB\n");
+ preSampleBin1 = new string("$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");
}
// Set the error to init fail and if the init process goes without problem
@@ -102,6 +107,12 @@ bool VN300::init()
return false;
}
+ if (!setBinaryOutput())
+ {
+ LOG_ERR(logger, "Unable to set binary output register");
+ return false;
+ }
+
if (!setAntennaA(antPosA))
{
LOG_ERR(logger, "Unable to set antenna A position");
@@ -234,6 +245,37 @@ bool VN300::selfTest()
}
VN300Data VN300::sampleImpl()
+{
+
+ if (!isInit)
+ {
+ lastError = SensorErrors::NOT_INIT;
+ LOG_WARN(logger,
+ "Unable to sample due to not initialized VN300 sensor");
+ return lastSample;
+ }
+
+ // Before sampling i check for errors
+ if (lastError != SensorErrors::NO_ERRORS)
+ {
+ return lastSample;
+ }
+
+ VN300Data data;
+
+ if (isBinary == true)
+ {
+ data = sampleBinary();
+ }
+ else
+ {
+ data = sampleASCII();
+ }
+
+ return data;
+}
+
+VN300Data VN300::sampleBinary()
{
if (!isInit)
{
@@ -249,6 +291,45 @@ VN300Data VN300::sampleImpl()
return lastSample;
}
+ clearBuffer();
+
+ usart.writeString(preSampleBin1->c_str());
+
+ BinaryData bindata = sampleBin();
+
+ QuaternionData quat{TimestampTimer::getTimestamp(), bindata.quatW_bin,
+ bindata.quatX_bin, bindata.quatY_bin,
+ bindata.quatZ_bin};
+ AccelerometerData acc{TimestampTimer::getTimestamp(), bindata.accx,
+ bindata.accy, bindata.accz};
+
+ MagnetometerData mag{TimestampTimer::getTimestamp(), bindata.magx,
+ bindata.magy, bindata.magz};
+
+ GyroscopeData gyro{TimestampTimer::getTimestamp(), bindata.angx,
+ bindata.angy, bindata.angz};
+
+ Ins_Lla ins{TimestampTimer::getTimestamp(),
+ bindata.fix,
+ bindata.ins_status,
+ bindata.ins_status,
+ bindata.yaw_bin,
+ bindata.pitch_bin,
+ bindata.roll_bin,
+ static_cast<float>(bindata.latitude_bin),
+ static_cast<float>(bindata.longitude_bin),
+ static_cast<float>(bindata.altitude_bin),
+ bindata.velx,
+ bindata.vely,
+ bindata.velz};
+
+
+ return VN300Data(quat, mag, acc, gyro, ins);
+}
+
+VN300Data VN300::sampleASCII()
+{
+ clearBuffer();
// Returns Quaternion, Magnetometer, Accelerometer and Gyro
usart.writeString(preSampleImuString->c_str());
@@ -274,6 +355,7 @@ VN300Data VN300::sampleImpl()
AccelerometerData acc = sampleAccelerometer();
GyroscopeData gyro = sampleGyroscope();
+ clearBuffer();
// Returns INS LLA message
usart.writeString(preSampleINSlla->c_str());
@@ -395,6 +477,7 @@ bool VN300::configBaudRate(int baudRate)
LOG_WARN(logger, "Unable to sample due to serial communication error");
return false;
}
+
if (strncmp(command.c_str(), recvString + modelNumberOffset,
strlen(command.c_str())) != 0)
{
@@ -602,6 +685,47 @@ bool VN300::setReferenceFrame(Eigen::Matrix3f rotMat)
return true;
}
+bool VN300::setBinaryOutput()
+{
+ uint16_t outputGroup =
+ VN300Defs::BINARYGROUP_COMMON | VN300Defs::BINARYGROUP_GPS;
+
+ uint16_t commonGroup =
+ VN300Defs::COMMONGROUP_YAWPITCHROLL |
+ VN300Defs::COMMONGROUP_QUATERNION | VN300Defs::COMMONGROUP_ANGULARRATE |
+ VN300Defs::COMMONGROUP_VELOCITY | VN300Defs::COMMONGROUP_ACCEL |
+ VN300Defs::COMMONGROUP_MAGPRES | VN300Defs::COMMONGROUP_INSSTATUS;
+
+ uint16_t gnssGroup = VN300Defs::GPSGROUP_NUMSATS | VN300Defs::GPSGROUP_FIX |
+ VN300Defs::GPSGROUP_POSLLA;
+
+ std::string command;
+ command = fmt::format("{},{},{:x},{:x}", "VNWRG,75,0,8", outputGroup,
+ commonGroup, gnssGroup);
+
+ miosix::Thread::sleep(50);
+ clearBuffer();
+ // I can send the command
+ if (!sendStringCommand(command))
+ {
+ return false;
+ }
+
+ if (!recvStringCommand(recvString, recvStringMaxDimension))
+ {
+ LOG_WARN(logger, "Unable to sample due to serial communication error");
+ return false;
+ }
+ std::string comp = "VNWRG,75,0,8";
+ if (strncmp(comp.c_str(), recvString + 1, strlen(comp.c_str())) != 0)
+ {
+ LOG_WARN(logger, "The message is wrong {}", recvString);
+ return false;
+ }
+
+ return true;
+}
+
bool VN300::selfTestImpl()
{
char modelNumber[] = "VN-300T-CR";
@@ -784,16 +908,17 @@ Ins_Lla VN300::sampleIns()
// Parse the data
data.insTimestamp = TimestampTimer::getTimestamp();
- data.time_gps = strtod(recvString + indexStart, &nextNumber);
- data.week = static_cast<uint16_t>(strtol(nextNumber + 1, &nextNumber, 16));
+ double time_gps = strtod(recvString + indexStart, &nextNumber);
+ uint16_t week =
+ static_cast<uint16_t>(strtol(nextNumber + 1, &nextNumber, 16));
data.status =
static_cast<uint16_t>(strtol(nextNumber + 1, &nextNumber, 16));
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.latitude = strtod(nextNumber + 1, &nextNumber);
+ data.longitude = strtod(nextNumber + 1, &nextNumber);
+ data.altitude = strtod(nextNumber + 1, &nextNumber);
data.nedVelX = strtof(nextNumber + 1, &nextNumber);
data.nedVelY = strtof(nextNumber + 1, &nextNumber);
data.nedVelZ = strtof(nextNumber + 1, NULL);
@@ -801,6 +926,39 @@ Ins_Lla VN300::sampleIns()
return data;
}
+BinaryData VN300::sampleBin()
+{
+
+ BinaryData bindata;
+ int i = 0;
+ int j = 1;
+ bool read = false;
+ unsigned char initByte;
+
+ miosix::GpioPin dbg(GPIOB_BASE, 8);
+ dbg.mode(miosix::Mode::OUTPUT);
+
+ dbg.high();
+ miosix::Thread::sleep(2);
+ while (read == false && i < 10000)
+ {
+ if (usart.read(&initByte, 1))
+ {
+ dbg.low();
+ while (usart.read(&bindata, sizeof(BinaryData)) && j < 200)
+ {
+ read = true;
+ }
+
+ break;
+ }
+ }
+
+ i++;
+
+ return bindata;
+}
+
bool VN300::sendStringCommand(std::string command)
{
if (crc == CRCOptions::CRC_ENABLE_8)
@@ -846,13 +1004,14 @@ bool VN300::recvStringCommand(char *command, int maxLength)
command[0] = '\0';
int i = 0;
+ miosix::Thread::sleep(2);
while (read == false && i < 10000)
{
// Read the first char
if (usart.read(&initChar, 1) && initChar == '$')
{
command[0] = '$';
-
+
while (usart.read(&initChar, 1) && initChar != '\n' &&
j < maxLength)
{
@@ -879,6 +1038,44 @@ bool VN300::recvStringCommand(char *command, int maxLength)
return true;
}
+int VN300::recvBinaryCommand(uint8_t *command)
+{
+ uint8_t initByte;
+ int i = 0;
+ int j = 1;
+ bool read = false;
+ memset(command, '-', sizeof(command));
+
+ miosix::Thread::sleep(2);
+ while (read == false && i < 10000)
+ {
+ if (usart.read(&initByte, 1))
+ {
+ command[0] = initByte;
+
+ while (usart.read(&initByte, 1) && j < 200)
+ {
+ command[j] = initByte;
+ j++;
+ read = true;
+ }
+
+ break;
+ }
+
+ i++;
+ }
+
+ if (read)
+ {
+ return --j;
+ }
+ else
+ {
+ return 0;
+ }
+}
+
uint8_t VN300::checkErrorVN(const char *message)
{
if (strncmp(message, "$VNERR,", 7) == 0)
diff --git a/src/shared/sensors/VN300/VN300.h b/src/shared/sensors/VN300/VN300.h
index 762425fa1888602ee7e6b4ff74a4239c945f94e3..7b2f615859d482a0a84ec1d9fbea060cb298e1d8 100644
--- a/src/shared/sensors/VN300/VN300.h
+++ b/src/shared/sensors/VN300/VN300.h
@@ -61,6 +61,7 @@
#include <Eigen/Core>
#include "VN300Data.h"
+#include "VN300Defs.h"
#include "drivers/usart/USART.h"
namespace Boardcore
@@ -77,7 +78,7 @@ public:
CRC_ENABLE_8 = 0x08,
CRC_ENABLE_16 = 0x10
};
-
+
std::array<uint32_t, 9> BaudrateList = {
9600, 19200, 38400, 57600, 115200, 128000, 230400, 460800, 921600};
@@ -91,7 +92,7 @@ public:
* @param samplePeriod Sampling period in ms
* @param antPos antenna A position
*/
- VN300(USART &usart, int userBaudRate,
+ VN300(USART &usart, int userBaudRate, bool isBinary = true,
CRCOptions crc = CRCOptions::CRC_ENABLE_8, uint16_t samplePeriod = 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},
@@ -191,15 +192,21 @@ private:
bool setCompassBaseline(AntennaPosition antPos);
/**
- * @brief set the reference frame rotation of the sensor in order to have
+ * @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.
+ * @return True if operation succeeded.
*/
bool setReferenceFrame(Eigen::Matrix3f rotMat);
+ /**
+ * @brief Set the binary output register
+ *
+ * @return True if operation succeeded.
+ */
+ bool setBinaryOutput();
/**
* @brief Method implementation of self test.
*
@@ -216,6 +223,13 @@ private:
GyroscopeData sampleGyroscope();
Ins_Lla sampleIns();
+
+ BinaryData sampleBin();
+
+ VN300Data sampleBinary();
+
+ VN300Data sampleASCII();
+
/**
* @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 '$'
@@ -238,6 +252,8 @@ private:
*/
bool recvStringCommand(char *command, int maxLength);
+ int recvBinaryCommand(uint8_t *command);
+
/**
* @brief check if the VN-300 returned an error and differentiate between
* them. The error is formatted as $VNERR,xx*XX
@@ -299,6 +315,7 @@ private:
uint16_t samplePeriod;
CRCOptions crc;
bool isInit = false;
+ bool isBinary = true;
AntennaPosition antPosA;
AntennaPosition antPosB;
@@ -315,12 +332,16 @@ private:
*/
string *preSampleINSlla = nullptr;
+ string *preSampleBin1 = nullptr;
+
/**
* @brief Pointer to the received string by the sensor. Allocated 1 time
* only (200 bytes).
*/
char *recvString = nullptr;
+ unsigned char *recvBin = nullptr;
+
/**
* @brief Actual strlen() of the recvString.
*/
diff --git a/src/shared/sensors/VN300/VN300Data.h b/src/shared/sensors/VN300/VN300Data.h
index 26f552e3c6fdb8d9bea3a4c75c34c004367e1095..2fd1851a70b4d4ac399bbe88679510ad09e7bd23 100644
--- a/src/shared/sensors/VN300/VN300Data.h
+++ b/src/shared/sensors/VN300/VN300Data.h
@@ -55,8 +55,8 @@ struct AntennaPosition
struct Ins_Lla
{
uint64_t insTimestamp;
- double time_gps;
- uint16_t week;
+ uint8_t fix_gps;
+ uint8_t fix_ins;
uint16_t status;
float yaw;
float pitch;
@@ -69,6 +69,41 @@ struct Ins_Lla
float nedVelZ;
};
+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;
+};
+
/**
* @brief data type class
*/
@@ -87,7 +122,7 @@ struct VN300Data : public QuaternionData,
: 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}
+ Ins_Lla{0, 0, 0, 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}
{
}
@@ -112,7 +147,7 @@ struct VN300Data : public QuaternionData,
"accelerationTimestamp,accelerationX,accelerationY,"
"accelerationZ,angularSpeedTimestamp,angularSpeedX,"
"angularSpeedY,angularSpeedZ,insTimeStamp,"
- "time_gps,week,status,yaw,pitch,roll,latitude,"
+ "status,yaw,pitch,roll,latitude,"
"longitude,altitude,nedVelX,nedVelY,nedVelZ\n";
}
@@ -125,9 +160,9 @@ struct VN300Data : public QuaternionData,
<< accelerationY << "," << accelerationZ << ","
<< angularSpeedTimestamp << "," << angularSpeedX << ","
<< angularSpeedY << "," << angularSpeedZ << "," << insTimestamp
- << "," << time_gps << "," << week << "," << status << yaw << pitch
- << roll << latitude << longitude << altitude << nedVelX << nedVelY
- << nedVelZ << "\n";
+ << "," << status << "," << yaw << "," << pitch << "," << roll << ","
+ << latitude << "," << longitude << "," << altitude << "," << nedVelX
+ << "," << nedVelY << "," << nedVelZ << "\n";
}
};
diff --git a/src/shared/sensors/VN300/VN300Defs.h b/src/shared/sensors/VN300/VN300Defs.h
new file mode 100644
index 0000000000000000000000000000000000000000..2b173e83c9c2b4ebead9f3180ab838444647fbf0
--- /dev/null
+++ b/src/shared/sensors/VN300/VN300Defs.h
@@ -0,0 +1,167 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Lorenzo Cucchi
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+namespace Boardcore
+{
+
+namespace VN300Defs
+{
+
+/// \brief The available binary output groups.
+enum BinaryGroup
+{
+ BINARYGROUP_COMMON = 0x01, ///< Common group.
+ BINARYGROUP_TIME = 0x02, ///< Time group.
+ BINARYGROUP_IMU = 0x04, ///< IMU group.
+ BINARYGROUP_GPS = 0x08, ///< GPS group.
+ BINARYGROUP_ATTITUDE = 0x10, ///< Attitude group.
+ BINARYGROUP_INS = 0x20, ///< INS group.
+ BINARYGROUP_GPS2 = 0x40 ///< GPS2 group.
+};
+
+/// \brief Flags for the binary group 1 'Common' in the binary output registers.
+enum CommonGroup
+{
+ COMMONGROUP_NONE = 0x0000, ///< None.
+ COMMONGROUP_TIMESTARTUP = 0x0001, ///< TimeStartup.
+ COMMONGROUP_TIMEGPS = 0x0002, ///< TimeGps.
+ COMMONGROUP_TIMESYNCIN = 0x0004, ///< TimeSyncIn.
+ COMMONGROUP_YAWPITCHROLL = 0x0008, ///< YawPitchRoll.
+ COMMONGROUP_QUATERNION = 0x0010, ///< Quaternion.
+ COMMONGROUP_ANGULARRATE = 0x0020, ///< AngularRate.
+ COMMONGROUP_POSITION = 0x0040, ///< Position.
+ COMMONGROUP_VELOCITY = 0x0080, ///< Velocity.
+ COMMONGROUP_ACCEL = 0x0100, ///< Accel.
+ COMMONGROUP_IMU = 0x0200, ///< Imu.
+ COMMONGROUP_MAGPRES = 0x0400, ///< MagPres.
+ COMMONGROUP_DELTATHETA = 0x0800, ///< DeltaTheta.
+ COMMONGROUP_INSSTATUS = 0x1000, ///< InsStatus.
+ COMMONGROUP_SYNCINCNT = 0x2000, ///< SyncInCnt.
+ COMMONGROUP_TIMEGPSPPS = 0x4000 ///< TimeGpsPps.
+};
+
+/// \brief Flags for the binary group 2 'Time' in the binary output registers.
+enum TimeGroup
+{
+ TIMEGROUP_NONE = 0x0000, ///< None.
+ TIMEGROUP_TIMESTARTUP = 0x0001, ///< TimeStartup.
+ TIMEGROUP_TIMEGPS = 0x0002, ///< TimeGps.
+ TIMEGROUP_GPSTOW = 0x0004, ///< GpsTow.
+ TIMEGROUP_GPSWEEK = 0x0008, ///< GpsWeek.
+ TIMEGROUP_TIMESYNCIN = 0x0010, ///< TimeSyncIn.
+ TIMEGROUP_TIMEGPSPPS = 0x0020, ///< TimeGpsPps.
+ TIMEGROUP_TIMEUTC = 0x0040, ///< TimeUTC.
+ TIMEGROUP_SYNCINCNT = 0x0080, ///< SyncInCnt.
+ TIMEGROUP_SYNCOUTCNT = 0x0100, ///< SyncOutCnt.
+ TIMEGROUP_TIMESTATUS = 0x0200 ///< TimeStatus.
+};
+
+/// \brief Flags for the binary group 3 'IMU' in the binary output registers.
+enum ImuGroup
+{
+ IMUGROUP_NONE = 0x0000, ///< None.
+ IMUGROUP_IMUSTATUS = 0x0001, ///< ImuStatus.
+ IMUGROUP_UNCOMPMAG = 0x0002, ///< UncompMag.
+ IMUGROUP_UNCOMPACCEL = 0x0004, ///< UncompAccel.
+ IMUGROUP_UNCOMPGYRO = 0x0008, ///< UncompGyro.
+ IMUGROUP_TEMP = 0x0010, ///< Temp.
+ IMUGROUP_PRES = 0x0020, ///< Pres.
+ IMUGROUP_DELTATHETA = 0x0040, ///< DeltaTheta.
+ IMUGROUP_DELTAVEL = 0x0080, ///< DeltaVel.
+ IMUGROUP_MAG = 0x0100, ///< Mag.
+ IMUGROUP_ACCEL = 0x0200, ///< Accel.
+ IMUGROUP_ANGULARRATE = 0x0400, ///< AngularRate.
+ IMUGROUP_SENSSAT = 0x0800, ///< SensSat.
+};
+
+/// \brief Flags for the binary group 4 'GPS' and group 7 'GPS2' in the binary
+/// output registers.
+enum GpsGroup
+{
+ GPSGROUP_NONE = 0x0000, ///< None.
+ GPSGROUP_UTC = 0x0001, ///< UTC.
+ GPSGROUP_TOW = 0x0002, ///< Tow.
+ GPSGROUP_WEEK = 0x0004, ///< Week.
+ GPSGROUP_NUMSATS = 0x0008, ///< NumSats.
+ GPSGROUP_FIX = 0x0010, ///< Fix.
+ GPSGROUP_POSLLA = 0x0020, ///< PosLla.
+ GPSGROUP_POSECEF = 0x0040, ///< PosEcef.
+ GPSGROUP_VELNED = 0x0080, ///< VelNed.
+ GPSGROUP_VELECEF = 0x0100, ///< VelEcef.
+ GPSGROUP_POSU = 0x0200, ///< PosU.
+ GPSGROUP_VELU = 0x0400, ///< VelU.
+ GPSGROUP_TIMEU = 0x0800, ///< TimeU.
+ GPSGROUP_TIMEINFO = 0x1000, ///< TimeInfo.
+ GPSGROUP_DOP = 0x2000, ///< Dop.
+};
+
+/// \brief Flags for the binary group 5 'Attitude' in the binary output
+/// registers.
+enum AttitudeGroup
+{
+ ATTITUDEGROUP_NONE = 0x0000, ///< None.
+ ATTITUDEGROUP_VPESTATUS = 0x0001, ///< VpeStatus.
+ ATTITUDEGROUP_YAWPITCHROLL = 0x0002, ///< YawPitchRoll.
+ ATTITUDEGROUP_QUATERNION = 0x0004, ///< Quaternion.
+ ATTITUDEGROUP_DCM = 0x0008, ///< DCM.
+ ATTITUDEGROUP_MAGNED = 0x0010, ///< MagNed.
+ ATTITUDEGROUP_ACCELNED = 0x0020, ///< AccelNed.
+ ATTITUDEGROUP_LINEARACCELBODY = 0x0040, ///< LinearAccelBody.
+ ATTITUDEGROUP_LINEARACCELNED = 0x0080, ///< LinearAccelNed.
+ ATTITUDEGROUP_YPRU = 0x0100, ///< YprU.
+ ATTITUDEGROUP_HEAVE = 0x1000, ///< Heave.
+};
+
+/// \brief Flags for the binary group 6 'INS' in the binary output registers.
+enum InsGroup
+{
+ INSGROUP_NONE = 0x0000, ///< None.
+ INSGROUP_INSSTATUS = 0x0001, ///< InsStatus.
+ INSGROUP_POSLLA = 0x0002, ///< PosLla.
+ INSGROUP_POSECEF = 0x0004, ///< PosEcef.
+ INSGROUP_VELBODY = 0x0008, ///< VelBody.
+ INSGROUP_VELNED = 0x0010, ///< VelNed.
+ INSGROUP_VELECEF = 0x0020, ///< VelEcef.
+ INSGROUP_MAGECEF = 0x0040, ///< MagEcef.
+ INSGROUP_ACCELECEF = 0x0080, ///< AccelEcef.
+ INSGROUP_LINEARACCELECEF = 0x0100, ///< LinearAccelEcef.
+ INSGROUP_POSU = 0x0200, ///< PosU.
+ INSGROUP_VELU = 0x0400, ///< VelU.
+};
+
+const unsigned char
+ BinaryGroupLengths[sizeof(uint8_t) * 8][sizeof(uint16_t) * 15] = {
+ {8, 8, 8, 12, 16, 12, 24, 12, 12, 24, 20, 28, 2, 4, 8}, // Group 1
+ {8, 8, 8, 2, 8, 8, 8, 4, 4, 1, 0, 0, 0, 0, 0}, // Group 2
+ {2, 12, 12, 12, 4, 4, 16, 12, 12, 12, 12, 2, 40, 0, 0}, // Group 3
+ {8, 8, 2, 1, 1, 24, 24, 12, 12, 12, 4, 4, 2, 28, 0}, // Group 4
+ {2, 12, 16, 36, 12, 12, 12, 12, 12, 12, 28, 24, 12, 0, 0}, // Group 5
+ {2, 24, 24, 12, 12, 12, 12, 12, 12, 4, 4, 68, 64, 0, 0}, // Group 6
+ {8, 8, 2, 1, 1, 24, 24, 12, 12, 12, 4, 4, 2, 28, 0}, // Group 7
+ {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} // Invalid group
+};
+
+} // namespace VN300Defs
+
+} // namespace Boardcore
diff --git a/src/tests/sensors/test-vn300.cpp b/src/tests/sensors/test-vn300.cpp
index 6c89455b99263713ba9b0f7f49a8ccea33775c1d..d8de586ce3d847cb63d0a0d289758964836420a3 100644
--- a/src/tests/sensors/test-vn300.cpp
+++ b/src/tests/sensors/test-vn300.cpp
@@ -25,6 +25,8 @@
#include <sensors/VN300/VN300.h>
#include <utils/Stats/Stats.h>
+#include <iostream>
+
#include "diagnostic/CpuMeter/CpuMeter.h"
#define ENABLE_CPU_METER
@@ -36,16 +38,18 @@ int main()
VN300Data sample;
string sampleRaw;
+ GpioPin dbg(GPIOB_BASE, 4);
GpioPin u6tx1(GPIOG_BASE, 14);
GpioPin u6rx1(GPIOG_BASE, 9);
u6rx1.alternateFunction(8);
- u6rx1.mode(Mode::ALTERNATE);
+ u6rx1.mode(Mode::ALTERNATE_PULL_UP);
u6tx1.alternateFunction(8);
u6tx1.mode(Mode::ALTERNATE);
+ dbg.mode(Mode::OUTPUT);
USART usart(USART6, 115200);
- VN300 sensor(usart, 230400, VN300::CRCOptions::CRC_ENABLE_8);
+ VN300 sensor(usart, 230400, true, VN300::CRCOptions::CRC_ENABLE_8);
// Let the sensor start up
Thread::sleep(1000);
@@ -83,21 +87,33 @@ int main()
Thread::sleep(1000);
uint64_t time_start = getTick();
- for (int i = 0; i < 30; i++)
+ CpuMeter::resetCpuStats();
+
+ for (int i = 0; i < 10000; i++)
{
- ledOn();
+ dbg.high();
sensor.sample();
+ dbg.low();
+
sample = sensor.getLastSample();
- printf("Sample %i\n", i);
+ // printf("Sample %i\n", i);
printf("acc: %" PRIu64 ", %.3f, %.3f, %.3f\n",
sample.accelerationTimestamp, sample.accelerationX,
sample.accelerationY, sample.accelerationZ);
printf("ang: %.6f, %.6f, %.6f\n", sample.angularSpeedX,
sample.angularSpeedY, sample.angularSpeedZ);
- printf("ins: %.6f, %.6f, %.6f\n", sample.yaw, sample.pitch,
- sample.roll);
+ // printf("ins: %.6f, %.6f, %.6f\n", sample.yaw, sample.pitch,
+ // sample.roll);
+ //// printf("gps: %.6f, %.6f, %.6f\n", sample.latitude,
+ ///sample.longitude,
+ // sample.altitude);
+ // sample.print(std::cout);
+
+ printf("CPU: %5.1f\n", CpuMeter::getCpuStats().mean);
+
+ Thread::sleep(10);
}
ledOff();