X-Git-Url: http://git.droids-corp.org/?p=protos%2Fimu.git;a=blobdiff_plain;f=mpu6050.c;h=143d0e99be63b40d67859236b724d4f0b93b2a56;hp=4f1ab817c221c626f84e153d2527c2aab06f1780;hb=1efaecd600f90de4aa0f18d694355c52f2447b5a;hpb=96d834bdfb8df4e3369ca1b3c7bc7bc8534fda31

diff --git a/mpu6050.c b/mpu6050.c
index 4f1ab81..143d0e9 100644
--- a/mpu6050.c
+++ b/mpu6050.c
@@ -16,9 +16,9 @@
 
 // fs_sel = 1
 // 500°/s
-#define gyro_x_resolution 66.5
-#define gyro_y_resolution 66.5
-#define gyro_z_resolution 66.5
+/* #define gyro_x_resolution 66.5 */
+/* #define gyro_y_resolution 66.5 */
+/* #define gyro_z_resolution 66.5 */
 
 // fs_sel = 3
 // 2000°/s
@@ -26,9 +26,9 @@
 #define gyro_y_resolution 16.4
 #define gyro_z_resolution 16.4
 
-#define accel_x_resolution 8192.0
-#define accel_y_resolution 8192.0
-#define accel_z_resolution 8192.0
+#define accel_x_resolution 2048.0
+#define accel_y_resolution 2048.0
+#define accel_z_resolution 2048.0
 
 
 
@@ -37,24 +37,24 @@
 #define MPU6050_OFF_LSB 0
 
 
-#define MPU6050_ADDRESS 0x68
-#define MPU6050_MAGNETO_ADDRESS 0x0C
+#define MPU6050_ADDRESS 0x69
+#define MPU6050_MAGNETO_ADDRESS 0x0D
 
 #define SWAP_16(a) ((( (a) & 0xff)<<8) | (( (a) >> 8) & 0xff))
 
-float mpu6050_gx;
-float mpu6050_gy;
-float mpu6050_gz;
+double mpu6050_gx;
+double mpu6050_gy;
+double mpu6050_gz;
 
-float mpu6050_ax;
-float mpu6050_ay;
-float mpu6050_az;
+double mpu6050_ax;
+double mpu6050_ay;
+double mpu6050_az;
 
-float mpu6050_mx;
-float mpu6050_my;
-float mpu6050_mz;
+double mpu6050_mx;
+double mpu6050_my;
+double mpu6050_mz;
 
-float mpu6050_temp;
+double mpu6050_temp;
 
 int16_t drift_g[3] = {0, 0, 0};
 
@@ -81,19 +81,19 @@ uint8_t mpu6050_read_all_axes(int16_t *values)
 		values[i] = SWAP_16(values[i]);
 	}
 
-	mpu6050_ax = 9.81 * (float)values[0] / accel_x_resolution ;
-	mpu6050_ay = 9.81 * (float)values[1] / accel_y_resolution ;
-	mpu6050_az = 9.81 * (float)values[2] / accel_z_resolution ;
+	mpu6050_ax = 9.81 * (double)values[0] / accel_x_resolution ;
+	mpu6050_ay = 9.81 * (double)values[1] / accel_y_resolution ;
+	mpu6050_az = 9.81 * (double)values[2] / accel_z_resolution ;
 
-	mpu6050_temp = (float)values[3]/340. + 36.5;
+	mpu6050_temp = (double)values[3]/340. + 36.5;
 
-	mpu6050_gx = ToRad((float)(values[4] - drift_g[0]) / gyro_x_resolution );
-	mpu6050_gy = ToRad((float)(values[5] - drift_g[1]) / gyro_y_resolution );
-	mpu6050_gz = ToRad((float)(values[6] - drift_g[2]) / gyro_z_resolution );
+	mpu6050_gx = ToRad((double)(values[4] - drift_g[0]) / gyro_x_resolution );
+	mpu6050_gy = ToRad((double)(values[5] - drift_g[1]) / gyro_y_resolution );
+	mpu6050_gz = ToRad((double)(values[6] - drift_g[2]) / gyro_z_resolution );
 
-	mpu6050_mx = (float) values[7] * 0.3;
-	mpu6050_my = (float) values[8] * 0.3;
-	mpu6050_mz = (float) values[9] * 0.3;
+	mpu6050_mx = (double) values[7] * 0.3;
+	mpu6050_my = (double) values[8] * 0.3;
+	mpu6050_mz = (double) values[9] * 0.3;
 
 
 	return err;
@@ -192,7 +192,7 @@ uint8_t mpu6050_init(void)
 
 
 	while(1) {
-		err = read_reg(MPU6050_ADDRESS, 0x75, &id);
+		err = read_reg(MPU6050_ADDRESS, MPU6050_RA_WHO_AM_I, &id);
 		if (err)
 			continue;
 		if (id == 0x68)
@@ -210,9 +210,9 @@ uint8_t mpu6050_init(void)
 
 	//Disable gyro self tests, scale of 2000 degrees/s
 	send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_GYRO_CONFIG, 0b00011000);
+	//Disable accel self tests, scale of +-16g, no DHPF
+	send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ACCEL_CONFIG, 0b00011000);
 
-	//Disable accel self tests, scale of +-4g, no DHPF
-	send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ACCEL_CONFIG, 0b00001000);
 	//Freefall threshold of <|0mg|
 	send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FF_THR, 0x00);
 	//Freefall duration limit of 0