From 5ec06bf075b9f9b4b451d13690a6a88fd949745c Mon Sep 17 00:00:00 2001 From: Eiren Rain Date: Tue, 23 Feb 2021 17:57:44 +0300 Subject: [PATCH] Added new offset finder code for MPU6050 Added code for BNO08X Added code to send serial to server --- include/6axismotion.cpp | 13 +- include/9axismotion.cpp | 2 +- include/BNO080.cpp | 1637 ++++++++++++++++++++++ include/BNO080.h | 303 ++++ include/MPU6050OffsetFinder.cpp | 367 +++++ include/bno085.cpp | 82 ++ include/{motionbase.cpp => motionbase.h} | 7 +- include/udpclient.cpp | 27 + include/udpclient.h | 1 + src/main.cpp | 6 +- 10 files changed, 2438 insertions(+), 7 deletions(-) create mode 100644 include/BNO080.cpp create mode 100644 include/BNO080.h create mode 100644 include/MPU6050OffsetFinder.cpp create mode 100644 include/bno085.cpp rename include/{motionbase.cpp => motionbase.h} (88%) diff --git a/include/6axismotion.cpp b/include/6axismotion.cpp index 8b66816..f31f8dc 100644 --- a/include/6axismotion.cpp +++ b/include/6axismotion.cpp @@ -1,4 +1,5 @@ -#include "motionbase.cpp" +#include "motionbase.h" +#include "MPU6050OffsetFinder.cpp" // MPU control/status vars bool dmpReady = false; // set true if DMP init was successful @@ -69,7 +70,7 @@ void motionLoop() { // track FIFO count here in case there is > 1 packet available // (this lets us immediately read more without waiting for an interrupt) fifoCount -= packetSize; - + accelgyro.dmpGetQuaternion(&rawQuat, fifoBuffer); q[0] = rawQuat.x; q[1] = rawQuat.y; @@ -86,6 +87,13 @@ void sendData() { void performCalibration() { digitalWrite(CALIBRATING_LED, LOW); + Serial.println("Starting offset finder"); + findOffset(); + Serial.println("Process is over"); + digitalWrite(CALIBRATING_LED, HIGH); +} + +void gatherCalibrationData() { Serial.println("Gathering raw data for device calibration..."); int calibrationSamples = 500; // Reset values @@ -139,5 +147,4 @@ void performCalibration() { delay(50); } Serial.println("Calibration data gathered and sent"); - digitalWrite(CALIBRATING_LED, HIGH); } \ No newline at end of file diff --git a/include/9axismotion.cpp b/include/9axismotion.cpp index 835c16a..5d48d00 100644 --- a/include/9axismotion.cpp +++ b/include/9axismotion.cpp @@ -1,4 +1,4 @@ -#include "motionbase.cpp" +#include "motionbase.h" //raw data and scaled as vector int16_t ax, ay, az; diff --git a/include/BNO080.cpp b/include/BNO080.cpp new file mode 100644 index 0000000..1e64615 --- /dev/null +++ b/include/BNO080.cpp @@ -0,0 +1,1637 @@ +/* + This is a library written for the BNO080 + SparkFun sells these at its website: www.sparkfun.com + Do you like this library? Help support SparkFun. Buy a board! + https://www.sparkfun.com/products/14686 + + Written by Nathan Seidle @ SparkFun Electronics, December 28th, 2017 + + The BNO080 IMU is a powerful triple axis gyro/accel/magnetometer coupled with an ARM processor + to maintain and complete all the complex calculations for various VR, inertial, step counting, + and movement operations. + + This library handles the initialization of the BNO080 and is able to query the sensor + for different readings. + + https://github.com/sparkfun/SparkFun_BNO080_Arduino_Library + + Development environment specifics: + Arduino IDE 1.8.5 + + SparkFun code, firmware, and software is released under the MIT License. + Please see LICENSE.md for further details. +*/ + +#include "BNO080.h" + +//Attempt communication with the device +//Return true if we got a 'Polo' back from Marco +boolean BNO080::begin(uint8_t deviceAddress, TwoWire &wirePort, uint8_t intPin) +{ + _deviceAddress = deviceAddress; //If provided, store the I2C address from user + _i2cPort = &wirePort; //Grab which port the user wants us to use + _int = intPin; //Get the pin that the user wants to use for interrupts. By default, it's 255 and we'll not use it in dataAvailable() function. + if (_int != 255) + { + pinMode(_int, INPUT_PULLUP); + } + + //We expect caller to begin their I2C port, with the speed of their choice external to the library + //But if they forget, we start the hardware here. + //_i2cPort->begin(); + + //Begin by resetting the IMU + softReset(); + + //Check communication with device + shtpData[0] = SHTP_REPORT_PRODUCT_ID_REQUEST; //Request the product ID and reset info + shtpData[1] = 0; //Reserved + + //Transmit packet on channel 2, 2 bytes + sendPacket(CHANNEL_CONTROL, 2); + + //Now we wait for response + if (receivePacket() == true) + { + if (shtpData[0] == SHTP_REPORT_PRODUCT_ID_RESPONSE) + { + if (_printDebug == true) + { + _debugPort->print(F("SW Version Major: 0x")); + _debugPort->print(shtpData[2], HEX); + _debugPort->print(F(" SW Version Minor: 0x")); + _debugPort->print(shtpData[3], HEX); + uint32_t SW_Part_Number = ((uint32_t)shtpData[7] << 24) | ((uint32_t)shtpData[6] << 16) | ((uint32_t)shtpData[5] << 8) | ((uint32_t)shtpData[4]); + _debugPort->print(F(" SW Part Number: 0x")); + _debugPort->print(SW_Part_Number, HEX); + uint32_t SW_Build_Number = ((uint32_t)shtpData[11] << 24) | ((uint32_t)shtpData[10] << 16) | ((uint32_t)shtpData[9] << 8) | ((uint32_t)shtpData[8]); + _debugPort->print(F(" SW Build Number: 0x")); + _debugPort->print(SW_Build_Number, HEX); + uint16_t SW_Version_Patch = ((uint16_t)shtpData[13] << 8) | ((uint16_t)shtpData[12]); + _debugPort->print(F(" SW Version Patch: 0x")); + _debugPort->println(SW_Version_Patch, HEX); + } + return (true); + } + } + + return (false); //Something went wrong +} + +boolean BNO080::beginSPI(uint8_t user_CSPin, uint8_t user_WAKPin, uint8_t user_INTPin, uint8_t user_RSTPin, uint32_t spiPortSpeed, SPIClass &spiPort) +{ + _i2cPort = NULL; //This null tells the send/receive functions to use SPI + + //Get user settings + _spiPort = &spiPort; + _spiPortSpeed = spiPortSpeed; + if (_spiPortSpeed > 3000000) + _spiPortSpeed = 3000000; //BNO080 max is 3MHz + + _cs = user_CSPin; + _wake = user_WAKPin; + _int = user_INTPin; + _rst = user_RSTPin; + + pinMode(_cs, OUTPUT); + pinMode(_wake, OUTPUT); + pinMode(_int, INPUT_PULLUP); + pinMode(_rst, OUTPUT); + + digitalWrite(_cs, HIGH); //Deselect BNO080 + + //Configure the BNO080 for SPI communication + digitalWrite(_wake, HIGH); //Before boot up the PS0/WAK pin must be high to enter SPI mode + digitalWrite(_rst, LOW); //Reset BNO080 + delay(2); //Min length not specified in datasheet? + digitalWrite(_rst, HIGH); //Bring out of reset + + //Wait for first assertion of INT before using WAK pin. Can take ~104ms + waitForSPI(); + + //if(wakeBNO080() == false) //Bring IC out of sleep after reset + // Serial.println("BNO080 did not wake up"); + + _spiPort->begin(); //Turn on SPI hardware + + //At system startup, the hub must send its full advertisement message (see 5.2 and 5.3) to the + //host. It must not send any other data until this step is complete. + //When BNO080 first boots it broadcasts big startup packet + //Read it and dump it + waitForSPI(); //Wait for assertion of INT before reading advert message. + receivePacket(); + + //The BNO080 will then transmit an unsolicited Initialize Response (see 6.4.5.2) + //Read it and dump it + waitForSPI(); //Wait for assertion of INT before reading Init response + receivePacket(); + + //Check communication with device + shtpData[0] = SHTP_REPORT_PRODUCT_ID_REQUEST; //Request the product ID and reset info + shtpData[1] = 0; //Reserved + + //Transmit packet on channel 2, 2 bytes + sendPacket(CHANNEL_CONTROL, 2); + + //Now we wait for response + waitForSPI(); + if (receivePacket() == true) + { + if (shtpData[0] == SHTP_REPORT_PRODUCT_ID_RESPONSE) + if (_printDebug == true) + { + _debugPort->print(F("SW Version Major: 0x")); + _debugPort->print(shtpData[2], HEX); + _debugPort->print(F(" SW Version Minor: 0x")); + _debugPort->print(shtpData[3], HEX); + uint32_t SW_Part_Number = ((uint32_t)shtpData[7] << 24) | ((uint32_t)shtpData[6] << 16) | ((uint32_t)shtpData[5] << 8) | ((uint32_t)shtpData[4]); + _debugPort->print(F(" SW Part Number: 0x")); + _debugPort->print(SW_Part_Number, HEX); + uint32_t SW_Build_Number = ((uint32_t)shtpData[11] << 24) | ((uint32_t)shtpData[10] << 16) | ((uint32_t)shtpData[9] << 8) | ((uint32_t)shtpData[8]); + _debugPort->print(F(" SW Build Number: 0x")); + _debugPort->print(SW_Build_Number, HEX); + uint16_t SW_Version_Patch = ((uint16_t)shtpData[13] << 8) | ((uint16_t)shtpData[12]); + _debugPort->print(F(" SW Version Patch: 0x")); + _debugPort->println(SW_Version_Patch, HEX); + } + return (true); + } + + return (false); //Something went wrong +} + +//Calling this function with nothing sets the debug port to Serial +//You can also call it with other streams like Serial1, SerialUSB, etc. +void BNO080::enableDebugging(Stream &debugPort) +{ + _debugPort = &debugPort; + _printDebug = true; +} + +//Updates the latest variables if possible +//Returns false if new readings are not available +bool BNO080::dataAvailable(void) +{ + return (getReadings() != 0); +} + +uint16_t BNO080::getReadings(void) +{ + //If we have an interrupt pin connection available, check if data is available. + //If int pin is not set, then we'll rely on receivePacket() to timeout + //See issue 13: https://github.com/sparkfun/SparkFun_BNO080_Arduino_Library/issues/13 + if (_int != 255) + { + if (digitalRead(_int) == HIGH) + return 0; + } + + if (receivePacket() == true) + { + //Check to see if this packet is a sensor reporting its data to us + if (shtpHeader[2] == CHANNEL_REPORTS && shtpData[0] == SHTP_REPORT_BASE_TIMESTAMP) + { + return parseInputReport(); //This will update the rawAccelX, etc variables depending on which feature report is found + } + else if (shtpHeader[2] == CHANNEL_CONTROL) + { + return parseCommandReport(); //This will update responses to commands, calibrationStatus, etc. + } + else if(shtpHeader[2] == CHANNEL_GYRO) + { + return parseInputReport(); //This will update the rawAccelX, etc variables depending on which feature report is found + } + } + return 0; +} + +//This function pulls the data from the command response report + +//Unit responds with packet that contains the following: +//shtpHeader[0:3]: First, a 4 byte header +//shtpData[0]: The Report ID +//shtpData[1]: Sequence number (See 6.5.18.2) +//shtpData[2]: Command +//shtpData[3]: Command Sequence Number +//shtpData[4]: Response Sequence Number +//shtpData[5 + 0]: R0 +//shtpData[5 + 1]: R1 +//shtpData[5 + 2]: R2 +//shtpData[5 + 3]: R3 +//shtpData[5 + 4]: R4 +//shtpData[5 + 5]: R5 +//shtpData[5 + 6]: R6 +//shtpData[5 + 7]: R7 +//shtpData[5 + 8]: R8 +uint16_t BNO080::parseCommandReport(void) +{ + if (shtpData[0] == SHTP_REPORT_COMMAND_RESPONSE) + { + //The BNO080 responds with this report to command requests. It's up to use to remember which command we issued. + uint8_t command = shtpData[2]; //This is the Command byte of the response + + if (command == COMMAND_ME_CALIBRATE) + { + calibrationStatus = shtpData[5 + 0]; //R0 - Status (0 = success, non-zero = fail) + } + return shtpData[0]; + } + else + { + //This sensor report ID is unhandled. + //See reference manual to add additional feature reports as needed + } + + //TODO additional feature reports may be strung together. Parse them all. + return 0; +} + +//This function pulls the data from the input report +//The input reports vary in length so this function stores the various 16-bit values as globals + +//Unit responds with packet that contains the following: +//shtpHeader[0:3]: First, a 4 byte header +//shtpData[0:4]: Then a 5 byte timestamp of microsecond clicks since reading was taken +//shtpData[5 + 0]: Then a feature report ID (0x01 for Accel, 0x05 for Rotation Vector) +//shtpData[5 + 1]: Sequence number (See 6.5.18.2) +//shtpData[5 + 2]: Status +//shtpData[3]: Delay +//shtpData[4:5]: i/accel x/gyro x/etc +//shtpData[6:7]: j/accel y/gyro y/etc +//shtpData[8:9]: k/accel z/gyro z/etc +//shtpData[10:11]: real/gyro temp/etc +//shtpData[12:13]: Accuracy estimate +uint16_t BNO080::parseInputReport(void) +{ + //Calculate the number of data bytes in this packet + int16_t dataLength = ((uint16_t)shtpHeader[1] << 8 | shtpHeader[0]); + dataLength &= ~(1 << 15); //Clear the MSbit. This bit indicates if this package is a continuation of the last. + //Ignore it for now. TODO catch this as an error and exit + + dataLength -= 4; //Remove the header bytes from the data count + + timeStamp = ((uint32_t)shtpData[4] << (8 * 3)) | ((uint32_t)shtpData[3] << (8 * 2)) | ((uint32_t)shtpData[2] << (8 * 1)) | ((uint32_t)shtpData[1] << (8 * 0)); + + // The gyro-integrated input reports are sent via the special gyro channel and do no include the usual ID, sequence, and status fields + if(shtpHeader[2] == CHANNEL_GYRO) { + rawQuatI = (uint16_t)shtpData[1] << 8 | shtpData[0]; + rawQuatJ = (uint16_t)shtpData[3] << 8 | shtpData[2]; + rawQuatK = (uint16_t)shtpData[5] << 8 | shtpData[4]; + rawQuatReal = (uint16_t)shtpData[7] << 8 | shtpData[6]; + rawFastGyroX = (uint16_t)shtpData[9] << 8 | shtpData[8]; + rawFastGyroY = (uint16_t)shtpData[11] << 8 | shtpData[10]; + rawFastGyroZ = (uint16_t)shtpData[13] << 8 | shtpData[12]; + + return SENSOR_REPORTID_GYRO_INTEGRATED_ROTATION_VECTOR; + } + + uint8_t status = shtpData[5 + 2] & 0x03; //Get status bits + uint16_t data1 = (uint16_t)shtpData[5 + 5] << 8 | shtpData[5 + 4]; + uint16_t data2 = (uint16_t)shtpData[5 + 7] << 8 | shtpData[5 + 6]; + uint16_t data3 = (uint16_t)shtpData[5 + 9] << 8 | shtpData[5 + 8]; + uint16_t data4 = 0; + uint16_t data5 = 0; //We would need to change this to uin32_t to capture time stamp value on Raw Accel/Gyro/Mag reports + + if (dataLength - 5 > 9) + { + data4 = (uint16_t)shtpData[5 + 11] << 8 | shtpData[5 + 10]; + } + if (dataLength - 5 > 11) + { + data5 = (uint16_t)shtpData[5 + 13] << 8 | shtpData[5 + 12]; + } + + //Store these generic values to their proper global variable + if (shtpData[5] == SENSOR_REPORTID_ACCELEROMETER) + { + accelAccuracy = status; + rawAccelX = data1; + rawAccelY = data2; + rawAccelZ = data3; + } + else if (shtpData[5] == SENSOR_REPORTID_LINEAR_ACCELERATION) + { + accelLinAccuracy = status; + rawLinAccelX = data1; + rawLinAccelY = data2; + rawLinAccelZ = data3; + } + else if (shtpData[5] == SENSOR_REPORTID_GYROSCOPE) + { + gyroAccuracy = status; + rawGyroX = data1; + rawGyroY = data2; + rawGyroZ = data3; + } + else if (shtpData[5] == SENSOR_REPORTID_MAGNETIC_FIELD) + { + magAccuracy = status; + rawMagX = data1; + rawMagY = data2; + rawMagZ = data3; + } + else if (shtpData[5] == SENSOR_REPORTID_ROTATION_VECTOR || + shtpData[5] == SENSOR_REPORTID_GAME_ROTATION_VECTOR || + shtpData[5] == SENSOR_REPORTID_AR_VR_STABILIZED_ROTATION_VECTOR || + shtpData[5] == SENSOR_REPORTID_AR_VR_STABILIZED_GAME_ROTATION_VECTOR) + { + quatAccuracy = status; + rawQuatI = data1; + rawQuatJ = data2; + rawQuatK = data3; + rawQuatReal = data4; + + //Only available on rotation vector and ar/vr stabilized rotation vector, + // not game rot vector and not ar/vr stabilized rotation vector + rawQuatRadianAccuracy = data5; + } + else if (shtpData[5] == SENSOR_REPORTID_TAP_DETECTOR) + { + tapDetector = shtpData[5 + 4]; //Byte 4 only + } + else if (shtpData[5] == SENSOR_REPORTID_STEP_COUNTER) + { + stepCount = data3; //Bytes 8/9 + } + else if (shtpData[5] == SENSOR_REPORTID_STABILITY_CLASSIFIER) + { + stabilityClassifier = shtpData[5 + 4]; //Byte 4 only + } + else if (shtpData[5] == SENSOR_REPORTID_PERSONAL_ACTIVITY_CLASSIFIER) + { + activityClassifier = shtpData[5 + 5]; //Most likely state + + //Load activity classification confidences into the array + for (uint8_t x = 0; x < 9; x++) //Hardcoded to max of 9. TODO - bring in array size + _activityConfidences[x] = shtpData[5 + 6 + x]; //5 bytes of timestamp, byte 6 is first confidence byte + } + else if (shtpData[5] == SENSOR_REPORTID_RAW_ACCELEROMETER) + { + memsRawAccelX = data1; + memsRawAccelY = data2; + memsRawAccelZ = data3; + } + else if (shtpData[5] == SENSOR_REPORTID_RAW_GYROSCOPE) + { + memsRawGyroX = data1; + memsRawGyroY = data2; + memsRawGyroZ = data3; + } + else if (shtpData[5] == SENSOR_REPORTID_RAW_MAGNETOMETER) + { + memsRawMagX = data1; + memsRawMagY = data2; + memsRawMagZ = data3; + } + else if (shtpData[5] == SHTP_REPORT_COMMAND_RESPONSE) + { + if (_printDebug == true) + { + _debugPort->println(F("!")); + } + //The BNO080 responds with this report to command requests. It's up to use to remember which command we issued. + uint8_t command = shtpData[5 + 2]; //This is the Command byte of the response + + if (command == COMMAND_ME_CALIBRATE) + { + if (_printDebug == true) + { + _debugPort->println(F("ME Cal report found!")); + } + calibrationStatus = shtpData[5 + 5]; //R0 - Status (0 = success, non-zero = fail) + } + } + else + { + //This sensor report ID is unhandled. + //See reference manual to add additional feature reports as needed + return 0; + } + + //TODO additional feature reports may be strung together. Parse them all. + return shtpData[5]; +} + +// Quaternion to Euler conversion +// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles +// https://github.com/sparkfun/SparkFun_MPU-9250-DMP_Arduino_Library/issues/5#issuecomment-306509440 +// Return the roll (rotation around the x-axis) in Radians +float BNO080::getRoll() +{ + float dqw = getQuatReal(); + float dqx = getQuatI(); + float dqy = getQuatJ(); + float dqz = getQuatK(); + + float norm = sqrt(dqw*dqw + dqx*dqx + dqy*dqy + dqz*dqz); + dqw = dqw/norm; + dqx = dqx/norm; + dqy = dqy/norm; + dqz = dqz/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); + float roll = atan2(t0, t1); + + return (roll); +} + +// Return the pitch (rotation around the y-axis) in Radians +float BNO080::getPitch() +{ + float dqw = getQuatReal(); + float dqx = getQuatI(); + float dqy = getQuatJ(); + float dqz = getQuatK(); + + float norm = sqrt(dqw*dqw + dqx*dqx + dqy*dqy + dqz*dqz); + dqw = dqw/norm; + dqx = dqx/norm; + dqy = dqy/norm; + dqz = dqz/norm; + + float ysqr = dqy * dqy; + + // 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; + float pitch = asin(t2); + + return (pitch); +} + +// Return the yaw / heading (rotation around the z-axis) in Radians +float BNO080::getYaw() +{ + float dqw = getQuatReal(); + float dqx = getQuatI(); + float dqy = getQuatJ(); + float dqz = getQuatK(); + + float norm = sqrt(dqw*dqw + dqx*dqx + dqy*dqy + dqz*dqz); + dqw = dqw/norm; + dqx = dqx/norm; + dqy = dqy/norm; + dqz = dqz/norm; + + float ysqr = dqy * dqy; + + // yaw (z-axis rotation) + float t3 = +2.0 * (dqw * dqz + dqx * dqy); + float t4 = +1.0 - 2.0 * (ysqr + dqz * dqz); + float yaw = atan2(t3, t4); + + return (yaw); +} + +//Gets the full quaternion +//i,j,k,real output floats +void BNO080::getQuat(float &i, float &j, float &k, float &real, float &radAccuracy, uint8_t &accuracy) +{ + i = qToFloat(rawQuatI, rotationVector_Q1); + j = qToFloat(rawQuatJ, rotationVector_Q1); + k = qToFloat(rawQuatK, rotationVector_Q1); + real = qToFloat(rawQuatReal, rotationVector_Q1); + radAccuracy = qToFloat(rawQuatRadianAccuracy, rotationVector_Q1); + accuracy = quatAccuracy; +} + +//Return the rotation vector quaternion I +float BNO080::getQuatI() +{ + float quat = qToFloat(rawQuatI, rotationVector_Q1); + return (quat); +} + +//Return the rotation vector quaternion J +float BNO080::getQuatJ() +{ + float quat = qToFloat(rawQuatJ, rotationVector_Q1); + return (quat); +} + +//Return the rotation vector quaternion K +float BNO080::getQuatK() +{ + float quat = qToFloat(rawQuatK, rotationVector_Q1); + return (quat); +} + +//Return the rotation vector quaternion Real +float BNO080::getQuatReal() +{ + float quat = qToFloat(rawQuatReal, rotationVector_Q1); + return (quat); +} + +//Return the rotation vector accuracy +float BNO080::getQuatRadianAccuracy() +{ + float quat = qToFloat(rawQuatRadianAccuracy, rotationVectorAccuracy_Q1); + return (quat); +} + +//Return the acceleration component +uint8_t BNO080::getQuatAccuracy() +{ + return (quatAccuracy); +} + +//Gets the full acceleration +//x,y,z output floats +void BNO080::getAccel(float &x, float &y, float &z, uint8_t &accuracy) +{ + x = qToFloat(rawAccelX, accelerometer_Q1); + y = qToFloat(rawAccelY, accelerometer_Q1); + z = qToFloat(rawAccelZ, accelerometer_Q1); + accuracy = accelAccuracy; +} + +//Return the acceleration component +float BNO080::getAccelX() +{ + float accel = qToFloat(rawAccelX, accelerometer_Q1); + return (accel); +} + +//Return the acceleration component +float BNO080::getAccelY() +{ + float accel = qToFloat(rawAccelY, accelerometer_Q1); + return (accel); +} + +//Return the acceleration component +float BNO080::getAccelZ() +{ + float accel = qToFloat(rawAccelZ, accelerometer_Q1); + return (accel); +} + +//Return the acceleration component +uint8_t BNO080::getAccelAccuracy() +{ + return (accelAccuracy); +} + +// linear acceleration, i.e. minus gravity + +//Gets the full lin acceleration +//x,y,z output floats +void BNO080::getLinAccel(float &x, float &y, float &z, uint8_t &accuracy) +{ + x = qToFloat(rawLinAccelX, linear_accelerometer_Q1); + y = qToFloat(rawLinAccelY, linear_accelerometer_Q1); + z = qToFloat(rawLinAccelZ, linear_accelerometer_Q1); + accuracy = accelLinAccuracy; +} + +//Return the acceleration component +float BNO080::getLinAccelX() +{ + float accel = qToFloat(rawLinAccelX, linear_accelerometer_Q1); + return (accel); +} + +//Return the acceleration component +float BNO080::getLinAccelY() +{ + float accel = qToFloat(rawLinAccelY, linear_accelerometer_Q1); + return (accel); +} + +//Return the acceleration component +float BNO080::getLinAccelZ() +{ + float accel = qToFloat(rawLinAccelZ, linear_accelerometer_Q1); + return (accel); +} + +//Return the acceleration component +uint8_t BNO080::getLinAccelAccuracy() +{ + return (accelLinAccuracy); +} + +//Gets the full gyro vector +//x,y,z output floats +void BNO080::getGyro(float &x, float &y, float &z, uint8_t &accuracy) +{ + x = qToFloat(rawGyroX, gyro_Q1); + y = qToFloat(rawGyroY, gyro_Q1); + z = qToFloat(rawGyroZ, gyro_Q1); + accuracy = gyroAccuracy; +} + +//Return the gyro component +float BNO080::getGyroX() +{ + float gyro = qToFloat(rawGyroX, gyro_Q1); + return (gyro); +} + +//Return the gyro component +float BNO080::getGyroY() +{ + float gyro = qToFloat(rawGyroY, gyro_Q1); + return (gyro); +} + +//Return the gyro component +float BNO080::getGyroZ() +{ + float gyro = qToFloat(rawGyroZ, gyro_Q1); + return (gyro); +} + +//Return the gyro component +uint8_t BNO080::getGyroAccuracy() +{ + return (gyroAccuracy); +} + +//Gets the full mag vector +//x,y,z output floats +void BNO080::getMag(float &x, float &y, float &z, uint8_t &accuracy) +{ + x = qToFloat(rawMagX, magnetometer_Q1); + y = qToFloat(rawMagY, magnetometer_Q1); + z = qToFloat(rawMagZ, magnetometer_Q1); + accuracy = magAccuracy; +} + +//Return the magnetometer component +float BNO080::getMagX() +{ + float mag = qToFloat(rawMagX, magnetometer_Q1); + return (mag); +} + +//Return the magnetometer component +float BNO080::getMagY() +{ + float mag = qToFloat(rawMagY, magnetometer_Q1); + return (mag); +} + +//Return the magnetometer component +float BNO080::getMagZ() +{ + float mag = qToFloat(rawMagZ, magnetometer_Q1); + return (mag); +} + +//Return the mag component +uint8_t BNO080::getMagAccuracy() +{ + return (magAccuracy); +} + +//Gets the full high rate gyro vector +//x,y,z output floats +void BNO080::getFastGyro(float &x, float &y, float &z) +{ + x = qToFloat(rawFastGyroX, angular_velocity_Q1); + y = qToFloat(rawFastGyroY, angular_velocity_Q1); + z = qToFloat(rawFastGyroZ, angular_velocity_Q1); +} + +// Return the high refresh rate gyro component +float BNO080::getFastGyroX() +{ + float gyro = qToFloat(rawFastGyroX, angular_velocity_Q1); + return (gyro); +} + +// Return the high refresh rate gyro component +float BNO080::getFastGyroY() +{ + float gyro = qToFloat(rawFastGyroY, angular_velocity_Q1); + return (gyro); +} + +// Return the high refresh rate gyro component +float BNO080::getFastGyroZ() +{ + float gyro = qToFloat(rawFastGyroZ, angular_velocity_Q1); + return (gyro); +} + +//Return the tap detector +uint8_t BNO080::getTapDetector() +{ + uint8_t previousTapDetector = tapDetector; + tapDetector = 0; //Reset so user code sees exactly one tap + return (previousTapDetector); +} + +//Return the step count +uint16_t BNO080::getStepCount() +{ + return (stepCount); +} + +//Return the stability classifier +uint8_t BNO080::getStabilityClassifier() +{ + return (stabilityClassifier); +} + +//Return the activity classifier +uint8_t BNO080::getActivityClassifier() +{ + return (activityClassifier); +} + +//Return the time stamp +uint32_t BNO080::getTimeStamp() +{ + return (timeStamp); +} + +//Return raw mems value for the accel +int16_t BNO080::getRawAccelX() +{ + return (memsRawAccelX); +} +//Return raw mems value for the accel +int16_t BNO080::getRawAccelY() +{ + return (memsRawAccelY); +} +//Return raw mems value for the accel +int16_t BNO080::getRawAccelZ() +{ + return (memsRawAccelZ); +} + +//Return raw mems value for the gyro +int16_t BNO080::getRawGyroX() +{ + return (memsRawGyroX); +} +int16_t BNO080::getRawGyroY() +{ + return (memsRawGyroY); +} +int16_t BNO080::getRawGyroZ() +{ + return (memsRawGyroZ); +} + +//Return raw mems value for the mag +int16_t BNO080::getRawMagX() +{ + return (memsRawMagX); +} +int16_t BNO080::getRawMagY() +{ + return (memsRawMagY); +} +int16_t BNO080::getRawMagZ() +{ + return (memsRawMagZ); +} + +//Given a record ID, read the Q1 value from the metaData record in the FRS (ya, it's complicated) +//Q1 is used for all sensor data calculations +int16_t BNO080::getQ1(uint16_t recordID) +{ + //Q1 is always the lower 16 bits of word 7 + uint16_t q = readFRSword(recordID, 7) & 0xFFFF; //Get word 7, lower 16 bits + return (q); +} + +//Given a record ID, read the Q2 value from the metaData record in the FRS +//Q2 is used in sensor bias +int16_t BNO080::getQ2(uint16_t recordID) +{ + //Q2 is always the upper 16 bits of word 7 + uint16_t q = readFRSword(recordID, 7) >> 16; //Get word 7, upper 16 bits + return (q); +} + +//Given a record ID, read the Q3 value from the metaData record in the FRS +//Q3 is used in sensor change sensitivity +int16_t BNO080::getQ3(uint16_t recordID) +{ + //Q3 is always the upper 16 bits of word 8 + uint16_t q = readFRSword(recordID, 8) >> 16; //Get word 8, upper 16 bits + return (q); +} + +//Given a record ID, read the resolution value from the metaData record in the FRS for a given sensor +float BNO080::getResolution(uint16_t recordID) +{ + //The resolution Q value are 'the same as those used in the sensor's input report' + //This should be Q1. + int16_t Q = getQ1(recordID); + + //Resolution is always word 2 + uint32_t value = readFRSword(recordID, 2); //Get word 2 + + float resolution = qToFloat(value, Q); + + return (resolution); +} + +//Given a record ID, read the range value from the metaData record in the FRS for a given sensor +float BNO080::getRange(uint16_t recordID) +{ + //The resolution Q value are 'the same as those used in the sensor's input report' + //This should be Q1. + int16_t Q = getQ1(recordID); + + //Range is always word 1 + uint32_t value = readFRSword(recordID, 1); //Get word 1 + + float range = qToFloat(value, Q); + + return (range); +} + +//Given a record ID and a word number, look up the word data +//Helpful for pulling out a Q value, range, etc. +//Use readFRSdata for pulling out multi-word objects for a sensor (Vendor data for example) +uint32_t BNO080::readFRSword(uint16_t recordID, uint8_t wordNumber) +{ + if (readFRSdata(recordID, wordNumber, 1) == true) //Get word number, just one word in length from FRS + return (metaData[0]); //Return this one word + + return (0); //Error +} + +//Ask the sensor for data from the Flash Record System +//See 6.3.6 page 40, FRS Read Request +void BNO080::frsReadRequest(uint16_t recordID, uint16_t readOffset, uint16_t blockSize) +{ + shtpData[0] = SHTP_REPORT_FRS_READ_REQUEST; //FRS Read Request + shtpData[1] = 0; //Reserved + shtpData[2] = (readOffset >> 0) & 0xFF; //Read Offset LSB + shtpData[3] = (readOffset >> 8) & 0xFF; //Read Offset MSB + shtpData[4] = (recordID >> 0) & 0xFF; //FRS Type LSB + shtpData[5] = (recordID >> 8) & 0xFF; //FRS Type MSB + shtpData[6] = (blockSize >> 0) & 0xFF; //Block size LSB + shtpData[7] = (blockSize >> 8) & 0xFF; //Block size MSB + + //Transmit packet on channel 2, 8 bytes + sendPacket(CHANNEL_CONTROL, 8); +} + +//Given a sensor or record ID, and a given start/stop bytes, read the data from the Flash Record System (FRS) for this sensor +//Returns true if metaData array is loaded successfully +//Returns false if failure +bool BNO080::readFRSdata(uint16_t recordID, uint8_t startLocation, uint8_t wordsToRead) +{ + uint8_t spot = 0; + + //First we send a Flash Record System (FRS) request + frsReadRequest(recordID, startLocation, wordsToRead); //From startLocation of record, read a # of words + + //Read bytes until FRS reports that the read is complete + while (1) + { + //Now we wait for response + while (1) + { + uint8_t counter = 0; + while (receivePacket() == false) + { + if (counter++ > 100) + return (false); //Give up + delay(1); + } + + //We have the packet, inspect it for the right contents + //See page 40. Report ID should be 0xF3 and the FRS types should match the thing we requested + if (shtpData[0] == SHTP_REPORT_FRS_READ_RESPONSE) + if (((((uint16_t)shtpData[13]) << 8) | shtpData[12]) == recordID) + break; //This packet is one we are looking for + } + + uint8_t dataLength = shtpData[1] >> 4; + uint8_t frsStatus = shtpData[1] & 0x0F; + + uint32_t data0 = (uint32_t)shtpData[7] << 24 | (uint32_t)shtpData[6] << 16 | (uint32_t)shtpData[5] << 8 | (uint32_t)shtpData[4]; + uint32_t data1 = (uint32_t)shtpData[11] << 24 | (uint32_t)shtpData[10] << 16 | (uint32_t)shtpData[9] << 8 | (uint32_t)shtpData[8]; + + //Record these words to the metaData array + if (dataLength > 0) + { + metaData[spot++] = data0; + } + if (dataLength > 1) + { + metaData[spot++] = data1; + } + + if (spot >= MAX_METADATA_SIZE) + { + if (_printDebug == true) + _debugPort->println(F("metaData array over run. Returning.")); + return (true); //We have run out of space in our array. Bail. + } + + if (frsStatus == 3 || frsStatus == 6 || frsStatus == 7) + { + return (true); //FRS status is read completed! We're done! + } + } +} + +//Send command to reset IC +//Read all advertisement packets from sensor +//The sensor has been seen to reset twice if we attempt too much too quickly. +//This seems to work reliably. +void BNO080::softReset(void) +{ + shtpData[0] = 1; //Reset + + //Attempt to start communication with sensor + sendPacket(CHANNEL_EXECUTABLE, 1); //Transmit packet on channel 1, 1 byte + + //Read all incoming data and flush it + delay(50); + while (receivePacket() == true) + ; //delay(1); + delay(50); + while (receivePacket() == true) + ; //delay(1); +} + +//Set the operating mode to "On" +//(This one is for @jerabaul29) +void BNO080::modeOn(void) +{ + shtpData[0] = 2; //On + + //Attempt to start communication with sensor + sendPacket(CHANNEL_EXECUTABLE, 1); //Transmit packet on channel 1, 1 byte + + //Read all incoming data and flush it + delay(50); + while (receivePacket() == true) + ; //delay(1); + delay(50); + while (receivePacket() == true) + ; //delay(1); +} + +//Set the operating mode to "Sleep" +//(This one is for @jerabaul29) +void BNO080::modeSleep(void) +{ + shtpData[0] = 3; //Sleep + + //Attempt to start communication with sensor + sendPacket(CHANNEL_EXECUTABLE, 1); //Transmit packet on channel 1, 1 byte + + //Read all incoming data and flush it + delay(50); + while (receivePacket() == true) + ; //delay(1); + delay(50); + while (receivePacket() == true) + ; //delay(1); +} + +//Get the reason for the last reset +//1 = POR, 2 = Internal reset, 3 = Watchdog, 4 = External reset, 5 = Other +uint8_t BNO080::resetReason() +{ + shtpData[0] = SHTP_REPORT_PRODUCT_ID_REQUEST; //Request the product ID and reset info + shtpData[1] = 0; //Reserved + + //Transmit packet on channel 2, 2 bytes + sendPacket(CHANNEL_CONTROL, 2); + + //Now we wait for response + if (receivePacket() == true) + { + if (shtpData[0] == SHTP_REPORT_PRODUCT_ID_RESPONSE) + { + return (shtpData[1]); + } + } + + return (0); +} + +//Given a register value and a Q point, convert to float +//See https://en.wikipedia.org/wiki/Q_(number_format) +float BNO080::qToFloat(int16_t fixedPointValue, uint8_t qPoint) +{ + float qFloat = fixedPointValue; + qFloat *= pow(2, qPoint * -1); + return (qFloat); +} + +//Sends the packet to enable the rotation vector +void BNO080::enableRotationVector(uint16_t timeBetweenReports) +{ + setFeatureCommand(SENSOR_REPORTID_ROTATION_VECTOR, timeBetweenReports); +} + +//Sends the packet to enable the ar/vr stabilized rotation vector +void BNO080::enableARVRStabilizedRotationVector(uint16_t timeBetweenReports) +{ + setFeatureCommand(SENSOR_REPORTID_AR_VR_STABILIZED_ROTATION_VECTOR, timeBetweenReports); +} + +//Sends the packet to enable the rotation vector +void BNO080::enableGameRotationVector(uint16_t timeBetweenReports) +{ + setFeatureCommand(SENSOR_REPORTID_GAME_ROTATION_VECTOR, timeBetweenReports); +} + +//Sends the packet to enable the ar/vr stabilized rotation vector +void BNO080::enableARVRStabilizedGameRotationVector(uint16_t timeBetweenReports) +{ + setFeatureCommand(SENSOR_REPORTID_AR_VR_STABILIZED_GAME_ROTATION_VECTOR, timeBetweenReports); +} + +//Sends the packet to enable the accelerometer +void BNO080::enableAccelerometer(uint16_t timeBetweenReports) +{ + setFeatureCommand(SENSOR_REPORTID_ACCELEROMETER, timeBetweenReports); +} + +//Sends the packet to enable the accelerometer +void BNO080::enableLinearAccelerometer(uint16_t timeBetweenReports) +{ + setFeatureCommand(SENSOR_REPORTID_LINEAR_ACCELERATION, timeBetweenReports); +} + +//Sends the packet to enable the gyro +void BNO080::enableGyro(uint16_t timeBetweenReports) +{ + setFeatureCommand(SENSOR_REPORTID_GYROSCOPE, timeBetweenReports); +} + +//Sends the packet to enable the magnetometer +void BNO080::enableMagnetometer(uint16_t timeBetweenReports) +{ + setFeatureCommand(SENSOR_REPORTID_MAGNETIC_FIELD, timeBetweenReports); +} + +//Sends the packet to enable the high refresh-rate gyro-integrated rotation vector +void BNO080::enableGyroIntegratedRotationVector(uint16_t timeBetweenReports) +{ + setFeatureCommand(SENSOR_REPORTID_GYRO_INTEGRATED_ROTATION_VECTOR, timeBetweenReports); +} + +//Sends the packet to enable the tap detector +void BNO080::enableTapDetector(uint16_t timeBetweenReports) +{ + setFeatureCommand(SENSOR_REPORTID_TAP_DETECTOR, timeBetweenReports); +} + +//Sends the packet to enable the step counter +void BNO080::enableStepCounter(uint16_t timeBetweenReports) +{ + setFeatureCommand(SENSOR_REPORTID_STEP_COUNTER, timeBetweenReports); +} + +//Sends the packet to enable the Stability Classifier +void BNO080::enableStabilityClassifier(uint16_t timeBetweenReports) +{ + setFeatureCommand(SENSOR_REPORTID_STABILITY_CLASSIFIER, timeBetweenReports); +} + +//Sends the packet to enable the raw accel readings +//Note you must enable basic reporting on the sensor as well +void BNO080::enableRawAccelerometer(uint16_t timeBetweenReports) +{ + setFeatureCommand(SENSOR_REPORTID_RAW_ACCELEROMETER, timeBetweenReports); +} + +//Sends the packet to enable the raw accel readings +//Note you must enable basic reporting on the sensor as well +void BNO080::enableRawGyro(uint16_t timeBetweenReports) +{ + setFeatureCommand(SENSOR_REPORTID_RAW_GYROSCOPE, timeBetweenReports); +} + +//Sends the packet to enable the raw accel readings +//Note you must enable basic reporting on the sensor as well +void BNO080::enableRawMagnetometer(uint16_t timeBetweenReports) +{ + setFeatureCommand(SENSOR_REPORTID_RAW_MAGNETOMETER, timeBetweenReports); +} + +//Sends the packet to enable the various activity classifiers +void BNO080::enableActivityClassifier(uint16_t timeBetweenReports, uint32_t activitiesToEnable, uint8_t (&activityConfidences)[9]) +{ + _activityConfidences = activityConfidences; //Store pointer to array + + setFeatureCommand(SENSOR_REPORTID_PERSONAL_ACTIVITY_CLASSIFIER, timeBetweenReports, activitiesToEnable); +} + +//Sends the commands to begin calibration of the accelerometer +void BNO080::calibrateAccelerometer() +{ + sendCalibrateCommand(CALIBRATE_ACCEL); +} + +//Sends the commands to begin calibration of the gyro +void BNO080::calibrateGyro() +{ + sendCalibrateCommand(CALIBRATE_GYRO); +} + +//Sends the commands to begin calibration of the magnetometer +void BNO080::calibrateMagnetometer() +{ + sendCalibrateCommand(CALIBRATE_MAG); +} + +//Sends the commands to begin calibration of the planar accelerometer +void BNO080::calibratePlanarAccelerometer() +{ + sendCalibrateCommand(CALIBRATE_PLANAR_ACCEL); +} + +//See 2.2 of the Calibration Procedure document 1000-4044 +void BNO080::calibrateAll() +{ + sendCalibrateCommand(CALIBRATE_ACCEL_GYRO_MAG); +} + +void BNO080::endCalibration() +{ + sendCalibrateCommand(CALIBRATE_STOP); //Disables all calibrations +} + +//See page 51 of reference manual - ME Calibration Response +//Byte 5 is parsed during the readPacket and stored in calibrationStatus +boolean BNO080::calibrationComplete() +{ + if (calibrationStatus == 0) + return (true); + return (false); +} + +//Given a sensor's report ID, this tells the BNO080 to begin reporting the values +void BNO080::setFeatureCommand(uint8_t reportID, uint16_t timeBetweenReports) +{ + setFeatureCommand(reportID, timeBetweenReports, 0); //No specific config +} + +//Given a sensor's report ID, this tells the BNO080 to begin reporting the values +//Also sets the specific config word. Useful for personal activity classifier +void BNO080::setFeatureCommand(uint8_t reportID, uint16_t timeBetweenReports, uint32_t specificConfig) +{ + long microsBetweenReports = (long)timeBetweenReports * 1000L; + + shtpData[0] = SHTP_REPORT_SET_FEATURE_COMMAND; //Set feature command. Reference page 55 + shtpData[1] = reportID; //Feature Report ID. 0x01 = Accelerometer, 0x05 = Rotation vector + shtpData[2] = 0; //Feature flags + shtpData[3] = 0; //Change sensitivity (LSB) + shtpData[4] = 0; //Change sensitivity (MSB) + shtpData[5] = (microsBetweenReports >> 0) & 0xFF; //Report interval (LSB) in microseconds. 0x7A120 = 500ms + shtpData[6] = (microsBetweenReports >> 8) & 0xFF; //Report interval + shtpData[7] = (microsBetweenReports >> 16) & 0xFF; //Report interval + shtpData[8] = (microsBetweenReports >> 24) & 0xFF; //Report interval (MSB) + shtpData[9] = 0; //Batch Interval (LSB) + shtpData[10] = 0; //Batch Interval + shtpData[11] = 0; //Batch Interval + shtpData[12] = 0; //Batch Interval (MSB) + shtpData[13] = (specificConfig >> 0) & 0xFF; //Sensor-specific config (LSB) + shtpData[14] = (specificConfig >> 8) & 0xFF; //Sensor-specific config + shtpData[15] = (specificConfig >> 16) & 0xFF; //Sensor-specific config + shtpData[16] = (specificConfig >> 24) & 0xFF; //Sensor-specific config (MSB) + + //Transmit packet on channel 2, 17 bytes + sendPacket(CHANNEL_CONTROL, 17); +} + +//Tell the sensor to do a command +//See 6.3.8 page 41, Command request +//The caller is expected to set P0 through P8 prior to calling +void BNO080::sendCommand(uint8_t command) +{ + shtpData[0] = SHTP_REPORT_COMMAND_REQUEST; //Command Request + shtpData[1] = commandSequenceNumber++; //Increments automatically each function call + shtpData[2] = command; //Command + + //Caller must set these + /*shtpData[3] = 0; //P0 + shtpData[4] = 0; //P1 + shtpData[5] = 0; //P2 + shtpData[6] = 0; + shtpData[7] = 0; + shtpData[8] = 0; + shtpData[9] = 0; + shtpData[10] = 0; + shtpData[11] = 0;*/ + + //Transmit packet on channel 2, 12 bytes + sendPacket(CHANNEL_CONTROL, 12); +} + +//This tells the BNO080 to begin calibrating +//See page 50 of reference manual and the 1000-4044 calibration doc +void BNO080::sendCalibrateCommand(uint8_t thingToCalibrate) +{ + /*shtpData[3] = 0; //P0 - Accel Cal Enable + shtpData[4] = 0; //P1 - Gyro Cal Enable + shtpData[5] = 0; //P2 - Mag Cal Enable + shtpData[6] = 0; //P3 - Subcommand 0x00 + shtpData[7] = 0; //P4 - Planar Accel Cal Enable + shtpData[8] = 0; //P5 - Reserved + shtpData[9] = 0; //P6 - Reserved + shtpData[10] = 0; //P7 - Reserved + shtpData[11] = 0; //P8 - Reserved*/ + + for (uint8_t x = 3; x < 12; x++) //Clear this section of the shtpData array + shtpData[x] = 0; + + if (thingToCalibrate == CALIBRATE_ACCEL) + shtpData[3] = 1; + else if (thingToCalibrate == CALIBRATE_GYRO) + shtpData[4] = 1; + else if (thingToCalibrate == CALIBRATE_MAG) + shtpData[5] = 1; + else if (thingToCalibrate == CALIBRATE_PLANAR_ACCEL) + shtpData[7] = 1; + else if (thingToCalibrate == CALIBRATE_ACCEL_GYRO_MAG) + { + shtpData[3] = 1; + shtpData[4] = 1; + shtpData[5] = 1; + } + else if (thingToCalibrate == CALIBRATE_STOP) + ; //Do nothing, bytes are set to zero + + //Make the internal calStatus variable non-zero (operation failed) so that user can test while we wait + calibrationStatus = 1; + + //Using this shtpData packet, send a command + sendCommand(COMMAND_ME_CALIBRATE); +} + +//Request ME Calibration Status from BNO080 +//See page 51 of reference manual +void BNO080::requestCalibrationStatus() +{ + /*shtpData[3] = 0; //P0 - Reserved + shtpData[4] = 0; //P1 - Reserved + shtpData[5] = 0; //P2 - Reserved + shtpData[6] = 0; //P3 - 0x01 - Subcommand: Get ME Calibration + shtpData[7] = 0; //P4 - Reserved + shtpData[8] = 0; //P5 - Reserved + shtpData[9] = 0; //P6 - Reserved + shtpData[10] = 0; //P7 - Reserved + shtpData[11] = 0; //P8 - Reserved*/ + + for (uint8_t x = 3; x < 12; x++) //Clear this section of the shtpData array + shtpData[x] = 0; + + shtpData[6] = 0x01; //P3 - 0x01 - Subcommand: Get ME Calibration + + //Using this shtpData packet, send a command + sendCommand(COMMAND_ME_CALIBRATE); +} + +//This tells the BNO080 to save the Dynamic Calibration Data (DCD) to flash +//See page 49 of reference manual and the 1000-4044 calibration doc +void BNO080::saveCalibration() +{ + /*shtpData[3] = 0; //P0 - Reserved + shtpData[4] = 0; //P1 - Reserved + shtpData[5] = 0; //P2 - Reserved + shtpData[6] = 0; //P3 - Reserved + shtpData[7] = 0; //P4 - Reserved + shtpData[8] = 0; //P5 - Reserved + shtpData[9] = 0; //P6 - Reserved + shtpData[10] = 0; //P7 - Reserved + shtpData[11] = 0; //P8 - Reserved*/ + + for (uint8_t x = 3; x < 12; x++) //Clear this section of the shtpData array + shtpData[x] = 0; + + //Using this shtpData packet, send a command + sendCommand(COMMAND_DCD); //Save DCD command +} + +//Wait a certain time for incoming I2C bytes before giving up +//Returns false if failed +boolean BNO080::waitForI2C() +{ + for (uint8_t counter = 0; counter < 100; counter++) //Don't got more than 255 + { + if (_i2cPort->available() > 0) + return (true); + delay(1); + } + + if (_printDebug == true) + _debugPort->println(F("I2C timeout")); + return (false); +} + +//Blocking wait for BNO080 to assert (pull low) the INT pin +//indicating it's ready for comm. Can take more than 104ms +//after a hardware reset +boolean BNO080::waitForSPI() +{ + for (uint8_t counter = 0; counter < 125; counter++) //Don't got more than 255 + { + if (digitalRead(_int) == LOW) + return (true); + if (_printDebug == true) + _debugPort->println(F("SPI Wait")); + delay(1); + } + + if (_printDebug == true) + _debugPort->println(F("SPI INT timeout")); + return (false); +} + +//Check to see if there is any new data available +//Read the contents of the incoming packet into the shtpData array +boolean BNO080::receivePacket(void) +{ + if (_i2cPort == NULL) //Do SPI + { + if (digitalRead(_int) == HIGH) + return (false); //Data is not available + + //Old way: if (waitForSPI() == false) return (false); //Something went wrong + + //Get first four bytes to find out how much data we need to read + + _spiPort->beginTransaction(SPISettings(_spiPortSpeed, MSBFIRST, SPI_MODE3)); + digitalWrite(_cs, LOW); + + //Get the first four bytes, aka the packet header + uint8_t packetLSB = _spiPort->transfer(0); + uint8_t packetMSB = _spiPort->transfer(0); + uint8_t channelNumber = _spiPort->transfer(0); + uint8_t sequenceNumber = _spiPort->transfer(0); //Not sure if we need to store this or not + + //Store the header info + shtpHeader[0] = packetLSB; + shtpHeader[1] = packetMSB; + shtpHeader[2] = channelNumber; + shtpHeader[3] = sequenceNumber; + + //Calculate the number of data bytes in this packet + uint16_t dataLength = (((uint16_t)packetMSB) << 8) | ((uint16_t)packetLSB); + dataLength &= ~(1 << 15); //Clear the MSbit. + //This bit indicates if this package is a continuation of the last. Ignore it for now. + //TODO catch this as an error and exit + if (dataLength == 0) + { + //Packet is empty + printHeader(); + return (false); //All done + } + dataLength -= 4; //Remove the header bytes from the data count + + //Read incoming data into the shtpData array + for (uint16_t dataSpot = 0; dataSpot < dataLength; dataSpot++) + { + uint8_t incoming = _spiPort->transfer(0xFF); + if (dataSpot < MAX_PACKET_SIZE) //BNO080 can respond with upto 270 bytes, avoid overflow + shtpData[dataSpot] = incoming; //Store data into the shtpData array + } + + digitalWrite(_cs, HIGH); //Release BNO080 + + _spiPort->endTransaction(); + printPacket(); + } + else //Do I2C + { + _i2cPort->requestFrom((uint8_t)_deviceAddress, (size_t)4); //Ask for four bytes to find out how much data we need to read + if (waitForI2C() == false) + return (false); //Error + + //Get the first four bytes, aka the packet header + uint8_t packetLSB = _i2cPort->read(); + uint8_t packetMSB = _i2cPort->read(); + uint8_t channelNumber = _i2cPort->read(); + uint8_t sequenceNumber = _i2cPort->read(); //Not sure if we need to store this or not + + //Store the header info. + shtpHeader[0] = packetLSB; + shtpHeader[1] = packetMSB; + shtpHeader[2] = channelNumber; + shtpHeader[3] = sequenceNumber; + + //Calculate the number of data bytes in this packet + uint16_t dataLength = (((uint16_t)packetMSB) << 8) | ((uint16_t)packetLSB); + dataLength &= ~(1 << 15); //Clear the MSbit. + //This bit indicates if this package is a continuation of the last. Ignore it for now. + //TODO catch this as an error and exit + + // if (_printDebug == true) + // { + // _debugPort->print(F("receivePacket (I2C): dataLength is: ")); + // _debugPort->println(dataLength); + // } + + if (dataLength == 0) + { + //Packet is empty + return (false); //All done + } + dataLength -= 4; //Remove the header bytes from the data count + + getData(dataLength); + } + + return (true); //We're done! +} + +//Sends multiple requests to sensor until all data bytes are received from sensor +//The shtpData buffer has max capacity of MAX_PACKET_SIZE. Any bytes over this amount will be lost. +//Arduino I2C read limit is 32 bytes. Header is 4 bytes, so max data we can read per interation is 28 bytes +boolean BNO080::getData(uint16_t bytesRemaining) +{ + uint16_t dataSpot = 0; //Start at the beginning of shtpData array + + //Setup a series of chunked 32 byte reads + while (bytesRemaining > 0) + { + uint16_t numberOfBytesToRead = bytesRemaining; + if (numberOfBytesToRead > (I2C_BUFFER_LENGTH - 4)) + numberOfBytesToRead = (I2C_BUFFER_LENGTH - 4); + + _i2cPort->requestFrom((uint8_t)_deviceAddress, (size_t)(numberOfBytesToRead + 4)); + if (waitForI2C() == false) + return (0); //Error + + //The first four bytes are header bytes and are throw away + _i2cPort->read(); + _i2cPort->read(); + _i2cPort->read(); + _i2cPort->read(); + + for (uint8_t x = 0; x < numberOfBytesToRead; x++) + { + uint8_t incoming = _i2cPort->read(); + if (dataSpot < MAX_PACKET_SIZE) + { + shtpData[dataSpot++] = incoming; //Store data into the shtpData array + } + else + { + //Do nothing with the data + } + } + + bytesRemaining -= numberOfBytesToRead; + } + return (true); //Done! +} + +//Given the data packet, send the header then the data +//Returns false if sensor does not ACK +//TODO - Arduino has a max 32 byte send. Break sending into multi packets if needed. +boolean BNO080::sendPacket(uint8_t channelNumber, uint8_t dataLength) +{ + uint8_t packetLength = dataLength + 4; //Add four bytes for the header + + if (_i2cPort == NULL) //Do SPI + { + //Wait for BNO080 to indicate it is available for communication + if (waitForSPI() == false) + return (false); //Something went wrong + + //BNO080 has max CLK of 3MHz, MSB first, + //The BNO080 uses CPOL = 1 and CPHA = 1. This is mode3 + _spiPort->beginTransaction(SPISettings(_spiPortSpeed, MSBFIRST, SPI_MODE3)); + digitalWrite(_cs, LOW); + + //Send the 4 byte packet header + _spiPort->transfer(packetLength & 0xFF); //Packet length LSB + _spiPort->transfer(packetLength >> 8); //Packet length MSB + _spiPort->transfer(channelNumber); //Channel number + _spiPort->transfer(sequenceNumber[channelNumber]++); //Send the sequence number, increments with each packet sent, different counter for each channel + + //Send the user's data packet + for (uint8_t i = 0; i < dataLength; i++) + { + _spiPort->transfer(shtpData[i]); + } + + digitalWrite(_cs, HIGH); + _spiPort->endTransaction(); + } + else //Do I2C + { + //if(packetLength > I2C_BUFFER_LENGTH) return(false); //You are trying to send too much. Break into smaller packets. + + _i2cPort->beginTransmission(_deviceAddress); + + //Send the 4 byte packet header + _i2cPort->write(packetLength & 0xFF); //Packet length LSB + _i2cPort->write(packetLength >> 8); //Packet length MSB + _i2cPort->write(channelNumber); //Channel number + _i2cPort->write(sequenceNumber[channelNumber]++); //Send the sequence number, increments with each packet sent, different counter for each channel + + //Send the user's data packet + for (uint8_t i = 0; i < dataLength; i++) + { + _i2cPort->write(shtpData[i]); + } + + uint8_t i2cResult = _i2cPort->endTransmission(); + + if (i2cResult != 0) + { + if (_printDebug == true) + { + _debugPort->print(F("sendPacket(I2C): endTransmission returned: ")); + _debugPort->println(i2cResult); + } + return (false); + } + } + + return (true); +} + +//Pretty prints the contents of the current shtp header and data packets +void BNO080::printPacket(void) +{ + if (_printDebug == true) + { + uint16_t packetLength = (uint16_t)shtpHeader[1] << 8 | shtpHeader[0]; + + //Print the four byte header + _debugPort->print(F("Header:")); + for (uint8_t x = 0; x < 4; x++) + { + _debugPort->print(F(" ")); + if (shtpHeader[x] < 0x10) + _debugPort->print(F("0")); + _debugPort->print(shtpHeader[x], HEX); + } + + uint8_t printLength = packetLength - 4; + if (printLength > 40) + printLength = 40; //Artificial limit. We don't want the phone book. + + _debugPort->print(F(" Body:")); + for (uint8_t x = 0; x < printLength; x++) + { + _debugPort->print(F(" ")); + if (shtpData[x] < 0x10) + _debugPort->print(F("0")); + _debugPort->print(shtpData[x], HEX); + } + + if (packetLength & 1 << 15) + { + _debugPort->println(F(" [Continued packet] ")); + packetLength &= ~(1 << 15); + } + + _debugPort->print(F(" Length:")); + _debugPort->print(packetLength); + + _debugPort->print(F(" Channel:")); + if (shtpHeader[2] == 0) + _debugPort->print(F("Command")); + else if (shtpHeader[2] == 1) + _debugPort->print(F("Executable")); + else if (shtpHeader[2] == 2) + _debugPort->print(F("Control")); + else if (shtpHeader[2] == 3) + _debugPort->print(F("Sensor-report")); + else if (shtpHeader[2] == 4) + _debugPort->print(F("Wake-report")); + else if (shtpHeader[2] == 5) + _debugPort->print(F("Gyro-vector")); + else + _debugPort->print(shtpHeader[2]); + + _debugPort->println(); + } +} + +//Pretty prints the contents of the current shtp header (only) +void BNO080::printHeader(void) +{ + if (_printDebug == true) + { + //Print the four byte header + _debugPort->print(F("Header:")); + for (uint8_t x = 0; x < 4; x++) + { + _debugPort->print(F(" ")); + if (shtpHeader[x] < 0x10) + _debugPort->print(F("0")); + _debugPort->print(shtpHeader[x], HEX); + } + _debugPort->println(); + } +} diff --git a/include/BNO080.h b/include/BNO080.h new file mode 100644 index 0000000..025f9c8 --- /dev/null +++ b/include/BNO080.h @@ -0,0 +1,303 @@ +/* + This is a library written for the BNO080 + SparkFun sells these at its website: www.sparkfun.com + Do you like this library? Help support SparkFun. Buy a board! + https://www.sparkfun.com/products/14686 + + Written by Nathan Seidle @ SparkFun Electronics, December 28th, 2017 + + The BNO080 IMU is a powerful triple axis gyro/accel/magnetometer coupled with an ARM processor + to maintain and complete all the complex calculations for various VR, inertial, step counting, + and movement operations. + + This library handles the initialization of the BNO080 and is able to query the sensor + for different readings. + + https://github.com/sparkfun/SparkFun_BNO080_Arduino_Library + + Development environment specifics: + Arduino IDE 1.8.3 + + SparkFun code, firmware, and software is released under the MIT License. + Please see LICENSE.md for further details. +*/ + +#pragma once + +#if (ARDUINO >= 100) +#include "Arduino.h" +#else +#include "WProgram.h" +#endif + +#include +#include + +//The default I2C address for the BNO080 on the SparkX breakout is 0x4B. 0x4A is also possible. +#define BNO080_DEFAULT_ADDRESS 0x4B + +//Platform specific configurations + +//Define the size of the I2C buffer based on the platform the user has +//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) + +//I2C_BUFFER_LENGTH is defined in Wire.H +#define I2C_BUFFER_LENGTH BUFFER_LENGTH + +#else + +//The catch-all default is 32 +#define I2C_BUFFER_LENGTH 32 + +#endif +//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= + +//Registers +const byte CHANNEL_COMMAND = 0; +const byte CHANNEL_EXECUTABLE = 1; +const byte CHANNEL_CONTROL = 2; +const byte CHANNEL_REPORTS = 3; +const byte CHANNEL_WAKE_REPORTS = 4; +const byte CHANNEL_GYRO = 5; + +//All the ways we can configure or talk to the BNO080, figure 34, page 36 reference manual +//These are used for low level communication with the sensor, on channel 2 +#define SHTP_REPORT_COMMAND_RESPONSE 0xF1 +#define SHTP_REPORT_COMMAND_REQUEST 0xF2 +#define SHTP_REPORT_FRS_READ_RESPONSE 0xF3 +#define SHTP_REPORT_FRS_READ_REQUEST 0xF4 +#define SHTP_REPORT_PRODUCT_ID_RESPONSE 0xF8 +#define SHTP_REPORT_PRODUCT_ID_REQUEST 0xF9 +#define SHTP_REPORT_BASE_TIMESTAMP 0xFB +#define SHTP_REPORT_SET_FEATURE_COMMAND 0xFD + +//All the different sensors and features we can get reports from +//These are used when enabling a given sensor +#define SENSOR_REPORTID_ACCELEROMETER 0x01 +#define SENSOR_REPORTID_GYROSCOPE 0x02 +#define SENSOR_REPORTID_MAGNETIC_FIELD 0x03 +#define SENSOR_REPORTID_LINEAR_ACCELERATION 0x04 +#define SENSOR_REPORTID_ROTATION_VECTOR 0x05 +#define SENSOR_REPORTID_GRAVITY 0x06 +#define SENSOR_REPORTID_GAME_ROTATION_VECTOR 0x08 +#define SENSOR_REPORTID_GEOMAGNETIC_ROTATION_VECTOR 0x09 +#define SENSOR_REPORTID_GYRO_INTEGRATED_ROTATION_VECTOR 0x2A +#define SENSOR_REPORTID_TAP_DETECTOR 0x10 +#define SENSOR_REPORTID_STEP_COUNTER 0x11 +#define SENSOR_REPORTID_STABILITY_CLASSIFIER 0x13 +#define SENSOR_REPORTID_RAW_ACCELEROMETER 0x14 +#define SENSOR_REPORTID_RAW_GYROSCOPE 0x15 +#define SENSOR_REPORTID_RAW_MAGNETOMETER 0x16 +#define SENSOR_REPORTID_PERSONAL_ACTIVITY_CLASSIFIER 0x1E +#define SENSOR_REPORTID_AR_VR_STABILIZED_ROTATION_VECTOR 0x28 +#define SENSOR_REPORTID_AR_VR_STABILIZED_GAME_ROTATION_VECTOR 0x29 + +//Record IDs from figure 29, page 29 reference manual +//These are used to read the metadata for each sensor type +#define FRS_RECORDID_ACCELEROMETER 0xE302 +#define FRS_RECORDID_GYROSCOPE_CALIBRATED 0xE306 +#define FRS_RECORDID_MAGNETIC_FIELD_CALIBRATED 0xE309 +#define FRS_RECORDID_ROTATION_VECTOR 0xE30B + +//Command IDs from section 6.4, page 42 +//These are used to calibrate, initialize, set orientation, tare etc the sensor +#define COMMAND_ERRORS 1 +#define COMMAND_COUNTER 2 +#define COMMAND_TARE 3 +#define COMMAND_INITIALIZE 4 +#define COMMAND_DCD 6 +#define COMMAND_ME_CALIBRATE 7 +#define COMMAND_DCD_PERIOD_SAVE 9 +#define COMMAND_OSCILLATOR 10 +#define COMMAND_CLEAR_DCD 11 + +#define CALIBRATE_ACCEL 0 +#define CALIBRATE_GYRO 1 +#define CALIBRATE_MAG 2 +#define CALIBRATE_PLANAR_ACCEL 3 +#define CALIBRATE_ACCEL_GYRO_MAG 4 +#define CALIBRATE_STOP 5 + +#define MAX_PACKET_SIZE 128 //Packets can be up to 32k but we don't have that much RAM. +#define MAX_METADATA_SIZE 9 //This is in words. There can be many but we mostly only care about the first 9 (Qs, range, etc) + +class BNO080 +{ +public: + boolean begin(uint8_t deviceAddress = BNO080_DEFAULT_ADDRESS, TwoWire &wirePort = Wire, uint8_t intPin = 255); //By default use the default I2C addres, and use Wire port, and don't declare an INT pin + boolean beginSPI(uint8_t user_CSPin, uint8_t user_WAKPin, uint8_t user_INTPin, uint8_t user_RSTPin, uint32_t spiPortSpeed = 3000000, SPIClass &spiPort = SPI); + + void enableDebugging(Stream &debugPort = Serial); //Turn on debug printing. If user doesn't specify then Serial will be used. + + void softReset(); //Try to reset the IMU via software + uint8_t resetReason(); //Query the IMU for the reason it last reset + void modeOn(); //Use the executable channel to turn the BNO on + void modeSleep(); //Use the executable channel to put the BNO to sleep + + float qToFloat(int16_t fixedPointValue, uint8_t qPoint); //Given a Q value, converts fixed point floating to regular floating point number + + boolean waitForI2C(); //Delay based polling for I2C traffic + boolean waitForSPI(); //Delay based polling for INT pin to go low + boolean receivePacket(void); + boolean getData(uint16_t bytesRemaining); //Given a number of bytes, send the requests in I2C_BUFFER_LENGTH chunks + boolean sendPacket(uint8_t channelNumber, uint8_t dataLength); + void printPacket(void); //Prints the current shtp header and data packets + void printHeader(void); //Prints the current shtp header (only) + + void enableRotationVector(uint16_t timeBetweenReports); + void enableGameRotationVector(uint16_t timeBetweenReports); + void enableARVRStabilizedRotationVector(uint16_t timeBetweenReports); + void enableARVRStabilizedGameRotationVector(uint16_t timeBetweenReports); + void enableAccelerometer(uint16_t timeBetweenReports); + void enableLinearAccelerometer(uint16_t timeBetweenReports); + void enableGyro(uint16_t timeBetweenReports); + void enableMagnetometer(uint16_t timeBetweenReports); + void enableTapDetector(uint16_t timeBetweenReports); + void enableStepCounter(uint16_t timeBetweenReports); + void enableStabilityClassifier(uint16_t timeBetweenReports); + void enableActivityClassifier(uint16_t timeBetweenReports, uint32_t activitiesToEnable, uint8_t (&activityConfidences)[9]); + void enableRawAccelerometer(uint16_t timeBetweenReports); + void enableRawGyro(uint16_t timeBetweenReports); + void enableRawMagnetometer(uint16_t timeBetweenReports); + void enableGyroIntegratedRotationVector(uint16_t timeBetweenReports); + + bool dataAvailable(void); + uint16_t getReadings(void); + uint16_t parseInputReport(void); //Parse sensor readings out of report + uint16_t parseCommandReport(void); //Parse command responses out of report + + void getQuat(float &i, float &j, float &k, float &real, float &radAccuracy, uint8_t &accuracy); + float getQuatI(); + float getQuatJ(); + float getQuatK(); + float getQuatReal(); + float getQuatRadianAccuracy(); + uint8_t getQuatAccuracy(); + + void getAccel(float &x, float &y, float &z, uint8_t &accuracy); + float getAccelX(); + float getAccelY(); + float getAccelZ(); + uint8_t getAccelAccuracy(); + + void getLinAccel(float &x, float &y, float &z, uint8_t &accuracy); + float getLinAccelX(); + float getLinAccelY(); + float getLinAccelZ(); + uint8_t getLinAccelAccuracy(); + + void getGyro(float &x, float &y, float &z, uint8_t &accuracy); + float getGyroX(); + float getGyroY(); + float getGyroZ(); + uint8_t getGyroAccuracy(); + + void getFastGyro(float &x, float &y, float &z); + float getFastGyroX(); + float getFastGyroY(); + float getFastGyroZ(); + + void getMag(float &x, float &y, float &z, uint8_t &accuracy); + float getMagX(); + float getMagY(); + float getMagZ(); + uint8_t getMagAccuracy(); + + void calibrateAccelerometer(); + void calibrateGyro(); + void calibrateMagnetometer(); + void calibratePlanarAccelerometer(); + void calibrateAll(); + void endCalibration(); + void saveCalibration(); + void requestCalibrationStatus(); //Sends command to get status + boolean calibrationComplete(); //Checks ME Cal response for byte 5, R0 - Status + + uint8_t getTapDetector(); + uint32_t getTimeStamp(); + uint16_t getStepCount(); + uint8_t getStabilityClassifier(); + uint8_t getActivityClassifier(); + + int16_t getRawAccelX(); + int16_t getRawAccelY(); + int16_t getRawAccelZ(); + + int16_t getRawGyroX(); + int16_t getRawGyroY(); + int16_t getRawGyroZ(); + + int16_t getRawMagX(); + int16_t getRawMagY(); + int16_t getRawMagZ(); + + float getRoll(); + float getPitch(); + float getYaw(); + + void setFeatureCommand(uint8_t reportID, uint16_t timeBetweenReports); + void setFeatureCommand(uint8_t reportID, uint16_t timeBetweenReports, uint32_t specificConfig); + void sendCommand(uint8_t command); + void sendCalibrateCommand(uint8_t thingToCalibrate); + + //Metadata functions + int16_t getQ1(uint16_t recordID); + int16_t getQ2(uint16_t recordID); + int16_t getQ3(uint16_t recordID); + float getResolution(uint16_t recordID); + float getRange(uint16_t recordID); + uint32_t readFRSword(uint16_t recordID, uint8_t wordNumber); + void frsReadRequest(uint16_t recordID, uint16_t readOffset, uint16_t blockSize); + bool readFRSdata(uint16_t recordID, uint8_t startLocation, uint8_t wordsToRead); + + //Global Variables + uint8_t shtpHeader[4]; //Each packet has a header of 4 bytes + uint8_t shtpData[MAX_PACKET_SIZE]; + uint8_t sequenceNumber[6] = {0, 0, 0, 0, 0, 0}; //There are 6 com channels. Each channel has its own seqnum + uint8_t commandSequenceNumber = 0; //Commands have a seqNum as well. These are inside command packet, the header uses its own seqNum per channel + uint32_t metaData[MAX_METADATA_SIZE]; //There is more than 10 words in a metadata record but we'll stop at Q point 3 + +private: + //Variables + TwoWire *_i2cPort; //The generic connection to user's chosen I2C hardware + uint8_t _deviceAddress; //Keeps track of I2C address. setI2CAddress changes this. + + Stream *_debugPort; //The stream to send debug messages to if enabled. Usually Serial. + boolean _printDebug = false; //Flag to print debugging variables + + SPIClass *_spiPort; //The generic connection to user's chosen SPI hardware + unsigned long _spiPortSpeed; //Optional user defined port speed + uint8_t _cs; //Pins needed for SPI + uint8_t _wake; + uint8_t _int; + uint8_t _rst; + + //These are the raw sensor values (without Q applied) pulled from the user requested Input Report + uint16_t rawAccelX, rawAccelY, rawAccelZ, accelAccuracy; + uint16_t rawLinAccelX, rawLinAccelY, rawLinAccelZ, accelLinAccuracy; + uint16_t rawGyroX, rawGyroY, rawGyroZ, gyroAccuracy; + uint16_t rawMagX, rawMagY, rawMagZ, magAccuracy; + uint16_t rawQuatI, rawQuatJ, rawQuatK, rawQuatReal, rawQuatRadianAccuracy, quatAccuracy; + uint16_t rawFastGyroX, rawFastGyroY, rawFastGyroZ; + uint8_t tapDetector; + uint16_t stepCount; + uint32_t timeStamp; + uint8_t stabilityClassifier; + uint8_t activityClassifier; + uint8_t *_activityConfidences; //Array that store the confidences of the 9 possible activities + uint8_t calibrationStatus; //Byte R0 of ME Calibration Response + uint16_t memsRawAccelX, memsRawAccelY, memsRawAccelZ; //Raw readings from MEMS sensor + uint16_t memsRawGyroX, memsRawGyroY, memsRawGyroZ; //Raw readings from MEMS sensor + uint16_t memsRawMagX, memsRawMagY, memsRawMagZ; //Raw readings from MEMS sensor + + //These Q values are defined in the datasheet but can also be obtained by querying the meta data records + //See the read metadata example for more info + int16_t rotationVector_Q1 = 14; + int16_t rotationVectorAccuracy_Q1 = 12; //Heading accuracy estimate in radians. The Q point is 12. + int16_t accelerometer_Q1 = 8; + int16_t linear_accelerometer_Q1 = 8; + int16_t gyro_Q1 = 9; + int16_t magnetometer_Q1 = 4; + int16_t angular_velocity_Q1 = 10; +}; diff --git a/include/MPU6050OffsetFinder.cpp b/include/MPU6050OffsetFinder.cpp new file mode 100644 index 0000000..5c35707 --- /dev/null +++ b/include/MPU6050OffsetFinder.cpp @@ -0,0 +1,367 @@ +// MPU6050 offset-finder, based on Jeff Rowberg's MPU6050_RAW +// 2016-10-19 by Robert R. Fenichel (bob@fenichel.net) + +// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class +// 10/7/2011 by Jeff Rowberg +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// +// Changelog: +// 2019-07-11 - added PID offset generation at begninning Generates first offsets +// - in @ 6 seconds and completes with 4 more sets @ 10 seconds +// - then continues with origional 2016 calibration code. +// 2016-11-25 - added delays to reduce sampling rate to ~200 Hz +// added temporizing printing during long computations +// 2016-10-25 - requires inequality (Low < Target, High > Target) during expansion +// dynamic speed change when closing in +// 2016-10-22 - cosmetic changes +// 2016-10-19 - initial release of IMU_Zero +// 2013-05-08 - added multiple output formats +// - added seamless Fastwire support +// 2011-10-07 - initial release of MPU6050_RAW + +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2011 Jeff Rowberg + +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. + + If an MPU6050 + * is an ideal member of its tribe, + * is properly warmed up, + * is at rest in a neutral position, + * is in a location where the pull of gravity is exactly 1g, and + * has been loaded with the best possible offsets, +then it will report 0 for all accelerations and displacements, except for +Z acceleration, for which it will report 16384 (that is, 2^14). Your device +probably won't do quite this well, but good offsets will all get the baseline +outputs close to these target values. + + Put the MPU6050 on a flat and horizontal surface, and leave it operating for +5-10 minutes so its temperature gets stabilized. + + Run this program. A "----- done -----" line will indicate that it has done its best. +With the current accuracy-related constants (NFast = 1000, NSlow = 10000), it will take +a few minutes to get there. + + Along the way, it will generate a dozen or so lines of output, showing that for each +of the 6 desired offsets, it is + * first, trying to find two estimates, one too low and one too high, and + * then, closing in until the bracket can't be made smaller. + + The line just above the "done" line will look something like + [567,567] --> [-1,2] [-2223,-2223] --> [0,1] [1131,1132] --> [16374,16404] [155,156] --> [-1,1] [-25,-24] --> [0,3] [5,6] --> [0,4] +As will have been shown in interspersed header lines, the six groups making up this +line describe the optimum offsets for the X acceleration, Y acceleration, Z acceleration, +X gyro, Y gyro, and Z gyro, respectively. In the sample shown just above, the trial showed +that +567 was the best offset for the X acceleration, -2223 was best for Y acceleration, +and so on. + + The need for the delay between readings (usDelay) was brought to my attention by Nikolaus Doppelhammer. +=============================================== +*/ + +// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files +// for both classes must be in the include path of your project +#include "I2Cdev.h" +#include "MPU9250.h" +#include "motionbase.h" + +// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation +// is used in I2Cdev.h +#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE +#include "Wire.h" +#endif + +const char LBRACKET = '['; +const char RBRACKET = ']'; +const char COMMA = ','; +const char BLANK = ' '; +const char PERIOD = '.'; + +const int iAx = 0; +const int iAy = 1; +const int iAz = 2; +const int iGx = 3; +const int iGy = 4; +const int iGz = 5; + +const int usDelay = 3150; // empirical, to hold sampling to 200 Hz +const int NFast = 1000; // the bigger, the better (but slower) +const int NSlow = 10000; // .. +const int LinesBetweenHeaders = 5; +int LowValue[6]; +int HighValue[6]; +int Smoothed[6]; +int LowOffset[6]; +int HighOffset[6]; +int Target[6]; +int LinesOut; +int N; + +void ForceHeader() +{ + LinesOut = 99; +} + +void GetSmoothed() +{ + int16_t RawValue[6]; + int i; + long Sums[6]; + for (i = iAx; i <= iGz; i++) + { + Sums[i] = 0; + } + // unsigned long Start = micros(); + + for (i = 1; i <= N; i++) + { // get sums + accelgyro.getMotion6(&RawValue[iAx], &RawValue[iAy], &RawValue[iAz], + &RawValue[iGx], &RawValue[iGy], &RawValue[iGz]); + if ((i % 500) == 0) + Serial.print(PERIOD); + delayMicroseconds(usDelay); + for (int j = iAx; j <= iGz; j++) + Sums[j] = Sums[j] + RawValue[j]; + } // get sums + // unsigned long usForN = micros() - Start; + // Serial.print(" reading at "); + // Serial.print(1000000/((usForN+N/2)/N)); + // Serial.println(" Hz"); + for (i = iAx; i <= iGz; i++) + { + Smoothed[i] = (Sums[i] + N / 2) / N; + } +} // GetSmoothed + +void Initialize() +{ + Serial.println("PID tuning Each Dot = 100 readings"); + /*A tidbit on how PID (PI actually) tuning works. + When we change the offset in the MPU6050 we can get instant results. This allows us to use Proportional and + integral of the PID to discover the ideal offsets. Integral is the key to discovering these offsets, Integral + uses the error from set-point (set-point is zero), it takes a fraction of this error (error * ki) and adds it + to the integral value. Each reading narrows the error down to the desired offset. The greater the error from + set-point, the more we adjust the integral value. The proportional does its part by hiding the noise from the + integral math. The Derivative is not used because of the noise and because the sensor is stationary. With the + noise removed the integral value lands on a solid offset after just 600 readings. At the end of each set of 100 + readings, the integral value is used for the actual offsets and the last proportional reading is ignored due to + the fact it reacts to any noise. + */ + accelgyro.CalibrateAccel(6); + accelgyro.CalibrateGyro(6); + Serial.println("\nat 600 Readings"); + accelgyro.PrintActiveOffsets(); + Serial.println(); + accelgyro.CalibrateAccel(1); + accelgyro.CalibrateGyro(1); + Serial.println("700 Total Readings"); + accelgyro.PrintActiveOffsets(); + Serial.println(); + accelgyro.CalibrateAccel(1); + accelgyro.CalibrateGyro(1); + Serial.println("800 Total Readings"); + accelgyro.PrintActiveOffsets(); + Serial.println(); + accelgyro.CalibrateAccel(1); + accelgyro.CalibrateGyro(1); + Serial.println("900 Total Readings"); + accelgyro.PrintActiveOffsets(); + Serial.println(); + accelgyro.CalibrateAccel(1); + accelgyro.CalibrateGyro(1); + Serial.println("1000 Total Readings"); + accelgyro.PrintActiveOffsets(); + Serial.println("\n\n Any of the above offsets will work nice \n\n Lets proof the PID tuning using another method:"); +} // Initialize + +void SetOffsets(int TheOffsets[6]) +{ + accelgyro.setXAccelOffset(TheOffsets[iAx]); + accelgyro.setYAccelOffset(TheOffsets[iAy]); + accelgyro.setZAccelOffset(TheOffsets[iAz]); + accelgyro.setXGyroOffset(TheOffsets[iGx]); + accelgyro.setYGyroOffset(TheOffsets[iGy]); + accelgyro.setZGyroOffset(TheOffsets[iGz]); +} // SetOffsets + +void ShowProgress() +{ + if (LinesOut >= LinesBetweenHeaders) + { // show header + Serial.println("\tXAccel\t\t\tYAccel\t\t\t\tZAccel\t\t\tXGyro\t\t\tYGyro\t\t\tZGyro"); + LinesOut = 0; + } // show header + Serial.print(BLANK); + for (int i = iAx; i <= iGz; i++) + { + Serial.print(LBRACKET); + Serial.print(LowOffset[i]), + Serial.print(COMMA); + Serial.print(HighOffset[i]); + Serial.print("] --> ["); + Serial.print(LowValue[i]); + Serial.print(COMMA); + Serial.print(HighValue[i]); + if (i == iGz) + { + Serial.println(RBRACKET); + } + else + { + Serial.print("]\t"); + } + } + LinesOut++; +} // ShowProgress + +void SetAveraging(int NewN) +{ + N = NewN; + Serial.print("averaging "); + Serial.print(N); + Serial.println(" readings each time"); +} // SetAveraging + +void PullBracketsIn() +{ + boolean AllBracketsNarrow; + boolean StillWorking; + int NewOffset[6]; + + Serial.println("\nclosing in:"); + AllBracketsNarrow = false; + ForceHeader(); + StillWorking = true; + while (StillWorking) + { + StillWorking = false; + if (AllBracketsNarrow && (N == NFast)) + { + SetAveraging(NSlow); + } + else + { + AllBracketsNarrow = true; + } // tentative + for (int i = iAx; i <= iGz; i++) + { + if (HighOffset[i] <= (LowOffset[i] + 1)) + { + NewOffset[i] = LowOffset[i]; + } + else + { // binary search + StillWorking = true; + NewOffset[i] = (LowOffset[i] + HighOffset[i]) / 2; + if (HighOffset[i] > (LowOffset[i] + 10)) + { + AllBracketsNarrow = false; + } + } // binary search + } + SetOffsets(NewOffset); + GetSmoothed(); + for (int i = iAx; i <= iGz; i++) + { // closing in + if (Smoothed[i] > Target[i]) + { // use lower half + HighOffset[i] = NewOffset[i]; + HighValue[i] = Smoothed[i]; + } // use lower half + else + { // use upper half + LowOffset[i] = NewOffset[i]; + LowValue[i] = Smoothed[i]; + } // use upper half + } // closing in + ShowProgress(); + } // still working + +} // PullBracketsIn + +void PullBracketsOut() +{ + boolean Done = false; + int NextLowOffset[6]; + int NextHighOffset[6]; + + Serial.println("expanding:"); + ForceHeader(); + + while (!Done) + { + Done = true; + SetOffsets(LowOffset); + GetSmoothed(); + for (int i = iAx; i <= iGz; i++) + { // got low values + LowValue[i] = Smoothed[i]; + if (LowValue[i] >= Target[i]) + { + Done = false; + NextLowOffset[i] = LowOffset[i] - 1000; + } + else + { + NextLowOffset[i] = LowOffset[i]; + } + } // got low values + + SetOffsets(HighOffset); + GetSmoothed(); + for (int i = iAx; i <= iGz; i++) + { // got high values + HighValue[i] = Smoothed[i]; + if (HighValue[i] <= Target[i]) + { + Done = false; + NextHighOffset[i] = HighOffset[i] + 1000; + } + else + { + NextHighOffset[i] = HighOffset[i]; + } + } // got high values + ShowProgress(); + for (int i = iAx; i <= iGz; i++) + { + LowOffset[i] = NextLowOffset[i]; // had to wait until ShowProgress done + HighOffset[i] = NextHighOffset[i]; // .. + } + } // keep going +} // PullBracketsOut + +void findOffset() +{ + Initialize(); + for (int i = iAx; i <= iGz; i++) + { // set targets and initial guesses + Target[i] = 0; // must fix for ZAccel + HighOffset[i] = 0; + LowOffset[i] = 0; + } // set targets and initial guesses + Target[iAz] = 16384; + SetAveraging(NFast); + + PullBracketsOut(); + PullBracketsIn(); + + Serial.println("-------------- done --------------"); +} // setup \ No newline at end of file diff --git a/include/bno085.cpp b/include/bno085.cpp new file mode 100644 index 0000000..3bd3ac8 --- /dev/null +++ b/include/bno085.cpp @@ -0,0 +1,82 @@ +#include "motionbase.h" +#include "BNO080.cpp" + +BNO080 imu; + +void motionSetup() +{ + delay(500); + if(!imu.begin(BNO080_DEFAULT_ADDRESS, Wire)) { + Serial.println("Can't connect to BNO08X"); + for(int i = 0; i < 500; ++i) { + delay(50); + digitalWrite(LOADING_LED, LOW); + delay(50); + digitalWrite(LOADING_LED, HIGH); + } + } + Serial.println("Connected to BNO08X"); + Wire.setClock(400000); + imu.enableARVRStabilizedRotationVector(30); +} + +void motionLoop() +{ + //Look for reports from the IMU + if (imu.dataAvailable() == true) + { + cq.x = imu.getQuatI(); + cq.y = imu.getQuatJ(); + cq.z = imu.getQuatK(); + cq.w = imu.getQuatReal(); + cq *= rotationQuat; + } +} + +void sendData() { + sendQuat(&cq, PACKET_ROTATION); +} + +void performCalibration() { + for(int i = 0; i < 10; ++i) { + digitalWrite(CALIBRATING_LED, LOW); + delay(20); + digitalWrite(CALIBRATING_LED, HIGH); + delay(20); + } + digitalWrite(CALIBRATING_LED, LOW); + delay(2000); + digitalWrite(CALIBRATING_LED, HIGH); + imu.calibrateGyro(); + imu.requestCalibrationStatus(); // use this + while(!imu.calibrationComplete()) { + digitalWrite(CALIBRATING_LED, LOW); + delay(20); + digitalWrite(CALIBRATING_LED, HIGH); + delay(20); + imu.getReadings(); + } + digitalWrite(CALIBRATING_LED, LOW); + delay(2000); + digitalWrite(CALIBRATING_LED, HIGH); + imu.calibrateAccelerometer(); + while(!imu.calibrationComplete()) { + digitalWrite(CALIBRATING_LED, LOW); + delay(20); + digitalWrite(CALIBRATING_LED, HIGH); + delay(20); + imu.getReadings(); + } + digitalWrite(CALIBRATING_LED, LOW); + delay(2000); + digitalWrite(CALIBRATING_LED, HIGH); + imu.calibrateMagnetometer(); + while(!imu.calibrationComplete()) { + digitalWrite(CALIBRATING_LED, LOW); + delay(20); + digitalWrite(CALIBRATING_LED, HIGH); + delay(20); + imu.getReadings(); + } + imu.saveCalibration(); +} \ No newline at end of file diff --git a/include/motionbase.cpp b/include/motionbase.h similarity index 88% rename from include/motionbase.cpp rename to include/motionbase.h index 1c05dbf..972038f 100644 --- a/include/motionbase.cpp +++ b/include/motionbase.h @@ -1,3 +1,6 @@ +#ifndef _MOTIONBASE_H_ +#define _MOTIONBASE_H_ + #include "I2Cdev.cpp" #include "MPU9250MotionApps.h" #include "defines.h" @@ -20,4 +23,6 @@ void motionSetup(); void motionLoop(); -void sendData(); \ No newline at end of file +void sendData(); + +#endif \ No newline at end of file diff --git a/include/udpclient.cpp b/include/udpclient.cpp index 0485430..ceff982 100644 --- a/include/udpclient.cpp +++ b/include/udpclient.cpp @@ -20,6 +20,9 @@ bool connected = false; unsigned long lastConnectionAttemptMs; unsigned long lastPacketMs; +uint8_t serialBuffer[128]; +size_t serialLength = 0; + template unsigned char * convert_to_chars(T src, unsigned char * target) { @@ -206,6 +209,26 @@ void sendRawCalibrationData(int *const data, int type) } } +void sendSerial(uint8_t *const data, int length, int type) { + if (Udp.beginPacket(host, port) > 0) + { + sendType(type); + sendPacketNumber(); + Udp.write(convert_to_chars(length, buf), sizeof(length)); + Udp.write(data, length); + if (Udp.endPacket() == 0) + { + //Serial.print("Write error: "); + //Serial.println(Udp.getWriteError()); + } + } + else + { + //Serial.print("Write error: "); + //Serial.println(Udp.getWriteError()); + } +} + void returnLastPacket(int len) { if (Udp.beginPacket(host, port) > 0) { @@ -290,6 +313,10 @@ void clientUpdate() break; } } + //while(Serial.available()) { + // size_t bytesRead = Serial.readBytes(serialBuffer, min(Serial.available(), sizeof(serialBuffer))); + // sendSerial(serialBuffer, bytesRead, PACKET_SERIAL); + //} if(lastPacketMs + TIMEOUT < millis()) { connected = false; diff --git a/include/udpclient.h b/include/udpclient.h index dbeaa75..5197ebb 100644 --- a/include/udpclient.h +++ b/include/udpclient.h @@ -17,6 +17,7 @@ #define PACKET_CONFIG 8 #define PACKET_RAW_MAGENTOMETER 9 #define PACKET_PING_PONG 10 +#define PACKET_SERIAL 11 #define PACKET_RECIEVE_HEARTBEAT 1 #define PACKET_RECIEVE_VIBRATE 2 diff --git a/src/main.cpp b/src/main.cpp index 63c9d97..dcc9d04 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -20,7 +20,7 @@ #include "Wire.h" #include "ota.cpp" -#include "6axismotion.cpp" +#include "bno085.cpp" bool isCalibrating = false; bool blinking = false; @@ -61,7 +61,9 @@ void setup() digitalWrite(LOADING_LED, LOW); // join I2C bus (I2Cdev library doesn't do this automatically) - Wire.begin(D6, D5); + Wire.flush(); + Wire.begin(D2, D1); + Wire.setClockStretchLimit(4000); Serial.begin(serialBaudRate); while (!Serial) ; // wait for connection