From ac658834b9a23ea87b970319bd5a46d5cfd8b4af Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Atle=20H=2E=20Havs=C3=B8?= <atle@havso.net>
Date: Sat, 30 Aug 2014 14:44:16 +0200
Subject: SI units Accel and Gyro.
---
drivers/mpu6000.h | 30 ++++++++++++++++--------------
1 file changed, 16 insertions(+), 14 deletions(-)
diff --git a/drivers/mpu6000.h b/drivers/mpu6000.h
index a667b00..7194cb4 100644
--- a/drivers/mpu6000.h
+++ b/drivers/mpu6000.h
@@ -36,23 +36,25 @@ class MPU6000 {
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);
+ Time::sleep(100);
+ write_register(0x68, (1 << 2) | (1 << 1) | (1 << 0)); // SINGAL_PATH_RESET - Reset Gyro | Accel | Temp
+ Time::sleep(100);
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);
+ Time::sleep(50);
write_register(0x1A, (3 << 0)); // CONFIG - DLPF_CFG 42hz
- Time::sleep(1);
+ Time::sleep(50);
write_register(0x1B, (3 << 3)); // GYRO_CFG, 2000 deg/s
- Time::sleep(1);
- write_register(0x1C, (3 << 3)); // ACCEL_CFG, +-16g
- Time::sleep(1);
+ Time::sleep(50);
+ write_register(0x1C, (3 << 3)); // ACCEL_CFG, +-16g || Why u no work!
+ Time::sleep(50);
write_register(0x6A, (1 << 4)); // USER_CTRL - I2C Off
- Time::sleep(1);
+ Time::sleep(50);
write_register(0x6B, (3 << 0)); // PWR_MGMT_1 - CLKSEL PLL_Z
- Time::sleep(1);
+ Time::sleep(50);
write_register(0x6C, 0); // PWR_MGMT_2
Time::sleep(150);
}
@@ -67,23 +69,23 @@ class MPU6000 {
temp = raw_data[0] << 8;
temp |= raw_data[1];
- accel[0] = float(temp) * 2 / 32768.0f;
+ accel[0] = float(temp) * (9.81 / 16384.0); // Actually in +-2g for reasons unknown.
temp = raw_data[2] << 8;
temp |= raw_data[3];
- accel[1] = float(temp) * 2 / 32768.0f;
+ accel[1] = float(temp) * (9.81 / 16384.0);
temp = raw_data[4] << 8;
temp |= raw_data[5];
- accel[2] = float(temp) * 2 / 32768.0f;
+ accel[2] = float(temp) * (9.81 / 16384.0);
temp = raw_data[8] << 8;
temp |= raw_data[9];
- gyro[0] = float(temp) * 2000 / 32768.0f;
+ gyro[0] = float(temp) * (2000.0f / 32768.0f) * 0.0174532f;
temp = raw_data[10] << 8;
temp |= raw_data[11];
- gyro[1] = float(temp) * 2000 / 32768.0f;
+ gyro[1] = float(temp) * (2000.0f / 32768.0f) * 0.0174532f;
temp = raw_data[12] << 8;
temp |= raw_data[13];
- gyro[2] = float(temp) * 2000 / 32768.0f;
+ gyro[2] = float(temp) * (2000.0f / 32768.0f) * 0.0174532f;
}
};
--
cgit v1.2.3