Code:
//#define TFT_MISO 12
#define TFT_MOSI 11 //a12
#define TFT_SCK 13 //a13
#define TFT_DC 9
#define TFT_CS 10
#define TFT_RST 8
#define ST7735_BROWN 0x9A60
#define PlaneSquareL 61
#define PlaneSquareR 67
#define PlaneSquareT 77
#define PlaneSquareB 83
#define PlaneWingLeft 16
#define PlaneWingRight 112
#define PlaneWingWidth 30
#define TenWidth 40
#define FiveWidth 15
#define TwoPFiveWidth 8
#define HWF 102
int16_t Colour1, Colour2;
#include <ST7735_t3.h>
#include <SPI.h>
#include <Wire.h>
ST7735_t3 tft = ST7735_t3(TFT_CS, TFT_DC, TFT_RST);
float Pitch, Roll, Yaw;
int centerY, centerX;
int TestX1, TestY1, TestX2, TestY2;
double gradient;
int intercept, xintercept;
int RightIntercept, RightIntercept2;
void setup() {
tft.initR(INITR_BLACKTAB);
tft.useFrameBuffer(true); //use the frame buffer to prevent flickeing
tft.fillScreen(ST7735_BLACK);
Roll = 30;
Pitch = 0;
while (!Serial && millis() < 4000) ;
Serial.begin(115200);
}
uint32_t loop_count = 0;
void loop() {
//fill background colour (brown when upright)
Serial.printf("Loop Count %u\n", ++loop_count); Serial.flush();
tft.fillScreen(Colour1);
//pitch sensitivity
Pitch = Pitch * 4;
//calculate the center point of the horizon line
centerY = Pitch * cos(Roll * 0.0174533) + 80;
centerX = 65 - (Pitch * sin(Roll * 0.0174533));
//calculate the left and right points
TestX1 = centerX - (HWF * cos(Roll * 0.0174533));
TestY1 = centerY - (HWF * sin(Roll * 0.0174533));
TestX2 = centerX + (HWF * cos(Roll * 0.0174533));
TestY2 = centerY + (HWF * sin(Roll * 0.0174533));
//if we are upside down switch the colours
if (Roll < -90 || Roll > 90) {
Colour1 = ST7735_BLUE;
Colour2 = ST7735_BROWN;
}
else {
Colour2 = ST7735_BLUE;
Colour1 = ST7735_BROWN;
}
//calculate the gradient of the horizon
gradient = (double)(TestY2 - TestY1) / (double)(TestX2 - TestX1);
//calculate the left hand intercept of the horizon
intercept = TestY1 - (gradient * TestX1);
xintercept = -intercept / gradient;
if (xintercept < 0 || xintercept > 128) {
xintercept = 0;
}
//cqalculate the right intercept of teh horizon
RightIntercept = gradient * 128 + intercept;
//fill the background colour
if (RightIntercept > 0) {
tft.fillTriangle(xintercept, 0, 128, 0, 128, RightIntercept, Colour2);
}
if (xintercept == 0) {
xintercept = 128;
}
if (RightIntercept < 0) {
RightIntercept2 = 0;
}
else {
RightIntercept2 = RightIntercept;
}
if (intercept > 0) {
tft.fillTriangle(0, intercept, 0, 0, xintercept, RightIntercept2, Colour2);
}
//draw the horizon
Serial.println("Before horizon"); Serial.flush();
tft.drawLine(0, intercept, 128, RightIntercept, ST7735_WHITE); // Draw The horizon
//draw the 2.5 degree lines
for (int i = 10; i < 100; i += 10)
{
if (i % 20 != 0)
{
int center2P5X = centerX - (i * sin(Roll * 0.0174533));
int center2P5Y = centerY + (i * cos(Roll * 0.0174533));
tft.drawLine(center2P5X - (TwoPFiveWidth * sin((Roll + 90) * 0.0174533)), center2P5Y + (TwoPFiveWidth * cos((Roll + 90) * 0.0174533)), center2P5X + (TwoPFiveWidth * sin((Roll + 90) * 0.0174533)), center2P5Y - (TwoPFiveWidth * cos((Roll + 90) * 0.0174533)), ST7735_WHITE);
center2P5X = centerX + (i * sin(Roll * 0.0174533));
center2P5Y = centerY - (i * cos(Roll * 0.0174533));
tft.drawLine(center2P5X - (TwoPFiveWidth * sin((Roll + 90) * 0.0174533)), center2P5Y + (TwoPFiveWidth * cos((Roll + 90) * 0.0174533)), center2P5X + (TwoPFiveWidth * sin((Roll + 90) * 0.0174533)), center2P5Y - (TwoPFiveWidth * cos((Roll + 90) * 0.0174533)), ST7735_WHITE);
}
}
//draw the 5 degree lines
Serial.println("Before 5 degree"); Serial.flush();
for (int i = 20; i < 160; i += 20)
{
if (i % 40 != 0)
{
int center5X = centerX - (i * sin(Roll * 0.0174533));
int center5Y = centerY + (i * cos(Roll * 0.0174533));
Serial.printf(" %i %i:%i ", i, center5X, center5Y); Serial.flush();
tft.drawLine(center5X - (20 * sin((Roll + 90) * 0.0174533)), center5Y + (20 * cos((Roll + 90) * 0.0174533)), center5X + (20 * sin((Roll + 90) * 0.0174533)), center5Y - (20 * cos((Roll + 90) * 0.0174533)), ST7735_WHITE);
center5X = centerX + (i * sin(Roll * 0.0174533));
center5Y = centerY - (i * cos(Roll * 0.0174533));
Serial.printf(" $ %i:%i\n", center5X, center5Y); Serial.flush();
tft.drawLine(center5X - (20 * sin((Roll + 90) * 0.0174533)), center5Y + (20 * cos((Roll + 90) * 0.0174533)), center5X + (20 * sin((Roll + 90) * 0.0174533)), center5Y - (20 * cos((Roll + 90) * 0.0174533)), ST7735_WHITE);
}
}
//draw the 10 degree lines
Serial.println("Before 10 degree"); Serial.flush();
for (int i = 40; i < 280; i += 40)
{
int center10X = centerX - (i * sin(Roll * 0.0174533));
int center10Y = centerY + (i * cos(Roll * 0.0174533));
tft.drawLine(center10X - (TenWidth * sin((Roll + 90) * 0.0174533)), center10Y + (TenWidth * cos((Roll + 90) * 0.0174533)), center10X + (TenWidth * sin((Roll + 90) * 0.0174533)), center10Y - (TenWidth * cos((Roll + 90) * 0.0174533)), ST7735_WHITE);
center10X = centerX + (i * sin(Roll * 0.0174533));
center10Y = centerY - (i * cos(Roll * 0.0174533));
tft.drawLine(center10X - (TenWidth * sin((Roll + 90) * 0.0174533)), center10Y + (TenWidth * cos((Roll + 90) * 0.0174533)), center10X + (TenWidth * sin((Roll + 90) * 0.0174533)), center10Y - (TenWidth * cos((Roll + 90) * 0.0174533)), ST7735_WHITE);
}
Serial.println("Before cursor"); Serial.flush();
DrawPlaneCursor();
Serial.println("Before updateScreen"); Serial.flush();
tft.updateScreen();
}
void DrawPlaneCursor() {
int PlaneWingRightOffset = PlaneWingLeft + PlaneWingWidth;
int IndicatorThickness = PlaneSquareB - PlaneSquareT;
int PlaneSquareWidth = PlaneSquareR - PlaneSquareL;
//draw the center square
tft.drawFastHLine(PlaneSquareL, PlaneSquareT, PlaneSquareWidth, ST7735_WHITE);
tft.drawFastHLine(PlaneSquareL, PlaneSquareB - 1, PlaneSquareWidth, ST7735_WHITE);
tft.drawFastVLine(PlaneSquareL, PlaneSquareT + 1, IndicatorThickness - 2, ST7735_WHITE);
tft.drawFastVLine(PlaneSquareR - 1, PlaneSquareT + 1, IndicatorThickness - 2, ST7735_WHITE);
tft.fillRect(PlaneSquareL + 1, PlaneSquareT + 1, PlaneSquareWidth - 2, IndicatorThickness - 2, ST7735_BLACK);
//draw the left wing
tft.fillRect(PlaneWingLeft + 1, PlaneSquareT + 1, PlaneWingWidth, IndicatorThickness - 2, ST7735_BLACK);
tft.fillRect(PlaneWingRightOffset - PlaneSquareWidth + 3, PlaneSquareB - 1, IndicatorThickness - 2, IndicatorThickness - 2, ST7735_BLACK);
tft.drawFastHLine(PlaneWingLeft, PlaneSquareT, PlaneWingWidth + 1, ST7735_WHITE);
tft.drawFastVLine(PlaneWingLeft, PlaneSquareT + 1, IndicatorThickness - 1, ST7735_WHITE);
tft.drawFastHLine(PlaneWingLeft, PlaneSquareT + IndicatorThickness - 1, PlaneWingWidth - PlaneSquareWidth + 2, ST7735_WHITE);
tft.drawFastVLine(PlaneWingLeft + PlaneWingWidth, PlaneSquareT, 2 * (IndicatorThickness) - 3, ST7735_WHITE);
tft.drawFastHLine(PlaneWingRightOffset - PlaneSquareWidth + 2, PlaneSquareB + IndicatorThickness - 4, PlaneSquareR - PlaneSquareL - 2, ST7735_WHITE);
tft.drawFastVLine(PlaneWingRightOffset - PlaneSquareWidth + 1, PlaneSquareB - 1, IndicatorThickness - 2, ST7735_WHITE);
//draw the right wing
tft.fillRect(PlaneWingRight - PlaneWingWidth + 1, PlaneSquareT + 1, PlaneWingWidth - 1, IndicatorThickness - 2, ST7735_BLACK);
tft.fillRect(PlaneWingRight - PlaneWingWidth + 1, PlaneSquareB - 1, IndicatorThickness - 2, IndicatorThickness - 2, ST7735_BLACK);
tft.drawFastHLine(PlaneWingRight - PlaneWingWidth, PlaneSquareT, PlaneWingWidth, ST7735_WHITE);
tft.drawFastVLine(PlaneWingRight - 1, PlaneSquareT + 1, IndicatorThickness - 2, ST7735_WHITE);
tft.drawFastHLine(PlaneWingRight - PlaneWingWidth + PlaneSquareWidth - 1, PlaneSquareT + IndicatorThickness - 1, PlaneWingWidth - PlaneSquareWidth + 1, ST7735_WHITE);
tft.drawFastVLine(PlaneWingRight - PlaneWingWidth + PlaneSquareWidth - 1, PlaneSquareT + IndicatorThickness, IndicatorThickness - 3, ST7735_WHITE);
tft.drawFastHLine(PlaneWingRight - PlaneWingWidth, PlaneSquareT + 2 * IndicatorThickness - 3, PlaneSquareWidth, ST7735_WHITE);
tft.drawFastVLine(PlaneWingRight - PlaneWingWidth, PlaneSquareT + 1, IndicatorThickness * 2 - 4, ST7735_WHITE);
}
And the debug output: