diff --git a/LSM303/LSM303.cpp b/LSM303/LSM303.cpp index f9a00bf..79feb29 100644 --- a/LSM303/LSM303.cpp +++ b/LSM303/LSM303.cpp @@ -260,6 +260,31 @@ void LSM303::read(void) readMag(); } +int LSM303::pitch(void) +{ + vector temp_a = a; + // normalize + vector_normalize(&temp_a); + //vector_normalize(&m); + int pitch = round(atan2(temp_a.y,((temp_a.z > 0) - (temp_a.z < 0))*sqrt(pow(temp_a.x,2.0)+pow(temp_a.z,2.0)))* 180 / M_PI); + if (pitch < 0) pitch += 360; + pitch = 360 - pitch; + return pitch; +} + +int LSM303::roll(void) +{ + vector temp_a = a; + // normalize + vector_normalize(&temp_a); + //vector_normalize(&m); + int roll = round(atan2(-1*temp_a.x,temp_a.z)* 180 / M_PI); + if (roll < 0) roll += 360; + roll = 360 - roll; + return roll; + +} + // Returns the number of degrees from the -Y axis that it // is pointing. int LSM303::heading(void) diff --git a/LSM303/LSM303.h b/LSM303/LSM303.h index f1bbb8f..85025b4 100644 --- a/LSM303/LSM303.h +++ b/LSM303/LSM303.h @@ -140,6 +140,8 @@ class LSM303 bool timeoutOccurred(void); int heading(void); + int pitch(void); + int roll(void); int heading(vector from); // vector functions @@ -157,6 +159,3 @@ class LSM303 }; #endif - - -