Revision 5:6a08a4c5b1e1, committed 2017-01-16

Comitter:

mrcrsch

Date:

Mon Jan 16 16:51:39 2017 +0000

Parent:

4:9e6fead1e93e

Commit message:

added extra functions;

Changed in this revision

diff -r 9e6fead1e93e -r 6a08a4c5b1e1 BNO055.cpp
--- 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

diff -r 9e6fead1e93e -r 6a08a4c5b1e1 BNO055.h
--- a/BNO055.h	Thu Apr 16 10:47:40 2015 +0000
+++ b/BNO055.h	Mon Jan 16 16:51:39 2017 +0000
@@ -62,6 +62,9 @@
 #define I_AM_BNO055_MAG         0x32    // MAG ID
 #define I_AM_BNO055_GYR         0x0f    // GYR ID
 
+
+
+
 ////////////// DATA TYPE DEFINITION ///////////////////////
 typedef struct {
     uint8_t  chip_id;
@@ -102,6 +105,21 @@
     int8_t gyr_chip;
 } BNO055_TEMPERATURE_TypeDef;
 
+typedef struct {
+    uint8_t Gscale;
+    uint8_t Gbw;
+    uint8_t GPwrMode;
+    
+    uint8_t Ascale;
+    uint8_t Abw;
+    uint8_t APwrMode;
+    
+    uint8_t MOpMode;
+    uint8_t MPwrMode;
+    uint8_t Modr;
+   
+} BNO055_CONFIG_TypeDef;
+
 enum {MT_P0 = 0, MT_P1, MT_P2, MT_P3, MT_P4, MT_P5, MT_P6, MT_P7};
 
 /** Interface for Bosch Sensortec Intelligent 9-axis absolute orientation sensor
@@ -258,7 +276,16 @@
       * @return register data
       */
     uint8_t write_reg1(uint8_t addr, uint8_t data);
-
+    
+    /** Calculate acceleration in Earth frame
+      * @param Linear acceleration structure
+    */
+    void get_abs_accel(BNO055_LIN_ACC_TypeDef *la);
+    
+    void get_quaternion_float(float *Q);
+    
+    void configure_accelerometer_range(uint8_t range);
+    
 protected:
     void initialize(void);
     void check_id(void);
@@ -283,6 +310,8 @@
     uint8_t  gyr_id;
     uint8_t  bootldr_rev_id;
     uint16_t sw_rev_id;
+    
+    
 
 };
 
@@ -465,6 +494,9 @@
 #define GYRO_ANY_MOTION_THRES   0x1e
 #define GYRO_ANY_MOTION_SET     0x1f
 
+
+
+
 //---------------------------------------------------------
 //----- Calibration example -------------------------------
 //---------------------------------------------------------