Add MPU6050 library and sample code

Sming/Libraries/MPU6050/README.md

@@ -0,0 +1,9 @@
+# MPU6050 Six-Axis (Gyro + Accelerometer)
+
+Based on code from [jrowberg/i2cdevlib](https://github.com/jrowberg/i2cdevlib/tree/master/ESP32_ESP-IDF/components/MPU6050). Most of the code is the same, except:
+
+- Removed MPU6050::ReadRegister function due to incompatibility. It is also not used anywhere in the original code.
+- Removed map function in favor of the Sming built-in one.
+- Adapted include path and applied clangformat
+
+

Sming/Libraries/MPU6050/component.mk

@@ -0,0 +1 @@
+COMPONENT_DEPENDS := I2Cdev

Sming/Libraries/MPU6050/helper_3dmath.h

@@ -0,0 +1,243 @@
+// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class, 3D
+// math helper 6/5/2012 by Jeff Rowberg <jeff@rowberg.net> Updates should
+// (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
+//
+// Changelog:
+//     2012-06-05 - add 3D math helper file to DMP6 example sketch
+
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+Copyright (c) 2012 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.
+===============================================
+*/
+
+#ifndef _HELPER_3DMATH_H_
+#define _HELPER_3DMATH_H_
+#include <math.h>
+#include <stdint.h>
+
+class Quaternion
+{
+public:
+	float w;
+	float x;
+	float y;
+	float z;
+
+	Quaternion()
+	{
+		w = 1.0f;
+		x = 0.0f;
+		y = 0.0f;
+		z = 0.0f;
+	}
+
+	Quaternion(float nw, float nx, float ny, float nz)
+	{
+		w = nw;
+		x = nx;
+		y = ny;
+		z = nz;
+	}
+
+	Quaternion getProduct(Quaternion q)
+	{
+		// Quaternion multiplication is defined by:
+		//     (Q1 * Q2).w = (w1w2 - x1x2 - y1y2 - z1z2)
+		//     (Q1 * Q2).x = (w1x2 + x1w2 + y1z2 - z1y2)
+		//     (Q1 * Q2).y = (w1y2 - x1z2 + y1w2 + z1x2)
+		//     (Q1 * Q2).z = (w1z2 + x1y2 - y1x2 + z1w2
+		return Quaternion(w * q.w - x * q.x - y * q.y - z * q.z,  // new w
+						  w * q.x + x * q.w + y * q.z - z * q.y,  // new x
+						  w * q.y - x * q.z + y * q.w + z * q.x,  // new y
+						  w * q.z + x * q.y - y * q.x + z * q.w); // new z
+	}
+
+	Quaternion getConjugate()
+	{
+		return Quaternion(w, -x, -y, -z);
+	}
+
+	float getMagnitude()
+	{
+		return sqrt(w * w + x * x + y * y + z * z);
+	}
+
+	void normalize()
+	{
+		float m = getMagnitude();
+		w /= m;
+		x /= m;
+		y /= m;
+		z /= m;
+	}
+
+	Quaternion getNormalized()
+	{
+		Quaternion r(w, x, y, z);
+		r.normalize();
+		return r;
+	}
+};
+
+class VectorInt16
+{
+public:
+	int16_t x;
+	int16_t y;
+	int16_t z;
+
+	VectorInt16()
+	{
+		x = 0;
+		y = 0;
+		z = 0;
+	}
+
+	VectorInt16(int16_t nx, int16_t ny, int16_t nz)
+	{
+		x = nx;
+		y = ny;
+		z = nz;
+	}
+
+	float getMagnitude()
+	{
+		return sqrt(x * x + y * y + z * z);
+	}
+
+	void normalize()
+	{
+		float m = getMagnitude();
+		x /= m;
+		y /= m;
+		z /= m;
+	}
+
+	VectorInt16 getNormalized()
+	{
+		VectorInt16 r(x, y, z);
+		r.normalize();
+		return r;
+	}
+
+	void rotate(Quaternion* q)
+	{
+		// http://www.cprogramming.com/tutorial/3d/quaternions.html
+		// http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm
+		// http://content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation
+		// ^ or:
+		// http://webcache.googleusercontent.com/search?q=cache:xgJAp3bDNhQJ:content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation&hl=en&gl=us&strip=1
+
+		// P_out = q * P_in * conj(q)
+		// - P_out is the output vector
+		// - q is the orientation quaternion
+		// - P_in is the input vector (a*aReal)
+		// - conj(q) is the conjugate of the orientation quaternion (q=[w,x,y,z],
+		// q*=[w,-x,-y,-z])
+		Quaternion p(0, x, y, z);
+
+		// quaternion multiplication: q * p, stored back in p
+		p = q->getProduct(p);
+
+		// quaternion multiplication: p * conj(q), stored back in p
+		p = p.getProduct(q->getConjugate());
+
+		// p quaternion is now [0, x', y', z']
+		x = p.x;
+		y = p.y;
+		z = p.z;
+	}
+
+	VectorInt16 getRotated(Quaternion* q)
+	{
+		VectorInt16 r(x, y, z);
+		r.rotate(q);
+		return r;
+	}
+};
+
+class VectorFloat
+{
+public:
+	float x;
+	float y;
+	float z;
+
+	VectorFloat()
+	{
+		x = 0;
+		y = 0;
+		z = 0;
+	}
+
+	VectorFloat(float nx, float ny, float nz)
+	{
+		x = nx;
+		y = ny;
+		z = nz;
+	}
+
+	float getMagnitude()
+	{
+		return sqrt(x * x + y * y + z * z);
+	}
+
+	void normalize()
+	{
+		float m = getMagnitude();
+		x /= m;
+		y /= m;
+		z /= m;
+	}
+
+	VectorFloat getNormalized()
+	{
+		VectorFloat r(x, y, z);
+		r.normalize();
+		return r;
+	}
+
+	void rotate(Quaternion* q)
+	{
+		Quaternion p(0, x, y, z);
+
+		// quaternion multiplication: q * p, stored back in p
+		p = q->getProduct(p);
+
+		// quaternion multiplication: p * conj(q), stored back in p
+		p = p.getProduct(q->getConjugate());
+
+		// p quaternion is now [0, x', y', z']
+		x = p.x;
+		y = p.y;
+		z = p.z;
+	}
+
+	VectorFloat getRotated(Quaternion* q)
+	{
+		VectorFloat r(x, y, z);
+		r.rotate(q);
+		return r;
+	}
+};
+
+#endif /* _HELPER_3DMATH_H_ */

samples/Accel_Gyro_MPU6050/Makefile

@@ -0,0 +1,9 @@
+#####################################################################
+#### Please don't change this file. Use component.mk instead ####
+#####################################################################
+
+ifndef SMING_HOME
+$(error SMING_HOME is not set: please configure it as an environment variable)
+endif
+
+include $(SMING_HOME)/project.mk

samples/Accel_Gyro_MPU6050/README.rst

@@ -0,0 +1,7 @@
+MPU6050 Six-Axis (Gyro + Accelerometer) 
+================
+
+MPU6050 sensor reader.
+
+.. image:: mpu6050.jpg
+   :height: 192px

samples/Accel_Gyro_MPU6050/app/application.cpp

@@ -0,0 +1,42 @@
+#include <Libraries/MPU6050/MPU6050.h>
+#include <SmingCore.h>
+
+constexpr float main_loop_interval = 0.02; // sec
+SimpleTimer main_loop_timer;
+MPU6050 mpu;
+
+void print_accel_gyro(int16_t ax, int16_t ay, int16_t az, int16_t gx, int16_t gy, int16_t gz)
+{
+	Serial.print("a/g:\t");
+	Serial.print(ax);
+	Serial.print("\t");
+	Serial.print(ay);
+	Serial.print("\t");
+	Serial.print(az);
+	Serial.print("\t");
+	Serial.print(gx);
+	Serial.print("\t");
+	Serial.print(gy);
+	Serial.print("\t");
+	Serial.println(gz);
+}
+
+void main_loop()
+{
+	int16_t ax, ay, az;
+	int16_t gx, gy, gz;
+	mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
+	print_accel_gyro(ax, ay, az, gx, gy, gz);
+}
+
+void init()
+{
+	Serial.begin(SERIAL_BAUD_RATE); // 115200 by default
+	Serial.systemDebugOutput(true); // Enable debug output to serial
+
+	Wire.begin(DEFAULT_SDA_PIN, DEFAULT_SCL_PIN);
+	mpu.initialize();
+	Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
+
+	main_loop_timer.initializeMs(static_cast<int>(main_loop_interval * 1000), main_loop).start();
+}

samples/Accel_Gyro_MPU6050/component.mk

@@ -0,0 +1,2 @@
+ARDUINO_LIBRARIES := MPU6050
+DISABLE_NETWORK := 1