From 1a2b915655378c0a258e70af1abba3b1a33463ac Mon Sep 17 00:00:00 2001 From: Tao Qian Date: Tue, 18 Jun 2013 19:16:23 -0400 Subject: [PATCH 1/2] Added the methods for the calculation of pitch and roll. --- LSM303/LSM303.cpp | 23 +++++++++++++++++++++++ LSM303/LSM303.h | 5 ++--- 2 files changed, 25 insertions(+), 3 deletions(-) diff --git a/LSM303/LSM303.cpp b/LSM303/LSM303.cpp index f9a00bf..510f58e 100644 --- a/LSM303/LSM303.cpp +++ b/LSM303/LSM303.cpp @@ -260,6 +260,29 @@ 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; + 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; + 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 - - - From de7fa510da2e084f5a48fccc5b027da4fa1765a2 Mon Sep 17 00:00:00 2001 From: Tao Qian Date: Thu, 25 Jul 2013 10:19:50 +0900 Subject: [PATCH 2/2] Used a different aeronautical convention for pitch and roll. --- LSM303/LSM303.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/LSM303/LSM303.cpp b/LSM303/LSM303.cpp index 510f58e..79feb29 100644 --- a/LSM303/LSM303.cpp +++ b/LSM303/LSM303.cpp @@ -263,22 +263,24 @@ void LSM303::read(void) int LSM303::pitch(void) { vector temp_a = a; - // normalize + // normalize vector_normalize(&temp_a); - //vector_normalize(&m); + //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 + // normalize vector_normalize(&temp_a); - //vector_normalize(&m); + //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; }