11.05.2022г. ВНИМАНИЕ! Если у вас не отображаются иллюстрации к статьям, для просмотра сайта используйте TOR браузер
Для навигации, роверу достаточно одного компаса что бы двигаться из точки А в точку В. Нужно лишь вычислить азимут, сделать разворот и проехать определенное расстояние по выбранному курсу.
В качестве электронного компаса может служить магнитометр 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.