summaryrefslogtreecommitdiff
path: root/drivers
diff options
context:
space:
mode:
Diffstat (limited to 'drivers')
-rw-r--r--drivers/mpu6000.h90
1 files changed, 90 insertions, 0 deletions
diff --git a/drivers/mpu6000.h b/drivers/mpu6000.h
new file mode 100644
index 0000000..1297810
--- /dev/null
+++ b/drivers/mpu6000.h
@@ -0,0 +1,90 @@
+#ifndef MPU6000_H
+#define MPU6000_H
+
+#include <spi/spi.h>
+#include <gpio/gpio.h>
+#include <os/time.h>
+
+class MPU6000 {
+ private:
+ Pin cs;
+ SPI_t& spi;
+ uint8_t raw_data[14];
+
+ void read_register(uint8_t reg, uint8_t len, uint8_t *data) {
+ cs.off();
+ spi.transfer_byte(reg | (1 << 7)); // Read bit
+ while(len--) {
+ *data++ = spi.transfer_byte();
+ }
+ cs.on();
+ }
+
+ void write_register(uint8_t reg, uint8_t data) {
+ cs.off();
+ spi.transfer_byte(reg);
+ spi.transfer_byte(data);
+ cs.on();
+ }
+
+ public:
+ MPU6000(Pin cs_pin, SPI_t& spi_bus) : cs(cs_pin), spi(spi_bus) {}
+ float gyro [3] = {0,0,0}; //X Y Z
+ float accel [3] = {0,0,0}; //X Y Z
+
+ void init() {
+ spi.reg.CR1 = (1 << 9) | (1 << 8) | (1 << 6) | (2 << 3) | (1 << 2) | (1 << 1) | (1 << 0); // SSM On | SSI 1 | SPE On | BR /8 | CPOL CK 1 Idle | CPHA Second clock data capture
+ spi.reg.CR2 = (1 << 12) | ( 7 << 8); // FRXTH RXNE if FIFO >= 8bit | DS 8-bit
+ write_register(0x6B, (1 << 7)); // Device Reset
+ Time::sleep(150);
+ uint8_t who_am_i = 0x00;
+ while (who_am_i != 0x68) {
+ read_register(0x75, 1, &who_am_i); //WHO_AM_I register
+ }
+ write_register(0x19, 4); // SAMPLE DIV 4
+ Time::sleep(1);
+ write_register(0x1A, (3 << 0)); // CONFIG - DLPF_CFG 42hz
+ Time::sleep(1);
+ write_register(0x1B, 0); // GYRO_CFG
+ Time::sleep(1);
+ write_register(0x1C, 0); // ACCEL_CFG
+ Time::sleep(1);
+ write_register(0x6A, (1 << 4)); // USER_CTRL - I2C Off
+ Time::sleep(1);
+ write_register(0x6B, (3 << 0)); // PWR_MGMT_1 - CLKSEL PLL_Z
+ Time::sleep(1);
+ write_register(0x6C, 0); // PWR_MGMT_2
+ Time::sleep(150);
+ }
+
+
+ void update() {
+ uint16_t temp;
+ spi.reg.CR1 = (1 << 9) | (1 << 8) | (1 << 6) | (2 << 3) | (1 << 2) | (1 << 1) | (1 << 0); // SSM On | SSI 1 | SPE On | BR /8 | CPOL CK 1 Idle | CPHA Second clock data capture
+ spi.reg.CR2 = (1 << 12) | ( 7 << 8); // FRXTH RXNE if FIFO >= 8bit | DS 8-bit
+
+ read_register(0x3B, 14, raw_data); // Accel 0-5, Temp 6-7, Gyro 8-13
+
+ temp = raw_data[0] << 8;
+ temp |= raw_data[1];
+ accel[0] = float(temp);
+ temp = raw_data[2] << 8;
+ temp |= raw_data[3];
+ accel[1] = float(temp);
+ temp = raw_data[4] << 8;
+ temp |= raw_data[5];
+ accel[2] = float(temp);
+
+ temp = raw_data[8] << 8;
+ temp |= raw_data[9];
+ gyro[0] = float(temp);
+ temp = raw_data[10] << 8;
+ temp |= raw_data[11];
+ gyro[1] = float(temp);
+ temp = raw_data[12] << 8;
+ temp |= raw_data[13];
+ gyro[2] = float(temp);
+ }
+};
+
+#endif