ESP32 Arduino: 地磁気センサ QMC5883L その2

平成30年度公開講座で演習する内容です。

地磁気センサQMC5883Lのテストに用いたコードです。地磁気センサをESP8266に取り付けた状態で，キャリブレーションを行います。一周360度回転をさせて最大値・最小値を調べ，maxX, maxY, minX, minY に値を入力します。また，静止状態のmag.xAxis,mag.yAxisの値をオフセット値としてoffX, offYも入力します。0度～360度で方位度が出力されます。0度，360度が北，180度が南を示します。

#include <Wire.h>
#include <DFRobot_QMC5883.h>

DFRobot_QMC5883 compass;

int maxX = 2550;
int maxY = 7480;
int minX = -10150;
int minY = -5060;
//int offX = -6466;
//int offY = 4349;
int offX = 0;
int offY = 0;

void setup() {
  delay(1000);
  Serial.begin(9600);

  while (!compass.begin()) {
    Serial.println("Could not find a valid QMC5883 sensor, check wiring!");
    delay(1000);
    yield();
  }
  if (compass.isQMC()) {
    compass.setRange(QMC5883_RANGE_2GA);
    compass.setMeasurementMode(QMC5883_CONTINOUS);
    compass.setDataRate(QMC5883_DATARATE_50HZ);
    compass.setSamples(QMC5883_SAMPLES_8);
  }
}

void loop() {
  Vector mag = compass.readRaw();
//  Serial.print(mag.XAxis);
//  Serial.print(",");
//  Serial.print(mag.YAxis);
//  Serial.print(",");
//  Serial.print(mag.ZAxis);
//  Serial.println(",");

  if (mag.XAxis > maxX)  maxX = mag.XAxis;
  if (mag.YAxis > maxY)  maxY = mag.YAxis;

  if (mag.XAxis < minX)  minX = mag.XAxis;
  if (mag.YAxis < minY)  minY = mag.YAxis;

//  Serial.print(maxX);
//  Serial.print(",");
//  Serial.print(maxY);
//  Serial.print(",");
//  Serial.print(minX);
//  Serial.print(",");
//  Serial.println(minY);

  Vector norm = compass.readNormalize();
  // Calculate heading
  float heading = atan2(norm.YAxis, norm.XAxis);
  float declinationAngle = (4.0 + (26.0 / 60.0)) / (180 / PI);
  heading += declinationAngle;
  // Correct for heading < 0deg and heading > 360deg
  if (heading < 0) {
    heading += 2 * PI;
  }
  if (heading > 2 * PI) {
    heading -= 2 * PI;
  }
  // Convert to degrees
  float headingDegrees = heading * 180 / PI;
  // Output
  ////  Serial.print(" Heading = ");
  Serial.print(heading);
  Serial.print(",");
  ////  Serial.print(" Degress = ");
  Serial.print(headingDegrees);
  Serial.println();
  delay(30);
}