CCS C Software and Maintenance Offers
FAQFAQ   FAQForum Help   FAQOfficial CCS Support   SearchSearch  RegisterRegister 

ProfileProfile   Log in to check your private messagesLog in to check your private messages   Log inLog in 

CCS does not monitor this forum on a regular basis.

Please do not post bug reports on this forum. Send them to support@ccsinfo.com

HMC5883L Convert to heading

 
Post new topic   Reply to topic    CCS Forum Index -> General CCS C Discussion
View previous topic :: View next topic  
Author Message
Lykos1986



Joined: 26 Nov 2005
Posts: 68

View user's profile Send private message

HMC5883L Convert to heading
PostPosted: Fri Aug 02, 2013 10:41 am     Reply with quote

Hi! I was writing a driver for the HMC5883L in order to get heading readings in degrees.

So far I am getting back X,Y,Z magnitude values and I am storing them into three (compass_x, compass_y, compass_z) signed int16 variables. The gain is set to 1.3.

It seems that so far everything is working great!
Now, I want to convert those into heading in degrees (in order to have a 0 - 360 result). I was searching the web and found the regular atan(Y/X) formula. Together with that I am correcting the output value when I am having negative results. Something like:

Code:

   compass_x = (X_MSB << 8) | X_LSB;
   compass_y = (Y_MSB << 8) | Y_LSB;
   compass_z = (Z_MSB << 8) | Z_LSB;

   head = atan((float)compass_y/(float)compass_x);

   if(head < 0.0)
              head += 2*PI;

   Heading_Degrees = head * 180.0/3.14159;


head and Heading_Degrees are float variables. The problem is that the output results are totally wrong! I am getting 0.0 for almost a quarter rotation, then it jumps to 45 then it tracks (almost perfect) from 210 to 345...

Any info of what is going wrong? Maybe the formula? The variable types are wrong?
PCM programmer



Joined: 06 Sep 2003
Posts: 21708

View user's profile Send private message

PostPosted: Fri Aug 02, 2013 1:09 pm     Reply with quote

This type of problem can be completely tested in MPLAB simulator with
numeric output displayed in the MPLAB Output Window. You go into the
MPLAB Debugger menu and setup UART1 to redirect printf's (etc) to the
Output Window. It's a just a pure math problem and/or coding problem.

The first thing to do is, make your code snippet into a short compilable
test program. Put in the #include, #fuses, #use delay, main(), variable
declarations, #include for math library routines, etc. And put in printf
statements at the end to display the results of the calculations.

Then at the beginning of main(), load X_MSB, X_LSB, etc., with values
that you know are causing the problem. You didn't tell us what those are.
Run the program in MPLAB Simulator (make sure you have it set for the
correct PIC in the Configure/Select Device menu). Then post the results
that you see in the Output window. Also post what you expect it to be.

For sure, remember to post your CCS compiler version.
Ttelmah



Joined: 11 Mar 2010
Posts: 19195

View user's profile Send private message

PostPosted: Sat Aug 03, 2013 4:32 am     Reply with quote

No guarantees on this, and you need to work out what is causing your problems first, but this is an alternative to using atan for this problem.
It is a generic function, that accepts an X and Y co-ordinate, and returns the corresponding angle in degrees.

I may well have got the quadrant solutions wrong, but a quick test suggests it looks right. The accuracy is better than half a degree, and it is _fast_. Takes about 70uSec on a 40Mhz PIC, while atan takes at least ten times longer. It is also smaller.... It only gives positive angles, from 0 to 360 as it's result.

Code:

//Processor defines here....
#include <math.h>
float angle(float X, float Y)
{
   //routine to give a fast solution for angle, from X/Y co-ordinates - result in degrees
   float AX,AY,ival,oval,aival;
   int8 quad;
   AX=fabs(X);
   AY=fabs(Y);
   //Now the approximation used works for tan from -1 to 1, so we have to keep the
   //values inside this range and adjust the input/output.
   //Four 'quadrants' are decoded -1 to 1, (315 to 45 degrees), then 45 to 135,
   //135 to 225, 225 to 315
   If (X >= 0) //Right hand half of the circle
   {
      If (AY > X)
      {
         If (Y < 0)
         {
             quad = 4;
             ival = -X / Y;
         }
         Else
         {
             quad = 2;
             ival = X / -Y;
         }
      }
      Else
      {
         If (AY > X)
         {
             quad = 4;
             ival = -Y / X;
         }
         else
         {
             quad = 1;
             ival = Y / X;
         }
      }
   }
   else
   {
      //Now the others
      If (Y > AX)
      {
         quad = 2;
         ival = X / -Y;
      }
      Else
      { 
         If (AY > AX)
         {         
             quad = 4;
             ival = -X / Y;
         }
         Else
         {
             quad = 3;
             ival = -Y / -X;
         }
      }
   }
   //A lot of lines of code, but small and quick really.....
   //Now the solution
   //Now approximation for atan from -1 to +1, giving an answer in degrees.

   aival = fAbs(ival);
   oval = 45 * ival - ival * (aival - 1) * (14.02 + 3.79 * aival);
   
   //Now solve back to the final result
   If (quad != 1)
   {
      If (quad == 2)
          oval = oval + 90;
      Else
      {
          If (quad == 3)
              oval = oval + 180;
          Else
              oval = oval + 270;
      }
   }
   if (oval<0)
      oval+=360;
   return oval;
}   

//Demo program using pairs of numbers from the array to test
void main()
{
   const signed int16 source[] = {0,300,600,1000,0,-300,-600,-1000};
   int8 ctr,ctr2=0;
   signed int16 X,Y;
   while (TRUE)
   {
      for (ctr=0;ctr<8;ctr++)
      {
         //Now loop through the array, using pairs from two counters as X/Y
         X=source[ctr];
         Y=source[ctr2];
         printf("X %ld, Y %ld, angle %5.1f\n\r", X,Y,angle(X,Y));
      }
      if (ctr2<7)
         ctr2++;
      else
         ctr2=0;
   }
}


Best Wishes
dorinm



Joined: 07 Jan 2006
Posts: 38

View user's profile Send private message

PostPosted: Sat Aug 03, 2013 5:41 pm     Reply with quote

excellent!
RF_Developer



Joined: 07 Feb 2011
Posts: 839

View user's profile Send private message

PostPosted: Mon Aug 05, 2013 2:42 am     Reply with quote

The function atan2(y, x) is intended for just this sort of problem, but if the approximation works accurately enough then use it.
Ttelmah



Joined: 11 Mar 2010
Posts: 19195

View user's profile Send private message

PostPosted: Mon Aug 05, 2013 4:08 am     Reply with quote

The approximation is more accurate than the chip. It's about half the size in ROM.
The maximum error, for the entire range of the chip output (-2047 to 2048), is under 0.1 degrees.
As a comment though, using atan, could be the reason for the original problems. atan2, is specifically written to 'cope' with the special cases where X=0, while a division, feeding atan, will return a maths error, which unless trapped, will result in garbage results. The approximation also handles this case.

Best Wishes
Display posts from previous:   
Post new topic   Reply to topic    CCS Forum Index -> General CCS C Discussion All times are GMT - 6 Hours
Page 1 of 1

 
Jump to:  
You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot vote in polls in this forum


Powered by phpBB © 2001, 2005 phpBB Group