Цифровой компас в реальном времени на Arduino и Processing

Сделаем красивый компас на Arduino, MEMS-магнетометре и Processing IDE. Демо-видео ниже.

Обзор

Нужен MEMS-магнетометр (например, HMC5883L на плате GY-80), Arduino и перемычки.

Как работает компас

Arduino часть

Считываем данные с датчика по I2C, вычисляем азимут (heading) по осям X и Y, отправляем в Processing через Serial. Код:

/* Arduino Compass by Dejan Nedelkovski, www.HowToMechatronics.com */
#include <Wire.h>
#define Magnetometer_mX0 0x03 #define Magnetometer_mX1 0x04
#define Magnetometer_mZ0 0x05 #define Magnetometer_mZ1 0x06
#define Magnetometer_mY0 0x07 #define Magnetometer_mY1 0x08
int mX0,mX1,mX_out; int mY0,mY1,mY_out; int mZ0,mZ1,mZ_out;
float heading,headingDegrees,headingFiltered,declination;
float Xm,Ym,Zm;
#define Magnetometer 0x1E // I2C адрес HMC5883

void setup(){
  Serial.begin(115200); Wire.begin(); delay(100);
  Wire.beginTransmission(Magnetometer);
  Wire.write(0x02); Wire.write(0x00); // Continuous measurement mode
  Wire.endTransmission();
}

void loop(){
  // Чтение X, Y, Z (по 2 байта на ось)
  Wire.beginTransmission(Magnetometer); Wire.write(Magnetometer_mX1); Wire.endTransmission();
  Wire.requestFrom(Magnetometer,1); if(Wire.available()<=1){mX0=Wire.read();}
  Wire.beginTransmission(Magnetometer); Wire.write(Magnetometer_mX0); Wire.endTransmission();
  Wire.requestFrom(Magnetometer,1); if(Wire.available()<=1){mX1=Wire.read();}
  // Y
  Wire.beginTransmission(Magnetometer); Wire.write(Magnetometer_mY1); Wire.endTransmission();
  Wire.requestFrom(Magnetometer,1); if(Wire.available()<=1){mY0=Wire.read();}
  Wire.beginTransmission(Magnetometer); Wire.write(Magnetometer_mY0); Wire.endTransmission();
  Wire.requestFrom(Magnetometer,1); if(Wire.available()<=1){mY1=Wire.read();}
  // Z
  Wire.beginTransmission(Magnetometer); Wire.write(Magnetometer_mZ1); Wire.endTransmission();
  Wire.requestFrom(Magnetometer,1); if(Wire.available()<=1){mZ0=Wire.read();}
  Wire.beginTransmission(Magnetometer); Wire.write(Magnetometer_mZ0); Wire.endTransmission();
  Wire.requestFrom(Magnetometer,1); if(Wire.available()<=1){mZ1=Wire.read();}

  // Сборка 16-битных значений и перевод в Гауссы (0.92 мГс/бит)
  mX1=mX1<<8; mX_out=mX0+mX1; Xm=mX_out*0.00092;
  mY1=mY1<<8; mY_out=mY0+mY1; Ym=mY_out*0.00092;
  mZ1=mZ1<<8; mZ_out=mZ0+mZ1; Zm=mZ_out*0.00092;

  // Азимут
  heading = atan2(Ym, Xm);
  declination = 0.073; // магнитное склонение для вашей локации (рад)
  heading += declination;
  if(heading < 0) heading += 2*PI;
  if(heading > 2*PI) heading -= 2*PI;
  headingDegrees = heading * 180/PI;
  // Низкочастотный фильтр (сглаживание)
  headingFiltered = headingFiltered*0.85 + headingDegrees*0.15;
  Serial.println(headingFiltered);
  delay(50);
}

Подробнее о работе магнетометра — в туториале по MEMS-сенсорам.

Processing IDE часть

Принимаем heading через Serial. Компас — набор прозрачных PNG: фон, циферблат, стрелка. В draw() загружаем фон, сдвигаем систему координат в центр, вращаем циферблат на -heading, стрелка остаётся неподвижной (popMatrix).

/* Arduino Compass - Processing code */
import processing.serial.*; import java.awt.event.KeyEvent; import java.io.IOException;
Serial myPort; PImage imgCompass, imgCompassArrow, background;
String data=""; float heading;

void setup() {
  size(1920,1080,P3D); smooth();
  imgCompass=loadImage("Compass.png");
  imgCompassArrow=loadImage("CompassArrow.png");
  background=loadImage("Background.png");
  myPort=new Serial(this,"COM4",115200);
  myPort.bufferUntil('\n');
}

void draw() {
  image(background,0,0);
  pushMatrix(); translate(width/2,height/2,0);
  rotateZ(radians(-heading));
  image(imgCompass,-960,-540);
  popMatrix();
  image(imgCompassArrow,0,0);
  textSize(30); text("Heading: "+heading,40,40);
  delay(40);
}

void serialEvent(Serial myPort) {
  data=myPort.readStringUntil('\n');
  heading=float(data);
}

Скачать файлы проекта (изображения, код):