Merge branch 'mx-firm' into mx-firm-moapp-6_12

README.md

@@ -12,13 +12,12 @@ Compatible and tested with these IMUs (select during compilation):
 * BNO080
   * Using any fusion in internal DMP. Doesn't have BNO085's ARVR stabilization, but still gives good results.
 * BNO055
-  * Work in progress. Should be roughly equal BNO080, but cheaper
+  * Should be roughly equal BNO080, but cheaper. Not tested thoroughly, please report your results on Discord if you're willing to try.
 * MPU-9250
-  * Using Mahony sensor fusion of Gyroscope, Magnetometer and Accelerometer, requires good magnetic environment
-* MPU-6500
-  * Using internal DMP to fuse Gyroscope and Accelerometer, can be used with MPU-9250, can drift substantially
-* MPU-6050
-  * Using internal DMP to fuse Gyro and Accelerometer. This fork uses I2CDev's MPU6050 library
-  * NOTE: Currently the MPU will auto calibrate when powered on. You *must* place it on the group and *DO NOT* move it until calibration is complete
+  * Using Mahony sensor fusion of Gyroscope, Magnetometer and Accelerometer, requires good magnetic environment.
+  * NOTE: Currently can't be calibrated due to lack of proper commands from the server. Work in progress. Can be ran as MPU-6050 below w/o magnetometer.
+* MPU-6500 & MPU-6050
+  * Using internal DMP to fuse Gyroscope and Accelerometer, can be used with MPU-9250 to use it without accelerometer, can drift substantially.
+  * NOTE: Currently the MPU will auto calibrate when powered on. You *must* place it on the ground and *DO NOT* move it until calibration is complete (for a few seconds). **LED on the ESP will blink 5 times after calibration is over.**
 
 Firmware can work with both ESP8266 and ESP32. Please edit defines.h and set your pinout properly according to how you connected the IMU.

lib/bno080/BNO080.cpp

@@ -354,6 +354,7 @@ uint16_t BNO080::parseInputReport(void)
 		shtpData[5] == SENSOR_REPORTID_AR_VR_STABILIZED_ROTATION_VECTOR ||
 		shtpData[5] == SENSOR_REPORTID_AR_VR_STABILIZED_GAME_ROTATION_VECTOR)
 	{
+		hasNewQuaternion = true;
 		quatAccuracy = status;
 		rawQuatI = data1;
 		rawQuatJ = data2;
@@ -363,10 +364,29 @@ uint16_t BNO080::parseInputReport(void)
 		//Only available on rotation vector and ar/vr stabilized rotation vector,
 		// not game rot vector and not ar/vr stabilized rotation vector
 		rawQuatRadianAccuracy = data5;
+
+		if(shtpData[5] == SENSOR_REPORTID_ROTATION_VECTOR || shtpData[5] == SENSOR_REPORTID_AR_VR_STABILIZED_ROTATION_VECTOR) {
+			hasNewMagQuaternion = true;
+			quatMagAccuracy = status;
+			rawMagQuatI = data1;
+			rawMagQuatJ = data2;
+			rawMagQuatK = data3;
+			rawMagQuatReal = data4;
+			rawMagQuatRadianAccuracy = data5;
+		}
+		if(shtpData[5] == SENSOR_REPORTID_GAME_ROTATION_VECTOR || shtpData[5] == SENSOR_REPORTID_AR_VR_STABILIZED_GAME_ROTATION_VECTOR) {
+			hasNewGameQuaternion = true;
+			quatGameAccuracy = status;
+			rawGameQuatI = data1;
+			rawGameQuatJ = data2;
+			rawGameQuatK = data3;
+			rawGameQuatReal = data4;
+		}
 	}
 	else if (shtpData[5] == SENSOR_REPORTID_TAP_DETECTOR)
 	{
 		tapDetector = shtpData[5 + 4]; //Byte 4 only
+		hasNewTap = true;
 	}
 	else if (shtpData[5] == SENSOR_REPORTID_STEP_COUNTER)
 	{
@@ -517,6 +537,40 @@ void BNO080::getQuat(float &i, float &j, float &k, float &real, float &radAccura
 	real = qToFloat(rawQuatReal, rotationVector_Q1);
 	radAccuracy = qToFloat(rawQuatRadianAccuracy, rotationVector_Q1);
 	accuracy = quatAccuracy;
+	hasNewQuaternion = false;
+}
+
+void BNO080::getGameQuat(float &i, float &j, float &k, float &real, uint8_t &accuracy)
+{
+	i = qToFloat(rawGameQuatI, rotationVector_Q1);
+	j = qToFloat(rawGameQuatJ, rotationVector_Q1);
+	k = qToFloat(rawGameQuatK, rotationVector_Q1);
+	real = qToFloat(rawGameQuatReal, rotationVector_Q1);
+	accuracy = quatGameAccuracy;
+	hasNewGameQuaternion = false;
+}
+
+void BNO080::getMagQuat(float &i, float &j, float &k, float &real, float &radAccuracy, uint8_t &accuracy)
+{
+	i = qToFloat(rawMagQuatI, rotationVector_Q1);
+	j = qToFloat(rawMagQuatJ, rotationVector_Q1);
+	k = qToFloat(rawMagQuatK, rotationVector_Q1);
+	real = qToFloat(rawMagQuatReal, rotationVector_Q1);
+	radAccuracy = qToFloat(rawMagQuatRadianAccuracy, rotationVector_Q1);
+	accuracy = quatMagAccuracy;
+	hasNewMagQuaternion = false;
+}
+
+bool BNO080::hasNewQuat() {
+	return hasNewQuaternion;
+}
+
+bool BNO080::hasNewGameQuat() {
+	return hasNewGameQuaternion;
+}
+
+bool BNO080::hasNewMagQuat() {
+	return hasNewMagQuaternion;
 }
 
 //Return the rotation vector quaternion I
@@ -745,9 +799,14 @@ uint8_t BNO080::getTapDetector()
 {
 	uint8_t previousTapDetector = tapDetector;
 	tapDetector = 0; //Reset so user code sees exactly one tap
+	hasNewTap = false;
 	return (previousTapDetector);
 }
 
+bool BNO080::getTapDetected() {
+	return hasNewTap;
+}
+
 //Return the step count
 uint16_t BNO080::getStepCount()
 {
@@ -1341,6 +1400,7 @@ void BNO080::saveCalibration()
 //Returns false if failed
 boolean BNO080::waitForI2C()
 {
+	i2cTimedOut = false;
 	for (uint8_t counter = 0; counter < 100; counter++) //Don't got more than 255
 	{
 		if (_i2cPort->available() > 0)
@@ -1350,9 +1410,15 @@ boolean BNO080::waitForI2C()
 
 	if (_printDebug == true)
 		_debugPort->println(F("I2C timeout"));
+	i2cTimedOut = true;
 	return (false);
 }
 
+boolean BNO080::I2CTimedOut()
+{
+	return i2cTimedOut;
+}
+
 //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

lib/bno080/BNO080.h

@@ -155,6 +155,7 @@ class BNO080
 	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 I2CTimedOut(); // Check if last time I2C timed out
 	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
@@ -184,7 +185,12 @@ class BNO080
 	uint16_t parseInputReport(void);   //Parse sensor readings out of report
 	uint16_t parseCommandReport(void); //Parse command responses out of report
 
+	bool hasNewQuat();
+	bool hasNewGameQuat();
+	bool hasNewMagQuat();
 	void getQuat(float &i, float &j, float &k, float &real, float &radAccuracy, uint8_t &accuracy);
+	void getGameQuat(float &i, float &j, float &k, float &real, uint8_t &accuracy);
+	void getMagQuat(float &i, float &j, float &k, float &real, float &radAccuracy, uint8_t &accuracy);
 	float getQuatI();
 	float getQuatJ();
 	float getQuatK();
@@ -232,6 +238,7 @@ class BNO080
 	boolean calibrationComplete();   //Checks ME Cal response for byte 5, R0 - Status
 
 	uint8_t getTapDetector();
+	bool getTapDetected();
 	uint32_t getTimeStamp();
 	uint16_t getStepCount();
 	uint8_t getStabilityClassifier();
@@ -286,6 +293,7 @@ class BNO080
 	//Variables
 	TwoWire *_i2cPort;		//The generic connection to user's chosen I2C hardware
 	uint8_t _deviceAddress; //Keeps track of I2C address. setI2CAddress changes this.
+	bool i2cTimedOut = false;
 
 	Stream *_debugPort;			 //The stream to send debug messages to if enabled. Usually Serial.
 	boolean _printDebug = false; //Flag to print debugging variables
@@ -303,8 +311,12 @@ class BNO080
 	uint16_t rawGyroX, rawGyroY, rawGyroZ, gyroAccuracy;
 	uint16_t rawMagX, rawMagY, rawMagZ, magAccuracy;
 	uint16_t rawQuatI, rawQuatJ, rawQuatK, rawQuatReal, rawQuatRadianAccuracy, quatAccuracy;
+	uint16_t rawGameQuatI, rawGameQuatJ, rawGameQuatK, rawGameQuatReal, quatGameAccuracy;
+	uint16_t rawMagQuatI, rawMagQuatJ, rawMagQuatK, rawMagQuatReal, rawMagQuatRadianAccuracy, quatMagAccuracy;
+	bool hasNewQuaternion, hasNewGameQuaternion, hasNewMagQuaternion;
 	uint16_t rawFastGyroX, rawFastGyroY, rawFastGyroZ;
 	uint8_t tapDetector;
+	bool hasNewTap;
 	uint16_t stepCount;
 	uint32_t timeStamp;
 	uint8_t stabilityClassifier;

lib/i2cscan/i2cscan.cpp

@@ -1,11 +1,11 @@
 #include "i2cscan.h"
 
 #ifdef ESP8266
-uint8_t portArray[] = {16, 5, 4, 0, 2, 14, 12, 13};
-String portMap[] = {"D0", "D1", "D2", "D3", "D4", "D5", "D6", "D7"};
+uint8_t portArray[] = {16, 5, 4, 2, 14, 12, 13};
+String portMap[] = {"D0", "D1", "D2", "D4", "D5", "D6", "D7"};
 #elif defined(ESP32)
-uint8_t portArray[] = {4, 12, 13, 14, 15, 16, 17, 18, 19, 21, 22, 23, 25, 26, 27, 32, 33};
-String portMap[] = {"4", "12", "13", "14", "15", "16", "17", "18", "19", "21", "22", "23", "25", "26", "27", "32", "33"};
+uint8_t portArray[] = {4, 13, 14, 15, 16, 17, 18, 19, 21, 22, 23, 25, 26, 27, 32, 33};
+String portMap[] = {"4", "13", "14", "15", "16", "17", "18", "19", "21", "22", "23", "25", "26", "27", "32", "33"};
 #endif
 
 namespace I2CSCAN
@@ -91,4 +91,76 @@ namespace I2CSCAN
             return true;
         return false;
     }
+
+    /**
+     * This routine turns off the I2C bus and clears it
+     * on return SCA and SCL pins are tri-state inputs.
+     * You need to call Wire.begin() after this to re-enable I2C
+     * This routine does NOT use the Wire library at all.
+     *
+     * returns 0 if bus cleared
+     *         1 if SCL held low.
+     *         2 if SDA held low by slave clock stretch for > 2sec
+     *         3 if SDA held low after 20 clocks.
+     * From: http://www.forward.com.au/pfod/ArduinoProgramming/I2C_ClearBus/index.html
+     * (c)2014 Forward Computing and Control Pty. Ltd.
+     * NSW Australia, www.forward.com.au
+     * This code may be freely used for both private and commerical use
+     */
+    int clearBus(uint8_t SDA, uint8_t SCL) {
+        #if defined(TWCR) && defined(TWEN)
+        TWCR &= ~(_BV(TWEN)); //Disable the Atmel 2-Wire interface so we can control the SDA and SCL pins directly
+        #endif
+
+        pinMode(SDA, INPUT_PULLUP); // Make SDA (data) and SCL (clock) pins Inputs with pullup.
+        pinMode(SCL, INPUT_PULLUP);
+
+        boolean SCL_LOW = (digitalRead(SCL) == LOW); // Check is SCL is Low.
+        if (SCL_LOW) { //If it is held low Arduno cannot become the I2C master. 
+            return 1; //I2C bus error. Could not clear SCL clock line held low
+        }
+
+        boolean SDA_LOW = (digitalRead(SDA) == LOW);  // vi. Check SDA input.
+        int clockCount = 20; // > 2x9 clock
+
+        while (SDA_LOW && (clockCount > 0)) { //  vii. If SDA is Low,
+            clockCount--;
+        // Note: I2C bus is open collector so do NOT drive SCL or SDA high.
+            pinMode(SCL, INPUT); // release SCL pullup so that when made output it will be LOW
+            pinMode(SCL, OUTPUT); // then clock SCL Low
+            delayMicroseconds(10); //  for >5uS
+            pinMode(SCL, INPUT); // release SCL LOW
+            pinMode(SCL, INPUT_PULLUP); // turn on pullup resistors again
+            // do not force high as slave may be holding it low for clock stretching.
+            delayMicroseconds(10); //  for >5uS
+            // The >5uS is so that even the slowest I2C devices are handled.
+            SCL_LOW = (digitalRead(SCL) == LOW); // Check if SCL is Low.
+            int counter = 20;
+            while (SCL_LOW && (counter > 0)) {  //  loop waiting for SCL to become High only wait 2sec.
+            counter--;
+            delay(100);
+            SCL_LOW = (digitalRead(SCL) == LOW);
+            }
+            if (SCL_LOW) { // still low after 2 sec error
+            return 2; // I2C bus error. Could not clear. SCL clock line held low by slave clock stretch for >2sec
+            }
+            SDA_LOW = (digitalRead(SDA) == LOW); //   and check SDA input again and loop
+        }
+        if (SDA_LOW) { // still low
+            return 3; // I2C bus error. Could not clear. SDA data line held low
+        }
+
+        // else pull SDA line low for Start or Repeated Start
+        pinMode(SDA, INPUT); // remove pullup.
+        pinMode(SDA, OUTPUT);  // and then make it LOW i.e. send an I2C Start or Repeated start control.
+        // When there is only one I2C master a Start or Repeat Start has the same function as a Stop and clears the bus.
+        /// A Repeat Start is a Start occurring after a Start with no intervening Stop.
+        delayMicroseconds(10); // wait >5uS
+        pinMode(SDA, INPUT); // remove output low
+        pinMode(SDA, INPUT_PULLUP); // and make SDA high i.e. send I2C STOP control.
+        delayMicroseconds(10); // x. wait >5uS
+        pinMode(SDA, INPUT); // and reset pins as tri-state inputs which is the default state on reset
+        pinMode(SCL, INPUT);
+        return 0; // all ok
+    }
 }

lib/i2cscan/i2cscan.h

@@ -9,6 +9,7 @@ namespace I2CSCAN {
     bool checkI2C(uint8_t i, uint8_t j);
     bool isI2CExist(uint8_t addr);
     uint8_t pickDevice(uint8_t addr1, uint8_t addr2, bool scanIfNotFound);
+    int clearBus(uint8_t SDA, uint8_t SCL);
 }
 
 #endif // _I2CSCAN_H_