Diff: BNO055.cpp

Revision:

5:6a08a4c5b1e1

Parent:

4:9e6fead1e93e

Child:

6:eae1a743876b

--- a/BNO055.cpp	Thu Apr 16 10:47:40 2015 +0000
+++ b/BNO055.cpp	Mon Jan 16 16:51:39 2017 +0000
@@ -481,3 +481,55 @@
     change_fusion_mode(current_mode);
     return d;
 }
+
+void BNO055::get_quaternion_float(float *Q){
+    BNO055_QUATERNION_TypeDef intQ;
+    get_quaternion(&intQ);
+    
+    Q[0] = (float)(intQ.w / 16384.0);
+    Q[1] = (float)(intQ.x / 16384.0);
+    Q[2] = (float)(intQ.y / 16384.0);
+    Q[3] = (float)(intQ.z / 16384.0);
+}
+
+
+void BNO055::configure_accelerometer_range(uint8_t range){
+    uint8_t current_mode;
+    
+    current_mode = check_operating_mode();
+    if(current_mode != CONFIGMODE)
+        change_fusion_mode(CONFIGMODE);
+        
+    select_page(1); 
+    dt[0] = ACCEL_CONFIG;
+    dt[1] = 0<<5 | 3 << 2 | range;
+    
+    _i2c.write(chip_addr, dt, 2, false);
+    
+    change_fusion_mode(current_mode);
+}
+
+void BNO055::get_abs_accel(BNO055_LIN_ACC_TypeDef *la){
+    //http://math.stackexchange.com/questions/40164/how-do-you-rotate-a-vector-by-a-unit-quaternion
+    //http://mathworld.wolfram.com/QuaternionConjugate.html
+    
+    float tempQuat[4];
+    float Quat[4];
+    
+    get_linear_accel(la);
+    get_quaternion_float(tempQuat);
+    
+    for( int i=0; i<4; i++)
+        Quat[i] = tempQuat[i];
+    
+    //http://es.mathworks.com/help/aeroblks/quaternionmultiplication.html   q=quat, r=la
+    tempQuat[0]=0 -la->x*Quat[1] -la->y*Quat[2] -la->z*Quat[3];
+    tempQuat[1]=0 +la->x*Quat[0] -la->y*Quat[3] +la->z*Quat[2];
+    tempQuat[2]=0 +la->x*Quat[3] +la->y*Quat[0] -la->z*Quat[1];
+    tempQuat[3]=0 -la->x*Quat[2] +la->y*Quat[1] +la->z*Quat[0];
+
+    //q=tempQuat, r=quatConj
+    la->x=Quat[0]*tempQuat[1] -Quat[1]*tempQuat[0] +Quat[2]*tempQuat[3] -Quat[3]*tempQuat[2];
+    la->y=Quat[0]*tempQuat[2] +Quat[1]*tempQuat[3] -Quat[2]*tempQuat[0] +Quat[3]*tempQuat[1];
+    la->z=Quat[0]*tempQuat[3] +Quat[1]*tempQuat[2] -Quat[2]*tempQuat[1] -Quat[3]*tempQuat[0];
+}
\ No newline at end of file