diff --git a/CMakeLists.txt b/CMakeLists.txt index 3695cb6d85a68baced219cc0d63dc3ad4d888c7f..2c858831ed86a80964624b7c2f84fdc3338b0aba 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -447,8 +447,11 @@ sbs_target(test-ubxgps-serial stm32f429zi_death_stack_v2) add_executable(test-ubxgps-spi src/tests/sensors/test-ubxgps-spi.cpp) sbs_target(test-ubxgps-spi stm32f429zi_death_stack_v2) -add_executable(test-vn100 src/tests/sensors/test-vn100.cpp) -sbs_target(test-vn100 stm32f407vg_stm32f4discovery) +add_executable(test-vn100-serial src/tests/sensors/test-vn100-serial.cpp) +sbs_target(test-vn100-serial stm32f407vg_stm32f4discovery) + +add_executable(test-vn100-spi src/tests/sensors/test-vn100-spi.cpp) +sbs_target(test-vn100-spi stm32f407vg_stm32f4discovery) add_executable(test-lis2mdl src/tests/sensors/test-lis2mdl.cpp) sbs_target(test-lis2mdl stm32f429zi_stm32f4discovery) diff --git a/cmake/boardcore.cmake b/cmake/boardcore.cmake index ae8a0541ac65e343a7de3a3caeafa9c5af91233e..1f725d32bd35de35b3412b66125c39c2b3956e3a 100644 --- a/cmake/boardcore.cmake +++ b/cmake/boardcore.cmake @@ -110,7 +110,8 @@ set(BOARDCORE_SRC ${BOARDCORE_PATH}/src/shared/sensors/SensorSampler.cpp ${BOARDCORE_PATH}/src/shared/sensors/UBXGPS/UBXGPSSerial.cpp ${BOARDCORE_PATH}/src/shared/sensors/UBXGPS/UBXGPSSpi.cpp - ${BOARDCORE_PATH}/src/shared/sensors/VN100/VN100.cpp + ${BOARDCORE_PATH}/src/shared/sensors/VN100/VN100Serial.cpp + ${BOARDCORE_PATH}/src/shared/sensors/VN100/VN100Spi.cpp ${BOARDCORE_PATH}/src/shared/sensors/LIS2MDL/LIS2MDL.cpp ${BOARDCORE_PATH}/src/shared/sensors/LPS28DFW/LPS28DFW.cpp ${BOARDCORE_PATH}/src/shared/sensors/LPS22DF/LPS22DF.cpp diff --git a/src/shared/logger/LogTypes.h b/src/shared/logger/LogTypes.h index c10289c254926df18a1e04eac9a5b5fd1540faa3..70e6426fbbdcab9915279a870c77cce2cf9296b5 100644 --- a/src/shared/logger/LogTypes.h +++ b/src/shared/logger/LogTypes.h @@ -59,7 +59,8 @@ #include <sensors/RotatedIMU/IMUData.h> #include <sensors/SensorData.h> #include <sensors/UBXGPS/UBXGPSData.h> -#include <sensors/VN100/VN100Data.h> +#include <sensors/VN100/VN100SerialData.h> +#include <sensors/VN100/VN100SpiData.h> #include <sensors/analog/AnalogLoadCellData.h> #include <sensors/analog/BatteryVoltageSensorData.h> #include <sensors/analog/Pitot/PitotData.h> @@ -165,7 +166,8 @@ void registerTypes(Deserializer& ds) ds.registerType<TemperatureData>(); ds.registerType<TimestampData>(); ds.registerType<UBXGPSData>(); - ds.registerType<VN100Data>(); + ds.registerType<VN100SerialData>(); + ds.registerType<VN100SpiData>(); ds.registerType<VoltageData>(); ds.registerType<Xbee::XbeeStatus>(); } diff --git a/src/shared/sensors/VN100/VN100.cpp b/src/shared/sensors/VN100/VN100Serial.cpp similarity index 93% rename from src/shared/sensors/VN100/VN100.cpp rename to src/shared/sensors/VN100/VN100Serial.cpp index 12ebff0ee883897658b221572133527d9ed31f86..e0df012ee2d2ac20a2ceefd91cb0c94c053328ec 100644 --- a/src/shared/sensors/VN100/VN100.cpp +++ b/src/shared/sensors/VN100/VN100Serial.cpp @@ -20,7 +20,7 @@ * THE SOFTWARE. */ -#include "VN100.h" +#include "VN100Serial.h" #include <drivers/timer/TimestampTimer.h> #include <utils/KernelTime.h> @@ -28,12 +28,13 @@ namespace Boardcore { -VN100::VN100(USART &usart, int baudRate, CRCOptions crc, uint16_t samplePeriod) +VN100Serial::VN100Serial(USART &usart, int baudRate, CRCOptions crc, + uint16_t samplePeriod) : usart(usart), baudRate(baudRate), samplePeriod(samplePeriod), crc(crc) { } -bool VN100::init() +bool VN100Serial::init() { SensorErrors backup = lastError; @@ -117,7 +118,7 @@ bool VN100::init() return true; } -void VN100::run() +void VN100Serial::run() { while (!shouldStop()) { @@ -132,7 +133,7 @@ void VN100::run() } } -bool VN100::sampleRaw() +bool VN100Serial::sampleRaw() { // Sensor not init if (!isInit) @@ -160,7 +161,7 @@ bool VN100::sampleRaw() return true; } -string VN100::getLastRawSample() +string VN100Serial::getLastRawSample() { // If not init i return the void string if (!isInit) @@ -171,7 +172,7 @@ string VN100::getLastRawSample() return string(recvString, recvStringLength); } -bool VN100::closeAndReset() +bool VN100Serial::closeAndReset() { // Sensor not init if (!isInit) @@ -196,7 +197,7 @@ bool VN100::closeAndReset() return true; } -bool VN100::selfTest() +bool VN100Serial::selfTest() { if (!selfTestImpl()) { @@ -208,13 +209,13 @@ bool VN100::selfTest() return true; } -VN100Data VN100::sampleImpl() +VN100SerialData VN100Serial::sampleImpl() { miosix::Lock<FastMutex> l(mutex); return threadSample; } -VN100Data VN100::sampleData() +VN100SerialData VN100Serial::sampleData() { if (!isInit) { @@ -282,10 +283,10 @@ VN100Data VN100::sampleData() TemperatureData temp = sampleTemperature(); PressureData press = samplePressure(); - return VN100Data(quat, mag, acc, gyro, temp, press); + return VN100SerialData(quat, mag, acc, gyro, temp, press); } -bool VN100::disableAsyncMessages(bool waitResponse) +bool VN100Serial::disableAsyncMessages(bool waitResponse) { // Command string std::string command = @@ -306,7 +307,7 @@ bool VN100::disableAsyncMessages(bool waitResponse) return true; } -bool VN100::configDefaultSerialPort() +bool VN100Serial::configDefaultSerialPort() { // Initial default settings usart.setBaudrate(115200); @@ -319,7 +320,7 @@ bool VN100::configDefaultSerialPort() * Even if the user configured baudrate is the default, I want to reset the * buffer to clean the junk. */ -bool VN100::configUserSerialPort() +bool VN100Serial::configUserSerialPort() { std::string command; @@ -339,7 +340,7 @@ bool VN100::configUserSerialPort() return true; } -bool VN100::setCrc(bool waitResponse) +bool VN100Serial::setCrc(bool waitResponse) { // Command for the crc change std::string command; @@ -397,7 +398,7 @@ bool VN100::setCrc(bool waitResponse) return true; } -bool VN100::selfTestImpl() +bool VN100Serial::selfTestImpl() { char modelNumber[] = "VN-100"; const int modelNumberOffset = 10; @@ -450,7 +451,7 @@ bool VN100::selfTestImpl() return true; } -QuaternionData VN100::sampleQuaternion() +QuaternionData VN100Serial::sampleQuaternion() { unsigned int indexStart = 0; char *nextNumber; @@ -478,7 +479,7 @@ QuaternionData VN100::sampleQuaternion() return data; } -MagnetometerData VN100::sampleMagnetometer() +MagnetometerData VN100Serial::sampleMagnetometer() { unsigned int indexStart = 0; char *nextNumber; @@ -505,7 +506,7 @@ MagnetometerData VN100::sampleMagnetometer() return data; } -AccelerometerData VN100::sampleAccelerometer() +AccelerometerData VN100Serial::sampleAccelerometer() { unsigned int indexStart = 0; char *nextNumber; @@ -532,7 +533,7 @@ AccelerometerData VN100::sampleAccelerometer() return data; } -GyroscopeData VN100::sampleGyroscope() +GyroscopeData VN100Serial::sampleGyroscope() { unsigned int indexStart = 0; char *nextNumber; @@ -559,7 +560,7 @@ GyroscopeData VN100::sampleGyroscope() return data; } -TemperatureData VN100::sampleTemperature() +TemperatureData VN100Serial::sampleTemperature() { unsigned int indexStart = 0; TemperatureData data; @@ -583,7 +584,7 @@ TemperatureData VN100::sampleTemperature() return data; } -PressureData VN100::samplePressure() +PressureData VN100Serial::samplePressure() { unsigned int indexStart = 0; PressureData data; @@ -607,7 +608,7 @@ PressureData VN100::samplePressure() return data; } -bool VN100::sendStringCommand(std::string command) +bool VN100Serial::sendStringCommand(std::string command) { if (crc == CRCOptions::CRC_ENABLE_8) { @@ -648,7 +649,7 @@ bool VN100::sendStringCommand(std::string command) return true; } -bool VN100::recvStringCommand(char *command, int maxLength) +bool VN100Serial::recvStringCommand(char *command, int maxLength) { int i = 0; // Read the buffer @@ -672,7 +673,7 @@ bool VN100::recvStringCommand(char *command, int maxLength) return true; } -bool VN100::verifyChecksum(char *command, int length) +bool VN100Serial::verifyChecksum(char *command, int length) { int checksumOffset = 0; @@ -730,7 +731,7 @@ bool VN100::verifyChecksum(char *command, int length) return true; } -uint8_t VN100::calculateChecksum8(uint8_t *message, int length) +uint8_t VN100Serial::calculateChecksum8(uint8_t *message, int length) { int i; uint8_t result = 0x00; @@ -745,7 +746,7 @@ uint8_t VN100::calculateChecksum8(uint8_t *message, int length) return result; } -uint16_t VN100::calculateChecksum16(uint8_t *message, int length) +uint16_t VN100Serial::calculateChecksum16(uint8_t *message, int length) { int i; uint16_t result = 0x0000; diff --git a/src/shared/sensors/VN100/VN100.h b/src/shared/sensors/VN100/VN100Serial.h similarity index 94% rename from src/shared/sensors/VN100/VN100.h rename to src/shared/sensors/VN100/VN100Serial.h index 64886b774eca270e46dba86435bd1019c308050f..cd0959f743c0efef266f30d92165c7de5fc2786f 100644 --- a/src/shared/sensors/VN100/VN100.h +++ b/src/shared/sensors/VN100/VN100Serial.h @@ -58,7 +58,7 @@ #include <string.h> #include <utils/Debug.h> -#include "VN100Data.h" +#include "VN100SerialData.h" #include "drivers/usart/USART.h" namespace Boardcore @@ -67,7 +67,7 @@ namespace Boardcore /** * @brief Driver class for VN100 IMU. */ -class VN100 : public Sensor<VN100Data>, public ActiveObject +class VN100Serial : public Sensor<VN100SerialData>, public ActiveObject { public: enum class CRCOptions : uint8_t @@ -86,8 +86,9 @@ public: * @param Redundancy check option. * @param samplePeriod Sampling period in ms */ - VN100(USART &usart, int baudrate, CRCOptions crc = CRCOptions::CRC_ENABLE_8, - uint16_t samplePeriod = 20); + VN100Serial(USART &usart, int baudrate, + CRCOptions crc = CRCOptions::CRC_ENABLE_8, + uint16_t samplePeriod = 20); bool init() override; @@ -119,7 +120,7 @@ private: /** * @brief Sample action implementation. */ - VN100Data sampleImpl() override; + VN100SerialData sampleImpl() override; /** * @brief Active object method, about the thread execution @@ -131,7 +132,7 @@ private: * * @return VN100Data The sampled data */ - VN100Data sampleData(); + VN100SerialData sampleData(); /** * @brief Disables the async messages that the vn100 is default configured @@ -274,9 +275,9 @@ private: * @brief Mutex to synchronize the reading and writing of the threadSample */ mutable miosix::FastMutex mutex; - VN100Data threadSample; + VN100SerialData threadSample; - PrintLogger logger = Logging::getLogger("vn100"); + PrintLogger logger = Logging::getLogger("vn100-serial"); static const unsigned int recvStringMaxDimension = 200; }; diff --git a/src/shared/sensors/VN100/VN100Data.h b/src/shared/sensors/VN100/VN100SerialData.h similarity index 86% rename from src/shared/sensors/VN100/VN100Data.h rename to src/shared/sensors/VN100/VN100SerialData.h index 0b1f190425d5cd0db4b451c2d69aae3d0af8ff60..31e903bdad3955a3caf466bfde899d1edab87573 100644 --- a/src/shared/sensors/VN100/VN100Data.h +++ b/src/shared/sensors/VN100/VN100SerialData.h @@ -30,19 +30,19 @@ namespace Boardcore /** * @brief data type class */ -struct VN100Data : public QuaternionData, - public MagnetometerData, - public AccelerometerData, - public GyroscopeData, - public TemperatureData, - public PressureData +struct VN100SerialData : public QuaternionData, + public MagnetometerData, + public AccelerometerData, + public GyroscopeData, + public TemperatureData, + public PressureData { /** * @brief Void parameters constructor */ // cppcheck-suppress uninitDerivedMemberVar - VN100Data() + VN100SerialData() : 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}, @@ -57,9 +57,9 @@ struct VN100Data : public QuaternionData, */ // cppcheck-suppress passedByValue // cppcheck-suppress uninitDerivedMemberVar - VN100Data(QuaternionData quat, MagnetometerData magData, - AccelerometerData accData, GyroscopeData gyro, - TemperatureData temp, PressureData pres) + VN100SerialData(QuaternionData quat, MagnetometerData magData, + AccelerometerData accData, GyroscopeData gyro, + TemperatureData temp, PressureData pres) : QuaternionData(quat), MagnetometerData(magData), AccelerometerData(accData), GyroscopeData(gyro), TemperatureData(temp), PressureData(pres) diff --git a/src/shared/sensors/VN100/VN100Spi.cpp b/src/shared/sensors/VN100/VN100Spi.cpp new file mode 100644 index 0000000000000000000000000000000000000000..7caea2a86bb2313020811e6a6c3a10973ff9bed3 --- /dev/null +++ b/src/shared/sensors/VN100/VN100Spi.cpp @@ -0,0 +1,375 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Author: 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 + * 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. + */ + +#include "VN100Spi.h" + +#include <drivers/timer/TimestampTimer.h> +#include <interfaces/endianness.h> +#include <utils/Debug.h> + +namespace Boardcore +{ + +VN100Spi::VN100Spi(SPIBus& bus, miosix::GpioPin csPin, + SPIBusConfig busConfiguration, uint16_t syncOutSkipFactor) + : spiSlave(bus, csPin, busConfiguration), + syncOutSkipFactor(syncOutSkipFactor) +{ +} + +bool VN100Spi::init() +{ + if (isInit) + { + LOG_ERR(logger, "init() should be called once"); + lastError = SensorErrors::ALREADY_INIT; + return false; + } + + // First communication after startup might fail + // Send dummy data to clean up + sendDummyPacket(); + + if (!checkModelNumber()) + { + LOG_ERR(logger, "Got bad CHIPID"); + lastError = SensorErrors::INVALID_WHOAMI; + return false; + } + + miosix::delayUs(100); + + if (!setInterrupt()) + { + LOG_ERR(logger, "Unable to set data ready interrupt"); + lastError = SensorErrors::INIT_FAIL; + return false; + } + + // Wait to ensure that enough time has passed before the next operation + miosix::delayUs(100); + + isInit = true; + lastError = SensorErrors::NO_ERRORS; + return true; +} + +bool VN100Spi::checkModelNumber() +{ + int i = 0; + char buf[VN100SpiDefs::MODEL_NUMBER_SIZE]; + for (i = 0; i < VN100SpiDefs::MODEL_NUMBER_SIZE; ++i) + { + buf[i] = 0; + } + + VN100SpiDefs::VNErrors err = readRegister(VN100SpiDefs::REG_MODEL_NUMBER, + reinterpret_cast<uint8_t*>(buf), + VN100SpiDefs::MODEL_NUMBER_SIZE); + if (err != VN100SpiDefs::VNErrors::NO_ERROR) + { + // An error occurred while attempting to service the request + LOG_ERR(logger, "Error while reading CHIPID"); + TRACE("Error code: %u\n", err); + return false; + } + + // Check the model number + if (strncmp(VN100SpiDefs::MODEL_NUMBER, buf, + strlen(VN100SpiDefs::MODEL_NUMBER)) != 0) + { + LOG_ERR(logger, "Error, invalid CHIPID"); + TRACE("%s != %s\n", VN100SpiDefs::MODEL_NUMBER, buf); + return false; + } + + return true; +} + +void VN100Spi::sendDummyPacket() +{ + SPITransaction transaction(spiSlave); + transaction.write32(0); + + /** + * After issuing a command the vn100 needs al least 100 microseconds + * before providing a reply. Considering this function is called only + * during the initialization phase I wait for a full millisecond, as a + * safety measure. + */ + miosix::Thread::sleep(1); +} + +bool VN100Spi::setInterrupt() +{ + /** + * The data ready interrupt is set via the synchronization control register, + * by setting the SyncOut mode. + * + * Imu data is sampled at 800Hz, while Attitude data (quaternion) is sampled + * at 400 Hz. Considering that attitude data has a lower rate we set the + * data ready to trigger on attitude data. + * + * We can set the SyncOutSkipFactor, that defines how many times the sync + * out event should be skipped before actually triggering the SyncOut pin. + * This way we can control the rate at which the data ready interrupt is + * triggered. + * + * SyncIn values, which aren't needed for the data ready of the register, + * will be set to default. + */ + + // Init struct and set default values + VN100SpiDefs::SynchronizationData sData; + sData.syncInMode = 3; // Set to: count number of trigger events on SYNC_IN. + sData.syncInEdge = 0; // Trigger on rising edge + sData.syncInSkipFactor = 0; // Don't skip + + // Set needed values + sData.syncOutMode = 3; // Trigger when attitude measurements are available + sData.syncOutPolarity = 1; // Positive output pulse on the SyncOut pin + sData.syncOutSkipFactor = syncOutSkipFactor; + sData.syncOutPulseWidth = VN100SpiDefs::SYNC_OUT_PULSE_WIDTH; + + VN100SpiDefs::VNErrors err = writeRegister( + VN100SpiDefs::REG_SYNC, reinterpret_cast<uint8_t*>(&sData), + sizeof(VN100SpiDefs::SynchronizationData)); + + if (err != VN100SpiDefs::VNErrors::NO_ERROR) + { + TRACE("setInterrupt() failed, error: %u\n", err); + return false; + } + + return true; +} + +bool VN100Spi::selfTest() { return true; } + +VN100SpiData VN100Spi::sampleImpl() +{ + D(assert(isInit && "init() was not called")); + + // Reset any errors. + lastError = SensorErrors::NO_ERRORS; + + VN100SpiData data; + data.accelerationTimestamp = TimestampTimer::getTimestamp(); + data.angularSpeedTimestamp = data.accelerationTimestamp; + data.magneticFieldTimestamp = data.accelerationTimestamp; + data.quaternionTimestamp = data.accelerationTimestamp; + + if (!getSample(data)) + { + // An error occurred while gathering data + lastError = NO_NEW_DATA; + return lastSample; + } + + return data; +} + +bool VN100Spi::getSample(VN100SpiData& data) +{ + VN100SpiDefs::RawImuQuatData rawData; + + VN100SpiDefs::VNErrors err = + readRegister(VN100SpiDefs::REG_QUAT_IMU_DATA, + reinterpret_cast<uint8_t*>(&rawData), sizeof(rawData)); + + if (err != VN100SpiDefs::VNErrors::NO_ERROR) + { + // An error occurred while reading data + TRACE("getSample() failed, error: %u\n", err); + return false; + } + + // Get measurements from raw data + data.quaternionX = rawData.quatX; + data.quaternionY = rawData.quatY; + data.quaternionZ = rawData.quatZ; + data.quaternionW = rawData.quatW; + data.magneticFieldX = rawData.magX; + data.magneticFieldY = rawData.magY; + data.magneticFieldZ = rawData.magZ; + data.accelerationX = rawData.accX; + data.accelerationY = rawData.accY; + data.accelerationZ = rawData.accZ; + data.angularSpeedX = rawData.gyrX; + data.angularSpeedY = rawData.gyrY; + data.angularSpeedZ = rawData.gyrZ; + + return true; +} + +TemperatureData VN100Spi::getTemperature() +{ + TemperatureData data; + + VN100SpiDefs::RawTempPressData rawData; + + // Get timestamp + data.temperatureTimestamp = TimestampTimer::getTimestamp(); + + VN100SpiDefs::VNErrors err = + readRegister(VN100SpiDefs::REG_TEMP_PRESS_DATA, + reinterpret_cast<uint8_t*>(&rawData), sizeof(rawData)); + + if (err != VN100SpiDefs::VNErrors::NO_ERROR) + { + // An error occurred while reading data + TRACE("getTemperature() failed, error: %u\n", err); + return data; + } + + // Get measurement from raw data + data.temperature = rawData.temp; + + return data; +} + +PressureData VN100Spi::getPressure() +{ + PressureData data; + + VN100SpiDefs::RawTempPressData rawData; + + // Get timestamp + data.pressureTimestamp = TimestampTimer::getTimestamp(); + + VN100SpiDefs::VNErrors err = + readRegister(VN100SpiDefs::REG_TEMP_PRESS_DATA, + reinterpret_cast<uint8_t*>(&rawData), sizeof(rawData)); + + if (err != VN100SpiDefs::VNErrors::NO_ERROR) + { + // An error occurred while reading data + TRACE("getPressure() failed, error: %u\n", err); + return data; + } + + // Get measurement from raw data + data.pressure = rawData.press; + + return data; +} + +VN100SpiDefs::VNErrors VN100Spi::readRegister(const uint8_t regId, + uint8_t* payloadBuf, + const uint32_t payloadSize) +{ + /** + * When reading from a sensor's register 2 spi transactions are needed. + * + * First I have to send the request packet, then wait at least 100 + * microseconds to let the sensor process the request. + * + * After this period of time we can proceed with the reading. First + * we receive a 4 bytes header, whit the first byte always 0, the second + * being the read register command, the third being the register we asked + * for and the fourth the error value. + * + * Finally we receive the content of the register. + * + * Low level spi is needed in order to issue multiple readings without + * raising the chip select. + */ + + const uint32_t requestPacket = + (VN100SpiDefs::READ_REG << 24) | // Read register command + (regId << 16); // Id of the register + + // 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 + VN100SpiDefs::VNErrors err = + (VN100SpiDefs::VNErrors)(spiSlave.bus.read32() & 255); + + if (err != VN100SpiDefs::VNErrors::NO_ERROR) + { + // An error occurred while attempting to service the request + spiSlave.bus.deselect(spiSlave.cs); + return err; + } + + spiSlave.bus.read(payloadBuf, payloadSize); + + spiSlave.bus.deselect(spiSlave.cs); + + return VN100SpiDefs::VNErrors::NO_ERROR; +} + +VN100SpiDefs::VNErrors VN100Spi::writeRegister(const uint8_t regId, + const uint8_t* payloadBuf, + const uint32_t payloadSize) +{ + /** + * When writing to a sensor's register 2 spi transactions are needed. + * + * First I have to send the request packet with the value to be written, + * then wait at least 100 microseconds to let the sensor process the + * request. + * + * After this period of time we proceed with reading the outcome of the + * operation. We receive a 4 bytes header, whit the first byte always 0, the + * second being the write register command, the third being the register we + * asked for and the fourth the error value. If the error value is 0 no + * error occurred and the operation is successful. + * + * Low level spi is needed in order to issue multiple readings and writings + * without raising the chip select. + */ + + const uint32_t requestPacket = + (VN100SpiDefs::WRITE_REG << 24) | // Read register command + (regId << 16); // Id of the register + + // Send request packet + spiSlave.bus.select(spiSlave.cs); + spiSlave.bus.write32(requestPacket); + spiSlave.bus.write(payloadBuf, payloadSize); + 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 + uint8_t err = spiSlave.bus.read32() & 255; + + spiSlave.bus.deselect(spiSlave.cs); + + return (VN100SpiDefs::VNErrors)err; +} + +} // namespace Boardcore diff --git a/src/shared/sensors/VN100/VN100Spi.h b/src/shared/sensors/VN100/VN100Spi.h new file mode 100644 index 0000000000000000000000000000000000000000..d90ea7c16ec5dc09f63393d0a5aed0a5e7cf21d0 --- /dev/null +++ b/src/shared/sensors/VN100/VN100Spi.h @@ -0,0 +1,171 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Author: 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 + * 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 + +/** + * Driver for the VN100 SPI IMU. + * + * The VN100 sensor is a calibrated IMU which includes accelerometer, + * magnetometer, gyroscope, barometer and temperature sensor. It also provides + * attitude data (yaw, pith, roll, quaternion). + * This driver samples imu compensated data (accelerometer, gyroscope and + * magnetometer) and quaternion data. + * + * The sampling rate is 400Hz. The data ready interrupt can be set to a lower + * rate by changing the syncOutSkipFactor parameter. + * + * ATTENTION: at least 100 microseconds has to pass between the read/write + * operations with the sensor. + */ + +#include <diagnostic/PrintLogger.h> +#include <drivers/spi/SPIDriver.h> +#include <sensors/Sensor.h> + +#include "VN100SpiData.h" +#include "VN100SpiDefs.h" + +namespace Boardcore +{ + +/** + * @brief Driver class for VN100 Spi IMU. + */ +class VN100Spi : public Sensor<VN100SpiData> +{ +public: + /** + * @brief VN100 constructor. + * + * @param bus SPI bus. + * @param csPin SPI chip select pin. + * @param busConfiguration SPI bus configuration. + * @param syncOutSkipFactor The SyncOutSkipFactor defines how many times the + * data ready event should be skipped before actually triggering the + * interrupt pin. + */ + VN100Spi(SPIBus& bus, miosix::GpioPin csPin, SPIBusConfig busConfiguration, + uint16_t syncOutSkipFactor); + + /** + * @brief Initialize the sensor. + */ + bool init() override; + + /** + * @brief Performs self test for the sensor. + * + * @return Return true if the test was successful. + */ + bool selfTest() override; + + /** + * @brief Retrieve temperature data from the sensor [°C]. + */ + TemperatureData getTemperature(); + + /** + * @brief Retrieve pressure data from the sensor [kPa]. + */ + PressureData getPressure(); + +protected: + /** + * @brief Gather data from the sensor. + */ + VN100SpiData sampleImpl() override; + +private: + /** + * @brief Check the model number register. + * + * @return Returns false if the red value is not valid. + */ + bool checkModelNumber(); + + /** + * @brief Utility function used to clean the junk before starting to + * communicate with the sensor. It send a 4 bytes packet of zeros to the + * sensor. + */ + void sendDummyPacket(); + + /** + * @brief Set the data ready interrupt. + * + * @return True if the operation is successful, false otherwise. + */ + bool setInterrupt(); + + /** + * @brief Get quaternion, accelerometer, gyroscope and magnetometer + * measurements from the sensor. + * + * @param data The variable where measurements will be stored. + * + * @return True if the operation is successful, false otherwise. + */ + bool getSample(VN100SpiData& data); + + /** + * @brief Utility function used to read from a register of the sensor. + * + * @param regId The id of the register to read from. + * @param payloadBuf The buffer where data will be stored. + * @param payloadSize The amount of data (in bytes) to be read from the + * register. + * + * @return Zero if the operation is successful, the error code otherwise. + */ + VN100SpiDefs::VNErrors readRegister(const uint8_t regId, + uint8_t* payloadBuf, + const uint32_t payloadSize); + + /** + * @brief Utility function used to write data to a register of the sensor. + * + * @param regId The id of the register to be written. + * @param payloadBuf The buffer containing the data to be written. + * @param payloadSize The amount of data (in bytes) to be written. + * + * @return Zero if the operation is successful, the error code otherwise. + */ + VN100SpiDefs::VNErrors writeRegister(const uint8_t regId, + const uint8_t* payloadBuf, + const uint32_t payloadSize); + + bool isInit = false; + + SPISlave spiSlave; + + /** + * @brief The SyncOutSkipFactor defines how many times the sync out event + * should be skipped before actually triggering the SyncOut pin (data + * ready). + */ + const uint16_t syncOutSkipFactor = 0; + + PrintLogger logger = Logging::getLogger("vn100-spi"); +}; + +} // namespace Boardcore diff --git a/src/shared/sensors/VN100/VN100SpiData.h b/src/shared/sensors/VN100/VN100SpiData.h new file mode 100644 index 0000000000000000000000000000000000000000..40376473b76155ea2dab640716cbe00cdd8aed41 --- /dev/null +++ b/src/shared/sensors/VN100/VN100SpiData.h @@ -0,0 +1,81 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Author: 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 + * 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 + +#include <sensors/SensorData.h> + +namespace Boardcore +{ + +/** + * @brief Data type class for VN100 Spi. + * + * Units of measurement: + * - Magnetometer [Gauss] + * - Accelerometer [m/s^2] + * - Gyroscope [rad/s] + */ +struct VN100SpiData : public QuaternionData, + public MagnetometerData, + public AccelerometerData, + public GyroscopeData +{ + + VN100SpiData() + : 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} + { + } + + VN100SpiData(QuaternionData quat, MagnetometerData magData, + AccelerometerData accData, GyroscopeData gyro, + TemperatureData temp, PressureData pres) + : QuaternionData(quat), MagnetometerData(magData), + AccelerometerData(accData), GyroscopeData(gyro) + { + } + + static std::string header() + { + return "quatTimestamp,quatX,quatY,quatZ,quatW,magneticFieldTimestamp," + "magneticFieldX,magneticFieldY,magneticFieldZ," + "accelerationTimestamp,accelerationX,accelerationY," + "accelerationZ,angularSpeedTimestamp,angularSpeedX," + "angularSpeedY,angularSpeedZ\n"; + } + + void print(std::ostream& os) const + { + os << quaternionTimestamp << "," << quaternionX << "," << quaternionY + << "," << quaternionZ << "," << quaternionW << "," + << magneticFieldTimestamp << "," << magneticFieldX << "," + << magneticFieldY << "," << magneticFieldZ << "," + << accelerationTimestamp << "," << accelerationX << "," + << accelerationY << "," << accelerationZ << "," + << angularSpeedTimestamp << "," << angularSpeedX << "," + << angularSpeedY << "," << angularSpeedZ << "\n"; + } +}; + +} // namespace Boardcore diff --git a/src/shared/sensors/VN100/VN100SpiDefs.h b/src/shared/sensors/VN100/VN100SpiDefs.h new file mode 100644 index 0000000000000000000000000000000000000000..f39d7adc09961b59215a72136a20fbfdc27337c9 --- /dev/null +++ b/src/shared/sensors/VN100/VN100SpiDefs.h @@ -0,0 +1,155 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Author: 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 + * 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 VN100SpiDefs +{ + +/** + * @brief Internal registers definitions. + */ +enum Registers : uint8_t +{ + REG_MODEL_NUMBER = 1, ///< WhoAmI register + REG_QUAT_IMU_DATA = 15, ///< Quaternion, accelerometer, gyroscope and + ///< magnetometer data register + REG_SYNC = 32, ///< Used to set data ready interrupt + REG_TEMP_PRESS_DATA = 54, ///< Temperature and pressure data register +}; + +/** + * @brief Commands available for the sensor. + */ +enum Commands : uint8_t +{ + READ_REG = 1, + WRITE_REG = 2, +}; + +/** + * @brief Error codes of the sensor. + */ +enum class VNErrors : uint8_t +{ + NO_ERROR = 0, + HARD_FAULT = 1, + SERIAL_BUFFER_OVERFLOW = 2, + INVALID_CHECKSUM = 3, + INVALID_COMMAND = 4, + NOT_ENOUGH_PARAMETERS = 5, + TOO_MANY_PARAMETERS = 6, + INVALID_PARAMETER = 7, + INVALID_REGISTER = 8, + UNAUTHORIZED_ACCESS = 9, + WATCHDOG_RESET = 10, + OUTPUT_BUFFER_OVERFLOW = 11, + INSUFFICIENT_BAUDRATE = 12, + ERROR_BUFFER_OVERFLOW = 255, +}; + +/** + * @brief Data format of the synchronization control register, used for read + * and write operations. + */ +struct __attribute__((packed)) SynchronizationData +{ + uint8_t syncInMode; ///< Behaviour of the syncIn event + uint8_t syncInEdge; ///< Trigger syncIn on rising or falling edge + uint16_t syncInSkipFactor; ///< How many times trigger edges defined by + ///< SyncInEdge should occur prior to triggering + ///< a SyncIn event + const uint32_t RESERVED = 0; ///< Reserved, do not use + uint8_t syncOutMode; ///< Behavior of the SyncOut event + uint8_t syncOutPolarity; ///< The polarity of the output pulse on the + ///< SyncOut pin (positive or negative) + uint16_t syncOutSkipFactor; ///< how many times the sync out event should + ///< be skipped before actually triggering the + ///< SyncOut pin + uint32_t + syncOutPulseWidth; ///< Controls the desired width of the SyncOut pulse + const uint32_t RESERVED2 = 0; ///< Reserved, do not use +}; + +/** + * @brief Data format of the data register (imu and quaternion), used for + * sampling operations. + */ +struct __attribute__((packed)) RawImuQuatData +{ + float quatX; + float quatY; + float quatZ; + float quatW; + float magX; + float magY; + float magZ; + float accX; + float accY; + float accZ; + float gyrX; + float gyrY; + float gyrZ; +}; + +/** + * @brief Data format of the data register (temperature and pressure), used + * for sampling operations. + */ +struct __attribute__((packed)) RawTempPressData +{ + float magX; + float magY; + float magZ; + float accX; + float accY; + float accZ; + float gyrX; + float gyrY; + float gyrZ; + float temp; + float press; +}; + +/** + * @brief The expected model number to be red from the sensor. + */ +const char* const MODEL_NUMBER = "VN-100"; + +/** + * @brief Size of the buffer used to retrieve the model number from the sensor. + * It corresponds to the size of the register, see the datasheet for details. + */ +const int MODEL_NUMBER_SIZE = 24; + +/** + * @brief Width of the SyncOut pulse in nanoseconds. Now is set to 1 + * millisecond. + */ +const uint32_t SYNC_OUT_PULSE_WIDTH = 1000000; + +} // namespace VN100SpiDefs + +} // namespace Boardcore diff --git a/src/tests/sensors/test-vn100.cpp b/src/tests/sensors/test-vn100-serial.cpp similarity index 94% rename from src/tests/sensors/test-vn100.cpp rename to src/tests/sensors/test-vn100-serial.cpp index 5f5fd14ada39d8be9b93487304a685e9819db712..c4aa36e8728d16ae0033b1ea56b26519a6943fef 100644 --- a/src/tests/sensors/test-vn100.cpp +++ b/src/tests/sensors/test-vn100-serial.cpp @@ -22,14 +22,14 @@ #include <drivers/timer/TimestampTimer.h> #include <inttypes.h> -#include <sensors/VN100/VN100.h> +#include <sensors/VN100/VN100Serial.h> using namespace miosix; using namespace Boardcore; int main() { - VN100Data sample; + VN100SerialData sample; string sampleRaw; GpioPin u2tx1(GPIOA_BASE, 2); @@ -41,7 +41,7 @@ int main() u2tx1.mode(Mode::ALTERNATE); USART usart(USART2, 115200); - VN100 sensor{usart, 115200, VN100::CRCOptions::CRC_ENABLE_16}; + VN100Serial sensor{usart, 115200, VN100Serial::CRCOptions::CRC_ENABLE_16}; // Let the sensor start up Thread::sleep(1000); diff --git a/src/tests/sensors/test-vn100-spi.cpp b/src/tests/sensors/test-vn100-spi.cpp new file mode 100644 index 0000000000000000000000000000000000000000..dd0a7e091cb5b55e3d05da3ad47da15b6c92fca9 --- /dev/null +++ b/src/tests/sensors/test-vn100-spi.cpp @@ -0,0 +1,92 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Author: 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 + * 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. + */ + +#include <drivers/spi/SPIDriver.h> +#include <sensors/VN100/VN100Spi.h> + +using namespace miosix; +using namespace Boardcore; + +int main() +{ + // spi setup + SPIBus bus(SPI3); + + GpioPin csPin(GPIOE_BASE, 3); // PE3 CS + csPin.mode(Mode::OUTPUT); + GpioPin clockPin(GPIOB_BASE, 3); // PB3 CK + clockPin.mode(Mode::ALTERNATE); + clockPin.alternateFunction(6); + GpioPin misoPin(GPIOB_BASE, 4); // PB4 MISO + misoPin.mode(Mode::ALTERNATE); + misoPin.alternateFunction(6); + GpioPin mosiPin(GPIOB_BASE, 5); // PB5 MOSI + mosiPin.mode(Mode::ALTERNATE); + mosiPin.alternateFunction(6); + + GpioPin intPin(GPIOC_BASE, 15); // PC15 interrupt pin + intPin.mode(Mode::INPUT); + + SPIBusConfig busConfiguration; + busConfiguration.clockDivider = SPI::ClockDivider::DIV_64; + busConfiguration.mode = SPI::Mode::MODE_3; + + VN100Spi sensor(bus, csPin, busConfiguration, 200); + + // Let the sensor start up + Thread::sleep(1000); + + if (!sensor.init()) + { + printf("Error, cannot initialize the sensor\n\n"); + return 0; + } + printf("Sensor initialized\n"); + + if (!sensor.selfTest()) + { + printf("Error while performing self test\n\n"); + return 0; + } + printf("Self test successful\n"); + + for (int i = 0; i < 100; ++i) + { + sensor.sample(); + VN100SpiData sample = sensor.getLastSample(); + + printf("sample %d:\n", i + 1); + 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\n", sample.quaternionX, + sample.quaternionY, sample.quaternionZ, sample.quaternionW); + + Thread::sleep(500); + } + + return 0; +}