Lolin S2 Mini & Sparkfun BNO080
Photo
Sketch
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| // LiLon ESP32-S2 with BNO080 IMU - Quaternion & Euler Angles | |
| // Copyright (C) 2023 https://www.roboticboat.uk | |
| // 44bfb3ca-2c8f-4e36-b123-6f2655c631e4 | |
| // | |
| // This program is free software: you can redistribute it and/or modify | |
| // it under the terms of the GNU General Public License as published by | |
| // the Free Software Foundation, either version 3 of the License, or | |
| // (at your option) any later version. | |
| // | |
| // This program is distributed in the hope that it will be useful, | |
| // but WITHOUT ANY WARRANTY; without even the implied warranty of | |
| // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | |
| // GNU General Public License for more details. | |
| // | |
| // You should have received a copy of the GNU General Public License | |
| // along with this program. If not, see <https://www.gnu.org/licenses/>. | |
| // These Terms shall be governed and construed in accordance with the laws of | |
| // England and Wales, without regard to its conflict of law provisions. | |
| // IDE Arduino 1.8.19 | |
| // Register information (for example which register does what) was obtained from BNO080 datasheet v1.3 | |
| // Copyright © 2017, Hillcrest Laboratories, Inc. All rights reserved | |
| #include <Wire.h> | |
| #define I2C_SDA 3 | |
| #define I2C_SCL 5 | |
| #define COMMUNICATION_TIMEOUT 1000 | |
| #define I2C_BUFFER_SIZE 16 | |
| #define PACKET_BUFFER_SIZE 1000 | |
| #define COMMAND_PACKET_SIZE 21 | |
| struct Quaternion { | |
| float real, i, j, k; | |
| }; | |
| struct Euler { | |
| float roll, pitch, yaw; | |
| float roll2, pitch2, yaw2; | |
| }; | |
| uint8_t nZeroReadings = 0; | |
| uint8_t nReceived = 0; | |
| uint8_t _i2cAddress = 0x4B; | |
| uint8_t receiveBuffer[14]; | |
| uint8_t packetBuffer[PACKET_BUFFER_SIZE]; | |
| uint8_t commandPacket[COMMAND_PACKET_SIZE]; | |
| uint8_t packetSequencialNumber = 0; | |
| uint8_t resetSequencialNumber = 0; | |
| uint16_t packetBufferPtr; | |
| float accelX, accelY, accelZ; | |
| float gyroX, gyroY, gyroZ; | |
| float magnetX, magnetY, magnetZ; | |
| float linearAccelX, linearAccelY, linearAccelZ; | |
| byte accel_status; | |
| byte gyro_status; | |
| byte magnet_status; | |
| byte linearAccel_status; | |
| byte quantRotationVec_status; | |
| byte quantGameRotationVec_status; | |
| int16_t accelXraw, accelYraw, accelZraw; | |
| int16_t gyroXraw, gyroYraw, gyroZraw; | |
| int16_t magnetXraw, magnetYraw, magnetZraw; | |
| int16_t quantRotationVec_accuracy; | |
| Quaternion quantRotationVec; | |
| Quaternion quantGameRotationVec; | |
| Euler euler; | |
| void setup() | |
| { | |
| // Update the User | |
| Serial.begin(115200); | |
| // Start the i2c network | |
| Wire.begin(I2C_SDA, I2C_SCL); | |
| // Set the clock frequency for I2C communication to 400 Khz | |
| Wire.setClock(400000); | |
| // Initialise the BMO080 IMU module | |
| SetupBMO080(); | |
| } | |
| void loop() | |
| { | |
| receivePacket(); | |
| // Check quality of data | |
| if (accelX == 0 && accelY == 0){nZeroReadings++;} | |
| // If we have too many zeros, then restart the BNO080 | |
| if (nZeroReadings > 20){ | |
| // We are not synconised correctly | |
| Serial.println("Out of synchronous with data received. Restart BNO080"); | |
| SetupBMO080(); | |
| nZeroReadings = 0; | |
| } | |
| // print the data | |
| Serial.print("$ACC,"); | |
| Serial.print(accelX); | |
| Serial.print(","); | |
| Serial.print(accelY); | |
| Serial.print(","); | |
| Serial.print(accelZ); | |
| Serial.print(","); | |
| printStatus(accel_status); | |
| Serial.print(" m/s^2\t"); | |
| // Serial.print("$lnrACC,"); | |
| // Serial.print(linearAccelX); | |
| // Serial.print(","); | |
| // Serial.print(linearAccelY); | |
| // Serial.print(","); | |
| // Serial.print(linearAccelZ); | |
| // Serial.print(" m/s^2\t"); | |
| // | |
| // Serial.print("$GYR,"); | |
| // Serial.print(gyroX); | |
| // Serial.print(","); | |
| // Serial.print(gyroY); | |
| // Serial.print(","); | |
| // Serial.print(gyroZ); | |
| // Serial.print(" rad/s\t"); | |
| Serial.print("$MAG,"); | |
| Serial.print(magnetX); | |
| Serial.print(","); | |
| Serial.print(magnetY); | |
| Serial.print(","); | |
| Serial.print(magnetZ); | |
| Serial.print(","); | |
| printStatus(magnet_status); | |
| Serial.print(" uTesla \t"); | |
| Serial.print("$QUA,"); | |
| Serial.print(quantRotationVec.real, 4); | |
| Serial.print(","); | |
| Serial.print(quantRotationVec.i, 4); | |
| Serial.print(","); | |
| Serial.print(quantRotationVec.j, 4); | |
| Serial.print(","); | |
| Serial.print(quantRotationVec.k, 4); | |
| Serial.print(","); | |
| printStatus(quantRotationVec_status); | |
| Serial.print(","); | |
| Serial.print(quantRotationVec_accuracy); | |
| Serial.print(" rijk\t\t"); | |
| Quanternion2Euler(quantRotationVec); | |
| Serial.print("$EUL,"); | |
| Serial.print(euler.yaw, 1); | |
| Serial.print(","); | |
| Serial.print(euler.roll, 1); | |
| Serial.print(","); | |
| Serial.print(euler.pitch, 1); | |
| Serial.println(" degrees yaw,roll,pitch"); | |
| delay(10); | |
| } | |
| void SetupBMO080() | |
| { | |
| // Set the commandPacket to zero | |
| memset(commandPacket, 0, sizeof(commandPacket)); | |
| delay(1000); | |
| // Restart the BMO080 module | |
| Restart(); | |
| // Set Accelerator, reportID 0x01 at 50ms intervals | |
| SetPacketInterval(0x01, 50); | |
| // Set Gyroscope, reportID 0x02 at 50ms intervals | |
| SetPacketInterval(0x02, 50); | |
| // Set Magnet, reportID 0x03 at 50ms intervals | |
| SetPacketInterval(0x03, 50); | |
| // Set Linear Accelerator, reportID 0x04 at 50ms intervals | |
| SetPacketInterval(0x04, 50); | |
| // Set Raw Accelerator, reportID 0x14 at 50ms intervals | |
| SetPacketInterval(0x14, 50); | |
| // Set Raw Gyroscope, reportID 0x15 at 50ms intervals | |
| SetPacketInterval(0x15, 50); | |
| // Set Raw Gyroscope, reportID 0x16 at 50ms intervals | |
| SetPacketInterval(0x16, 50); | |
| // Set Rotation Vector, reportID 0x08 at 50ms intervals | |
| SetPacketInterval(0x05, 50); | |
| // Set Game Rotation Vector, reportID 0x08 at 50ms intervals | |
| SetPacketInterval(0x08, 50); | |
| } | |
| void Restart() { | |
| // Allocate memory | |
| uint8_t resetPacket[5]; | |
| // Size of data transmission | |
| resetPacket[0] = 0x05; | |
| resetPacket[1] = 0x00; | |
| // Channel 1 | |
| resetPacket[2] = 0x01; | |
| // Sequence number | |
| resetPacket[3] = resetSequencialNumber++; | |
| // Data command | |
| resetPacket[4] = 0x01; | |
| // Send the packet | |
| uint8_t status = writeBytes(5, resetPacket); | |
| if (status != 0){ | |
| Serial.println("Restart status error"); | |
| } | |
| // Wait for the BNO080 to restart. This delay is essential | |
| delay(2000); | |
| } | |
| void SetPacketInterval(uint8_t reportID, uint16_t milliSeconds) | |
| { | |
| // Create header | |
| // Least significant byte (LSB) size of data | |
| commandPacket[0] = (COMMAND_PACKET_SIZE) & 0xFF; | |
| // Most significant byte (MSB) size of data | |
| commandPacket[1] = ((COMMAND_PACKET_SIZE) >> 8) & 0xFF; | |
| // Channel number hard coded at 2 | |
| commandPacket[2] = 2; | |
| // Sequencial 8-bit number to indicate following packet | |
| commandPacket[3] = packetSequencialNumber++; | |
| // Set options Command reference | |
| commandPacket[4] = 0xFD; | |
| // ReportID reference | |
| commandPacket[5] = reportID; | |
| // Time interval in microseconds | |
| uint32_t microSeconds = (uint32_t)milliSeconds * 1000; | |
| // Set the microsecond interval | |
| commandPacket[9] = microSeconds & 0xFF; | |
| commandPacket[10] = (microSeconds >> 8) & 0xFF; | |
| commandPacket[11] = (microSeconds >> 16) & 0xFF; | |
| commandPacket[12] = (microSeconds >> 24) & 0xFF; | |
| // Send the packet | |
| uint8_t status = writeBytes(COMMAND_PACKET_SIZE, commandPacket); | |
| if (status != 0){ | |
| Serial.print("SetPacketInterval status error"); | |
| } | |
| } | |
| float Convert2float(int16_t intvalue, float divisor) { | |
| float fvalue = intvalue; | |
| return fvalue / divisor; | |
| } | |
| uint8_t receivePacket() | |
| { | |
| // Header bytes to receive | |
| uint8_t numHeaderBytes = 4; | |
| uint8_t headerBuffer[numHeaderBytes]; | |
| uint8_t dataBuffer[I2C_BUFFER_SIZE]; | |
| //char showHex[3]; | |
| // Reset packet buffer pointer | |
| packetBufferPtr = 0; | |
| // Number of bytes to receive | |
| nReceived = Wire.requestFrom(_i2cAddress, numHeaderBytes); | |
| if (nReceived != numHeaderBytes) { | |
| Serial.println("failed 3"); | |
| //Wire.flush(); | |
| return 0; | |
| } | |
| // Setup timeout parameter | |
| int timeout = COMMUNICATION_TIMEOUT; | |
| // Wait for the bytes to arrive. | |
| // Don't wait forever as this will hang the whole program | |
| while ((Wire.available() < numHeaderBytes) && (timeout-- > 0)) | |
| delay(1); | |
| // Waited too long | |
| if (timeout <= 0) return (uint8_t)5; | |
| // Read the data into the headerBuffer | |
| for (uint8_t i = 0; i < numHeaderBytes; i++) | |
| { | |
| headerBuffer[i] = Wire.read(); | |
| //sprintf (showHex, "%02x", headerBuffer[i]); | |
| //Serial.print(showHex); | |
| } | |
| // Data package length | |
| uint16_t numDataBytes = (uint16_t)(headerBuffer[1] << 8 | headerBuffer[0]); | |
| if (numDataBytes == 0) return (uint8_t)8; | |
| numDataBytes = numDataBytes - numHeaderBytes; | |
| // Need to break up into 32 byte reads | |
| uint16_t bytesRemaining = numDataBytes; | |
| while (bytesRemaining > 0) | |
| { | |
| uint16_t dataBytes = bytesRemaining; | |
| if (dataBytes > I2C_BUFFER_SIZE - 4) dataBytes = I2C_BUFFER_SIZE - 4; | |
| // Number of bytes to receive | |
| nReceived = Wire.requestFrom(_i2cAddress, (uint8_t)(dataBytes + 4)); | |
| if (nReceived != dataBytes + 4) { | |
| Serial.println("failed 4"); | |
| return (uint8_t)55; | |
| } | |
| // Skip over the SHTP header bytes 0,1,2 & 3 | |
| for (uint8_t i = 0; i < 4; i++) Wire.read(); | |
| // Read the data into the headerBuffer | |
| for (uint8_t i = 0; i < dataBytes; i++) | |
| { | |
| dataBuffer[i] = Wire.read(); | |
| packetBuffer[packetBufferPtr] = dataBuffer[i]; | |
| packetBufferPtr++; | |
| if (packetBufferPtr == PACKET_BUFFER_SIZE) packetBufferPtr = PACKET_BUFFER_SIZE - 1; | |
| } | |
| bytesRemaining = bytesRemaining - dataBytes; | |
| } | |
| // Lets try and process this packetBuffer. | |
| uint16_t tmp = 5; | |
| uint8_t inc = 0; | |
| while (tmp < packetBufferPtr) { | |
| //Serial.print("["); | |
| //sprintf (showHex, "%02x", packetBuffer[tmp]); | |
| //Serial.print(showHex); | |
| //Serial.print(": size "); | |
| inc = processReport(packetBuffer[tmp], tmp); | |
| //Serial.print(inc); | |
| //Serial.print("] "); | |
| if (inc == 0) break; | |
| tmp = tmp + inc; | |
| } | |
| //Serial.println(""); | |
| return (uint8_t)0; | |
| } | |
| uint8_t processReport(uint8_t reportID, uint16_t tmp) | |
| { | |
| // Want to know the number of bytes in the report | |
| switch (reportID) { | |
| case 0x01: | |
| // Accelerometer (0x01) | |
| // The Q point is 8. 2^8=256 | |
| accel_status = packetBuffer[tmp + 2] & 0x03; | |
| accelX = Convert2float((int16_t) packetBuffer[tmp + 5] << 8 | packetBuffer[tmp + 4], 256.0f); | |
| accelY = Convert2float((int16_t) packetBuffer[tmp + 7] << 8 | packetBuffer[tmp + 6], 256.0f); | |
| accelZ = Convert2float((int16_t) packetBuffer[tmp + 9] << 8 | packetBuffer[tmp + 8], 256.0f); | |
| return (uint8_t)10; | |
| case 0x02: | |
| // Gyroscope Calibrated (0x02) | |
| // The Q point is 9. 2^9=512 | |
| gyro_status = packetBuffer[tmp + 2] & 0x03; | |
| gyroX = Convert2float((int16_t) packetBuffer[tmp + 5] << 8 | packetBuffer[tmp + 4], 512.0f); | |
| gyroY = Convert2float((int16_t) packetBuffer[tmp + 7] << 8 | packetBuffer[tmp + 6], 512.0f); | |
| gyroZ = Convert2float((int16_t) packetBuffer[tmp + 9] << 8 | packetBuffer[tmp + 8], 512.0f); | |
| return (uint8_t)10; | |
| case 0x03: | |
| // Magnetic Field Calibrated (0x03) | |
| // The Q point is 4. 2^4 = 16 | |
| magnet_status = packetBuffer[tmp + 2] & 0x03; | |
| magnetX = Convert2float((int16_t) packetBuffer[tmp + 5] << 8 | packetBuffer[tmp + 4], 16.0f); | |
| magnetY = Convert2float((int16_t) packetBuffer[tmp + 7] << 8 | packetBuffer[tmp + 6], 16.0f); | |
| magnetZ = Convert2float((int16_t) packetBuffer[tmp + 9] << 8 | packetBuffer[tmp + 8], 16.0f); | |
| return (uint8_t)10; | |
| case 0x04: | |
| // Linear Acceleration (0x04) | |
| // The Q point is 8. 2^8=256 | |
| linearAccel_status = packetBuffer[tmp + 2] & 0x03; | |
| linearAccelX = Convert2float((int16_t) packetBuffer[tmp + 5] << 8 | packetBuffer[tmp + 4], 256.0f); | |
| linearAccelY = Convert2float((int16_t) packetBuffer[tmp + 7] << 8 | packetBuffer[tmp + 6], 256.0f); | |
| linearAccelZ = Convert2float((int16_t) packetBuffer[tmp + 9] << 8 | packetBuffer[tmp + 8], 256.0f); | |
| return (uint8_t)10; | |
| case 0x05: | |
| // Rotation Vector (0x05) | |
| quantRotationVec_status = packetBuffer[tmp + 2] & 0x03; | |
| quantRotationVec.i = Convert2float((int16_t) packetBuffer[tmp + 5] << 8 | packetBuffer[tmp + 4], 16384.0f); | |
| quantRotationVec.j = Convert2float((int16_t) packetBuffer[tmp + 7] << 8 | packetBuffer[tmp + 6], 16384.0f); | |
| quantRotationVec.k = Convert2float((int16_t) packetBuffer[tmp + 9] << 8 | packetBuffer[tmp + 8], 16384.0f); | |
| quantRotationVec.real = Convert2float((int16_t) packetBuffer[tmp + 11] << 8 | packetBuffer[tmp + 10], 16384.0f); | |
| quantRotationVec_accuracy = (int16_t) packetBuffer[tmp + 13] << 8 | packetBuffer[tmp + 12]; | |
| return (uint8_t)14; | |
| case 0x06: | |
| // Gravity (0x06) | |
| return (uint8_t)10; | |
| case 0x07: | |
| // Gravity Uncalibrated (0x07) | |
| return (uint8_t)16; | |
| case 0x08: | |
| // Game Rotation Vector (0x08) | |
| // The Q point is 14. 2^14 = 16384 | |
| quantGameRotationVec_status = packetBuffer[tmp + 2] & 0x03; | |
| quantGameRotationVec.i = Convert2float((int16_t) packetBuffer[tmp + 5] << 8 | packetBuffer[tmp + 4], 16384.0f); | |
| quantGameRotationVec.j = Convert2float((int16_t) packetBuffer[tmp + 7] << 8 | packetBuffer[tmp + 6], 16384.0f); | |
| quantGameRotationVec.k = Convert2float((int16_t) packetBuffer[tmp + 9] << 8 | packetBuffer[tmp + 8], 16384.0f); | |
| quantGameRotationVec.real = Convert2float((int16_t) packetBuffer[tmp + 11] << 8 | packetBuffer[tmp + 10], 16384.0f); | |
| return (uint8_t)12; | |
| case 0x09: | |
| // Geomagnetic Rotation Vec (0x09) | |
| return (uint8_t)14; | |
| case 0x0F: | |
| // Magnetic Field Uncalibrat (0x0F) | |
| return (uint8_t)16; | |
| case 0x10: | |
| // Tap Detector (0x10) | |
| return (uint8_t)5; | |
| case 0x11: | |
| // Step Counter (0x11) | |
| return (uint8_t)12; | |
| case 0x12: | |
| // Significant Motion (0x12) | |
| return (uint8_t)6; | |
| case 0x13: | |
| // Stability Classifier (0x13) | |
| return (uint8_t)6; | |
| case 0x14: | |
| // Raw Accelerometer (0x14) | |
| accelXraw = (int16_t) packetBuffer[tmp + 5] << 8 | packetBuffer[tmp + 4]; | |
| accelYraw = (int16_t) packetBuffer[tmp + 7] << 8 | packetBuffer[tmp + 6]; | |
| accelZraw = (int16_t) packetBuffer[tmp + 9] << 8 | packetBuffer[tmp + 8]; | |
| return (uint8_t)16; | |
| case 0x15: | |
| // Raw Gyroscope (0x15) | |
| gyroXraw = (int16_t) packetBuffer[tmp + 5] << 8 | packetBuffer[tmp + 4]; | |
| gyroYraw = (int16_t) packetBuffer[tmp + 7] << 8 | packetBuffer[tmp + 6]; | |
| gyroZraw = (int16_t) packetBuffer[tmp + 9] << 8 | packetBuffer[tmp + 8]; | |
| return (uint8_t)16; | |
| case 0x16: | |
| // Raw Magnetometer (0x16) | |
| magnetXraw = (int16_t) packetBuffer[tmp + 5] << 8 | packetBuffer[tmp + 4]; | |
| magnetYraw = (int16_t) packetBuffer[tmp + 7] << 8 | packetBuffer[tmp + 6]; | |
| magnetZraw = (int16_t) packetBuffer[tmp + 9] << 8 | packetBuffer[tmp + 8]; | |
| return (uint8_t)16; | |
| case 0x18: | |
| // Step Detector (0x18) | |
| return (uint8_t)8; | |
| case 0x19: | |
| // Shake Detector (0x19) | |
| return (uint8_t)6; | |
| case 0x1A: | |
| // Flip Detector (0x1A) | |
| return (uint8_t)6; | |
| case 0x1B: | |
| // Pickup Detector (0x1B) | |
| return (uint8_t)6; | |
| case 0x1C: | |
| // Stability Detector (0x1C) | |
| return (uint8_t)6; | |
| case 0x1F: | |
| // Sleep Detector (0x1F) | |
| return (uint8_t)6; | |
| case 0x20: | |
| // Tilt Detector (0x20) | |
| return (uint8_t)6; | |
| case 0x28: | |
| // ARVR-Stabilized Rotation Vector (0x28) | |
| return (uint8_t)14; | |
| case 0x29: | |
| // ARVR-Stabilized Game Rotation Vector (0x29) | |
| return (uint8_t)12; | |
| case 0x2A: | |
| // Gyro-Integrated Rotation Vector (0x2A) | |
| return (uint8_t)14; | |
| case 0xFA: | |
| // Timestamp Rebase (0xFA) | |
| return (uint8_t)5; | |
| default: | |
| return 0; | |
| } | |
| } | |
| //========== i2c commands =============== | |
| // | |
| // NACK errors are | |
| // 0: OK - successful send | |
| // 1: Send receiveBuffer too large for the twi receiveBuffer. | |
| // 2: Register address was sent and a NACK received. Master should send a STOP condition. | |
| // 3: Data was sent and a NACK received. Slave has sent all data. Master can send a STOP condition. | |
| // 4: A different twi error took place | |
| // 5: Time out on waiting for data to be received. | |
| // 6: Checking data was correctly written failed | |
| uint8_t readRegister(uint8_t registerAddress, uint8_t* dest) | |
| { | |
| // Initialise the transmit message buffer | |
| Wire.beginTransmission(_i2cAddress); | |
| // Add the i2c module Register address to transmit message buffer | |
| Wire.write(registerAddress); | |
| // Transmit message and accept NACK response | |
| // The Wire.endTransmission() is used when writing data only | |
| // Send a restart to keep connection alive | |
| uint8_t nackCatcher = Wire.endTransmission(false); | |
| // Return if we have a connection problem | |
| if (nackCatcher != 0) return nackCatcher; | |
| // Number of bytes to receive | |
| nReceived = Wire.requestFrom(_i2cAddress, (uint8_t)1); | |
| if (nReceived == 0) { | |
| Serial.println("failed 1"); | |
| return (uint8_t)5; | |
| } | |
| // Read the data | |
| *dest = Wire.read(); | |
| // All ok | |
| return (uint8_t)0; | |
| } | |
| uint8_t readRegisters(uint8_t registerAddress, uint8_t numBytes, uint8_t* dest) | |
| { | |
| // Initialise the transmit message buffer | |
| Wire.beginTransmission(_i2cAddress); | |
| // Add the i2c module Register address to transmit message buffer | |
| Wire.write(registerAddress); | |
| // Transmit message and accept NACK response | |
| // The Wire.endTransmission() is used when writing data only | |
| // Send a restart to keep connection alive | |
| uint8_t nackCatcher = Wire.endTransmission(false); | |
| // Return if we have a connection problem | |
| if (nackCatcher != 0) return nackCatcher; | |
| // Number of bytes to receive | |
| nReceived = Wire.requestFrom(_i2cAddress, numBytes); | |
| if (nReceived != numBytes) { | |
| Serial.println("failed 2"); | |
| return (uint8_t)5; | |
| } | |
| uint8_t i = 0; | |
| // read the data into the receiveBuffer | |
| while ( Wire.available() ) | |
| { | |
| dest[i++] = Wire.read(); | |
| } | |
| // All ok | |
| return (uint8_t)0; | |
| } | |
| uint8_t writeRegister(uint8_t registerAddress, uint8_t dataByte) { | |
| // Initialise the transmit message buffer | |
| Wire.beginTransmission(_i2cAddress); | |
| // Add the i2c module Register address to transmit message buffer | |
| Wire.write(registerAddress); | |
| // Add the command data to the transmit message buffer | |
| Wire.write(dataByte); | |
| // Transmit message and accept NACK response | |
| // The Wire.endTransmission() is used when writing data only | |
| uint8_t nackCatcher = Wire.endTransmission(); | |
| // Return if we have a connection problem | |
| if (nackCatcher != 0) return nackCatcher; | |
| // Wait for 10ms | |
| delay(10); | |
| // Allocate memory. | |
| uint8_t checkByte; | |
| // We are now going to check the operation was successful | |
| // This is not really required, but most of the time we are debugging | |
| // Read the Register | |
| readRegister(registerAddress, &checkByte); | |
| // Check the data read back is the same | |
| if (checkByte == dataByte) return (uint8_t)0; | |
| // Update the User | |
| Serial.println("ERROR: i2c module register not accepting writes"); | |
| // An error happened | |
| return (uint8_t)6; | |
| } | |
| uint8_t writeBytes(uint8_t numBytes, uint8_t* dataBytes) { | |
| // Initialise the transmit message buffer | |
| Wire.beginTransmission(_i2cAddress); | |
| // Loop over the bytes to send | |
| for (uint8_t i = 0; i < numBytes; i++) | |
| { | |
| // Add the command data to the transmit message buffer | |
| Wire.write(dataBytes[i]); | |
| } | |
| // Transmit message and accept NACK response | |
| // The Wire.endTransmission() is used when writing data only | |
| uint8_t nackCatcher = Wire.endTransmission(); | |
| // Return nack | |
| return nackCatcher; | |
| } | |
| void Quanternion2Euler(Quaternion q) | |
| { | |
| // Calculates the Euler ZYX angles | |
| // Derived by comparing the Quaternion and Euler rotation matrices | |
| // Return angles in degrees (not radians) | |
| float norm = sqrt(q.real * q.real + q.i * q.i + q.j * q.j + q.k * q.k); | |
| float dqw = q.real / norm; | |
| float dqx = q.i / norm; | |
| float dqy = q.j / norm; | |
| float dqz = q.k / norm; | |
| float ysqr = dqy * dqy; | |
| // roll (x-axis rotation) | |
| float t0 = +2.0 * (dqw * dqx + dqy * dqz); | |
| float t1 = +1.0 - 2.0 * (dqx * dqx + ysqr); | |
| euler.roll = atan2(t0, t1) * 180.0f / PI; | |
| // pitch (y-axis rotation) | |
| float t2 = +2.0 * (dqw * dqy - dqz * dqx); | |
| t2 = t2 > 1.0 ? 1.0 : t2; | |
| t2 = t2 < -1.0 ? -1.0 : t2; | |
| euler.pitch = asin(t2) * 180.0f / PI; | |
| // yaw (z-axis rotation) | |
| float t3 = +2.0 * (dqw * dqz + dqx * dqy); | |
| float t4 = +1.0 - 2.0 * (ysqr + dqz * dqz); | |
| euler.yaw = atan2(t3, t4) * 180.0f / PI; | |
| euler.yaw = -euler.yaw; | |
| if (euler.yaw > 360) euler.yaw -= 360; | |
| if (euler.yaw < 0) euler.yaw += 360; | |
| } | |
| void printStatus(byte level) { | |
| // Want to know the number of bytes in the report | |
| switch (level) { | |
| case 0x00: | |
| Serial.print("D"); | |
| return; | |
| case 0x01: | |
| Serial.print("C"); | |
| return; | |
| case 0x02: | |
| Serial.print("B"); | |
| return; | |
| case 0x03: | |
| Serial.print("A"); | |
| return; | |
| } | |
| } |