GY-25 Tilt Angle Module MPU6050 Gyro Acceleration

				
					// GY-25 ARDUINO TEST CODE SSD1306 OLED DISPLAY WITH GRAPHICS
// Improved roll angle visualization

#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET     -1

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

int YPR[3];
unsigned char Re_buf[8], counter = 0;
unsigned char sign = 0;

//-----------------------------------------------------------
void setup()
{
  Serial.begin(115200);
  
  // Initialize OLED
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    for(;;);
  }
  
  display.display();
  delay(1000);
  display.clearDisplay();
  
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.println("GY-25 Initializing...");
  display.display();
  
  delay(2000);
  Serial.write(0XA5);
  Serial.write(0X52);    // Initialize GY25, continuous output mode
  
  display.clearDisplay();
  display.setCursor(0, 0);
  display.println("GY-25 Ready");
  display.display();
  delay(1000);
}

//-------------------------------------------------------------
void loop() {
  if(sign)
  {  
     sign = 0;
     if(Re_buf[0] == 0xAA && Re_buf[7] == 0x55)
     {           
        YPR[0] = (Re_buf[1] << 8 | Re_buf[2]) / 100;  // Yaw
        YPR[1] = (Re_buf[3] << 8 | Re_buf[4]) / 100;  // Pitch
        YPR[2] = (Re_buf[5] << 8 | Re_buf[6]) / 100;  // Roll
        
        // Serial output
        Serial.print("YPR:\t");
        Serial.print(YPR[0]); Serial.print("\t");
        Serial.print(YPR[1]); Serial.print("\t");
        Serial.println(YPR[2]);
        
        // Draw gyroscope visualization
        drawGyroscope();
        
        delay(50); // Slightly longer delay for smoother display
     }
  } 
}

//---------------------------------------------------------------
void drawGyroscope() {
  display.clearDisplay();
  
  int centerX = SCREEN_WIDTH / 2;
  int centerY = SCREEN_HEIGHT / 2;
  
  // Draw reference horizon line (for roll visualization)
  int horizonY = centerY;
  display.drawLine(0, horizonY, SCREEN_WIDTH, horizonY, SSD1306_WHITE);
  
  // Draw roll indicator (airplane wing representation)
  drawRollIndicator(centerX, centerY, YPR[2]);
  
  // Draw pitch indicator (simple up/down marker)
  drawPitchIndicator(centerX, centerY, YPR[1]);
  
  // Draw yaw compass at the bottom
  drawYawCompass(YPR[0]);
  
  // Draw angle values
  display.setTextSize(1);
  display.setCursor(0, 0);
  display.print("Y:"); display.print(YPR[0]); display.print((char)247);
  display.print(" P:"); display.print(YPR[1]); display.print((char)247);
  display.print(" R:"); display.print(YPR[2]); display.print((char)247);
  
  display.display();
}

//---------------------------------------------------------------
void drawRollIndicator(int centerX, int centerY, int roll) {
  // Draw a simple airplane representation for roll
  int wingLength = 20;
  
  // Calculate wing positions based on roll angle
  float rollRad = roll * PI / 180.0;
  
  // Left wing
  int leftWingX = centerX - wingLength * cos(rollRad);
  int leftWingY = centerY - wingLength * sin(rollRad);
  
  // Right wing
  int rightWingX = centerX + wingLength * cos(rollRad);
  int rightWingY = centerY + wingLength * sin(rollRad);
  
  // Draw fuselage (center line)
  display.drawLine(centerX - 5, centerY, centerX + 5, centerY, SSD1306_WHITE);
  
  // Draw wings
  display.drawLine(centerX, centerY, leftWingX, leftWingY, SSD1306_WHITE);
  display.drawLine(centerX, centerY, rightWingX, rightWingY, SSD1306_WHITE);
  
  // Draw wing tips
  display.drawCircle(leftWingX, leftWingY, 2, SSD1306_WHITE);
  display.drawCircle(rightWingX, rightWingY, 2, SSD1306_WHITE);
}

//---------------------------------------------------------------
void drawPitchIndicator(int centerX, int centerY, int pitch) {
  // Simple pitch indicator (up/down marker)
  int pitchOffset = map(constrain(pitch, -30, 30), -30, 30, -15, 15);
  
  // Draw pitch reference lines
  for (int i = -2; i <= 2; i++) {
    int lineY = centerY + i * 10;
    if (lineY >= 0 && lineY < SCREEN_HEIGHT) {
      display.drawLine(centerX - 5, lineY, centerX + 5, lineY, SSD1306_WHITE);
    }
  }
  
  // Draw current pitch position
  int pitchY = centerY - pitchOffset;
  if (pitchY >= 0 && pitchY < SCREEN_HEIGHT) {
    display.drawTriangle(
      centerX - 8, pitchY,
      centerX + 8, pitchY,
      centerX, pitchY - 6, SSD1306_WHITE
    );
  }
}

//---------------------------------------------------------------
void drawYawCompass(int yaw) {
  // Draw a simple compass at the bottom
  int compassY = SCREEN_HEIGHT - 15;
  int compassWidth = SCREEN_WIDTH - 20;
  int compassCenter = SCREEN_WIDTH / 2;
  
  // Draw compass base line
  display.drawLine(10, compassY, SCREEN_WIDTH - 10, compassY, SSD1306_WHITE);
  
  // Draw compass needle (normalized to 0-360)
  int normalizedYaw = yaw % 360;
  if (normalizedYaw < 0) normalizedYaw += 360;
  
  // Map yaw to compass position
  int needlePos = map(normalizedYaw, 0, 360, 10, SCREEN_WIDTH - 10);
  
  // Draw needle
  display.drawTriangle(
    needlePos, compassY - 8,
    needlePos - 3, compassY,
    needlePos + 3, compassY,
    SSD1306_WHITE
  );
  
  // Draw compass markers
  display.setCursor(compassCenter - 5, compassY + 2);
  display.print("N");
  display.setCursor(10, compassY + 2);
  display.print("W");
  display.setCursor(SCREEN_WIDTH - 20, compassY + 2);
  display.print("E");
}

//---------------------------------------------------------------
void serialEvent() {
  while (Serial.available()) {   
    Re_buf[counter] = (unsigned char)Serial.read();
    if(counter == 0 && Re_buf[0] != 0xAA) return;
    counter++;       
    if(counter == 8)
    {    
       counter = 0;
       sign = 1;
    }      
  }
}