Diff: modSensData.h

Revision:

0:e79311aae7ed

Child:

2:503a5ac6c3b6

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/modSensData.h	Sat Feb 21 01:14:46 2015 +0000
@@ -0,0 +1,187 @@
+#include "mbed.h"
+#include "math.h"
+#include "L3GD20.h"
+#include "LSM303DLHC.h"
+
+L3GD20 gyro(PTE25, PTE24);
+LSM303DLHC compass(PTE25, PTE24);
+
+#define XBIAS 0
+#define YBIAS 0
+#define ZBIAS 0
+
+#define M_PI            3.1415926535897932384626433832795f
+#define TWO_PI          6.283185307179586476925286766559f
+
+#define RADTODEGREE     57.295779513082320876798154814105f
+
+#define GRAVITY       1.0f
+
+Timer t1;
+
+float vC[3], cC[3], magnetom[3], inertialAccel[3];
+
+float aP[3], vP[3], cP[3], deltaT, timeStamp, degGyro[3], angPos[3], prevAngPos[3];
+
+double roll = 0, pitch = 0, yaw = 0;
+
+float magCheck;
+
+float accel[3], bias[3], magnDiff;
+
+int printStamp = 0, stopStamp = 0, dangerAhead = 0, howLong = 0, count[3], pos[5][3]= { {0,0,0}, {0,0,0}, {0,0,0}, {0,0,0}, {0,0,0} };
+
+int flag = 0, signBit = 0;
+
+/********************************************************************************************************************************/
+//                                          Get Accelerometer and Magnetometer Data
+/********************************************************************************************************************************/
+
+void readIMU()
+{
+    compass.read(&accel[0], &accel[1], &accel[2], &magnetom[0], &magnetom[1], &magnetom[2]);
+    gyro.read(&degGyro[0], &degGyro[1], &degGyro[2]);
+}
+
+/********************************************************************************************************************************/
+//                                                      Compass Heading
+/********************************************************************************************************************************/
+
+void Compass_Heading()
+{
+  float mag_x;
+  float mag_y;
+  float cos_roll;
+  float sin_roll;
+  float cos_pitch;
+  float sin_pitch;
+  
+  //saves a few processor cycles by calculating and storing values
+  cos_roll = cos(roll);
+  sin_roll = sin(roll);
+  cos_pitch = cos(pitch);
+  sin_pitch = sin(pitch);
+  
+  // Tilt compensated magnetic field X
+  mag_x = magnetom[0] * cos_pitch + magnetom[1] * sin_roll * sin_pitch + magnetom[2] * cos_roll * sin_pitch;
+  // Tilt compensated magnetic field Y
+  mag_y = magnetom[1] * cos_roll - magnetom[2] * sin_roll;
+  // Magnetic Heading
+  yaw = atan2(-mag_x, mag_y);
+  
+  //make negative angles positive
+  if(yaw < 0) yaw = yaw + TWO_PI;
+  
+  yaw = yaw * RADTODEGREE;
+}
+
+/********************************************************************************************************************************/
+//                                                      getAttitude
+/********************************************************************************************************************************/
+
+void getAttitude()
+{
+  float temp1[3];
+  float temp2[3];
+  float xAxis[3] = {1.0f, 0.0f, 0.0f};
+  
+  // GET PITCH
+  // Using y-z-plane-component/x-component of gravity vector    
+  pitch = -atan2(accel[0], sqrt((accel[1] * accel[1]) + (accel[2] * accel[2])));
+  
+  //printf("P = %f", pitch);
+  
+  // GET ROLL
+  // Compensate pitch of gravity vector 
+  temp1[0] = (accel[1] * xAxis[2]) - (accel[2] * xAxis[1]);
+  temp1[1] = (accel[2] * xAxis[0]) - (accel[0] * xAxis[2]);
+  temp1[2] = (accel[0] * xAxis[1]) - (accel[1] * xAxis[0]);
+  
+  temp2[0] = (xAxis[1] * temp1[2]) - (xAxis[2] * temp1[1]);
+  temp2[1] = (xAxis[2] * temp1[0]) - (xAxis[0] * temp1[2]);
+  temp2[2] = (xAxis[0] * temp1[1]) - (xAxis[1] * temp1[0]);
+  
+  // Since we compensated for pitch, x-z-plane-component equals z-component:
+  roll = atan2(temp2[1], temp2[2]);
+}
+
+
+/********************************************************************************************************************************/
+//                                                      compensateAcc
+/********************************************************************************************************************************/
+/*
+void compensateAcc() 
+{
+    // Compensate accelerometer error
+    accel[0] = (accel[0] - ACCEL_X_OFFSET) * ACCEL_X_SCALE;
+    accel[1] = (accel[1] - ACCEL_Y_OFFSET) * ACCEL_Y_SCALE;
+    accel[2] = (accel[2] - ACCEL_Z_OFFSET) * ACCEL_Z_SCALE;
+}
+*/
+
+/********************************************************************************************************************************/
+//                                                      rotateBTI
+/********************************************************************************************************************************/
+
+//rotation from the body frame to the inertial frame
+void rotateBTI()
+{
+  float cos_roll, sin_roll, cos_pitch, sin_pitch, cos_yaw, sin_yaw;
+  
+  float gravity[3] = {0, 0, -GRAVITY};
+  
+  float rIB[3][3];  
+  
+  //saves a few processor cycles by calculating and storing values
+  
+  //Serial.print("Roll");
+  cos_roll = cos(roll);
+  //Serial.print(cos_roll, 4);
+  //Serial.print("    ");
+  sin_roll = sin(roll);
+  //Serial.print(sin_roll, 4);
+  //Serial.print("    ");
+  
+  //Serial.print("Pitch");
+  cos_pitch = cos(pitch);
+  //Serial.print(cos_pitch, 4);
+  //Serial.print("    ");
+  sin_pitch = sin(pitch);
+  //Serial.print(sin_pitch, 4);
+  //Serial.print("    ");
+  
+  //Serial.print("YAW");
+  cos_yaw = cos(yaw);
+  //Serial.print(cos_yaw, 4);
+  //Serial.print("    ");
+  sin_yaw = sin(yaw);
+  //Serial.print(sin_yaw, 4);
+  //Serial.print("    ");
+  
+  rIB[0][0] = cos_yaw * cos_pitch;
+  rIB[0][1] = (cos_yaw * sin_roll * sin_pitch) - (cos_roll * sin_yaw);
+  rIB[0][2] = (sin_roll * sin_yaw) + (cos_roll * cos_yaw * sin_pitch);
+  rIB[1][0] = cos_pitch * sin_yaw;
+  rIB[1][1] = (cos_roll * cos_yaw) + (sin_roll * sin_yaw * sin_pitch);
+  rIB[1][2] = (cos_roll * sin_yaw * sin_pitch) - (cos_yaw * sin_roll);
+  rIB[2][0] = -1 * sin_pitch;
+  rIB[2][1] = cos_pitch * sin_roll;
+  rIB[2][2] = cos_roll * cos_pitch;
+  
+  for(int i = 0;  i < 3; i++)
+  {
+    aP[i] = inertialAccel[i];
+    inertialAccel[i] = rIB[i][0] * accel[0] + rIB[i][1] * accel[1] + rIB[i][2] * accel[2];
+    inertialAccel[i] = inertialAccel[i] + gravity[i];
+  }
+}
+
+
+
+void updateAngles()
+{
+    readIMU();
+    getAttitude();
+    Compass_Heading();    
+    //rotateBTI();
+}
\ No newline at end of file