#include <Wire.h>
#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_ST7735.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <math.h>
// ===== TFT PINS =====
#define TFT_CS 10
#define TFT_DC 8
#define TFT_RST 9
Adafruit_ST7735 tft = Adafruit_ST7735(TFT_CS, TFT_DC, TFT_RST);
Adafruit_MPU6050 mpu;
// ===== Colors =====
#define SKY ST77XX_BLUE
#define GROUND 0xA145
#define LINE ST77XX_YELLOW
#define AIRCRAFT ST77XX_WHITE
int cx = 80;
int cy = 40;
void drawHorizon(float pitch, float roll)
{
tft.fillScreen(ST77XX_BLACK);
float rad = roll * 0.01745;
int horizonY = cy + pitch * 2;
int x1 = 0;
int y1 = horizonY + tan(rad) * (-cx);
int x2 = 160;
int y2 = horizonY + tan(rad) * (160 - cx);
tft.fillTriangle(0,0,160,0,x1,y1,SKY);
tft.fillTriangle(160,0,160,80,x2,y2,SKY);
tft.fillTriangle(0,80,160,80,x1,y1,GROUND);
tft.fillTriangle(160,80,160,0,x2,y2,GROUND);
tft.drawLine(x1,y1,x2,y2,LINE);
// Aircraft symbol
tft.drawLine(cx-10,cy,cx+10,cy,AIRCRAFT);
tft.drawLine(cx,cy-5,cx,cy+5,AIRCRAFT);
}
void setup()
{
Serial.begin(9600);
Wire.begin();
tft.initR(INITR_MINI160x80);
tft.setRotation(1);
tft.fillScreen(ST77XX_BLACK);
if (!mpu.begin())
{
Serial.println("MPU6050 not found");
while(1);
}
}
void loop()
{
sensors_event_t a, g, temp;
mpu.getEvent(&a, &g, &temp);
float pitch = atan2(a.acceleration.x,
sqrt(a.acceleration.y * a.acceleration.y +
a.acceleration.z * a.acceleration.z)) * 57.3;
float roll = atan2(a.acceleration.y, a.acceleration.z) * 57.3;
drawHorizon(pitch, roll);
delay(40);
}