/*Учебная программа. Работа с магнитометром и гироскопом акселерометром:
MPU6050 даташит:https://www.invensense.com/wp-content/uploads/2015/02/MPU-6000-Register-Map1.pdf
QMC5883 даташит:https://img.filipeflop.com/files/download/Datasheet-QMC5883L-1.0%20.pdf
Для корректировки данных с магнитометра используется матрица трансформации.
Коэффициенты получены обработкой сырых данных в Magneto ver. 1.2
подсмотрел тут:
http://www.germersogorb.de/html/kalibrierung_des_hcm5883l.html
http://robotclass.ru/articles/magnetometer-and-compass/
https://sites.google.com/site/diyheadtracking/home/kalibrovka-sensorov/magnetometr-calibration
http://www.count-zero.ru/2016/hmc5883l/
Для гироскопа коэффициенты дрейфа рассчитываются программно в setup() {}
функцией Corect_fo_Ac() на основании 1000 измерений.
Для данных магнитометра по осям X;Y применен фильтр Калмана.
Для корректировки данных с акселерометра применен комплементарный фильтр с опорой на данные с акселерометра.
В результате работы программы получаем данные отклонения в градусах от направления на магнитный полюс, и от горизонта, где 0 горизонт.
ATmega328P Arduino Uno
Скетч использует 8824 байт (27%) памяти устройства. Всего доступно 32256 байт.
Глобальные переменные используют 522 байт (25%) динамической памяти, оставляя 1526 байт для локальных переменных. Максимум: 2048 байт.
______________________
Stepan_Sotnikov
*/
#include <Wire.h>
//#include <Servo.h>
// Servo myservo;
//настройка Акселерометра гироскопа
const uint8_t Adres_for_AcGy = 0x68; // MPU6050
const uint8_t Reg_of_AcGy_PWR_MGMT = 0x6B; //регистр управления питание
const uint8_t RD_of_AcGy = 0x3B; // 0x3B (ACCEL_XOUT_H)
//настройка магнитометра
const uint8_t Adres_for_Mg = 0x0D; // QMC5883
const uint8_t Reg_of_Mg_Set_Reset = 0x0B; //регистр управления настройками
const uint8_t REG_CONTROL_of_Mg = 0x09;
const uint8_t RD_of_Mg = 0x00;
const float TO_Degrees = 57.2958;
const float TO_Radians = 0.01745;
//const float Pi = 3.14159;
//поправки дрейфа акс.
static int16_t Corect_fo_Ac_X; //=144;
static int16_t Corect_fo_Ac_Y; //=-145;
static int16_t Corect_fo_Ac_Z; //=251;
//переменные для функции calculate
static int16_t MGx, MGy, MGz, AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;
static float deltaTimer = 0.0;
static float Gy_roll_X = 0;
static float Gy_roll_Y = 0;
static float Gy_roll_Z = 0;
static uint32_t timer;
static float azimuth;
//коэффициент перехода от сырых данных гироскопа,
const float Transform_Gy_roll_to_Deg = 0.00763; // 1/131
const float Gy_roll_camp_fo_Z = 0.01; // коэф камп
const float Gy_roll_camp_fo_XY = 0.01; //кохф камп для осей X Y
void initMagnetometr() { //функция иниц. магн.
Wire.begin();
Wire.beginTransmission(Adres_for_Mg);
Wire.write(Reg_of_Mg_Set_Reset);
Wire.write(0b00000001); //
Wire.endTransmission(true);
//------------------------------------
Wire.beginTransmission(Adres_for_Mg);
Wire.write(REG_CONTROL_of_Mg);
Wire.write(0b00011101); // Mode_Continuous,ODR_200Hz,RNG_8G,OSR_512
Wire.endTransmission(true);
}
void initAccelgyro() { //функция иниц. магн.
Wire.begin();
Wire.beginTransmission(Adres_for_AcGy);
Wire.write(Reg_of_AcGy_PWR_MGMT);
Wire.write(0b00000000);
Wire.endTransmission(true);
}
//чтение данных маг.
boolean readMag(int16_t* MGx, int16_t* MGy, int16_t* MGz) {
Wire.beginTransmission(Adres_for_Mg);
Wire.write(RD_of_Mg);
int err = Wire.endTransmission();
if (err) {
return false;
}
Wire.requestFrom(Adres_for_Mg, 7);
*MGx = (Wire.read() | Wire.read() << 8);
*MGy = (Wire.read() | Wire.read() << 8);
*MGz = (Wire.read() | Wire.read() << 8);
return true;
}
boolean readAccGir(int16_t* AcX, int16_t* AcY, int16_t* AcZ, int16_t* Tmp,
int16_t* GyX, int16_t* GyY, int16_t* GyZ) {
Wire.beginTransmission(Adres_for_AcGy);
Wire.write(RD_of_AcGy);
Wire.endTransmission(false);
int err = Wire.endTransmission();
if (err) {
return false;
}
Wire.requestFrom(Adres_for_AcGy, 14, true);
*AcX = Wire.read() << 8 | Wire.read();
*AcY = Wire.read() << 8 | Wire.read();
*AcZ = Wire.read() << 8 | Wire.read();
*Tmp = Wire.read() << 8 | Wire.read();
*GyY = Wire.read() << 8 | Wire.read();//зависит от взаиморасположение датчиков
*GyX = Wire.read() << 8 | Wire.read();
*GyZ = Wire.read() << 8 | Wire.read();
return true;
}
void Corect_fo_Ac(int16_t* Corect_fo_Ac_X, int16_t* Corect_fo_Ac_Y,
int16_t* Corect_fo_Ac_Z) {
long f_fo_GyX = 0;
long f_fo_GyY = 0;
long f_fo_GyZ = 0;
for (int i = 0; i < 1000; i++) {
//читаем сырые данные из гироакселерометра
readAccGir(&AcX, &AcY, &AcZ, &Tmp, &GyX, &GyY, &GyZ);
f_fo_GyX = f_fo_GyX + GyX;
f_fo_GyY = f_fo_GyY + GyY;
f_fo_GyZ = f_fo_GyZ + GyZ;
}
*Corect_fo_Ac_X = (int16_t)(-f_fo_GyX / 1000);
*Corect_fo_Ac_Y = (int16_t)(-f_fo_GyY / 1000);
*Corect_fo_Ac_Z = (int16_t)(-f_fo_GyZ / 1000);
}
//функция калибровки данных магнитметра
void calibrate(int16_t* MGx, int16_t* MGy, int16_t* MGz) {
float calibr_matrix[3][3] = {{ 1.117512, -0.032663, 0.003151},
{-0.032663, 1.095694, -0.034088},
{ 0.003151, -0.034088, 1.102701}};
float CombBias[3] = { -241.661816, -373.203215, 41.251631};
float val[3];
val[0] = *MGx;
val[1] = *MGy;
val[2] = *MGz;
for (uint8_t i = 0; i < 3; ++i) { //умножения матриц
val[i] = val[i] - CombBias[i];
}
float result[3] = {0, 0, 0};
for (uint8_t i = 0; i < 3; ++i) {
for (uint8_t j = 0; j < 3; ++j) {
result[i] += calibr_matrix[i][j] * val[j];
}
}
for (uint8_t i = 0; i < 3; ++i) {
val[i] = result[i];
}
*MGx = (int16_t)val[0];
*MGy = (int16_t)val[1];
*MGz = (int16_t)val[2];
}
// переменные для Калмана
// скорость реакции на изменение (подбирается вручную) для 2 фильтров
float varProcess = 0.1;
float varVoltX = 7.80255; //среднее отклонение вычисляем из опыта
float PcX = 0.0, GX = 0.0, PX = 1.0, XpX = 0.0, ZpX = 0.0, XeX = 0.0;
// переменные для Калмана X
// Функция фильтрации
int16_t filterKalmanX(int16_t valX) {// Функция фильтрации
PcX = PX + varProcess;
GX = PcX / (PcX + varVoltX);
PX = (1 - GX) * PcX;
XpX = XeX;
ZpX = XpX;
XeX = GX * ((float)valX - ZpX) + XpX;
return (int16_t)(XeX);
}
// переменные для Калмана Y
float varVoltY = 13.29522; // среднее отклонение вычисляем из опыта
float PcY = 0.0, GY = 0.0, PY = 1.0, XpY = 0.0, ZpY = 0.0, XeY = 0.0;
int16_t filterKalmanY(int16_t valY) { // Функция фильтрации
PcY = PY + varProcess;
GY = PcY / (PcY + varVoltY);
PY = (1 - GY) * PcY;
XpY = XeY;
ZpY = XpY;
XeY = GY * ((float)valY - ZpY) + XpY;
return (int16_t)(XeY);
}
void calculate() {
//трансформируем данные магнитометра согласно калибр. матри.
calibrate(&MGx, &MGy, &MGz);
//фильтруем данные маг.
MGx = filterKalmanX(MGx);
MGy = filterKalmanY(MGy);
//вычисляем углы наклона по акс.
float Ac_roll_X = atan(((float)(AcX)) / ((float)(AcZ))) * TO_Degrees;
float Ac_roll_Y = atan(((float)(AcY)) / ((float)(AcZ))) * TO_Degrees;
//вычисляем углы по гиро.
Gy_roll_X -= (float)(GyX + Corect_fo_Ac_X) * deltaTimer *
Transform_Gy_roll_to_Deg; //здесь внимательно следить за +/-
Gy_roll_Y += (float)(GyY + Corect_fo_Ac_Y) * deltaTimer *
Transform_Gy_roll_to_Deg; //зависит от взаиморасположения датчиков
Gy_roll_Z -= (float)(GyZ + Corect_fo_Ac_Z) * deltaTimer *
Transform_Gy_roll_to_Deg;
//Получаем углы вращения по гироакселеромету
//вычисл. параметры наклонов A=A*(1-k)+B*(k)
//где к-коэф. А-корректируемой значение, В-корректировочное значения
Gy_roll_X = Gy_roll_X * (1 - Gy_roll_camp_fo_XY) +
Ac_roll_X * (Gy_roll_camp_fo_XY); // внимательно смотрим как
// расположены между собой
// датчики
Gy_roll_Y = Gy_roll_Y * (1 - Gy_roll_camp_fo_XY) +
Ac_roll_Y * (Gy_roll_camp_fo_XY); //
float SINx = sin(Gy_roll_X * TO_Radians);
float COSx = cos(Gy_roll_X * TO_Radians);
float SINy = sin(Gy_roll_Y * TO_Radians);
float COSy = cos(Gy_roll_Y * TO_Radians);
//доворачиваем вектор МП до положение датчика
MGx = (int16_t)((float)MGx * COSy +
(float)MGy * SINx * SINy +
(float)MGz * COSx * SINy);
MGy = (int16_t)((float)MGy * COSx - (float)MGz * SINx);
//по превед. вектору вычисляем отклонение оси Х от Направление на север
//получили +/- отклонения Х от направ. на маг.Север
azimuth = atan2((float)MGy, (float)MGx) * TO_Degrees;
// значение Gy_roll_Z-azimuth подбирается из расчета скорости реакции
if (abs(Gy_roll_Z - azimuth) > 15) {
Gy_roll_Z = azimuth;
}
//на случай критического отставания
Gy_roll_Z =
Gy_roll_Z * (1 - Gy_roll_camp_fo_Z) + azimuth * (Gy_roll_camp_fo_Z);
if (Gy_roll_Z < 0) {
azimuth += 360;
}
}
void setup() {
initMagnetometr(); //инициализация магн.
initAccelgyro(); //инициализация гиро.
//вычисляем дрейф гироскопа
Corect_fo_Ac(&Corect_fo_Ac_X, &Corect_fo_Ac_Y, &Corect_fo_Ac_Z);
Wire.begin();
Serial.begin(230400);
//инициализация серво.
// myservo.attach(9);
// myservo.write(0);
}
void loop() {
timer = micros();
readMag(&MGx, &MGy, &MGz); //читаем сырые данные из магнитометра
//читаем сырые данные из гироакселерометра
readAccGir(&AcX, &AcY, &AcZ, &Tmp, &GyX, &GyY, &GyZ);
calculate(); //пересчитываем полученные данные
// int G=(int)(-Gy_roll_Z+90);
// myservo.write(G);
//выводм данные
Serial.print(" ");
Serial.print(Gy_roll_X);
Serial.print(" ");
Serial.print(Gy_roll_Y);
Serial.print(" ");
Serial.print(Gy_roll_Z);
// Serial.print(" ");
// Serial.print(azimuth);
// Serial.print(" ");*/
Serial.println(" ");
deltaTimer =
(float)(micros() - timer) / 1000000.0; //получаем время между измерениями
}
