many updates..

new sensors.. display.. led.. drawing.. stuff..

ext/sens/MPU6050.h

@@ -0,0 +1,154 @@
+#ifndef SENS_MPU6050
+#define SENS_MPU6050
+
+#include "../../io/SoftI2C.h"
+#include "../../Debug.h"
+
+/**
+ * accelereomter/gyroscope
+ * https://www.invensense.com/wp-content/uploads/2015/02/MPU-6000-Datasheet1.pdf
+ * https://www.invensense.com/wp-content/uploads/2015/02/MPU-6000-Register-Map1.pdf
+ * https://github.com/kriswiner/MPU6050/blob/master/MPU6050BasicExample.ino
+ */
+class MPU6050 {
+
+private:
+
+	static constexpr const char* NAME		= "MPU6050";
+
+	static constexpr uint8_t ADDR			= 0b1101000 ;
+
+	static constexpr uint8_t REG_CFG_GYRO		= 0x1B;
+	static constexpr uint8_t REG_CFG_ACC		= 0x1C;
+	static constexpr uint8_t REG_ACCEL_XOUT_H	= 0x3B;
+
+	static constexpr uint8_t REG_SMPLRT_DIV		= 0x19;
+	static constexpr uint8_t REG_CONFIG			= 0x1A;
+
+	static constexpr uint8_t REG_PWR_MGMT_1		= 0x6B;
+
+	bool inited = false;
+
+
+private:
+
+	void init() {
+
+		debugMod(NAME, "init()");
+
+		// set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001
+		writeRegister(REG_PWR_MGMT_1, 0b001);		// table on page 40
+
+		// Configure Gyro and Accelerometer
+		// Disable FSYNC and set accelerometer and gyro bandwidth to 44 and 42 Hz, respectively;
+		// DLPF_CFG = bits 2:0 = 010; this sets the sample rate at 1 kHz for both
+		//writeRegister(REG_CONFIG, 0x03);			// table on page 13
+		writeRegister(REG_CONFIG, 0x00);			// table on page 13
+
+		// Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV)
+		writeRegister(REG_SMPLRT_DIV, 0x04); // Use a 200 Hz sample rate
+
+		// configure gyro as +/- 4G -> +32767 = +4G, -32768 = -4G
+		writeRegister(REG_CFG_ACC, 0b01 << 3);		// table on page 15
+		#define oneGto1000(x)	((int)x)*1000/(32768/4)
+	}
+
+	bool writeRegister(const uint8_t addr, const uint8_t val) {
+
+		bool ok;
+
+		// address the slave in write mode and select the first register to read
+		ok = i2c::startWrite(ADDR);
+		if (!ok) {os_printf("failed start write\n"); return false;}
+		ok = i2c::writeByteAndCheck(addr);
+		if (!ok) {os_printf("failed to select register %d\n", addr); return false;}
+		ok = i2c::writeBytesAndCheck(&val, 1);
+		if (!ok) {os_printf("failed to write register contents \n"); return false;}
+
+		// done
+		i2c::stop();
+		return true;
+
+	}
+
+
+
+
+public:
+
+	struct Acc {
+		int16_t x;
+		int16_t y;
+		int16_t z;
+	};
+
+	struct Gyro {
+		int16_t x;
+		int16_t y;
+		int16_t z;
+	};
+
+	struct Res {
+		Acc acc;
+		int16_t temp;
+		Gyro gyro;
+	};
+
+	void initOnce() {
+		if (inited) {return;}
+		init();
+		inited = true;
+	}
+
+
+	bool isPresent() {
+		return i2c::query(ADDR);
+	}
+
+	bool query(Res& res) {
+
+		bool ok;
+
+		// select register(s) to read
+		//i2c::start();
+		ok = i2c::startWrite(ADDR);
+		if (!ok) {os_printf("failed start write1\n"); return false;}
+		ok = i2c::writeByteAndCheck(REG_ACCEL_XOUT_H);
+		if (!ok) {os_printf("failed select register\n"); return false;}
+		i2c::stop();
+
+		// read registers
+		uint8_t buf[14];
+		//i2c::start();
+		ok = i2c::startRead(ADDR);
+		if (!ok) {os_printf("failed start write2\n"); return false;}
+		i2c::readBytes(buf, 14);
+		i2c::stop();
+
+		res.acc.x = (buf[0] << 8) | buf[1];
+		res.acc.y = (buf[2] << 8) | buf[3];
+		res.acc.z = (buf[4] << 8) | buf[5];
+
+		res.temp = (buf[6] << 8) | buf[7];
+
+		res.gyro.x = (buf[8] << 8) | buf[9];
+		res.gyro.y = (buf[10] << 8) | buf[11];
+		res.gyro.z = (buf[12] << 8) | buf[13];
+
+		res.acc.x = oneGto1000(res.acc.x);
+		res.acc.y = oneGto1000(res.acc.y);
+		res.acc.z = oneGto1000(res.acc.z);
+
+
+		//os_printf("Acc  (%d, %d, %d)\n", res.acc.x, res.acc.y, res.acc.z);
+		//os_printf("Gyro (%d, %d, %d)\n", res.gyro.x, res.gyro.y, res.gyro.z);
+		//os_printf("Temp (%d)\n", res.temp);
+
+		return true;
+
+	}
+
+
+};
+
+#endif