Diff: IMUs.cpp

Revision:

6:0ebecfecadc9

Child:

7:0e9af5986488

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IMUs.cpp	Tue Feb 26 01:22:53 2019 +0000
@@ -0,0 +1,130 @@
+#include "Structures.h"
+#include "IMUs.h"
+#include "mbed.h"
+
+//IMU Class Constructor
+IMU::IMU(char IMU_ID, double OffsetAX, double OffsetAY, double OffsetAZ, double OffsetGX, double OffsetGY, double OffsetGZ, char SSFA, char SSFG) {
+
+    AccelerometerOffset.x = OffsetAX;
+    AccelerometerOffset.y = OffsetAY;
+    AccelerometerOffset.z = OffsetAZ;
+
+    GyroscopeOffset.x = OffsetGX;
+    GyroscopeOffset.y = OffsetGY;
+    GyroscopeOffset.z = OffsetGZ;
+    
+    
+    IMU_Identifier = IMU_ID;
+    
+    switch(SSFA) {
+     case 0:
+        accelSSF = 0.00006103515625f;     
+     break;
+     case 1:
+        accelSSF = 0.0001220703125f;
+     break;
+     case 2:
+         accelSSF = 0.000244140625f; 
+     break;
+     case 3:
+        accelSSF = 0.00048828125f; 
+     break;
+     default:
+     break;   
+    }
+    
+    switch(SSFG) {
+     case 0:
+        gyroSSF = 0.00763358778625954198473282442748f;     
+     break;
+     case 1:
+        gyroSSF = 0.01526717557251908396946564885496f;
+     break;
+     case 2:
+         gyroSSF = 0.03048780487804878048780487804878f; 
+     break;
+     case 3:
+        gyroSSF = 0.06097560975609756097560975609756f; 
+     break;
+     default:
+     break;   
+    }
+    
+}
+
+//void IMU::CFAngle() {
+    
+    
+    
+//}
+
+//----------------------------------------------------------------------------------------------------------------------------------------------------------
+IMU_Data IMU::concatenateData(int16_t SamplesPieces[12]) {
+//Local-Variables------Local-Variables------Local-Variables------Local-Variables
+
+    int16_t MSB = 0;                                                            //Store Most Significant Byte of data piece in this variable for processing
+    int16_t LSB = 0;                                                            //Store Least Significant Byte of data piece in this variable for processing
+    char    arrPointer = 0;                                                     //Array Pointer
+    
+    IMU_Data LocalD;
+    
+//Procedure--------------Procedure------------Procedure-------------Procedure---      
+        for(char x = 0; x <= 5; x++) {
+        MSB = SamplesPieces[arrPointer];                                        //Odd data pieces are MSBs
+        MSB &= ~(255<<8);                                                       //Mask the MSB bits as they are not part of data
+        MSB = MSB << 8;                                                         //Shift the Value as its the MSB of the data piece
+        arrPointer++;                                                           //Increment array pointer
+        LSB = SamplesPieces[arrPointer];                                        //Even data pieces are LSBs
+        LSB &= ~(255 << 8);                                                     //Mask the MSB bits as they are not part of data
+        arrPointer++;                                                           //Increment array pointer
+        
+        switch(x) {
+            case 0:
+                LocalD.Ax = (MSB + LSB) + AccelerometerOffset.x;                 //Combine Accelerometer x-axis data
+            break;
+            case 1:
+                LocalD.Ay = (MSB + LSB) + AccelerometerOffset.y;                 //Combine Accelerometer y-axis data
+            break;
+            case 2:
+                LocalD.Az = (MSB + LSB) + AccelerometerOffset.z;                 //Combine Accelerometer z-axis data
+            break;
+            case 3:
+                LocalD.Gx = (MSB + LSB) + GyroscopeOffset.x;                      //Combine Gyroscope x-axis data
+            break;
+            case 4:
+                LocalD.Gy = (MSB + LSB) + GyroscopeOffset.y;                      //Combine Gyroscope y-axis data
+            break;
+            case 5:
+                LocalD.Gz = (MSB + LSB) + GyroscopeOffset.z;                      //Combine Gyroscope z-axis data
+            break;
+            default:
+            break; 
+        }//switch(x) 
+    }//for(char x = 0; x <= 5; x++)
+             
+    return LocalD;
+}
+//----------------------------------------------------------------------------------------------------------------------------------------------------------
+
+
+
+
+
+//----------------------------------------------------------------------------------------------------------------------------------------------------------
+IMU_Data IMU::SSFmultiply(IMU_Data RawData) {
+    
+    IMU_Data localD;  
+    
+
+    localD.Ax = RawData.Ax * accelSSF;
+    localD.Ay = RawData.Ay * accelSSF;
+    localD.Az = RawData.Az * accelSSF;
+    
+    localD.Gx = RawData.Gx * gyroSSF;
+    localD.Gy = RawData.Gy * gyroSSF;
+    localD.Gz = RawData.Gz * gyroSSF;
+        
+        printf("Accel X: %+2f,    Accel Y: %+2f,    Accel Z: %+2f,    Gyro X: %+2f,    Gyro Y: %+2f,    Gyro Z: %+2f\n\r", LocalD.Ax, LocalD.Ay, LocalD.Az, LocalD.Gx, LocalD.Gy, LocalD.Gz);
+    return localD;
+}
+//----------------------------------------------------------------------------------------------------------------------------------------------------------
\ No newline at end of file