Merge pull request #2 from AvSquirrel/working

Add runtime toggle for IMU fusion
cyoung · Feb 3, 2016 · 56658ff · 56658ff
2 parents af55080 + 03f2241
commit 56658ff
Show file tree

Hide file tree

Showing 5 changed files with 55 additions and 2 deletions.
diff --git a/libimu.c b/libimu.c
@@ -34,12 +34,14 @@
 #include "libimu.h"
 
 
+
 // static void fused_euler_angles(mpudata_t *mpu);
 // static void fused_quaternion(mpudata_t *mpu);
 // static void calibrated_accel(mpudata_t *mpu);
 // static void calibrated_mag(mpudata_t *mpu);
 
 
+
 int i2c_bus;
 int sample_rate;
 int yaw_mix_factor;
@@ -52,6 +54,8 @@ void close_mpu(void)
 
 
 
+
+
 int init_mpu(int sample_rate, int yaw_mix_factor)
 {
 	int ret;
@@ -71,6 +75,19 @@ int init_mpu(int sample_rate, int yaw_mix_factor)
 	return ret;
 }
 
+int enableFusion(void) {
+	if (enableAccelerometerFusion()) {
+		return -1;
+	}
+	return 0;
+}
+
+int disableFusion(void) {
+	if (disableAccelerometerFusion()) {
+		return -1;
+	}
+	return 0;
+}
 int read_mpu(float *pitch, float *roll, float *heading)
 {
 	int ret;
@@ -94,6 +111,7 @@ int read_mpu(float *pitch, float *roll, float *heading)
 // 	mpu->fusedEuler[VEC3_Z] * RAD_TO_DEGREE;
 // }
 
+
 // void fused_quaternions(mpudata_t *mpu)
 // {
 // 	mpu->fusedQuat[QUAT_W];
@@ -151,6 +169,7 @@ int set_cal(int mag, char *cal_file)
 		}
 	}
 
+
 	memset(buff, 0, sizeof(buff));
 
 	for (i = 0; i < 6; i++) {
@@ -159,6 +178,7 @@ int set_cal(int mag, char *cal_file)
 			break;
 		}
 
+
 		val[i] = atoi(buff);
 
 		if (val[i] == 0) {

diff --git a/libimu.h b/libimu.h
@@ -21,6 +21,8 @@ extern int init_mpu(int sample_rate, int yaw_mix_factor);
 extern int read_mpu(float *pitch, float *roll, float *heading);
 extern int set_cal(int mag, char *cal_file);
 
+extern int enableFusion(void);
+extern int disableFusion(void);
 #ifdef __cplusplus
 }
 #endif

diff --git a/mpu/mpu.go b/mpu/mpu.go
@@ -32,13 +32,21 @@ import (
 )
 
 // Current version.
-var PackageVersion = "v0.1"
+var PackageVersion = "v0.2b"
 
 // InitMPU
 func InitMPU(sample_rate, yaw_mix_factor int) int {
 	return int(C.init_mpu(C.int(sample_rate), C.int(yaw_mix_factor)))
 }
 
+func EnableFusion() int {
+	return int(C.enableFusion())
+}
+
+func DisableFusion() int {
+	return int(C.disableFusion())
+}
+
 // CloseMPU
 func CloseMPU() {
 	C.close_mpu()

diff --git a/mpu9150/mpu9150.c b/mpu9150/mpu9150.c
@@ -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_LP_QUAT |  DMP_FEATURE_SEND_RAW_ACCEL 
+  	if (dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL 
 						| DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) {
 		printf("\ndmp_enable_feature() failed\n");
 		return -1;
@@ -161,6 +161,27 @@ int mpu9150_init(int i2c_bus, int sample_rate, int mix_factor)
 	return 0;
 }
 
+/* New functions to enable / disable 6axis on the fly */
+
+
+int enableAccelerometerFusion(void) {
+	if (dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) {
+		printf("Failure enabling accelerometer fusion\n");
+		return -1;
+	}
+	printf("mpu9150.c: Accelerometer fusion enabled\n");
+	return 0;
+}
+
+int disableAccelerometerFusion(void) {
+	if (dmp_enable_feature(DMP_FEATURE_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) {
+		printf("Failure disabling accelerometer fusion\n");
+		return -1;
+	}
+	printf("mpu9150.c: Accelerometer fusion disabled\n");
+	return 0;
+}
+
 void mpu9150_exit()
 {
 	// turn off the DMP on exit 

diff --git a/mpu9150/mpu9150.h b/mpu9150/mpu9150.h
@@ -63,5 +63,7 @@ int mpu9150_read_mag(mpudata_t *mpu);
 void mpu9150_set_accel_cal(caldata_t *cal);
 void mpu9150_set_mag_cal(caldata_t *cal);
 
+int enableAccelerometerFusion(void);
+int disableAccelerometerFusion(void);
 #endif /* MPU9150_H */