Всё кругом враньё! Форум проекта www.falsehood.me

/*
HMC5883L.h - Header file for the HMC5883L Triple Axis Magnetometer Arduino Library.
Copyright (C) 2011 Love Electronics (loveelectronics.co.uk)

This program is free software: you can redistribute it and/or modify
it under the terms of the version 3 GNU General Public License as
published by the Free Software Foundation.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program.  If not, see <http://www.gnu.org/licenses/>.

  WARNING: THE HMC5883L IS NOT IDENTICAL TO THE HMC5883!
  Datasheet for HMC5883L:
  http://www51.honeywell.com/aero/common/documents/myaerospacecatalog-documents/Defense_Brochures-documents/HMC5883L_3-Axis_Digital_Compass_IC.pdf
  http://c48754.r54.cf3.rackcdn.com/HMC5883L.pdf

*/

#ifndef HMC5883L_h
#define HMC5883L_h

#include <inttypes.h>
#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#define printByte(args) Serial.write(args)
#define printlnByte(args)  Serial.write(args),Serial.println()
#else
#include "WProgram.h"
#define printByte(args) Serial.print(args,BYTE)
#define printlnByte(args)  Serial.println(args,BYTE)
#endif

#define HMC5883L_Address 0x1E
#define ConfigurationRegisterA 0x00
#define ConfigurationRegisterB 0x01
#define ModeRegister 0x02
#define DataRegisterBegin 0x03
#define IdentityRegister 0x0A
#define IdentityRegisterValue 0x48

#define Measurement_Continuous 0x00
#define Measurement_SingleShot 0x01
#define Measurement_Idle 0x03

#define ErrorCode_1 "Entered scale was not valid, valid gauss values are: 0.88, 1.3, 1.9, 2.5, 4.0, 4.7, 5.6, 8.1"
#define ErrorCode_1_Num 1

struct MagnetometerScaled
{
  float XAxis;
  float YAxis;
  float ZAxis;
};

struct MagnetometerRaw
{
  int XAxis;
  int YAxis;
  int ZAxis;
};

class HMC5883L
{
  public:
    HMC5883L();

    MagnetometerRaw ReadRawAxis();
    MagnetometerScaled ReadScaledAxis();
    
    int SetMeasurementMode(uint8_t mode);
    int SetScale(float gauss);

    char* GetErrorText(int errorCode);

    uint8_t EnsureConnected();
    uint8_t IsConnected;
  protected:
    void Write(int address, int byte);
    uint8_t* Read(int address, int length);

  private:
    float m_Scale;
};
#endif

/*
HMC5883L.cpp - Class file for the HMC5883L Triple Axis Magnetometer Arduino Library.
Copyright (C) 2011 Love Electronics (loveelectronics.co.uk)

This program is free software: you can redistribute it and/or modify
it under the terms of the version 3 GNU General Public License as
published by the Free Software Foundation.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program.  If not, see <http://www.gnu.org/licenses/>.

  WARNING: THE HMC5883L IS NOT IDENTICAL TO THE HMC5883!
  Datasheet for HMC5883L:
  http://www51.honeywell.com/aero/common/documents/myaerospacecatalog-documents/Defense_Brochures-documents/HMC5883L_3-Axis_Digital_Compass_IC.pdf
  http://c48754.r54.cf3.rackcdn.com/HMC5883L.pdf

*/
#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#include "Wire.h"
#define printByte(args) Serial.write(args)
#define printlnByte(args)  Serial.write(args),Serial.println()

#define write_I2C(args) Wire.write(args)
#define read_I2C() Wire.read()

#else
#include "WProgram.h"
#define printByte(args) Serial.print(args,BYTE)
#define printlnByte(args)  Serial.println(args,BYTE)

#define write_I2C(args) Wire.send(args)
#define read_I2C() Wire.receive()

#endif

#include "HMC5883L.h"

HMC5883L::HMC5883L()
{
   m_Scale = 1;
}

MagnetometerRaw HMC5883L::ReadRawAxis()
{
   uint8_t* buffer = Read(DataRegisterBegin, 6);
   MagnetometerRaw raw = MagnetometerRaw();
   raw.XAxis = (buffer[0] << 8) | buffer[1];
   raw.ZAxis = (buffer[2] << 8) | buffer[3];
   raw.YAxis = (buffer[4] << 8) | buffer[5];
   return raw;
}

MagnetometerScaled HMC5883L::ReadScaledAxis()
{
   MagnetometerRaw raw = ReadRawAxis();
   MagnetometerScaled scaled = MagnetometerScaled();
   scaled.XAxis = raw.XAxis * m_Scale;
   scaled.ZAxis = raw.ZAxis * m_Scale;
   scaled.YAxis = raw.YAxis * m_Scale;
   return scaled;
}

int HMC5883L::SetScale(float gauss)
{
  uint8_t regValue = 0x00;
  if(gauss == 0.88)
  {
   regValue = 0x00;
   m_Scale = 0.73;
  }
  else if(gauss == 1.3)
  {
   regValue = 0x01;
   m_Scale = 0.92;
  }
  else if(gauss == 1.9)
  {
   regValue = 0x02;
   m_Scale = 1.22;
  }
  else if(gauss == 2.5)
  {
   regValue = 0x03;
   m_Scale = 1.52;
  }
  else if(gauss == 4.0)
  {
   regValue = 0x04;
   m_Scale = 2.27;
  }
  else if(gauss == 4.7)
  {
   regValue = 0x05;
   m_Scale = 2.56;
  }
  else if(gauss == 5.6)
  {
   regValue = 0x06;
   m_Scale = 3.03;
  }
  else if(gauss == 8.1)
  {
   regValue = 0x07;
   m_Scale = 4.35;
  }
  else
   return ErrorCode_1_Num;
   
  // Setting is in the top 3 bits of the register.
  regValue = regValue << 5;
  Write(ConfigurationRegisterB, regValue);
}

int HMC5883L::SetMeasurementMode(uint8_t mode)
{
  Write(ModeRegister, mode);
}

uint8_t HMC5883L::EnsureConnected()
{
  uint8_t data = Read(IdentityRegister, 1)[0];

  if(data == IdentityRegisterValue)
   IsConnected = 1;
  else
   IsConnected = 0;

  return IsConnected;
}

void HMC5883L::Write(int address, int data)
{
   Wire.beginTransmission(HMC5883L_Address);
   write_I2C(address);
   write_I2C(data);
   Wire.endTransmission();
}

uint8_t* HMC5883L::Read(int address, int length)
{
   Wire.beginTransmission(HMC5883L_Address);
   write_I2C(address);
   Wire.endTransmission();
    
   Wire.beginTransmission(HMC5883L_Address);
   Wire.requestFrom(HMC5883L_Address, length);

   uint8_t buffer[length];
   if(Wire.available() == length)
   {
    for(uint8_t i = 0; i < length; i++)
    {
     buffer[i] = read_I2C();
    }
   }
   Wire.endTransmission();

   return buffer;
}

char* HMC5883L::GetErrorText(int errorCode)
{
  if(errorCode == ErrorCode_1_Num)
   return ErrorCode_1;
   
  return "Error not defined.";
}

/*
HMC5883L_Example.pde - Example sketch for integration with an HMC5883L triple axis magnetomerwe.
Copyright (C) 2011 Love Electronics (loveelectronics.co.uk)

This program is free software: you can redistribute it and/or modify
it under the terms of the version 3 GNU General Public License as
published by the Free Software Foundation.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program.  If not, see <http://www.gnu.org/licenses/>.

*/

// Reference the I2C Library
#include <Wire.h>
// Reference the HMC5883L Compass Library
#include <HMC5883L.h>

// Store our compass as a variable.
HMC5883L compass;
// Record any errors that may occur in the compass.
int error = 0;

// Out setup routine, here we will configure the microcontroller and compass.
void setup()
{
   // Initialize the serial port.
   Serial.begin(9600);

   Serial.println("Starting the I2C interface.");
   Wire.begin(); // Start the I2C interface.

   Serial.println("Constructing new HMC5883L");
   compass = HMC5883L(); // Construct a new HMC5883 compass.
      
   Serial.println("Setting scale to +/- 8.1 Ga");
   error = compass.SetScale(8.1); // Set the scale of the compass.
   if(error != 0) // If there is an error, print it out.
     Serial.println(compass.GetErrorText(error));
    
   Serial.println("Setting measurement mode to continous.");
   error = compass.SetMeasurementMode(Measurement_Continuous); // Set the measurement mode to Continuous
   if(error != 0) // If there is an error, print it out.
     Serial.println(compass.GetErrorText(error));
}

// Our main program loop.
void loop()
{
   // Retrive the raw values from the compass (not scaled).
   MagnetometerRaw raw = compass.ReadRawAxis();
   // Retrived the scaled values from the compass (scaled to the configured scale).
   MagnetometerScaled scaled = compass.ReadScaledAxis();
    
   // Values are accessed like so:
   int MilliGauss_OnThe_XAxis = scaled.XAxis;// (or YAxis, or ZAxis)

   // Calculate heading when the magnetometer is level, then correct for signs of axis.
   float heading = atan2(scaled.YAxis, scaled.XAxis);
    
   // Once you have your heading, you must then add your 'Declination Angle', which is the 'Error' of the magnetic field in your location.
   // Find yours here: http://www.magnetic-declination.com/
   // Mine is: 2? 37' W, which is 2.617 Degrees, or (which we need) 0.0456752665 radians, I will use 0.0457
   // If you cannot find your Declination, comment out these two lines, your compass will be slightly off.
   float declinationAngle = 0; //.224566;
   heading += declinationAngle;
    
   // Correct for when signs are reversed.
   if(heading < 0)
     heading += 2*PI;
      
   // Check for wrap due to addition of declination.
   if(heading > 2*PI)
     heading -= 2*PI;
     
   // Convert radians to degrees for readability.
   float headingDegrees = heading * 180/M_PI;  

   // Output the data via the serial port.
   Output(raw, scaled, heading, headingDegrees);

   // Normally we would delay the application by 66ms to allow the loop
   // to run at 15Hz (default bandwidth for the HMC5883L).
   // However since we have a long serial out (104ms at 9600) we will let
   // it run at its natural speed.
   delay(500);
}

// Output the data down the serial port.
void Output(MagnetometerRaw raw, MagnetometerScaled scaled, float heading, float headingDegrees)
{
    Serial.print("Raw:\t");
    Serial.print(raw.XAxis);
    Serial.print(" : ");    
    Serial.print(raw.YAxis);
    Serial.print(" : ");    
    Serial.print(raw.ZAxis);
    Serial.print(" \t\tScaled:  \t");
     
    Serial.print(scaled.XAxis,3);
    Serial.print(" : ");    
    Serial.print(scaled.YAxis,3);
    Serial.print(" : ");    
    Serial.print(scaled.ZAxis,3);

    Serial.print("\t  Heading:  \t");
    Serial.print(heading,3);
    Serial.print(" Radians\t");
    Serial.print(headingDegrees,3);
    Serial.println(" Degrees");
}