Arduino Reading 9DOF Sensors

MPU6050 + HMC5883L

Arduino IDE - Reading Compass Deflection Function

 COMPASS SENSORS READING ALGORITHM

 ILLUSTRATING COMPASS DEFLECTION READING

Let's follow the first table below, if you are facing on a direction and look at your compass heading and reads 2deg, assume that this is your set-point. Then turn right so that you will be facing at compass heading of 355deg, counting on the compass reference marks with your finger you know that you have turned (or deflect) 7deg right from your original pace (see shaded yellow).

On the second table below, if you are facing on a direction and look at your compass heading which reads 356deg and assume that this is your set-point, then  then turn left so that you will be facing at compass heading of 3deg, counting on the compass reference marks with your finger, you know that you have turned (or deflect) 7deg left from your original pace(see shaded yellow)..

Sometimes the real world is much easier to visualize and understand at almost an instance than to put it into mathematical language - indeed it took me a while with the help of excel spreadsheet slowly plotting and figure out as shown below.




The table shows a simulated reading of deflection from known set point where compass was deflecting passing through end-points (i.e. 0/360) which without proper algorithm leads to erroneous reading as shown on second rows (i.e. Reading Difference). With proper algorithm we can actually measure the deflection  (highlighted in yellow) from set-point to any point even as it passes the 0/360 endpoints of the compass as shown on third row of the table.

Heading - is a virtual representation  of 0-360 degree compass

Reading Difference - is the numerical calculation of compass travel base on heading value.

Deflection - is the angle changed from set-point.

Setpoint - is your fix or desired orientation Deflection - Also as error, is angle away from desired or reference orientation. ReadNow - is your current orientation which is then compare with desired/fixed orientation. if Setpoint and ReadNow  is the same, then you are maintaining your orientation.

ALGORITHM IN ARDUINO C:

From the table above, I have derived a function that can be of use for application about compass as shown below.

double az=0; //"az" is variable that read or store real-time data from compass sensor (range 0 to 360)

void setup()
{ Serial.begin(115200);}

void loop()
{double error=0, readNow=0, Setpoint=0;

readNow = az;

error = readCompass(readNow, Setpoint); 

Serial.println(error);
}

//Function to calculate the deflection of needle from setpoint 

double readCompass(double readNow, double Setpoint)
{double eRR;
 
  if (abs(readNow-Setpoint)<=180)  

       {eRR = readNow - Setpoint; Serial.print("readNow - Setpoint");Serial.print("\t");}
  else 
        if (Setpoint<readNow)
        {eRR = readNow - Setpoint - 360; Serial.print("readNow - Setpoint - 360");Serial.print("\t");}
        else
        {eRR = readNow - Setpoint + 360; Serial.print("readNow - Setpoint + 360");Serial.print("\t");}  
  return eRR;
 }


Notes: The algorithm sense the deflection up to maximum of 180 degrees turn from a defined set-points. These algorithm is good enough for Quad-copter application (i.e. to maintain it's heading) as an application example.

TO BE CONTINUE.....................

 

void setup()  { }

void   loop()  { }         

 

void Compute_Rudder()

{float Output_ZMax=100, Output_ZMin=-100, Output_zz;

 

   /*IDENTIFY SETPOINT*/

    if (RD_Pulse>=1493 && RD_Pulse<=1507 && StickCenter==true)

    {//SETPOINT AT CENTER STICK

         if (n<1)

         {SetRuddrAbout_Z=Mag_z;       

          StickCenter=false;  n=+1;

         }//catch/reads value only once while stick on center

    } 

    else if (RD_Pulse<1493 || RD_Pulse>1507)

    {//SETPOINT AT MOVING STICK, MCU

     //exit only when stick back within center range

     SetRuddrAbout_Z=Mag_z; StickCenter=true; n=0;

    }

   

       

   /*COMPUTE ERROR or DEFLECTION*/

   error_Z = readCompass (Mag_z, SetRuddrAbout_Z);

 
 

 /*Filter  error_Z – Rejects low value error*/

int  filter=0.50;

   if (abs(error_Z)< filter)

    {

       error_Z=0; //disregard error_Z less than 0.50

     }

   else

     {

      if (error_Z<0)  error_Z=error_Z + filter; //example   -1 + 0.50 = - 0.50

      if (error_Z>0)  error_Z=error_Z - filter;  // example  +1 – 0.50 = + 0.50

     }//End filter

                        

 

/*COMPUTE PID*/

    if (TH_Pulse>1170)

    {

            errSum_Z = errSum_Z + (error_Z * timeChange/1000);

            dErr_Z = (error_Z - lastErr_Z)*1000 / timeChange;

       

            Output_zz = kpz * error_Z + kiz * errSum_Z - kdz * dErr_Z; 

                    

            if          (Output_zz>Output_ZMax)     {Output_Z= Output_ZMax; }

            else if  (Output_zz<Output_ZMin)  {Output_Z= Output_ZMin; }

            else      Output_Z=Output_zz;

     }

     else

     {

      Reset_Z();

     }//End of if TH_Pulse>1170

}//End Compute Rudder                             

 

 

  

/*COMPASS DEFLECTION FUNCTION*/

float readCompass(float readNow, float SetRuddrAbout_Z)

 {float eRR;

  

  if (abs(readNow-SetRuddrAbout_Z)<=180) 

       {eRR = readNow - SetRuddrAbout_Z;}

   else

        if (SetRuddrAbout_Z<readNow)

        {eRR = readNow - SetRuddrAbout_Z - 360; }

        else

        {eRR = readNow - SetRuddrAbout_Z + 360; } 

   return eRR;

  }//End Compass function