Updated test.go to match recent changes

Makefile-native-shared

@@ -7,9 +7,8 @@ CFLAGS = -Wall -fPIC
 
 # add -DI2C_DEBUG for debugging
 #for MPU9150:
-#DEFS = -DEMPL_TARGET_LINUX -DMPU9150 -DAK8975_SECONDARY
-#for MPU6050 with no magnetometer:
-DEFS = -DEMPL_TARGET_LINUX -DMPU6050
+DEFS = -DEMPL_TARGET_LINUX -DMPU9150 -DAK8975_SECONDARY
+#DEFS = -DEMPL_TARGET_LINUX -DMPU9250 -DAK8963_SECONDARY
 
 EMPLDIR = eMPL
 GLUEDIR = glue

eMPL/inv_mpu.c

@@ -718,6 +718,8 @@ int mpu_init(struct int_param_s *int_param)
             st.chip_cfg.accel_half = 1;
         else if (rev == 2)
             st.chip_cfg.accel_half = 0;
+        else if (rev >= 3)
+            st.chip_cfg.accel_half = 0;
         else {
             log_e("Unsupported software product rev %d.\n", rev);
             return -1;

eMPL/inv_mpu_dmp_motion_driver.c

@@ -467,7 +467,7 @@ static const unsigned short sStartAddress = 0x0400;
 
 #define MAX_PACKET_LENGTH   (32)
 
-#define DMP_SAMPLE_RATE     (200)
+#define DMP_SAMPLE_RATE     (500)
 #define GYRO_SF             (46850825LL * 200 / DMP_SAMPLE_RATE)
 
 #define FIFO_CORRUPTION_CHECK

mpu9150/mpu9150.c

@@ -59,7 +59,7 @@ int mpu9150_init(int i2c_bus, int sample_rate, int mix_factor)
 	if (i2c_bus < 0 || i2c_bus > 3)
 		return -1;
 
-	if (sample_rate < 2 || sample_rate > 50)
+	if (sample_rate < 2 || sample_rate > 1000)
 		return -1;
 
 	if (mix_factor < 0 || mix_factor > 100)
@@ -69,7 +69,7 @@ int mpu9150_init(int i2c_bus, int sample_rate, int mix_factor)
 
 	linux_set_i2c_bus(i2c_bus);
 
-	printf("\nInitializing IMU .");
+	printf("\nInitializing IMU ...\n");
 	fflush(stdout);
 
 	if (mpu_init(NULL)) {
@@ -120,7 +120,7 @@ int mpu9150_init(int i2c_bus, int sample_rate, int mix_factor)
 
 	if (dmp_load_motion_driver_firmware()) {
 		printf("\ndmp_load_motion_driver_firmware() failed\n");
-		return -1;
+		//return -1;
 	}
 
 	printf(".");
@@ -134,7 +134,7 @@ int mpu9150_init(int i2c_bus, int sample_rate, int mix_factor)
 	printf(".");
 	fflush(stdout);
 
-  	if (dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL 
+  	if (dmp_enable_feature(DMP_FEATURE_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL 
 						| DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) {
 		printf("\ndmp_enable_feature() failed\n");
 		return -1;
@@ -145,15 +145,15 @@ int mpu9150_init(int i2c_bus, int sample_rate, int mix_factor)
 
 	if (dmp_set_fifo_rate(sample_rate)) {
 		printf("\ndmp_set_fifo_rate() failed\n");
-		return -1;
+		//return -1;
 	}
 
 	printf(".");
 	fflush(stdout);
 
-	if (mpu_set_dmp_state(1)) {
+	if (mpu_set_dmp_state(0)) {
 		printf("\nmpu_set_dmp_state(1) failed\n");
-		return -1;
+		//return -1;
 	}
 
 	printf(" done\n\n");
@@ -264,7 +264,7 @@ int mpu9150_read_dmp(mpudata_t *mpu)
 
 	if (!data_ready()) {
 		printf("data_ready() - data not ready.\n");
-		return -1;
+		//return -1;
 	}
 
 	if (dmp_read_fifo(mpu->rawGyro, mpu->rawAccel, mpu->rawQuat, &mpu->dmpTimestamp, &sensors, &more) < 0) {

test.go

@@ -1,12 +1,13 @@
 package main
 
 import (
-	"./mpu"
 	"fmt"
 	"math"
 	"net"
 	"os"
 	"time"
+
+	"./mpu"
 )
 
 type SituationUpdate struct {
@@ -58,17 +59,17 @@ func main() {
 	}
 	go updateSender(os.Args[1])
 
-	mpu.InitMPU()
+	mpu.InitMPU(500, 0)
 	defer mpu.CloseMPU()
 	time.Sleep(98 * time.Millisecond)
-	for {
-		pitch, roll, heading, err := mpu.ReadMPU()
+	for i := 0; i < 10; i++ {
+		d, err := mpu.ReadMPURaw()
 		if err == nil {
 			//			fmt.Printf("%s\n", err.Error())
 			//			time.Sleep(1 * time.Second)
 			//			continue
-			fmt.Printf("%f, %f, %f\n", pitch, roll, heading)
-			sendUpdate(pitch, roll, heading)
+			fmt.Printf("%f, %f, %f\n", d.Gx, d.Gy, d.Gz)
+			//sendUpdate(pitch, roll, heading)
 		}
 		time.Sleep(98 * time.Millisecond)
 	}