diff --git a/src/shared/sensors/VN100/VN100Spi.cpp b/src/shared/sensors/VN100/VN100Spi.cpp
index 81184a37dc6f14955d28d3ec20984c2976e9ceea..752eda5fe8aac1ee8e30d4eb598b28dc6d2bb9ac 100644
--- a/src/shared/sensors/VN100/VN100Spi.cpp
+++ b/src/shared/sensors/VN100/VN100Spi.cpp
@@ -111,35 +111,83 @@ bool VN100Spi::selfTest()
VN100Data VN100Spi::sampleImpl()
{
- // TODO
+ D(assert(isInit && "init() was not called"));
+
+ // Reset any errors.
+ lastError = SensorErrors::NO_ERRORS;
+
+ VN100Data data;
+ data.accelerationTimestamp = TimestampTimer::getTimestamp();
+ data.angularSpeedTimestamp = data.accelerationTimestamp;
+ data.magneticFieldTimestamp = data.accelerationTimestamp;
+ data.pressureTimestamp = data.accelerationTimestamp;
+ data.quaternionTimestamp = data.accelerationTimestamp;
+ data.temperatureTimestamp = data.accelerationTimestamp;
- return VN100Data();
+ if (!getImuSample(data))
+ {
+ // An error occurred while gathering data
+ lastError = NO_NEW_DATA;
+ return lastSample;
+ }
+
+ if (!getQuaternionSample(data))
+ {
+ // An error occurred while gathering data
+ lastError = NO_NEW_DATA;
+ return lastSample;
+ }
+
+ return data;
}
-AccelerometerData VN100Spi::readAcc()
+bool VN100Spi::getImuSample(VN100Data& data)
{
- AccelerometerData data;
+ uint8_t buf[VN100SpiDefs::IMU_SAMPLE_SIZE];
- const int REG_ID = 18;
- const int PAYLOAD_SIZE = 12; // 3 float (4 bytes) values
- uint8_t buf[PAYLOAD_SIZE];
+ if (readRegister(VN100SpiDefs::REG_IMU_DATA, buf,
+ VN100SpiDefs::IMU_SAMPLE_SIZE) != 0)
+ {
+ // An error occurred while reading data
+ return false;
+ }
- data.accelerationTimestamp = TimestampTimer::getTimestamp();
+ // Get measurements from raw data
+ uint32_t* ptr = (uint32_t*)buf;
+ data.magneticFieldX = extractMeasurement(ptr[0]);
+ data.magneticFieldY = extractMeasurement(ptr[1]);
+ data.magneticFieldZ = extractMeasurement(ptr[2]);
+ data.accelerationX = extractMeasurement(ptr[3]);
+ data.accelerationY = extractMeasurement(ptr[4]);
+ data.accelerationZ = extractMeasurement(ptr[5]);
+ data.angularSpeedX = extractMeasurement(ptr[6]);
+ data.angularSpeedY = extractMeasurement(ptr[7]);
+ data.angularSpeedZ = extractMeasurement(ptr[8]);
+ data.temperature = extractMeasurement(ptr[9]);
+ data.pressure = extractMeasurement(ptr[10]);
+
+ return true;
+}
- if (readRegister(REG_ID, buf, PAYLOAD_SIZE) != 0)
+bool VN100Spi::getQuaternionSample(VN100Data& data)
+{
+ uint8_t buf[VN100SpiDefs::QUATERNION_SAMPLE_SIZE];
+
+ if (readRegister(VN100SpiDefs::REG_QUATERNION_DATA, buf,
+ VN100SpiDefs::QUATERNION_SAMPLE_SIZE) != 0)
{
- // An error occurred
- lastError = NO_NEW_DATA;
- return AccelerometerData();
+ // An error occurred while reading data
+ return false;
}
// Get measurements from raw data
- uint32_t* ptr = (uint32_t*)buf;
- data.accelerationX = extractMeasurement(ptr[0]);
- data.accelerationY = extractMeasurement(ptr[1]);
- data.accelerationZ = extractMeasurement(ptr[2]);
+ uint32_t* ptr = (uint32_t*)buf;
+ data.quaternionX = extractMeasurement(ptr[0]);
+ data.quaternionY = extractMeasurement(ptr[1]);
+ data.quaternionZ = extractMeasurement(ptr[2]);
+ data.quaternionW = extractMeasurement(ptr[3]);
- return data;
+ return true;
}
float VN100Spi::extractMeasurement(uint32_t rawData)
diff --git a/src/shared/sensors/VN100/VN100Spi.h b/src/shared/sensors/VN100/VN100Spi.h
index f5a779c3ee4b585f5816594dff132ca9063cd1e4..1a0ddcf8b9286ad9fc742c697557f7838610ff48 100644
--- a/src/shared/sensors/VN100/VN100Spi.h
+++ b/src/shared/sensors/VN100/VN100Spi.h
@@ -63,9 +63,6 @@ public:
*/
VN100Data sampleImpl() override;
- // TODO: remove this, only for testing.
- AccelerometerData readAcc();
-
private:
/**
* @brief Check the model number register.
@@ -81,6 +78,25 @@ private:
*/
void sendDummyPacket();
+ /**
+ * @brief Get accelerometer, gyroscope, magnetometer, pressure and
+ * temperature measurements from the sensor.
+ *
+ * @param data The variable where measurements will be stored.
+ *
+ * @return True if the operation is successful, false otherwise.
+ */
+ bool getImuSample(VN100Data& data);
+
+ /**
+ * @brief Get quaternion measurements from the sensor.
+ *
+ * @param data The variable where measurements will be stored.
+ *
+ * @return True if the operation is successful, false otherwise.
+ */
+ bool getQuaternionSample(VN100Data& data);
+
/**
* @brief Extracts floating point measurement from the data received from
* the sensor.
diff --git a/src/shared/sensors/VN100/VN100SpiDefs.h b/src/shared/sensors/VN100/VN100SpiDefs.h
index 3c62836a3392e0d64072552dbd0b991b2fcb268a..64e8c86027811ec6870aa37a34fad96e8591b51b 100644
--- a/src/shared/sensors/VN100/VN100SpiDefs.h
+++ b/src/shared/sensors/VN100/VN100SpiDefs.h
@@ -33,7 +33,9 @@ namespace VN100SpiDefs
*/
enum Registers
{
- REG_MODEL_NUMBER = 1,
+ REG_MODEL_NUMBER = 1,
+ REG_QUATERNION_DATA = 9,
+ REG_IMU_DATA = 54,
};
/**
@@ -48,13 +50,23 @@ enum Commands
/**
* @brief The expected model number to be red from the sensor.
*/
-const char* MODEL_NUMBER = "VN-100";
+const char* const MODEL_NUMBER = "VN-100";
/**
* @brief Size of the buffer used to retrieve the model number from the sensor.
*/
const int MODEL_NUMBER_SIZE = 24;
+/**
+ * @brief Size of the buffer used to retrieve imu data from the sensor.
+ */
+const int IMU_SAMPLE_SIZE = 44;
+
+/**
+ * @brief Size of the buffer used to retrieve quaternion data from the sensor.
+ */
+const int QUATERNION_SAMPLE_SIZE = 16;
+
} // namespace VN100SpiDefs
} // namespace Boardcore
diff --git a/src/tests/sensors/test-vn100-spi.cpp b/src/tests/sensors/test-vn100-spi.cpp
index f517c44607485792914f4f30c5e4a55b47067ac7..4334c78f5d3e60b84c18ad96a7e36d60c1cca774 100644
--- a/src/tests/sensors/test-vn100-spi.cpp
+++ b/src/tests/sensors/test-vn100-spi.cpp
@@ -62,12 +62,22 @@ int main()
for (int i = 0; i < 100; ++i)
{
- AccelerometerData data = sensor.readAcc();
+ // AccelerometerData data = sensor.readAcc();
+ sensor.sample();
+ VN100Data sample = sensor.getLastSample();
printf("sample %d:\n", i + 1);
- printf("AccX: %f\n", data.accelerationX);
- printf("AccY: %f\n", data.accelerationY);
- printf("AccZ: %f\n\n", data.accelerationZ);
+ printf("acc: %llu, %.3f, %.3f, %.3f\n", sample.accelerationTimestamp,
+ sample.accelerationX, 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("press: %.3f\n", sample.pressure);
+ printf("temp: %.3f\n\n", sample.temperature);
Thread::sleep(500);
}