Магнитометр HMC5883L - калибровка и использование в Arduino

разделы: HMC5883L , Arduino , дата: 30 марта 2016г.


hmc5883l

Для навигации, роверу достаточно одного компаса что бы двигаться из точки А в точку В. Нужно лишь вычислить азимут, сделать разворот и проехать определенное расстояние по выбранному курсу.

В качестве электронного компаса может служить магнитометр HMC5883L. В данном посте я хочу разобрать документацию, процесс калибровки и программный код работы с датчиком под Arduino чтобы впоследствии портировать его под AVR/STM8/STM32.

Подробное видео по описанию и калибровке магнитометра можно посмотреть здесь:

Итак, магнитометр -это датчик, который измеряет вектор магнитной индукции. При отсутствии внешних помех и располагаясь в горизонтальной плоскости, проекция вектора магнитной индукции на плоскость XOY будет указывать на север.

HMC5883L работает по I2C протоколу, описание регистров датчика можно найти здесь: Работа с магнитометром HMC5883L

По этой ссылке можно скачать библиотеку для Arduino http://code.bildr.org/project/HMC5883L/Arduino

Если посмотреть ее код, то ничего сложного там не увидим:

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

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

*/

#include <Arduino.h>
#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);
}

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

uint8_t* HMC5883L::Read(int address, int length)
{
  Wire.beginTransmission(HMC5883L_Address);
  Wire.write(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] = Wire.read();
   }
  }
  Wire.endTransmission();

  return buffer;
}

char* HMC5883L::GetErrorText(int errorCode)
{
 if(ErrorCode_1_Num == 1)
  return ErrorCode_1;

 return "Error not defined.";
}

протокол HMC5883L походит на протокол RTC DS1307: сначала в счетчик пишется адрес первого нужного нам регистра, после чего считывается массив из шести байт.

Скетч для работы с корректировочной матрицей извлеченый из архива с MagMaster:

#include "Wire.h"
#include "HMC5883L.h"

HMC5883L compass; //Copy the folder "HMC5883L" in the folder "C:\Program Files\Arduino\libraries" and restart the arduino IDE.

float xv, yv, zv;

//calibrated_values[3] is the global array where the calibrated data will be placed
//calibrated_values[3]: [0]=Xc, [1]=Yc, [2]=Zc
float calibrated_values[3];
//transformation(float uncalibrated_values[3]) is the function of the magnetometer data correction 
//uncalibrated_values[3] is the array of the non calibrated magnetometer data
//uncalibrated_values[3]: [0]=Xnc, [1]=Ync, [2]=Znc
void transformation(float uncalibrated_values[3])
{
  //calibration_matrix[3][3] is the transformation matrix
  //replace M11, M12,..,M33 with your transformation matrix data
  double calibration_matrix[3][3] =
  {
    {M11, M12, M13},
    {M21, M22, M23},
    {M31, M32, M33}
  };
  //bias[3] is the bias
  //replace Bx, By, Bz with your bias data
  double bias[3] =
  {
    Bx,
    By,
    Bz
  };
  //calculation
  for (int i=0; i<3; ++i) uncalibrated_values[i] = uncalibrated_values[i] - bias[i];
  float result[3] = {0, 0, 0};
  for (int i=0; i<3; ++i)
    for (int j=0; j<3; ++j)
      result[i] += calibration_matrix[i][j] * uncalibrated_values[j];
  for (int i=0; i<3; ++i) calibrated_values[i] = result[i];
}

void setup()
{
  Serial.begin(9600);
  Wire.begin();
  compass = HMC5883L();
  setupHMC5883L();
}

void loop()
{
  float values_from_magnetometer[3];

  getHeading();
  values_from_magnetometer[0] = xv;
  values_from_magnetometer[1] = yv;
  values_from_magnetometer[2] = zv;
  transformation(values_from_magnetometer);

  Serial.flush();
  Serial.print(calibrated_values[0]);
  Serial.print(",");
  Serial.print(calibrated_values[1]);
  Serial.print(",");
  Serial.print(calibrated_values[2]);
  Serial.println();

  delay(100);
}

void setupHMC5883L()
{
  compass.SetScale(0.88);
  compass.SetMeasurementMode(Measurement_Continuous);
}

void getHeading()
{
  MagnetometerRaw raw = compass.ReadRawAxis();
  xv = (float)raw.XAxis;
  yv = (float)raw.YAxis;
  zv = (float)raw.ZAxis;
}

Здесь массив calibration_matrix нужно заполнить своими значениями получеными в MagMaster, и здесь может быть не хватает расчета азимута. Несколько строк кода исправляют это дело:

#include "Wire.h"
#include "HMC5883L.h"

HMC5883L compass; //Copy the folder "HMC5883L" in the folder "C:\Program Files\Arduino\libraries" and restart the arduino IDE.

float xv, yv, zv;

//calibrated_values[3] is the global array where the calibrated data will be placed
//calibrated_values[3]: [0]=Xc, [1]=Yc, [2]=Zc
float calibrated_values[3];
//transformation(float uncalibrated_values[3]) is the function of the magnetometer data correction 
//uncalibrated_values[3] is the array of the non calibrated magnetometer data
//uncalibrated_values[3]: [0]=Xnc, [1]=Ync, [2]=Znc
void transformation(float uncalibrated_values[3])
{
  //calibration_matrix[3][3] is the transformation matrix
  //replace M11, M12,..,M33 with your transformation matrix data

     double calibration_matrix[3][3] =
  {
    {1.785, -0.001, 0.015},
    {0.147, 0.847, 0.016},
    {0.087, 0.026, 0.909}
  };
  //bias[3] is the bias
  //replace Bx, By, Bz with your bias data
  double bias[3] =
  {
    142.547,
    -147.29,
    -15.82
  };

  //calculation
  for (int i=0; i<3; ++i) uncalibrated_values[i] = uncalibrated_values[i] - bias[i];
  float result[3] = {0, 0, 0};
  for (int i=0; i<3; ++i)
    for (int j=0; j<3; ++j)
      result[i] += calibration_matrix[i][j] * uncalibrated_values[j];
  for (int i=0; i<3; ++i) calibrated_values[i] = result[i];
}

void setup()
{
  Serial.begin(9600);
  Wire.begin();
  compass = HMC5883L();
  setupHMC5883L();
}

void loop()
{
  float values_from_magnetometer[3];

  getHeading();
  values_from_magnetometer[0] = xv;
  values_from_magnetometer[1] = yv;
  values_from_magnetometer[2] = zv;
  transformation(values_from_magnetometer);

  float heading = atan2(calibrated_values[1], calibrated_values[0]);

  float declinationAngle = 0.0457;
  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;


 float headingDegrees = heading * 180/M_PI;

   Serial.print(headingDegrees);
   Serial.println(" Degrees   \t");

/*
  Serial.flush(); 
  Serial.print(calibrated_values[0]); 
  Serial.print(" ,");
  Serial.print(calibrated_values[1]);
  Serial.print(" ,");
  Serial.print(calibrated_values[2]);
  Serial.println();
*/


  delay(1000);
}

void setupHMC5883L()
{
  compass.SetScale(0.88);
  compass.SetMeasurementMode(Measurement_Continuous);
}

void getHeading()
{
  MagnetometerRaw raw = compass.ReadRawAxis();
  xv = (float)raw.XAxis;
  yv = (float)raw.YAxis;
  zv = (float)raw.ZAxis;
}

Само-собой, значения подставленые в корректировочную матрицу, верны только для моего датчика. После компиляции такой скетч весит 7766 байт. На следующем этапе задача будет состоять в том, чтобы отвязать этот код от Arduino.

поделиться: