Chris Charters
This is a revision of the UM6LT library written by acloitre. Revisions include changes to the getTwoBatches(...) function, additional changes carried over to its dependent functions, and an additional getPitchAngle() function.
Fork of
UM6LT
by 
Audren Cloitre
Revision 1:703ba8e8b2cd, committed 2015-07-21
- Comitter:
- chartersauce25
- Date:
- Tue Jul 21 17:38:31 2015 +0000
- Parent:
- 0:5651731cfb32
- Commit message:
- Revised getTwoBatches(...) function to call the appropriate data registers. Changes carried over to its dependent functions. A new getPitchAngle() function added.; ;
Changed in this revision
| UM6LT.cpp | Show annotated file Show diff for this revision Revisions of this file |
| UM6LT.h | Show annotated file Show diff for this revision Revisions of this file |
diff -r 5651731cfb32 -r 703ba8e8b2cd UM6LT.cpp
--- a/UM6LT.cpp Sun Sep 30 17:59:52 2012 +0000
+++ b/UM6LT.cpp Tue Jul 21 17:38:31 2015 +0000
@@ -328,7 +328,15 @@
//----------------------------------------------------------------------------------------------
int UM6LT::getTwoBatches(int address, int* dataToPost){
-
+
+ /*MAJOR CHANGE:
+ - UM6_QUAT_CD ADDED
+ - UM6_EULER_PSI ADDED
+ - UM6_MAG_PROC_Z ADDED
+ - UM6_ACCEL_PROC_Z ADDED
+ -
+ */
+
dataToPost[0] = 0;
dataToPost[1] = 0;
dataToPost[2] = 0;
@@ -355,28 +363,38 @@
case UM6_EULER_PHI_THETA:
dataToPost[0] = (int)(twosComplement(dataToRead[0], dataToRead[1])*UM6_ANGLE_FACTOR);
dataToPost[1] = (int)(twosComplement(dataToRead[2], dataToRead[3])*UM6_ANGLE_FACTOR);
- dataToPost[2] = (int)(twosComplement(dataToRead[4], dataToRead[5])*UM6_ANGLE_FACTOR);
+ break;
+ case UM6_EULER_PSI:
+ dataToPost[0] = (int)(twosComplement(dataToRead[0], dataToRead[1])*UM6_ANGLE_FACTOR);
break;
case UM6_ACCEL_PROC_XY:
dataToPost[0] = (int)(twosComplement(dataToRead[0], dataToRead[1])*UM6_ACCEL_FACTOR*1000);
dataToPost[1] = (int)(twosComplement(dataToRead[2], dataToRead[3])*UM6_ACCEL_FACTOR*1000);
- dataToPost[2] = (int)(twosComplement(dataToRead[4], dataToRead[5])*UM6_ACCEL_FACTOR*1000);
+ break;
+ case UM6_ACCEL_PROC_Z:
+ dataToPost[0] = (int)(twosComplement(dataToRead[0], dataToRead[1])*UM6_ACCEL_FACTOR*1000);
break;
case UM6_MAG_PROC_XY:
dataToPost[0] = (int)(twosComplement(dataToRead[0], dataToRead[1])*UM6_MAG_FACTOR*1000);
dataToPost[1] = (int)(twosComplement(dataToRead[2], dataToRead[3])*UM6_MAG_FACTOR*1000);
- dataToPost[2] = (int)(twosComplement(dataToRead[4], dataToRead[5])*UM6_MAG_FACTOR*1000);
+ break;
+ case UM6_MAG_PROC_Z:
+ dataToPost[0] = (int)(twosComplement(dataToRead[0], dataToRead[1])*UM6_MAG_FACTOR*1000);
break;
case UM6_GYRO_PROC_XY:
dataToPost[0] = (int)(twosComplement(dataToRead[0], dataToRead[1])*UM6_ANGLE_RATE_FACTOR);
dataToPost[1] = (int)(twosComplement(dataToRead[2], dataToRead[3])*UM6_ANGLE_RATE_FACTOR);
- dataToPost[2] = (int)(twosComplement(dataToRead[4], dataToRead[5])*UM6_ANGLE_RATE_FACTOR);
+ break;
+ case UM6_GYRO_PROC_Z:
+ dataToPost[0] = (int)(twosComplement(dataToRead[0], dataToRead[1])*UM6_ANGLE_RATE_FACTOR);
break;
case UM6_QUAT_AB:
dataToPost[0] = (int)(twosComplement(dataToRead[0], dataToRead[1])*UM6_QUATERNION_FACTOR*1000);
dataToPost[1] = (int)(twosComplement(dataToRead[2], dataToRead[3])*UM6_QUATERNION_FACTOR*1000);
- dataToPost[2] = (int)(twosComplement(dataToRead[4], dataToRead[5])*UM6_QUATERNION_FACTOR*1000);
- dataToPost[3] = (int)(twosComplement(dataToRead[6], dataToRead[7])*UM6_QUATERNION_FACTOR*1000);
+ break;
+ case UM6_QUAT_CD:
+ dataToPost[0] = (int)(twosComplement(dataToRead[0], dataToRead[1])*UM6_QUATERNION_FACTOR*1000);
+ dataToPost[1] = (int)(twosComplement(dataToRead[2], dataToRead[3])*UM6_QUATERNION_FACTOR*1000);
break;
default:
dataToPost[0] = -1;
@@ -740,7 +758,7 @@
return 0;
}
else{
- printf("No problem to report.\r\n");
+ //printf("No problem to report.\r\n");
return 1;
}
}
@@ -882,17 +900,21 @@
//----------------------------------------------------------------------------------------------
int UM6LT::getAngles(int& roll, int& pitch, int& yaw){
-
+/*MAJOR UPDATE:
+ - getTwoBatches(...) updated
+*/
roll = 0;
pitch= 0;
yaw= 0;
- int data[4] = {0, 0, 0, 0};
- int address = UM6_EULER_PHI_THETA;
+ int data_1[4] = {0, 0, 0, 0};
+ int data_2[4] = {0, 0, 0, 0};
+ int address_phi_theta = UM6_EULER_PHI_THETA;
+ int address_psi = UM6_EULER_PSI;
- if(getTwoBatches(address, data)){
- roll = data[0];
- pitch = data[1];
- yaw = data[2];
+ if(getTwoBatches(address_phi_theta, data_1) && getTwoBatches(address_psi, data_2)){
+ roll = data_1[0];
+ pitch = data_1[1];
+ yaw = data_2[0];
return 1;
}
else{
@@ -902,20 +924,46 @@
}
//----------------------------------------------------------------------------------------------
+/*
+ SUPER NEW!!!
+
+ MAJOR UPDATE:
+ - getTwoBatches(...) updated
+*/
+
+int UM6LT::getPitchAngle(){
+
+ int pitch = 0;
+ int data[4] = {0, 0, 0, 0};
+ int address_phi_theta = UM6_EULER_PHI_THETA;
+
+ if(getTwoBatches(address_phi_theta, data)){
+ return pitch = data[1];
+ }
+ else{
+ return 0;
+ }
+
+}
+//----------------------------------------------------------------------------------------------
int UM6LT::getAccel(int& accelX, int& accelY, int& accelZ){
-
+/*MAJOR UPDATE:
+ - getTwoBatches(...) updated
+*/
accelX = 0;
accelY = 0;
accelZ = 0;
- int data[4] = {0, 0, 0, 0};
- int address = UM6_ACCEL_PROC_XY;
+ int data_1[4] = {0, 0, 0, 0};
+ int data_2[4] = {0, 0, 0, 0};
+ int address_XY = UM6_ACCEL_PROC_XY;
+ int address_Z = UM6_ACCEL_PROC_Z;
- if(getTwoBatches(address, data)){
- accelX = data[0];
- accelY = data[1];
- accelZ = data[2];
+ if(getTwoBatches(address_XY, data_1) && getTwoBatches(address_Z, data_2)){
+ accelX = data_1[0];
+ accelY = data_1[1];
+ accelZ = data_2[0];
return 1;
}
else{
@@ -928,17 +976,21 @@
int UM6LT::getMag(int& magX, int& magY, int& magZ){
-
+/*MAJOR UPDATE:
+ - getTwoBatches(...) updated
+*/
magX = 0;
magY = 0;
magZ = 0;
- int data[4] = {0, 0, 0, 0};
- int address = UM6_MAG_PROC_XY;
+ int data_1[4] = {0, 0, 0, 0};
+ int data_2[4] = {0, 0, 0, 0};
+ int address_XY = UM6_MAG_PROC_XY;
+ int address_Z = UM6_MAG_PROC_Z;
- if(getTwoBatches(address, data)){
- magX = data[0];
- magY = data[1];
- magZ = data[2];
+ if(getTwoBatches(address_XY, data_1) && getTwoBatches(address_Z, data_2)){
+ magX = data_1[0];
+ magY = data_1[1];
+ magZ = data_2[0];
return 1;
}
else{
@@ -951,17 +1003,21 @@
int UM6LT::getAngleRates(int& rollRate, int& pitchRate, int& yawRate){
-
+/*MAJOR UPDATE:
+ - getTwoBatches(...) updated
+*/
rollRate = 0;
pitchRate = 0;
yawRate = 0;
- int data[4] = {0, 0, 0, 0};
- int address = UM6_GYRO_PROC_XY;
+ int data_1[4] = {0, 0, 0, 0};
+ int data_2[4] = {0, 0, 0, 0};
+ int address_XY = UM6_GYRO_PROC_XY;
+ int address_Z = UM6_GYRO_PROC_Z;
- if(getTwoBatches(address, data)){
- rollRate = data[0];
- pitchRate = data[1];
- yawRate = data[2];
+ if(getTwoBatches(address_XY, data_1) && getTwoBatches(address_Z, data_2)){
+ rollRate = data_1[0];
+ pitchRate = data_1[1];
+ yawRate = data_2[0];
return 1;
}
else{
@@ -973,19 +1029,23 @@
//----------------------------------------------------------------------------------------------
int UM6LT::getQuaternion(int& a, int& b, int& c, int& d){
-
+/*MAJOR UPDATE:
+ - getTwoBatches(...) updated
+*/
a = 0;
b = 0;
c = 0;
d = 0;
- int data[4] = {0, 0, 0, 0};
- int address = UM6_QUAT_AB;
+ int data_1[4] = {0, 0, 0, 0};
+ int data_2[4] = {0, 0, 0, 0};
+ int address_AB = UM6_QUAT_AB;
+ int address_CD = UM6_QUAT_CD;
- if(getTwoBatches(address, data)){
- a = data[0];
- b = data[1];
- c = data[2];
- d = data[3];
+ if(getTwoBatches(address_AB, data_1) && getTwoBatches(address_CD, data_2)){
+ a = data_1[0];
+ b = data_1[1];
+ c = data_2[0];
+ d = data_2[1];
return 1;
}
else{
diff -r 5651731cfb32 -r 703ba8e8b2cd UM6LT.h
--- a/UM6LT.h Sun Sep 30 17:59:52 2012 +0000
+++ b/UM6LT.h Tue Jul 21 17:38:31 2015 +0000
@@ -132,6 +132,7 @@
int resetToFactory(void);
int getAngles(int& roll, int& pitch, int& yaw); // in degrees
+ int getPitchAngle(); // in degrees
int getAccel(int& accelX, int& accelY, int& accelZ); // in milli-gravity
int getMag(int& magX, int& magY, int& magZ); // in milli-unit. the norm of the vector should be one
int getAngleRates(int& rollRate, int& pitchRate, int& yawRate); // in degree/sec