자파리 취미생활 관련 제품 사용기나 팁, 활용법 등을 소개하는 블로그입니다. 드론, RC, 드로잉, 자전거, 여행 등을 주제로 합니다.

아래 예제는 저서 '드론 제작과 운용' 에서 설명한 코딩 예제입니다.

#include <Wire.h>
#include <MPU9250_asukiaaa.h>
#include "HT_SSD1306Wire.h"
#include "HT_TinyGPS++.h"

// 핀 정의 (예시, 실제 연결된 핀 번호로 변경 필요)
#define SDA_PIN 41 
#define SCL_PIN 42 
#define CALIB_SEC 20 // 캘리브레이션 지속 시간 설정 (20초)

// I2C를 사용하여 OLED 디스플레이 초기화, RST_OLED도 반드시 적어주어야함
static SSD1306Wire display(0x3c, 400000, SDA_OLED, SCL_OLED, GEOMETRY_128_64, RST_OLED);

float accelXOffset = 0, accelYOffset = 0, accelZOffset = 0;
float gyroXOffset = 0, gyroYOffset = 0, gyroZOffset = 0;   

TwoWire myWire = TwoWire(1); 
MPU9250_asukiaaa mySensor;   
TinyGPSPlus gps; // GPS 객체 생성

void VextON(void) {
  // 외부 전원 켜기 (예: 센서 또는 주변 장치)
  pinMode(Vext, OUTPUT);
  digitalWrite(Vext, LOW); // Vext 핀을 LOW로 설정하여 전원 공급
}

void VextOFF(void) {
  // 외부 전원 끄기
  pinMode(Vext, OUTPUT);
  digitalWrite(Vext, HIGH); // Vext 핀을 HIGH로 설정하여 전원 차단
}

void calibrateAccel() {
  float sumX = 0, sumY = 0, sumZ = 0;
  int samples = 500;
  for (int i = 0; i < samples; i++) {
    mySensor.accelUpdate();
    sumX += mySensor.accelX();
    sumY += mySensor.accelY();
    sumZ += mySensor.accelZ();
    delay(10);
  }
  accelXOffset = sumX / samples;
  accelYOffset = sumY / samples;
  accelZOffset = sumZ / samples;
  Serial.print("accelXOffset : "); Serial.println(accelXOffset); 
  Serial.print("accelYOffset : "); Serial.println(accelYOffset); 
  Serial.print("accelZOffset : "); Serial.println(accelZOffset);
}

void calibrateGyro() {
  float sumX = 0, sumY = 0, sumZ = 0;
  int samples = 500;
  for (int i = 0; i < samples; i++) {
    mySensor.gyroUpdate();
    sumX += mySensor.gyroX();
    sumY += mySensor.gyroY();
    sumZ += mySensor.gyroZ();
    delay(10);
  }
  // 아래 자이로 옵셋은 수평 상태에서 모두 0이 나오는 것이 이상적
  gyroXOffset = sumX / samples;
  gyroYOffset = sumY / samples;
  gyroZOffset = sumZ / samples;
  Serial.print("gyroXOffset : "); Serial.println(gyroXOffset); 
  Serial.print("gyroYOffset : "); Serial.println(gyroYOffset); 
  Serial.print("gyroZOffset : "); Serial.println(gyroZOffset);
}

void setMagMinMaxAndSetOffset(MPU9250_asukiaaa* sensor, int seconds) {
  unsigned long calibStartAt = millis(); // 캘리브레이션 시작 시간 기록
  float magX, magXMin, magXMax, magY, magYMin, magYMax, magZ, magZMin, magZMax;
  
  // 초기 최소/최대값 설정
  sensor->magUpdate();
  magXMin = magXMax = sensor->magX();
  magYMin = magYMax = sensor->magY();
  magZMin = magZMax = sensor->magZ();
  
  // 캘리브레이션 루프
  while(millis() - calibStartAt < (unsigned long) seconds * 1000) {
    delay(100); // 데이터 갱신 간격
    sensor->magUpdate();
    magX = sensor->magX();
    magY = sensor->magY();
    magZ = sensor->magZ();
    // 최소/최대값 업데이트
    if(magX > magXMax) magXMax = magX;
    if(magY > magYMax) magYMax = magY;
    if(magZ > magZMax) magZMax = magZ;
    if(magX < magXMin) magXMin = magX;
    if(magY < magYMin) magYMin = magY;
    if(magZ < magZMin) magZMin = magZ;
  }
  
  // 오프셋 설정 (최소/최대값의 중간값 기준)
  sensor->magXOffset = - (magXMax + magXMin) / 2;
  sensor->magYOffset = - (magYMax + magYMin) / 2;
  sensor->magZOffset = - (magZMax + magZMin) / 2;
  
  // 오프셋값 출력
  Serial.println("Magnetometer Offsets:");
  Serial.print("X Offset: "); Serial.println(mySensor.magXOffset);
  Serial.print("Y Offset: "); Serial.println(mySensor.magYOffset);
  Serial.print("Z Offset: "); Serial.println(mySensor.magZOffset);
}

void calculateTiltCompensatedHeading() {
  const int MEASUREMENTS = 20;
  float sumRollX = 0.0, sumRollY = 0.0;
  float sumPitchX = 0.0, sumPitchY = 0.0;
  float sumYawX = 0.0, sumYawY = 0.0;
  unsigned long prevMillis = millis();

  for (int i = 0; i < MEASUREMENTS;) {
    unsigned long currentMillis = millis();

    // 0.005초(5ms) 경과 확인
    if (currentMillis - prevMillis >= 5) {
      prevMillis = currentMillis;

  mySensor.accelUpdate();
  float ax = mySensor.accelY();
  float ay = -mySensor.accelX();
  float az = mySensor.accelZ();

  // 자이로 데이터 업데이트
  mySensor.gyroUpdate();
  float gx = mySensor.gyroY();
  float gy = -mySensor.gyroX();
  float gz = mySensor.gyroZ();

  // 지자기계 데이터 업데이트
  mySensor.magUpdate();
  float mx = mySensor.magY();
  float my = -mySensor.magX();
  float mz = mySensor.magZ();

      // 2) 롤, 피치 라디안 계산
      float rollRad = atan2(-ax, az);
      // 0~2π 범위로 만들기
      if (rollRad < 0) rollRad += 2.0f * PI;

      float pitchRad = atan2(-ay, sqrt(ax*ax + az*az));
      // 아래 if문 등은 원래 코드에서 pitch 조정하는 부분을 상황에 맞게 변환
      // 예: pitch가 음수면 2π 더하는 식으로 0~2π 만들 수도 있음
      if (pitchRad < 0) pitchRad += 2.0f * PI;

      // 3) 야우(heading) 라디안 계산
      float yawRad = atan2(my, mx);
      // atan2() 결과가 -π ~ +π 이므로, 0~2π 맞추려면
      if (yawRad < 0) yawRad += 2.0f * PI;

      // (선택) 샘플별 오프셋/특별 보정이 꼭 필요하다면,
      // 여기서 yawRad에 +-, rollRad에 +- 하는 식으로 처리 가능.

      // 4) (cos, sin)으로 변환하여 누적
      sumRollX  += cos(rollRad);
      sumRollY  += sin(rollRad);

      sumPitchX += cos(pitchRad);
      sumPitchY += sin(pitchRad);

      sumYawX   += cos(yawRad);
      sumYawY   += sin(yawRad);

      i++; // 카운트 증가
    }
  }

// -- 모든 샘플을 누적한 뒤 평균 백터 계산 --

  // 롤
  float avgRollRad = atan2(sumRollY, sumRollX);
  float avgRollDeg = avgRollRad * 180.0 / PI;
  if (avgRollDeg < 0) avgRollDeg += 360.0;
  
  float rollOffset = 171.7; // 수평일 때 172도인 문제를 보정
  avgRollDeg -= rollOffset;
  if (avgRollDeg < 0) {
  avgRollDeg += 360.0;
  } 
  else if (avgRollDeg >= 360.0) {
  avgRollDeg -= 360.0;
  }

  // 피치
  float avgPitchRad = atan2(sumPitchY, sumPitchX);
  float avgPitchDeg = avgPitchRad * 180.0 / PI;
  if (avgPitchDeg < 0) avgPitchDeg += 360.0;

  float pitchOffset = 350.2; // 수평일 때 피치가 350.2도인 문제를 보정
  avgPitchDeg -= pitchOffset;
  if (avgPitchDeg < 0) {
  avgPitchDeg += 360.0;
  } 
  else if (avgPitchDeg >= 360.0) {
  avgPitchDeg -= 360.0;
  }

  // 요
  float avgYawRad = atan2(sumYawX, sumYawY);
  float avgYawDeg = avgYawRad * 180.0 / PI;
  if (avgYawDeg < 0) avgYawDeg += 360.0;

  float yawOffset = 270; // 270도가 북쪽인 문제를 보정
  avgYawDeg -= yawOffset;
  if (avgYawDeg < 0) {
  avgYawDeg += 360.0;
  } 
  else if (avgYawDeg >= 360.0) {
  avgYawDeg -= 360.0;
  }
  
  // 결과 출력
  Serial.print("Roll: "); Serial.print(avgRollDeg);
  Serial.print(" Pitch: "); Serial.print(avgPitchDeg);
  Serial.print(" Yaw: "); Serial.println(avgYawDeg);  

  // 디스플레이에 출력
  char rollStr[32];
  snprintf(rollStr, sizeof(rollStr), "%.1f°", avgRollDeg);
  display.drawString(30, 15, "Roll: ");
  display.drawString(60, 15, rollStr);
  char pitchStr[32];
  snprintf(pitchStr, sizeof(pitchStr), "%.1f°", avgPitchDeg);
  display.drawString(95, 15, "Pitch: ");
  display.drawString(125, 15, pitchStr);

  char yawStr[32];
  snprintf(yawStr, sizeof(yawStr), "%.0f°", avgYawDeg);
  display.drawString(30, 30, "Yaw: ");
  display.drawString(60, 30, yawStr);
  display.display();
}

void setup() {
  Serial.begin(115200);
  Serial1.begin(9600, SERIAL_8N1, 34, 33); // GPS 모듈과 통신 시작
  
  // I2C 설정 및 센서 초기화
  myWire.begin(SDA_PIN, SCL_PIN, 400000);
  mySensor.setWire(&myWire);
  mySensor.beginAccel(); 
  mySensor.beginMag(MAG_MODE_CONTINUOUS_100HZ);   
  mySensor.beginGyro();  
  
  VextON(); // 외부 전원 켜기
  delay(100); // 전원이 안정될 때까지 짧은 지연

  display.init(); // 디스플레이 초기화
  display.setFont(ArialMT_Plain_10); // 디스플레이의 기본 폰트 설정

  // DWARF 로고 송출
  display.setTextAlignment(TEXT_ALIGN_CENTER);
  display.setFont(ArialMT_Plain_16);
  display.drawString(display.getWidth()/2, 24, "DWARF");
  display.display();
  delay(3000);
  display.clear();
  display.display();

  // 가속도 및 자이로 캘리브레이션 준비 메시지 표시
  display.setFont(ArialMT_Plain_10);
  display.drawString(display.getWidth() / 2, 5, "Start scanning");
  display.drawString(display.getWidth() / 2, 20, "values of Accel & Gyro");
  display.drawString(display.getWidth() / 2, 35, "to get offset values.");
  display.drawString(display.getWidth() / 2, 50, "DO NOT TOUCH OR MOVE");
  display.display();
  
  // 가속도 및 자이로 캘리브레이션
  calibrateAccel(); 
  calibrateGyro();  

  display.clear();
  display.display();
  display.setFont(ArialMT_Plain_16);
  display.drawString(display.getWidth() / 2, 10, "Calibration");
  display.drawString(display.getWidth() / 2, 30, "1/2 finished");
  display.display();
  delay(2000);
  display.clear();
  display.display();

  display.setFont(ArialMT_Plain_10);
  display.drawString(display.getWidth()/2, 5, "Now, Rotate your device");
  display.drawString(display.getWidth()/2, 20, "360 degrees");
  display.drawString(display.getWidth()/2, 35, "in every direction for");
  display.drawString(display.getWidth()/2, 50, String(CALIB_SEC) + " seconds.");
  display.display();

  // 방위각 변화를 감지하여 캘리브레이션 시작
  mySensor.magUpdate();
  float initialDirection = mySensor.magHorizDirection();
  unsigned long startTime = millis(); // startTime 선언
  bool significantChange = false;

  while(millis() - startTime < (unsigned long)(CALIB_SEC * 1000)) {
    delay(100); // 0.1초 간격으로 방위각 측정
    mySensor.magUpdate();
    float currentDirection = mySensor.magHorizDirection();

    if(abs(currentDirection - initialDirection) > 15.0) {
      significantChange = true; // 15도 이상의 변화 감지
      break;
    }
  }

  if(significantChange) {
    setMagMinMaxAndSetOffset(&mySensor, CALIB_SEC); // 캘리브레이션 함수 호출
    // 캘리브레이션 완료 메시지 표시
    display.clear();
    display.display();
    display.setFont(ArialMT_Plain_16);
    display.drawString(display.getWidth() / 2, 10, "Calibration");
    display.drawString(display.getWidth() / 2, 30, "Finished.");
    Serial.println("Calibration Finished.");
    display.display();
    delay(4000);
    display.clear();
    display.display();
  }
  else {
    // 방위각 변화가 없을 경우 기본 오프셋값 설정
    mySensor.magXOffset = -81;
    mySensor.magYOffset = 47;
    mySensor.magZOffset = 81;
    display.clear();
    display.display();
    display.setFont(ArialMT_Plain_10);
    display.drawString(display.getWidth()/2, 5, "No significant direction");
    display.drawString(display.getWidth()/2, 20, "change detected.");
    display.drawString(display.getWidth()/2, 35, "Using default offsets.");
    display.display();
    delay(4000);
    display.clear();
    display.display();
    Serial.println("No significant direction change detected. Using default offsets:");
  }
  
  display.clear();
  display.display();
  Serial.println("Initialization complete.");
}

void loop() {

  // GPS 데이터 수신
  unsigned long start = millis(); // 현재 시간 기록
  static unsigned long lastUpdate = 0;
  if (millis() - lastUpdate > 100) { // 마지막 업데이트 이후 100ms가 지났는지 확인
    lastUpdate = millis(); // 마지막 업데이트 시간 갱신
    display.clear();  // 디스플레이 내용 초기화
    display.setTextAlignment(TEXT_ALIGN_LEFT);
    display.setFont(ArialMT_Plain_10);
    // GPS 위치 데이터 확인 및 출력

/*
    char gpsInfo[64];
      snprintf(gpsInfo, sizeof(gpsInfo), "Lat: %.6f\nLng: %.6f\nAlt: %.2f m",
        gps.location.lat(),  // 위도 정보 가져오기
        gps.location.lng(),  // 경도 정보 가져오기
        gps.altitude.meters());  // 고도 정보 가져오기
      display.drawString(0, 45, String(gpsInfo));  // GPS 정보를 디스플레이에 그리기
*/

    if (gps.location.isValid()) {  // GPS 위치 데이터가 유효한지 확인
      display.drawString(0, 0, "GPS OK");
      Serial.println("GPS OK");
    } 
     else {
      display.drawString(0, 0, "GPS Data Invalid"); // GPS 데이터가 유효하지 않은 경우
      Serial.println("GPS Data Invalid");
     }

    // 프로그램 시작 이후 경과 시간을 시:분:초 형식으로 표시
    unsigned long seconds = millis() / 1000;  // 초 단위로 경과 시간 계산
    unsigned int hours = seconds / 3600;  // 시간 계산
    unsigned int minutes = (seconds % 3600) / 60;  // 분 계산
    unsigned int secs = seconds % 60;  // 초 계산
    char timeString[9];
    sprintf(timeString, "%02u:%02u:%02u", hours, minutes, secs);  // 시간 형식 문자열 생성
    display.setTextAlignment(TEXT_ALIGN_RIGHT);  // 오른쪽 정렬
    display.setFont(ArialMT_Plain_10);
    display.drawString(128, 0, String(timeString)); // 디스플레이에 경과 시간 표시
  
   // 방향호출
  calculateTiltCompensatedHeading();
  
 
  delay(100); // 100ms 대기
 }
}