diff --git a/src/shared/sensors/VN100/VN100Spi.cpp b/src/shared/sensors/VN100/VN100Spi.cpp index 80a75b9820ca0a13ad1f6abf3fe77ad9b55472a4..d4a01e4f6950055d30ce301f82b2ce0a42cf3f63 100644 --- a/src/shared/sensors/VN100/VN100Spi.cpp +++ b/src/shared/sensors/VN100/VN100Spi.cpp @@ -22,6 +22,10 @@ #include "VN100Spi.h" +#include <drivers/timer/TimestampTimer.h> +#include <interfaces/endianness.h> +#include <utils/Debug.h> + #include "VN100SpiDefs.h" namespace Boardcore @@ -86,7 +90,8 @@ bool VN100Spi::checkModelNumber() spiSlave.bus.select(spiSlave.cs); // Discard the first 3 bytes of the response - spiSlave.bus.read24(); + spiSlave.bus + .read24(); // TODO: should I verify also the command and register? err = spiSlave.bus.read(); @@ -121,4 +126,78 @@ VN100Data VN100Spi::sampleImpl() return VN100Data(); } +AccelerometerData VN100Spi::readAcc() +{ + // Low level spi is needed in order to read multiple data + // without raising the chip select pin + + AccelerometerData data; + + // Variables used to store raw data extracted from the sensor + uint32_t rawDataX = 0; + uint32_t rawDataY = 0; + uint32_t rawDataZ = 0; + + constexpr uint32_t requestPacket = + (VN100SpiDefs::READ_REG << 24) | // Read register command + (18 << 16); // Id of the register + + data.accelerationTimestamp = TimestampTimer::getTimestamp(); + + // Send request packet + spiSlave.bus.select(spiSlave.cs); + spiSlave.bus.write32(requestPacket); + spiSlave.bus.deselect(spiSlave.cs); + + // Wait at least 100us + miosix::delayUs(100); + + // Read response + spiSlave.bus.select(spiSlave.cs); + + // Discard the first 3 bytes of the response + spiSlave.bus + .read24(); // TODO: should I verify also the command and register? + + uint8_t err = spiSlave.bus.read(); + + if (err != 0) + { + // An error occurred while attempting to service the request + spiSlave.bus.deselect(spiSlave.cs); + lastError = COMMAND_FAILED; + return AccelerometerData(); + } + + rawDataX = spiSlave.bus.read32(); + rawDataY = spiSlave.bus.read32(); + rawDataZ = spiSlave.bus.read32(); + + spiSlave.bus.deselect(spiSlave.cs); + + // Get measurements from raw data + data.accelerationX = extractMeasurement(rawDataX); + data.accelerationY = extractMeasurement(rawDataY); + data.accelerationZ = extractMeasurement(rawDataZ); + + return data; +} + +float VN100Spi::extractMeasurement(uint32_t rawData) +{ + // The floating point values received are stored as 32-bit IEEE + // floating point numbers in little endian byte order. + + // Ensure that the operation is legal + static_assert(sizeof(uint32_t) == sizeof(float) && + "Error, data size mismatch"); + + rawData = swapBytes32(rawData); + + float f; + std::memcpy(&f, &rawData, sizeof(uint32_t)); + + return f; +} + } // namespace Boardcore diff --git a/src/shared/sensors/VN100/VN100Spi.h b/src/shared/sensors/VN100/VN100Spi.h index d1fd0f209d0b6865072ae9a464d5cc1fa6c2a6ac..2fb076c8fae1d3f9962d8f675e016872386dc833 100644 --- a/src/shared/sensors/VN100/VN100Spi.h +++ b/src/shared/sensors/VN100/VN100Spi.h @@ -63,6 +63,9 @@ public: */ VN100Data sampleImpl() override; + // TODO: remove this, only for testing. + AccelerometerData readAcc(); + private: /** * @brief Check the model number register. @@ -71,6 +74,16 @@ private: */ bool checkModelNumber(); + /** + * @brief Extracts floating point measurement from the data received from + * the sensor. + * + * @param rawData The data received from the sensor. + * + * @return The floating point data. + */ + float extractMeasurement(uint32_t rawData); + bool isInit = false; SPISlave spiSlave; diff --git a/src/shared/sensors/VN100/VN100SpiDefs.h b/src/shared/sensors/VN100/VN100SpiDefs.h index 87201520cda6ce9d6f6f7e78e984b869b0184f09..3c62836a3392e0d64072552dbd0b991b2fcb268a 100644 --- a/src/shared/sensors/VN100/VN100SpiDefs.h +++ b/src/shared/sensors/VN100/VN100SpiDefs.h @@ -33,7 +33,7 @@ namespace VN100SpiDefs */ enum Registers { - REG_MODEL_NUMBER = 0x01, + REG_MODEL_NUMBER = 1, }; /** @@ -41,8 +41,8 @@ enum Registers */ enum Commands { - READ_REG = 0x01, - WRITE_REG = 0x02, + READ_REG = 1, + WRITE_REG = 2, }; /** diff --git a/src/tests/sensors/test-vn100-spi.cpp b/src/tests/sensors/test-vn100-spi.cpp index e4246a65507224daab8ec3d8105ae7296befba92..f517c44607485792914f4f30c5e4a55b47067ac7 100644 --- a/src/tests/sensors/test-vn100-spi.cpp +++ b/src/tests/sensors/test-vn100-spi.cpp @@ -50,6 +50,9 @@ int main() VN100Spi sensor(bus, csPin, busConfiguration); + // Let the sensor start up + Thread::sleep(1000); + if (!sensor.init()) { printf("Error, cannot initialize the sensor\n\n"); @@ -57,5 +60,17 @@ int main() } printf("Sensor initialized\n"); + for (int i = 0; i < 100; ++i) + { + AccelerometerData data = sensor.readAcc(); + + 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); + + Thread::sleep(500); + } + return 0; }