DIY Avionics Simulator with ESP32 - Artificial Horizon, Compass & Altimeter

The project successfully demonstrates the integration of graphical user interfaces, I2C sensors, and the ESP32-S3 microcontroller into a compact avionics instrument simulator.

Jun 18, 2026

•

14 views

•

0 respects

•

GPL3+

Communication

Embedded

Monitoring

Devices & Components

HMC5883L Magnetic Sensor

Crowpanel round display 480x480

6 DOF Sensor - MPU6050

Hardware & Tools

Soldering Iron Kit

Software & Tools

Arduino IDE

Project description

Code

Code Avionics

cpp

...

1// by miercemk May, 2026
2
3#include <Arduino.h>
4#include <Arduino_GFX_Library.h>
5#include <Wire.h>
6
7#define DISPLAY_WIDTH  480
8#define DISPLAY_HEIGHT 480
9#define CX 240
10#define CY 240
11
12#define BL_PIN     6
13#define PANEL_CS   16
14#define PANEL_SCK  2
15#define PANEL_SDA  1
16#define PCLK_NEG   1
17
18#define ENCODER_CLK 4
19#define ENCODER_DT  42
20
21#define I2C_SDA 38
22#define I2C_SCL 39
23#define PCF8574_ADDR 0x21
24#define MPU6050_ADDR 0x68
25#define QMC5883L_ADDR 0x0D
26
27Arduino_DataBus       *panelBus = nullptr;
28Arduino_ESP32RGBPanel *rgbpanel = nullptr;
29Arduino_RGB_Display   *gfx      = nullptr;
30uint16_t              *fb       = nullptr;
31
32float pitchDeg = 0;
33float rollDeg  = 0;
34
35float compassHeadingDeg = 0;
36unsigned long lastCompassRead = 0;
37
38float altitudeFt = 6300.0f;
39unsigned long lastAltitudeUpdate = 0;
40
41float lastDrawPitch = 999;
42float lastDrawRoll  = 999;
43
44volatile int lastEncoded = 0;
45volatile long encoderValue = 0;
46long lastEncoderValue = 0;
47
48int brightnessLevel = 255;
49
50unsigned long lastMPURead = 0;
51
52extern const uint8_t st7701_type7_init_operations[];
53
54uint16_t rgb565(uint8_t r, uint8_t g, uint8_t b) {
55  return ((r & 0xF8) << 8) | ((g & 0xFC) << 3) | (b >> 3);
56}
57
58const uint16_t COL_BLACK      = 0x0000;
59const uint16_t COL_WHITE      = 0xFFFF;
60const uint16_t COL_YELLOW     = 0xFFE0;
61const uint16_t COL_ORANGE     = 0xFD20;
62const uint16_t COL_SKY        = rgb565(20, 165, 225);
63const uint16_t COL_GROUND     = rgb565(150, 95, 45);
64const uint16_t COL_RING_OUTER = rgb565(170, 170, 170);
65const uint16_t COL_RING_INNER = rgb565(45, 45, 45);
66const uint16_t COL_RING_EDGE  = rgb565(15, 15, 15);
67
68enum ScreenMode {
69  SCREEN_ATTITUDE = 0,
70  SCREEN_COMPASS,
71  SCREEN_ALTIMETER,
72  SCREEN_AIRPLANE,
73  SCREEN_COUNT
74};
75
76ScreenMode currentScreen = SCREEN_ATTITUDE;
77
78static inline void putpix(int x, int y, uint16_t c) {
79  if ((unsigned)x < DISPLAY_WIDTH && (unsigned)y < DISPLAY_HEIGHT) {
80    fb[y * DISPLAY_WIDTH + x] = c;
81  }
82}
83
84void clearFB(uint16_t color = COL_BLACK) {
85  for (int i = 0; i < DISPLAY_WIDTH * DISPLAY_HEIGHT; i++) fb[i] = color;
86}
87
88void drawLine(int x0, int y0, int x1, int y1, uint16_t col) {
89  int dx = abs(x1 - x0), sx = x0 < x1 ? 1 : -1;
90  int dy = -abs(y1 - y0), sy = y0 < y1 ? 1 : -1;
91  int err = dx + dy;
92
93  while (true) {
94    putpix(x0, y0, col);
95    if (x0 == x1 && y0 == y1) break;
96
97    int e2 = 2 * err;
98    if (e2 >= dy) { err += dy; x0 += sx; }
99    if (e2 <= dx) { err += dx; y0 += sy; }
100  }
101}
102
103void drawThickLine(int x0, int y0, int x1, int y1, uint16_t col, int t) {
104  for (int i = -t / 2; i <= t / 2; i++) {
105    drawLine(x0, y0 + i, x1, y1 + i, col);
106  }
107}
108
109void drawCircle(int cx, int cy, int r, uint16_t col) {
110  int x = r, y = 0, err = 0;
111
112  while (x >= y) {
113    putpix(cx + x, cy + y, col);
114    putpix(cx + y, cy + x, col);
115    putpix(cx - y, cy + x, col);
116    putpix(cx - x, cy + y, col);
117    putpix(cx - x, cy - y, col);
118    putpix(cx - y, cy - x, col);
119    putpix(cx + y, cy - x, col);
120    putpix(cx + x, cy - y, col);
121
122    y++;
123    if (err <= 0) err += 2 * y + 1;
124    if (err > 0) {
125      x--;
126      err -= 2 * x + 1;
127    }
128  }
129}
130
131void fillCircle(int cx, int cy, int r, uint16_t col) {
132
133  int r2 = r * r;
134
135  for (int y = -r; y <= r; y++) {
136    for (int x = -r; x <= r; x++) {
137
138      if (x * x + y * y <= r2) {
139        putpix(cx + x, cy + y, col);
140      }
141    }
142  }
143}
144
145void fillTriangle(int x1, int y1, int x2, int y2, int x3, int y3, uint16_t col) {
146  int minX = min(x1, min(x2, x3));
147  int maxX = max(x1, max(x2, x3));
148  int minY = min(y1, min(y2, y3));
149  int maxY = max(y1, max(y2, y3));
150
151  int area = (x2 - x1) * (y3 - y1) - (y2 - y1) * (x3 - x1);
152
153  for (int y = minY; y <= maxY; y++) {
154    for (int x = minX; x <= maxX; x++) {
155      int w1 = (x2 - x1) * (y - y1) - (y2 - y1) * (x - x1);
156      int w2 = (x3 - x2) * (y - y2) - (y3 - y2) * (x - x2);
157      int w3 = (x1 - x3) * (y - y3) - (y1 - y3) * (x - x3);
158
159      if ((area >= 0 && w1 >= 0 && w2 >= 0 && w3 >= 0) ||
160          (area < 0 && w1 <= 0 && w2 <= 0 && w3 <= 0)) {
161        putpix(x, y, col);
162      }
163    }
164  }
165}
166
167void fillRing(int cx, int cy, int rInner, int rOuter, uint16_t col) {
168  int ri2 = rInner * rInner;
169  int ro2 = rOuter * rOuter;
170
171  for (int y = -rOuter; y <= rOuter; y++) {
172    for (int x = -rOuter; x <= rOuter; x++) {
173      int d2 = x * x + y * y;
174      if (d2 >= ri2 && d2 <= ro2) {
175        putpix(cx + x, cy + y, col);
176      }
177    }
178  }
179}
180
181void fillArtificialHorizonDisc() {
182  const int R = 206;
183  const int r2 = R * R;
184
185  float rollRad = rollDeg * PI / 180.0f;
186
187  float cr = cos(rollRad);
188  float sr = sin(rollRad);
189
190  float pitchOffset = pitchDeg * 4.0f;
191
192  // sky / ground
193  for (int y = -R; y <= R; y++) {
194    for (int x = -R; x <= R; x++) {
195
196      if (x * x + y * y <= r2) {
197
198        float yr = x * sr + y * cr;
199
200        if (yr + pitchOffset < 0)
201          putpix(CX + x, CY + y, COL_SKY);
202        else
203          putpix(CX + x, CY + y, COL_GROUND);
204      }
205    }
206  }
207
208  // helper transform
209  auto H = [&](float lx, float ly, int &sx, int &sy) {
210
211    sx = CX + lx * cr + ly * sr;
212    sy = CY - lx * sr + ly * cr;
213  };
214
215  // --------------------------------------------------
216  // VIRTUAL RUNWAY
217  // --------------------------------------------------
218
219  {
220    int x1, y1, x2, y2, x3, y3;
221
222    H(0, 10 - pitchOffset, x1, y1);
223    H(-95, 115 - pitchOffset, x2, y2);
224    H(95, 115 - pitchOffset, x3, y3);
225
226    fillTriangle(
227      x1, y1,
228      x2, y2,
229      x3, y3,
230      rgb565(80, 38, 25)
231    );
232
233    int xa, ya, xb, yb;
234
235    H(0, 10 - pitchOffset, xa, ya);
236    H(-32, 115 - pitchOffset, xb, yb);
237    drawThickLine(xa, ya, xb, yb, COL_ORANGE, 2);
238
239    H(0, 10 - pitchOffset, xa, ya);
240    H(32, 115 - pitchOffset, xb, yb);
241    drawThickLine(xa, ya, xb, yb, COL_ORANGE, 2);
242
243    H(-32, 115 - pitchOffset, xa, ya);
244    H(32, 115 - pitchOffset, xb, yb);
245    drawThickLine(xa, ya, xb, yb, COL_ORANGE, 2);
246  }
247
248  // --------------------------------------------------
249  // MAIN HORIZON LINE
250  // --------------------------------------------------
251
252  {
253    int x1, y1, x2, y2;
254
255    H(-R, -pitchOffset, x1, y1);
256    H(R,  -pitchOffset, x2, y2);
257
258    drawThickLine(x1, y1, x2, y2, COL_WHITE, 3);
259  }
260
261  // --------------------------------------------------
262  // PITCH LADDER
263  // --------------------------------------------------
264
265  int step = 32;
266
267  for (int i = -4; i <= 4; i++) {
268
269    if (i == 0) continue;
270
271    int value = abs(i) * 5;
272
273    int len =
274      (value == 10 || value == 20)
275      ? 120
276      : 58;
277
278    int localY = i * step - pitchOffset;
279
280    int x1, y1, x2, y2;
281
282    H(-len / 2, localY, x1, y1);
283    H( len / 2, localY, x2, y2);
284
285    drawThickLine(x1, y1, x2, y2, COL_WHITE, 3);
286
287    if (value == 10 || value == 20) {
288
289      int xl, yl;
290      int xr, yr;
291
292      H(-len / 2 - 50, localY - 12, xl, yl);
293      H( len / 2 + 24, localY - 12, xr, yr);
294
295      drawSmallNumber(value, xl, yl, COL_WHITE);
296      drawSmallNumber(value, xr, yr, COL_WHITE);
297    }
298  }
299}
300
301void drawSegDigit(int d, int x, int y, int s, uint16_t col) {
302  bool seg[7];
303
304  switch (d) {
305    case 0: seg[0]=1; seg[1]=1; seg[2]=1; seg[3]=1; seg[4]=1; seg[5]=1; seg[6]=0; break;
306    case 1: seg[0]=0; seg[1]=1; seg[2]=1; seg[3]=0; seg[4]=0; seg[5]=0; seg[6]=0; break;
307    case 2: seg[0]=1; seg[1]=1; seg[2]=0; seg[3]=1; seg[4]=1; seg[5]=0; seg[6]=1; break;
308    case 3: seg[0]=1; seg[1]=1; seg[2]=1; seg[3]=1; seg[4]=0; seg[5]=0; seg[6]=1; break;
309    case 4: seg[0]=0; seg[1]=1; seg[2]=1; seg[3]=0; seg[4]=0; seg[5]=1; seg[6]=1; break;
310    case 5: seg[0]=1; seg[1]=0; seg[2]=1; seg[3]=1; seg[4]=0; seg[5]=1; seg[6]=1; break;
311    case 6: seg[0]=1; seg[1]=0; seg[2]=1; seg[3]=1; seg[4]=1; seg[5]=1; seg[6]=1; break;
312    case 7: seg[0]=1; seg[1]=1; seg[2]=1; seg[3]=0; seg[4]=0; seg[5]=0; seg[6]=0; break;
313    case 8: seg[0]=1; seg[1]=1; seg[2]=1; seg[3]=1; seg[4]=1; seg[5]=1; seg[6]=1; break;
314    case 9: seg[0]=1; seg[1]=1; seg[2]=1; seg[3]=1; seg[4]=0; seg[5]=1; seg[6]=1; break;
315  }
316
317  int w = 5 * s;
318  int h = 9 * s;
319  int t = s;
320
321  if (seg[0]) drawThickLine(x, y, x + w, y, col, t);
322  if (seg[1]) drawThickLine(x + w, y, x + w, y + h / 2, col, t);
323  if (seg[2]) drawThickLine(x + w, y + h / 2, x + w, y + h, col, t);
324  if (seg[3]) drawThickLine(x, y + h, x + w, y + h, col, t);
325  if (seg[4]) drawThickLine(x, y + h / 2, x, y + h, col, t);
326  if (seg[5]) drawThickLine(x, y, x, y + h / 2, col, t);
327  if (seg[6]) drawThickLine(x, y + h / 2, x + w, y + h / 2, col, t);
328}
329
330void drawSmallNumber(int value, int x, int y, uint16_t col) {
331  if (value == 10) {
332    drawSegDigit(1, x, y, 2, col);
333    drawSegDigit(0, x + 14, y, 2, col);
334  }
335
336  if (value == 20) {
337    drawSegDigit(2, x, y, 2, col);
338    drawSegDigit(0, x + 14, y, 2, col);
339  }
340}
341
342void drawPitchScaleLine(int y, int value, uint16_t col) {
343  int longLen  = 120;
344  int shortLen = 58;
345
346  bool major = (value == 10 || value == 20);
347  int len = major ? longLen : shortLen;
348
349  drawThickLine(CX - len / 2, y, CX + len / 2, y, col, 3);
350
351  if (value == 10 || value == 20) {
352    drawSmallNumber(value, CX - len / 2 - 50, y - 12, col);
353    drawSmallNumber(value, CX + len / 2 + 24, y - 12, col);
354  }
355}
356
357void drawPitchLadder() {
358  int step = 32;
359
360  float rollRad = rollDeg * PI / 180.0f;
361  float cr = cos(rollRad);
362  float sr = sin(rollRad);
363
364  float pitchOffset = pitchDeg * 4.0f;
365
366  for (int i = -4; i <= 4; i++) {
367    if (i == 0) continue;
368
369    int value = abs(i) * 5;
370    int len = (value == 10 || value == 20) ? 120 : 58;
371
372    float localY = i * step - pitchOffset;
373
374    float x1r = -len / 2;
375    float y1r = localY;
376    float x2r =  len / 2;
377    float y2r = localY;
378
379    int x1 = CX + x1r * cr - y1r * sr;
380    int y1 = CY + x1r * sr + y1r * cr;
381    int x2 = CX + x2r * cr - y2r * sr;
382    int y2 = CY + x2r * sr + y2r * cr;
383
384    drawThickLine(x1, y1, x2, y2, COL_WHITE, 3);
385  }
386}
387
388void drawRotatedLine(float x1, float y1, float x2, float y2, float angleDeg, uint16_t col, int thick) {
389  float a = angleDeg * PI / 180.0f;
390  float cr = cos(a);
391  float sr = sin(a);
392
393  int sx1 = CX + x1 * cr - y1 * sr;
394  int sy1 = CY + x1 * sr + y1 * cr;
395  int sx2 = CX + x2 * cr - y2 * sr;
396  int sy2 = CY + x2 * sr + y2 * cr;
397
398  drawThickLine(sx1, sy1, sx2, sy2, col, thick);
399}
400
401void drawScreen_AirplaneDemo() {
402  clearFB(rgb565(10, 18, 28));
403
404// light gray frame like other instruments
405fillRing(CX, CY, 229, 239, COL_RING_OUTER);
406fillRing(CX, CY, 218, 228, COL_RING_INNER);
407
408drawCircle(CX, CY, 217, COL_RING_EDGE);
409drawCircle(CX, CY, 228, COL_RING_EDGE);
410drawCircle(CX, CY, 239, COL_WHITE);
411
412// inner dark display area
413fillCircle(CX, CY, 217, rgb565(10, 18, 28));
414
415  // background reference grid
416  drawCircle(CX, CY, 210, rgb565(80, 80, 80));
417  drawCircle(CX, CY, 140, rgb565(50, 50, 50));
418  drawThickLine(CX - 210, CY, CX + 210, CY, rgb565(60, 60, 60), 1);
419  drawThickLine(CX, CY - 210, CX, CY + 210, rgb565(60, 60, 60), 1);
420
421  // pitch moves airplane slightly up/down
422  int oldCY = CY;
423  int pitchMove = constrain((int)(pitchDeg * 3.0f), -90, 90);
424
425  // local center offset
426  int baseY = CY + pitchMove;
427
428  float roll = rollDeg;
429
430  auto L = [&](float x1, float y1, float x2, float y2, uint16_t col, int t) {
431    float a = roll * PI / 180.0f;
432    float cr = cos(a);
433    float sr = sin(a);
434
435    int sx1 = CX + x1 * cr - (y1 + pitchMove) * sr;
436    int sy1 = CY + x1 * sr + (y1 + pitchMove) * cr;
437    int sx2 = CX + x2 * cr - (y2 + pitchMove) * sr;
438    int sy2 = CY + x2 * sr + (y2 + pitchMove) * cr;
439
440    drawThickLine(sx1, sy1, sx2, sy2, col, t);
441  };
442
443  uint16_t bodyCol = rgb565(255, 180, 40);
444  uint16_t wingCol = rgb565(40, 220, 255);
445  uint16_t tailCol = rgb565(255, 80, 80);
446
447  // airplane body
448  L(0, -120, 0, 110, bodyCol, 8);
449
450  // nose
451  L(0, -120, -22, -75, bodyCol, 5);
452  L(0, -120,  22, -75, bodyCol, 5);
453
454  // main wings
455  L(-20, -25, -145, 35, wingCol, 8);
456  L( 20, -25,  145, 35, wingCol, 8);
457
458  // wing tips
459  L(-145, 35, -120, 55, wingCol, 5);
460  L( 145, 35,  120, 55, wingCol, 5);
461
462  // tail wings
463  L(-15, 80, -75, 125, tailCol, 6);
464  L( 15, 80,  75, 125, tailCol, 6);
465
466  // center dot
467  fillCircle(CX, CY + pitchMove, 8, COL_WHITE);
468
469  gfx->draw16bitRGBBitmap(0, 0, fb, DISPLAY_WIDTH, DISPLAY_HEIGHT);
470}
471
472void drawRollScaleOnRing() {
473  for (int a = -50; a <= 50; a += 10) {
474    float rad = (a - 90) * PI / 180.0;
475
476    int r1 = 211;
477    int r2 = 224;
478
479    if (a % 30 == 0) r1 = 207;
480
481    int x1 = CX + cos(rad) * r1;
482    int y1 = CY + sin(rad) * r1;
483    int x2 = CX + cos(rad) * r2;
484    int y2 = CY + sin(rad) * r2;
485
486    drawThickLine(x1, y1, x2, y2, COL_WHITE, 3);
487  }
488
489  // smaller static orange triangle
490  fillTriangle(CX, CY - 210, CX - 11, CY - 229, CX + 11, CY - 229, COL_ORANGE);
491
492  // smaller moving white triangle
493float rollRad = rollDeg * PI / 180.0f;
494float cr = cos(rollRad);
495float sr = sin(rollRad);
496
497auto ROLL = [&](float lx, float ly, int &sx, int &sy) {
498  sx = CX + lx * cr + ly * sr;
499  sy = CY - lx * sr + ly * cr;
500};
501
502int x1, y1, x2, y2, x3, y3;
503
504ROLL(0, -200, x1, y1);
505ROLL(-12, -181, x2, y2);
506ROLL(12, -181, x3, y3);
507
508fillTriangle(x1, y1, x2, y2, x3, y3, COL_WHITE);
509}
510
511void drawVirtualRunway() {
512  // dark runway body
513  fillTriangle(
514    CX, CY + 10,
515    CX - 95, CY + 115,
516    CX + 95, CY + 115,
517    rgb565(80, 38, 25)
518  );
519
520  // orange runway perspective lines
521  drawThickLine(CX, CY + 10, CX - 32, CY + 115, COL_ORANGE, 2);
522  drawThickLine(CX, CY + 10, CX + 32, CY + 115, COL_ORANGE, 2);
523  drawThickLine(CX - 32, CY + 115, CX + 32, CY + 115, COL_ORANGE, 2);
524}
525
526void drawFrameLetter(char c, int x, int y, int s, uint16_t col) {
527  switch (c) {
528    case 'N':
529      drawThickLine(x, y + 28*s, x, y, col, s);
530      drawThickLine(x, y, x + 18*s, y + 28*s, col, s);
531      drawThickLine(x + 18*s, y + 28*s, x + 18*s, y, col, s);
532      break;
533
534    case 'E':
535      drawThickLine(x, y, x, y + 28*s, col, s);
536      drawThickLine(x, y, x + 18*s, y, col, s);
537      drawThickLine(x, y + 14*s, x + 15*s, y + 14*s, col, s);
538      drawThickLine(x, y + 28*s, x + 18*s, y + 28*s, col, s);
539      break;
540
541    case 'S':
542      drawThickLine(x + 18*s, y, x, y, col, s);
543      drawThickLine(x, y, x, y + 14*s, col, s);
544      drawThickLine(x, y + 14*s, x + 18*s, y + 14*s, col, s);
545      drawThickLine(x + 18*s, y + 14*s, x + 18*s, y + 28*s, col, s);
546      drawThickLine(x + 18*s, y + 28*s, x, y + 28*s, col, s);
547      break;
548
549    case 'W':
550      drawThickLine(x, y, x + 4*s, y + 28*s, col, s);
551      drawThickLine(x + 4*s, y + 28*s, x + 9*s, y + 12*s, col, s);
552      drawThickLine(x + 9*s, y + 12*s, x + 14*s, y + 28*s, col, s);
553      drawThickLine(x + 14*s, y + 28*s, x + 18*s, y, col, s);
554      break;
555  }
556}
557
558void drawFrameNumberText(const char *txt, int x, int y, int s, uint16_t col) {
559  for (int i = 0; txt[i]; i++) {
560    int dx = i * 14 * s;
561    drawSegDigit(txt[i] - '0', x + dx, y, s, col);
562  }
563}
564
565void drawCompassText(const char *txt, int cx, int cy, int s, uint16_t col) {
566  int len = strlen(txt);
567  int w;
568
569  if (txt[0] >= '0' && txt[0] <= '9') {
570    w = len * 14 * s;
571    drawFrameNumberText(txt, cx - w / 2, cy - 9 * s, s, col);
572  } else {
573    w = 20 * s;
574    drawFrameLetter(txt[0], cx - w / 2, cy - 14 * s, s, col);
575  }
576}
577
578void drawAircraftSymbol() {
579  // longer and thicker horizontal yellow wings
580  drawThickLine(CX - 140, CY, CX - 35, CY, COL_YELLOW, 8);
581  drawThickLine(CX + 35, CY, CX + 140, CY, COL_YELLOW, 8);
582
583  // thicker central inverted V
584  drawThickLine(CX - 35, CY, CX, CY + 28, COL_YELLOW, 8);
585  drawThickLine(CX, CY + 28, CX + 35, CY, COL_YELLOW, 8);
586
587  // small center point / hub
588  fillTriangle(CX, CY - 4, CX - 7, CY + 6, CX + 7, CY + 6, COL_YELLOW);
589}
590
591void drawConcentricBezel() {
592  // dark ring is thin, and horizon disc touches it directly
593  fillRing(CX, CY, 207, 224, COL_RING_INNER);
594
595  // bright outer ring
596  fillRing(CX, CY, 225, 239, COL_RING_OUTER);
597
598  drawCircle(CX, CY, 206, COL_RING_EDGE);
599  drawCircle(CX, CY, 224, COL_RING_EDGE);
600  drawCircle(CX, CY, 239, COL_WHITE);
601}
602
603void drawCompassDot(int x, int y, uint16_t col) {
604  fillCircle(x, y, 2, col);
605}
606
607void drawCompassTicks(float headingDeg = 0) {
608  const int R_OUT  = 216;  // речиси до сивиот обрач
609  const int R_IN   = 194;  // подолги црти
610  const int R_DOT  = 194;  // точки на средина од цртите
611
612  // 36 црти = на секои 10 степени
613  // помеѓу N и E има точно 9 црти: 10,20,30,40,50,60,70,80,90
614  for (int deg = 0; deg < 360; deg += 10) {
615    float a = (deg - headingDeg - 90) * PI / 180.0;
616
617    int x1 = CX + cos(a) * R_IN;
618    int y1 = CY + sin(a) * R_IN;
619    int x2 = CX + cos(a) * R_OUT;
620    int y2 = CY + sin(a) * R_OUT;
621
622    drawThickLine(x1, y1, x2, y2, COL_WHITE, 3);
623  }
624
625  // дискретни точки точно на средина помеѓу секои две црти
626  // значи на 5,15,25...
627  for (int deg = 5; deg < 360; deg += 10) {
628    float a = (deg - headingDeg - 90) * PI / 180.0;
629
630    int x = CX + cos(a) * R_DOT;
631    int y = CY + sin(a) * R_DOT;
632
633    fillCircle(x, y, 2, COL_WHITE);
634  }
635}
636void drawCompassLetters(float headingDeg = 0) {
637  struct Mark {
638    int deg;
639    const char* txt;
640    uint16_t col;
641    int scale;
642    int radius;
643  };
644
645  Mark marks[] = {
646    // smaller N/E/S/W
647    {0,   "N",  COL_YELLOW, 1, 158},
648    {90,  "E",  COL_YELLOW, 1, 158},
649    {180, "S",  COL_YELLOW, 1, 158},
650    {270, "W",  COL_YELLOW, 1, 158},
651
652    // degree labels
653    {30,  "3",  COL_WHITE, 2, 160},
654    {60,  "6",  COL_WHITE, 2, 160},
655    {120, "12", COL_WHITE, 2, 160},
656    {150, "15", COL_WHITE, 2, 165},
657    {210, "21", COL_WHITE, 2, 155},
658    {240, "24", COL_WHITE, 2, 150},
659    {300, "30", COL_WHITE, 2, 150},
660    {330, "33", COL_WHITE, 2, 150}
661  };
662
663  for (int i = 0; i < 12; i++) {
664    float a = (marks[i].deg - headingDeg - 90) * PI / 180.0;
665
666    int x = CX + cos(a) * marks[i].radius;
667    int y = CY + sin(a) * marks[i].radius;
668
669    drawCompassText(marks[i].txt, x, y, marks[i].scale, marks[i].col);
670  }
671}
672
673void drawCompassAirplane() {
674  // static yellow airplane symbol
675  uint16_t c = COL_ORANGE;
676
677  // nose / fuselage
678 drawThickLine(CX, CY - 194, CX, CY + 65, c, 5);
679
680  // nose sides
681  drawThickLine(CX, CY - 130, CX - 28, CY - 40, c, 4);
682  drawThickLine(CX, CY - 130, CX + 28, CY - 40, c, 4);
683
684  // wings
685  drawThickLine(CX - 28, CY - 40, CX - 88, CY + 10, c, 4);
686  drawThickLine(CX + 28, CY - 40, CX + 88, CY + 10, c, 4);
687  drawThickLine(CX - 88, CY + 10, CX - 88, CY + 35, c, 4);
688  drawThickLine(CX + 88, CY + 10, CX + 88, CY + 35, c, 4);
689  drawThickLine(CX - 88, CY + 35, CX - 20, CY + 10, c, 4);
690  drawThickLine(CX + 88, CY + 35, CX + 20, CY + 10, c, 4);
691
692  // body lower part
693  drawThickLine(CX - 20, CY + 10, CX - 20, CY + 85, c, 4);
694  drawThickLine(CX + 20, CY + 10, CX + 20, CY + 85, c, 4);
695
696  // tail
697  drawThickLine(CX - 20, CY + 85, CX - 55, CY + 110, c, 4);
698  drawThickLine(CX + 20, CY + 85, CX + 55, CY + 110, c, 4);
699  drawThickLine(CX - 55, CY + 110, CX - 55, CY + 130, c, 4);
700  drawThickLine(CX + 55, CY + 110, CX + 55, CY + 130, c, 4);
701  drawThickLine(CX - 55, CY + 130, CX, CY + 108, c, 4);
702  drawThickLine(CX + 55, CY + 130, CX, CY + 108, c, 4);
703}
704
705void drawScreen_Compass() {
706  clearFB(COL_BLACK);
707
708  // thinner gray frame - половина од претходната дебелина
709  fillRing(CX, CY, 229, 239, COL_RING_OUTER);
710  fillRing(CX, CY, 218, 228, COL_RING_INNER);
711
712  drawCircle(CX, CY, 217, COL_RING_EDGE);
713  drawCircle(CX, CY, 228, COL_RING_EDGE);
714  drawCircle(CX, CY, 239, COL_WHITE);
715
716  fillCircle(CX, CY, 217, rgb565(42, 50, 52));
717
718  float heading = compassHeadingDeg;
719
720  drawCompassTicks(heading);
721  drawCompassLetters(heading);
722  drawCompassAirplane();
723
724  gfx->draw16bitRGBBitmap(0, 0, fb, DISPLAY_WIDTH, DISPLAY_HEIGHT);
725}
726
727void drawScreen_AttitudeIndicator() {
728  
729  clearFB(COL_BLACK);
730
731  drawConcentricBezel();
732
733  fillArtificialHorizonDisc();
734
735  drawAircraftSymbol();
736  drawRollScaleOnRing();
737
738  gfx->draw16bitRGBBitmap(0, 0, fb, DISPLAY_WIDTH, DISPLAY_HEIGHT);
739}
740
741void drawAltimeterNumbers() {
742  const int R_NUM = 155;
743
744  for (int n = 0; n <= 9; n++) {
745    float deg = n * 36.0;
746    float a = (deg - 90) * PI / 180.0;
747
748    int x = CX + cos(a) * R_NUM;
749    int y = CY + sin(a) * R_NUM;
750
751    drawSegDigit(n, x - 8, y - 14, 3, COL_WHITE);
752  }
753}
754
755void drawAltimeterTicks() {
756  const int R_OUT = 205;
757  const int R_IN_MAJOR = 178;
758  const int R_IN_MINOR = 192;
759
760  for (int i = 0; i < 100; i++) {
761    float deg = i * 3.6;
762    float a = (deg - 90) * PI / 180.0;
763
764    bool major = (i % 10 == 0);
765    bool medium = (i % 5 == 0);
766
767    int r1 = major ? R_IN_MAJOR : (medium ? 185 : R_IN_MINOR);
768    int r2 = R_OUT;
769
770    int x1 = CX + cos(a) * r1;
771    int y1 = CY + sin(a) * r1;
772    int x2 = CX + cos(a) * r2;
773    int y2 = CY + sin(a) * r2;
774
775    drawThickLine(x1, y1, x2, y2, COL_WHITE, major ? 4 : 2);
776  }
777}
778
779void drawAltimeterText() {
780  // simple static text with lines, framebuffer-safe
781  // ALT
782// ALT
783drawFrameLetter('A', CX - 48, CY - 78, 1, COL_WHITE);
784
785// L
786drawThickLine(CX - 10, CY - 78, CX - 10, CY - 50, COL_WHITE, 1);
787drawThickLine(CX - 10, CY - 50, CX + 8, CY - 50, COL_WHITE, 1);
788
789// T
790drawFrameLetter('T', CX + 28, CY - 78, 1, COL_WHITE);
791
792  // small “x1000 ft” imitation
793  drawThickLine(CX - 28, CY - 38, CX + 28, CY - 38, COL_WHITE, 1);
794  drawThickLine(CX - 18, CY - 30, CX + 18, CY - 30, COL_WHITE, 1);
795}
796
797void drawAltimeterHand(float angleDeg, int length, int width, uint16_t col) {
798  float a = (angleDeg - 90) * PI / 180.0;
799
800  int tipX = CX + cos(a) * length;
801  int tipY = CY + sin(a) * length;
802
803  float px = -sin(a);
804  float py = cos(a);
805
806  int leftX  = CX + px * width;
807  int leftY  = CY + py * width;
808  int rightX = CX - px * width;
809  int rightY = CY - py * width;
810
811  fillTriangle(leftX, leftY, rightX, rightY, tipX, tipY, col);
812}
813
814void drawAltimeterHands(int altitudeFt) {
815
816  // LONG thin hand
817  // one full rotation = 1000 ft
818  float longAngle =
819      ((altitudeFt % 1000) / 1000.0f) * 360.0f;
820
821  // SHORT thick hand
822  // one full rotation = 10000 ft
823  float shortAngle =
824      ((altitudeFt % 10000) / 10000.0f) * 360.0f;
825
826  // short thick hand
827  drawAltimeterHand(shortAngle, 95, 11, COL_WHITE);
828
829  // long thin hand
830  drawAltimeterHand(longAngle, 175, 4, COL_WHITE);
831
832  fillCircle(CX, CY, 14, rgb565(120,120,120));
833  fillCircle(CX, CY, 7, COL_WHITE);
834}
835
836void drawAltimeterSmallWindow() {
837  // small striped reference window at bottom, like aircraft altimeters
838  int x0 = CX - 42;
839  int y0 = CY + 72;
840
841 fillCircle(CX, CY + 88, 38, rgb565(25, 25, 25));
842
843  for (int i = 0; i < 5; i++) {
844    drawThickLine(x0 + i * 16, y0 + 35, x0 + i * 16 + 35, y0, COL_WHITE, 5);
845  }
846}
847
848void drawScreen_Altimeter() {
849  clearFB(COL_BLACK);
850
851  // concentric frame, same style as compass
852  fillRing(CX, CY, 229, 239, COL_RING_OUTER);
853  fillRing(CX, CY, 218, 228, COL_RING_INNER);
854
855  drawCircle(CX, CY, 217, COL_RING_EDGE);
856  drawCircle(CX, CY, 228, COL_RING_EDGE);
857  drawCircle(CX, CY, 239, COL_WHITE);
858
859  // dial background
860  fillCircle(CX, CY, 217, rgb565(25, 28, 30));
861  fillCircle(CX, CY, 105, rgb565(36, 40, 42));
862
863  drawAltimeterTicks();
864  drawAltimeterNumbers();
865  drawAltimeterText();
866  drawAltimeterSmallWindow();
867
868  
869  int altitudeDisplay = (int)altitudeFt;
870
871  drawAltimeterHands(altitudeDisplay);
872
873  gfx->draw16bitRGBBitmap(0, 0, fb, DISPLAY_WIDTH, DISPLAY_HEIGHT);
874
875  
876}
877
878void drawCurrentScreen() {
879  switch (currentScreen) {
880    case SCREEN_ATTITUDE:
881      drawScreen_AttitudeIndicator();
882      break;
883
884    case SCREEN_COMPASS:
885      drawScreen_Compass();
886      break;
887
888    case SCREEN_ALTIMETER:
889      drawScreen_Altimeter();
890      break;
891
892   case SCREEN_AIRPLANE:
893      drawScreen_AirplaneDemo();
894      break;     
895
896    default:
897      drawScreen_AttitudeIndicator();
898      break;
899  }
900}  
901
902void init_display() {
903 pinMode(BL_PIN, OUTPUT);
904ledcAttach(BL_PIN, 5000, 8);
905ledcWrite(BL_PIN, brightnessLevel);
906
907  panelBus = new Arduino_SWSPI(
908    GFX_NOT_DEFINED, PANEL_CS, PANEL_SCK, PANEL_SDA, GFX_NOT_DEFINED
909  );
910
911  rgbpanel = new Arduino_ESP32RGBPanel(
912      40,7,15,41,
913      46,3,8,18,17,
914      14,13,12,11,10,9,
915      5,45,48,47,21,
916      1,50,10,50,
917      1,30,10,30,
918      PCLK_NEG,6000000UL
919  );
920
921  gfx = new Arduino_RGB_Display(
922    DISPLAY_WIDTH, DISPLAY_HEIGHT, rgbpanel, 0, true,
923    panelBus, GFX_NOT_DEFINED,
924    st7701_type7_init_operations, sizeof(st7701_type7_init_operations)
925  );
926
927  gfx->begin(8000000);
928
929  fb = (uint16_t*)ps_malloc(DISPLAY_WIDTH * DISPLAY_HEIGHT * 2);
930  if (!fb) {
931    Serial.println("Framebuffer allocation failed!");
932    while (1);
933  }
934}
935
936void pcf8574_init() {
937  Wire.begin(I2C_SDA, I2C_SCL);
938
939  Wire.beginTransmission(PCF8574_ADDR);
940  Wire.write(0xFF);        // all pins high = inputs with pullups
941  Wire.endTransmission();
942}
943
944uint8_t pcf8574_read() {
945  Wire.requestFrom(PCF8574_ADDR, (uint8_t)1);
946
947  if (Wire.available()) {
948    return Wire.read();
949  }
950
951  return 0xFF;
952}
953
954bool isEncoderButtonPressed() {
955  uint8_t state = pcf8574_read();
956
957  // P5 low = button pressed
958  return !(state & (1 << 5));
959}
960
961bool mpu6050_init() {
962  Wire.beginTransmission(MPU6050_ADDR);
963  Wire.write(0x6B);      // PWR_MGMT_1
964  Wire.write(0x00);      // wake up
965  if (Wire.endTransmission() != 0) return false;
966
967  delay(100);
968
969  // accelerometer ±2g
970  Wire.beginTransmission(MPU6050_ADDR);
971  Wire.write(0x1C);
972  Wire.write(0x00);
973  Wire.endTransmission();
974
975  return true;
976}
977
978bool mpu6050_readAccel(float &ax, float &ay, float &az) {
979  Wire.beginTransmission(MPU6050_ADDR);
980  Wire.write(0x3B); // ACCEL_XOUT_H
981  if (Wire.endTransmission(false) != 0) return false;
982
983  Wire.requestFrom(MPU6050_ADDR, (uint8_t)6);
984  if (Wire.available() < 6) return false;
985
986  int16_t rawX = (Wire.read() << 8) | Wire.read();
987  int16_t rawY = (Wire.read() << 8) | Wire.read();
988  int16_t rawZ = (Wire.read() << 8) | Wire.read();
989
990  ax = rawX / 16384.0f;
991  ay = rawY / 16384.0f;
992  az = rawZ / 16384.0f;
993
994  return true;
995}
996
997void updateMPU6050() {
998  float ax, ay, az;
999
1000  if (!mpu6050_readAccel(ax, ay, az)) return;
1001
1002  float newRoll  = atan2(ay, az) * 180.0f / PI;
1003  float newPitch = atan2(-ax, sqrt(ay * ay + az * az)) * 180.0f / PI;
1004
1005  // smoothing
1006  rollDeg  = rollDeg  * 0.85f + newRoll  * 0.15f;
1007  pitchDeg = pitchDeg * 0.85f + newPitch * 0.15f;
1008}
1009
1010bool qmc5883l_init() {
1011  Wire.beginTransmission(QMC5883L_ADDR);
1012  Wire.write(0x0B);      // SET/RESET period register
1013  Wire.write(0x01);
1014  Wire.endTransmission();
1015
1016  Wire.beginTransmission(QMC5883L_ADDR);
1017  Wire.write(0x09);      // control register
1018  Wire.write(0x1D);      // continuous, 200Hz, 2G, 512 OSR
1019  if (Wire.endTransmission() != 0) return false;
1020
1021  return true;
1022}
1023
1024bool qmc5883l_readHeading(float &headingDeg) {
1025  Wire.beginTransmission(QMC5883L_ADDR);
1026  Wire.write(0x00);
1027  if (Wire.endTransmission(false) != 0) return false;
1028
1029  Wire.requestFrom(QMC5883L_ADDR, (uint8_t)6);
1030  if (Wire.available() < 6) return false;
1031
1032  int16_t x = Wire.read() | (Wire.read() << 8);
1033  int16_t y = Wire.read() | (Wire.read() << 8);
1034  int16_t z = Wire.read() | (Wire.read() << 8);
1035
1036  float heading = atan2((float)y, (float)x) * 180.0f / PI;
1037
1038  if (heading < 0) heading += 360.0f;
1039  if (heading >= 360) heading -= 360.0f;
1040
1041  headingDeg = heading;
1042
1043  Serial.print("QMC X=");
1044  Serial.print(x);
1045  Serial.print(" Y=");
1046  Serial.print(y);
1047  Serial.print(" Z=");
1048  Serial.print(z);
1049  Serial.print(" Heading=");
1050  Serial.println(headingDeg);
1051
1052  return true;
1053}
1054
1055void updateQMC5883L() {
1056  float h;
1057
1058  if (!qmc5883l_readHeading(h)) {
1059    Serial.println("QMC5883L read failed");
1060    return;
1061  }
1062
1063  float diff = h - compassHeadingDeg;
1064
1065  if (diff > 180.0f)  diff -= 360.0f;
1066  if (diff < -180.0f) diff += 360.0f;
1067
1068  compassHeadingDeg += diff * 0.15f;
1069
1070  if (compassHeadingDeg < 0) compassHeadingDeg += 360.0f;
1071  if (compassHeadingDeg >= 360.0f) compassHeadingDeg -= 360.0f;
1072}
1073
1074void updateSimulatedAltitude() {
1075  unsigned long now = millis();
1076
1077  if (lastAltitudeUpdate == 0) {
1078    lastAltitudeUpdate = now;
1079    return;
1080  }
1081
1082  float dt = (now - lastAltitudeUpdate) / 1000.0f;
1083  lastAltitudeUpdate = now;
1084
1085  // dead zone за да не “плива” кога е речиси рамно
1086  float p = pitchDeg;
1087
1088  if (abs(p) < 2.0f) {
1089    p = 0;
1090  }
1091
1092  // брзина на промена на висина
1093  // 1 степен pitch ≈ 12 ft/sec
1094  float climbRateFtPerSec = -p * 12.0f;
1095
1096  altitudeFt += climbRateFtPerSec * dt;
1097
1098  // ограничување
1099  if (altitudeFt < 0) altitudeFt = 0;
1100  if (altitudeFt > 9999) altitudeFt = 9999;
1101}
1102
1103void IRAM_ATTR updateEncoder() {
1104  int MSB = digitalRead(ENCODER_CLK);
1105  int LSB = digitalRead(ENCODER_DT);
1106
1107  int encoded = (MSB << 1) | LSB;
1108  int sum = (lastEncoded << 2) | encoded;
1109
1110  if (sum == 0b1101 || sum == 0b0100 || sum == 0b0010 || sum == 0b1011)
1111    encoderValue++;
1112
1113  if (sum == 0b1110 || sum == 0b0111 || sum == 0b0001 || sum == 0b1000)
1114    encoderValue--;
1115
1116  lastEncoded = encoded;
1117}
1118
1119void handleBrightnessEncoder() {
1120  noInterrupts();
1121  long currentValue = encoderValue;
1122  interrupts();
1123
1124  if (currentValue != lastEncoderValue) {
1125    int diff = currentValue - lastEncoderValue;
1126
1127    brightnessLevel += diff * 4;
1128
1129    if (brightnessLevel < 20) brightnessLevel = 20;
1130    if (brightnessLevel > 255) brightnessLevel = 255;
1131
1132    ledcWrite(BL_PIN, brightnessLevel);
1133
1134    lastEncoderValue = currentValue;
1135
1136    Serial.print("Brightness: ");
1137    Serial.println(brightnessLevel);
1138  }
1139}
1140
1141void setup() {
1142  Serial.begin(115200);
1143
1144  pinMode(ENCODER_CLK, INPUT_PULLUP);
1145  pinMode(ENCODER_DT, INPUT_PULLUP);
1146
1147  attachInterrupt(digitalPinToInterrupt(ENCODER_CLK), updateEncoder, CHANGE);
1148  attachInterrupt(digitalPinToInterrupt(ENCODER_DT), updateEncoder, CHANGE);
1149
1150  init_display();
1151
1152  pcf8574_init();
1153
1154//  scanI2C();
1155  
1156  if (mpu6050_init()) {
1157  Serial.println("MPU6050 OK");
1158} else {
1159  Serial.println("MPU6050 NOT FOUND");
1160}
1161
1162if (qmc5883l_init()) {
1163  Serial.println("QMC5883L OK");
1164} else {
1165  Serial.println("QMC5883L NOT FOUND");
1166}
1167
1168  drawCurrentScreen();
1169}
1170
1171void loop() {
1172
1173  handleBrightnessEncoder();
1174  
1175  static bool lastBtn = false;
1176
1177  bool btn = isEncoderButtonPressed();
1178
1179  if (btn && !lastBtn) {
1180    currentScreen = (ScreenMode)((currentScreen + 1) % SCREEN_COUNT);
1181    drawCurrentScreen();
1182    delay(250);
1183  }
1184
1185  lastBtn = btn;
1186
1187  if (currentScreen == SCREEN_ATTITUDE && millis() - lastMPURead > 120) {
1188    lastMPURead = millis();
1189    updateMPU6050();
1190    drawCurrentScreen();
1191  }
1192 if (currentScreen == SCREEN_COMPASS && millis() - lastCompassRead > 120) {
1193  lastCompassRead = millis();
1194  updateQMC5883L();
1195  drawCurrentScreen();
1196}
1197
1198
1199if (currentScreen == SCREEN_ALTIMETER && millis() - lastMPURead > 120) {
1200  lastMPURead = millis();
1201
1202  updateMPU6050();
1203  updateSimulatedAltitude();
1204
1205  drawCurrentScreen();
1206}
1207
1208if (currentScreen == SCREEN_AIRPLANE && millis() - lastMPURead > 120) {
1209  lastMPURead = millis();
1210  updateMPU6050();
1211  drawCurrentScreen();
1212}
1213
1214}

// by miercemk May, 2026

#include <Arduino.h>
#include <Arduino_GFX_Library.h>
#include <Wire.h>

#define DISPLAY_WIDTH  480
#define DISPLAY_HEIGHT 480
#define CX 240
#define CY 240

#define BL_PIN     6
#define PANEL_CS   16
#define PANEL_SCK  2
#define PANEL_SDA  1
#define PCLK_NEG   1

#define ENCODER_CLK 4
#define ENCODER_DT  42

#define I2C_SDA 38
#define I2C_SCL 39
#define PCF8574_ADDR 0x21
#define MPU6050_ADDR 0x68
#define QMC5883L_ADDR 0x0D

Arduino_DataBus       *panelBus = nullptr;
Arduino_ESP32RGBPanel *rgbpanel = nullptr;
Arduino_RGB_Display   *gfx      = nullptr;
uint16_t              *fb       = nullptr;

float pitchDeg = 0;
float rollDeg  = 0;

float compassHeadingDeg = 0;
unsigned long lastCompassRead = 0;

float altitudeFt = 6300.0f;
unsigned long lastAltitudeUpdate = 0;

float lastDrawPitch = 999;
float lastDrawRoll  = 999;

volatile int lastEncoded = 0;
volatile long encoderValue = 0;
long lastEncoderValue = 0;

int brightnessLevel = 255;

unsigned long lastMPURead = 0;

extern const uint8_t st7701_type7_init_operations[];

uint16_t rgb565(uint8_t r, uint8_t g, uint8_t b) {
  return ((r & 0xF8) << 8) | ((g & 0xFC) << 3) | (b >> 3);
}

const uint16_t COL_BLACK      = 0x0000;
const uint16_t COL_WHITE      = 0xFFFF;
const uint16_t COL_YELLOW     = 0xFFE0;
const uint16_t COL_ORANGE     = 0xFD20;
const uint16_t COL_SKY        = rgb565(20, 165, 225);
const uint16_t COL_GROUND     = rgb565(150, 95, 45);
const uint16_t COL_RING_OUTER = rgb565(170, 170, 170);
const uint16_t COL_RING_INNER = rgb565(45, 45, 45);
const uint16_t COL_RING_EDGE  = rgb565(15, 15, 15);

enum ScreenMode {
  SCREEN_ATTITUDE = 0,
  SCREEN_COMPASS,
  SCREEN_ALTIMETER,
  SCREEN_AIRPLANE,
  SCREEN_COUNT
};

ScreenMode currentScreen = SCREEN_ATTITUDE;

static inline void putpix(int x, int y, uint16_t c) {
  if ((unsigned)x < DISPLAY_WIDTH && (unsigned)y < DISPLAY_HEIGHT) {
    fb[y * DISPLAY_WIDTH + x] = c;
  }
}

void clearFB(uint16_t color = COL_BLACK) {
  for (int i = 0; i < DISPLAY_WIDTH * DISPLAY_HEIGHT; i++) fb[i] = color;
}

void drawLine(int x0, int y0, int x1, int y1, uint16_t col) {
  int dx = abs(x1 - x0), sx = x0 < x1 ? 1 : -1;
  int dy = -abs(y1 - y0), sy = y0 < y1 ? 1 : -1;
  int err = dx + dy;

  while (true) {
    putpix(x0, y0, col);
    if (x0 == x1 && y0 == y1) break;

    int e2 = 2 * err;
    if (e2 >= dy) { err += dy; x0 += sx; }
    if (e2 <= dx) { err += dx; y0 += sy; }
  }
}

void drawThickLine(int x0, int y0, int x1, int y1, uint16_t col, int t) {
  for (int i = -t / 2; i <= t / 2; i++) {
    drawLine(x0, y0 + i, x1, y1 + i, col);
  }
}

void drawCircle(int cx, int cy, int r, uint16_t col) {
  int x = r, y = 0, err = 0;

  while (x >= y) {
    putpix(cx + x, cy + y, col);
    putpix(cx + y, cy + x, col);
    putpix(cx - y, cy + x, col);
    putpix(cx - x, cy + y, col);
    putpix(cx - x, cy - y, col);
    putpix(cx - y, cy - x, col);
    putpix(cx + y, cy - x, col);
    putpix(cx + x, cy - y, col);

    y++;
    if (err <= 0) err += 2 * y + 1;
    if (err > 0) {
      x--;
      err -= 2 * x + 1;
    }
  }
}

void fillCircle(int cx, int cy, int r, uint16_t col) {

  int r2 = r * r;

  for (int y = -r; y <= r; y++) {
    for (int x = -r; x <= r; x++) {

      if (x * x + y * y <= r2) {
        putpix(cx + x, cy + y, col);
      }
    }
  }
}

void fillTriangle(int x1, int y1, int x2, int y2, int x3, int y3, uint16_t col) {
  int minX = min(x1, min(x2, x3));
  int maxX = max(x1, max(x2, x3));
  int minY = min(y1, min(y2, y3));
  int maxY = max(y1, max(y2, y3));

  int area = (x2 - x1) * (y3 - y1) - (y2 - y1) * (x3 - x1);

  for (int y = minY; y <= maxY; y++) {
    for (int x = minX; x <= maxX; x++) {
      int w1 = (x2 - x1) * (y - y1) - (y2 - y1) * (x - x1);
      int w2 = (x3 - x2) * (y - y2) - (y3 - y2) * (x - x2);
      int w3 = (x1 - x3) * (y - y3) - (y1 - y3) * (x - x3);

      if ((area >= 0 && w1 >= 0 && w2 >= 0 && w3 >= 0) ||
          (area < 0 && w1 <= 0 && w2 <= 0 && w3 <= 0)) {
        putpix(x, y, col);
      }
    }
  }
}

void fillRing(int cx, int cy, int rInner, int rOuter, uint16_t col) {
  int ri2 = rInner * rInner;
  int ro2 = rOuter * rOuter;

  for (int y = -rOuter; y <= rOuter; y++) {
    for (int x = -rOuter; x <= rOuter; x++) {
      int d2 = x * x + y * y;
      if (d2 >= ri2 && d2 <= ro2) {
        putpix(cx + x, cy + y, col);
      }
    }
  }
}

void fillArtificialHorizonDisc() {
  const int R = 206;
  const int r2 = R * R;

  float rollRad = rollDeg * PI / 180.0f;

  float cr = cos(rollRad);
  float sr = sin(rollRad);

  float pitchOffset = pitchDeg * 4.0f;

  // sky / ground
  for (int y = -R; y <= R; y++) {
    for (int x = -R; x <= R; x++) {

      if (x * x + y * y <= r2) {

        float yr = x * sr + y * cr;

        if (yr + pitchOffset < 0)
          putpix(CX + x, CY + y, COL_SKY);
        else
          putpix(CX + x, CY + y, COL_GROUND);
      }
    }
  }

  // helper transform
  auto H = [&](float lx, float ly, int &sx, int &sy) {

    sx = CX + lx * cr + ly * sr;
    sy = CY - lx * sr + ly * cr;
  };

  // --------------------------------------------------
  // VIRTUAL RUNWAY
  // --------------------------------------------------

  {
    int x1, y1, x2, y2, x3, y3;

    H(0, 10 - pitchOffset, x1, y1);
    H(-95, 115 - pitchOffset, x2, y2);
    H(95, 115 - pitchOffset, x3, y3);

    fillTriangle(
      x1, y1,
      x2, y2,
      x3, y3,
      rgb565(80, 38, 25)
    );

    int xa, ya, xb, yb;

    H(0, 10 - pitchOffset, xa, ya);
    H(-32, 115 - pitchOffset, xb, yb);
    drawThickLine(xa, ya, xb, yb, COL_ORANGE, 2);

    H(0, 10 - pitchOffset, xa, ya);
    H(32, 115 - pitchOffset, xb, yb);
    drawThickLine(xa, ya, xb, yb, COL_ORANGE, 2);

    H(-32, 115 - pitchOffset, xa, ya);
    H(32, 115 - pitchOffset, xb, yb);
    drawThickLine(xa, ya, xb, yb, COL_ORANGE, 2);
  }

  // --------------------------------------------------
  // MAIN HORIZON LINE
  // --------------------------------------------------

  {
    int x1, y1, x2, y2;

    H(-R, -pitchOffset, x1, y1);
    H(R,  -pitchOffset, x2, y2);

    drawThickLine(x1, y1, x2, y2, COL_WHITE, 3);
  }

  // --------------------------------------------------
  // PITCH LADDER
  // --------------------------------------------------

  int step = 32;

  for (int i = -4; i <= 4; i++) {

    if (i == 0) continue;

    int value = abs(i) * 5;

    int len =
      (value == 10 || value == 20)
      ? 120
      : 58;

    int localY = i * step - pitchOffset;

    int x1, y1, x2, y2;

    H(-len / 2, localY, x1, y1);
    H( len / 2, localY, x2, y2);

    drawThickLine(x1, y1, x2, y2, COL_WHITE, 3);

    if (value == 10 || value == 20) {

      int xl, yl;
      int xr, yr;

      H(-len / 2 - 50, localY - 12, xl, yl);
      H( len / 2 + 24, localY - 12, xr, yr);

      drawSmallNumber(value, xl, yl, COL_WHITE);
      drawSmallNumber(value, xr, yr, COL_WHITE);
    }
  }
}

void drawSegDigit(int d, int x, int y, int s, uint16_t col) {
  bool seg[7];

  switch (d) {
    case 0: seg[0]=1; seg[1]=1; seg[2]=1; seg[3]=1; seg[4]=1; seg[5]=1; seg[6]=0; break;
    case 1: seg[0]=0; seg[1]=1; seg[2]=1; seg[3]=0; seg[4]=0; seg[5]=0; seg[6]=0; break;
    case 2: seg[0]=1; seg[1]=1; seg[2]=0; seg[3]=1; seg[4]=1; seg[5]=0; seg[6]=1; break;
    case 3: seg[0]=1; seg[1]=1; seg[2]=1; seg[3]=1; seg[4]=0; seg[5]=0; seg[6]=1; break;
    case 4: seg[0]=0; seg[1]=1; seg[2]=1; seg[3]=0; seg[4]=0; seg[5]=1; seg[6]=1; break;
    case 5: seg[0]=1; seg[1]=0; seg[2]=1; seg[3]=1; seg[4]=0; seg[5]=1; seg[6]=1; break;
    case 6: seg[0]=1; seg[1]=0; seg[2]=1; seg[3]=1; seg[4]=1; seg[5]=1; seg[6]=1; break;
    case 7: seg[0]=1; seg[1]=1; seg[2]=1; seg[3]=0; seg[4]=0; seg[5]=0; seg[6]=0; break;
    case 8: seg[0]=1; seg[1]=1; seg[2]=1; seg[3]=1; seg[4]=1; seg[5]=1; seg[6]=1; break;
    case 9: seg[0]=1; seg[1]=1; seg[2]=1; seg[3]=1; seg[4]=0; seg[5]=1; seg[6]=1; break;
  }

  int w = 5 * s;
  int h = 9 * s;
  int t = s;

  if (seg[0]) drawThickLine(x, y, x + w, y, col, t);
  if (seg[1]) drawThickLine(x + w, y, x + w, y + h / 2, col, t);
  if (seg[2]) drawThickLine(x + w, y + h / 2, x + w, y + h, col, t);
  if (seg[3]) drawThickLine(x, y + h, x + w, y + h, col, t);
  if (seg[4]) drawThickLine(x, y + h / 2, x, y + h, col, t);
  if (seg[5]) drawThickLine(x, y, x, y + h / 2, col, t);
  if (seg[6]) drawThickLine(x, y + h / 2, x + w, y + h / 2, col, t);
}

void drawSmallNumber(int value, int x, int y, uint16_t col) {
  if (value == 10) {
    drawSegDigit(1, x, y, 2, col);
    drawSegDigit(0, x + 14, y, 2, col);
  }

  if (value == 20) {
    drawSegDigit(2, x, y, 2, col);
    drawSegDigit(0, x + 14, y, 2, col);
  }
}

void drawPitchScaleLine(int y, int value, uint16_t col) {
  int longLen  = 120;
  int shortLen = 58;

  bool major = (value == 10 || value == 20);
  int len = major ? longLen : shortLen;

  drawThickLine(CX - len / 2, y, CX + len / 2, y, col, 3);

  if (value == 10 || value == 20) {
    drawSmallNumber(value, CX - len / 2 - 50, y - 12, col);
    drawSmallNumber(value, CX + len / 2 + 24, y - 12, col);
  }
}

void drawPitchLadder() {
  int step = 32;

  float rollRad = rollDeg * PI / 180.0f;
  float cr = cos(rollRad);
  float sr = sin(rollRad);

  float pitchOffset = pitchDeg * 4.0f;

  for (int i = -4; i <= 4; i++) {
    if (i == 0) continue;

    int value = abs(i) * 5;
    int len = (value == 10 || value == 20) ? 120 : 58;

    float localY = i * step - pitchOffset;

    float x1r = -len / 2;
    float y1r = localY;
    float x2r =  len / 2;
    float y2r = localY;

    int x1 = CX + x1r * cr - y1r * sr;
    int y1 = CY + x1r * sr + y1r * cr;
    int x2 = CX + x2r * cr - y2r * sr;
    int y2 = CY + x2r * sr + y2r * cr;

    drawThickLine(x1, y1, x2, y2, COL_WHITE, 3);
  }
}

void drawRotatedLine(float x1, float y1, float x2, float y2, float angleDeg, uint16_t col, int thick) {
  float a = angleDeg * PI / 180.0f;
  float cr = cos(a);
  float sr = sin(a);

  int sx1 = CX + x1 * cr - y1 * sr;
  int sy1 = CY + x1 * sr + y1 * cr;
  int sx2 = CX + x2 * cr - y2 * sr;
  int sy2 = CY + x2 * sr + y2 * cr;

  drawThickLine(sx1, sy1, sx2, sy2, col, thick);
}

void drawScreen_AirplaneDemo() {
  clearFB(rgb565(10, 18, 28));

// light gray frame like other instruments
fillRing(CX, CY, 229, 239, COL_RING_OUTER);
fillRing(CX, CY, 218, 228, COL_RING_INNER);

drawCircle(CX, CY, 217, COL_RING_EDGE);
drawCircle(CX, CY, 228, COL_RING_EDGE);
drawCircle(CX, CY, 239, COL_WHITE);

// inner dark display area
fillCircle(CX, CY, 217, rgb565(10, 18, 28));

  // background reference grid
  drawCircle(CX, CY, 210, rgb565(80, 80, 80));
  drawCircle(CX, CY, 140, rgb565(50, 50, 50));
  drawThickLine(CX - 210, CY, CX + 210, CY, rgb565(60, 60, 60), 1);
  drawThickLine(CX, CY - 210, CX, CY + 210, rgb565(60, 60, 60), 1);

  // pitch moves airplane slightly up/down
  int oldCY = CY;
  int pitchMove = constrain((int)(pitchDeg * 3.0f), -90, 90);

  // local center offset
  int baseY = CY + pitchMove;

  float roll = rollDeg;

  auto L = [&](float x1, float y1, float x2, float y2, uint16_t col, int t) {
    float a = roll * PI / 180.0f;
    float cr = cos(a);
    float sr = sin(a);

    int sx1 = CX + x1 * cr - (y1 + pitchMove) * sr;
    int sy1 = CY + x1 * sr + (y1 + pitchMove) * cr;
    int sx2 = CX + x2 * cr - (y2 + pitchMove) * sr;
    int sy2 = CY + x2 * sr + (y2 + pitchMove) * cr;

    drawThickLine(sx1, sy1, sx2, sy2, col, t);
  };

  uint16_t bodyCol = rgb565(255, 180, 40);
  uint16_t wingCol = rgb565(40, 220, 255);
  uint16_t tailCol = rgb565(255, 80, 80);

  // airplane body
  L(0, -120, 0, 110, bodyCol, 8);

  // nose
  L(0, -120, -22, -75, bodyCol, 5);
  L(0, -120,  22, -75, bodyCol, 5);

  // main wings
  L(-20, -25, -145, 35, wingCol, 8);
  L( 20, -25,  145, 35, wingCol, 8);

  // wing tips
  L(-145, 35, -120, 55, wingCol, 5);
  L( 145, 35,  120, 55, wingCol, 5);

  // tail wings
  L(-15, 80, -75, 125, tailCol, 6);
  L( 15, 80,  75, 125, tailCol, 6);

  // center dot
  fillCircle(CX, CY + pitchMove, 8, COL_WHITE);

  gfx->draw16bitRGBBitmap(0, 0, fb, DISPLAY_WIDTH, DISPLAY_HEIGHT);
}

void drawRollScaleOnRing() {
  for (int a = -50; a <= 50; a += 10) {
    float rad = (a - 90) * PI / 180.0;

    int r1 = 211;
    int r2 = 224;

    if (a % 30 == 0) r1 = 207;

    int x1 = CX + cos(rad) * r1;
    int y1 = CY + sin(rad) * r1;
    int x2 = CX + cos(rad) * r2;
    int y2 = CY + sin(rad) * r2;

    drawThickLine(x1, y1, x2, y2, COL_WHITE, 3);
  }

  // smaller static orange triangle
  fillTriangle(CX, CY - 210, CX - 11, CY - 229, CX + 11, CY - 229, COL_ORANGE);

  // smaller moving white triangle
float rollRad = rollDeg * PI / 180.0f;
float cr = cos(rollRad);
float sr = sin(rollRad);

auto ROLL = [&](float lx, float ly, int &sx, int &sy) {
  sx = CX + lx * cr + ly * sr;
  sy = CY - lx * sr + ly * cr;
};

int x1, y1, x2, y2, x3, y3;

ROLL(0, -200, x1, y1);
ROLL(-12, -181, x2, y2);
ROLL(12, -181, x3, y3);

fillTriangle(x1, y1, x2, y2, x3, y3, COL_WHITE);
}

void drawVirtualRunway() {
  // dark runway body
  fillTriangle(
    CX, CY + 10,
    CX - 95, CY + 115,
    CX + 95, CY + 115,
    rgb565(80, 38, 25)
  );

  // orange runway perspective lines
  drawThickLine(CX, CY + 10, CX - 32, CY + 115, COL_ORANGE, 2);
  drawThickLine(CX, CY + 10, CX + 32, CY + 115, COL_ORANGE, 2);
  drawThickLine(CX - 32, CY + 115, CX + 32, CY + 115, COL_ORANGE, 2);
}

void drawFrameLetter(char c, int x, int y, int s, uint16_t col) {
  switch (c) {
    case 'N':
      drawThickLine(x, y + 28*s, x, y, col, s);
      drawThickLine(x, y, x + 18*s, y + 28*s, col, s);
      drawThickLine(x + 18*s, y + 28*s, x + 18*s, y, col, s);
      break;

    case 'E':
      drawThickLine(x, y, x, y + 28*s, col, s);
      drawThickLine(x, y, x + 18*s, y, col, s);
      drawThickLine(x, y + 14*s, x + 15*s, y + 14*s, col, s);
      drawThickLine(x, y + 28*s, x + 18*s, y + 28*s, col, s);
      break;

    case 'S':
      drawThickLine(x + 18*s, y, x, y, col, s);
      drawThickLine(x, y, x, y + 14*s, col, s);
      drawThickLine(x, y + 14*s, x + 18*s, y + 14*s, col, s);
      drawThickLine(x + 18*s, y + 14*s, x + 18*s, y + 28*s, col, s);
      drawThickLine(x + 18*s, y + 28*s, x, y + 28*s, col, s);
      break;

    case 'W':
      drawThickLine(x, y, x + 4*s, y + 28*s, col, s);
      drawThickLine(x + 4*s, y + 28*s, x + 9*s, y + 12*s, col, s);
      drawThickLine(x + 9*s, y + 12*s, x + 14*s, y + 28*s, col, s);
      drawThickLine(x + 14*s, y + 28*s, x + 18*s, y, col, s);
      break;
  }
}

void drawFrameNumberText(const char *txt, int x, int y, int s, uint16_t col) {
  for (int i = 0; txt[i]; i++) {
    int dx = i * 14 * s;
    drawSegDigit(txt[i] - '0', x + dx, y, s, col);
  }
}

void drawCompassText(const char *txt, int cx, int cy, int s, uint16_t col) {
  int len = strlen(txt);
  int w;

  if (txt[0] >= '0' && txt[0] <= '9') {
    w = len * 14 * s;
    drawFrameNumberText(txt, cx - w / 2, cy - 9 * s, s, col);
  } else {
    w = 20 * s;
    drawFrameLetter(txt[0], cx - w / 2, cy - 14 * s, s, col);
  }
}

void drawAircraftSymbol() {
  // longer and thicker horizontal yellow wings
  drawThickLine(CX - 140, CY, CX - 35, CY, COL_YELLOW, 8);
  drawThickLine(CX + 35, CY, CX + 140, CY, COL_YELLOW, 8);

  // thicker central inverted V
  drawThickLine(CX - 35, CY, CX, CY + 28, COL_YELLOW, 8);
  drawThickLine(CX, CY + 28, CX + 35, CY, COL_YELLOW, 8);

  // small center point / hub
  fillTriangle(CX, CY - 4, CX - 7, CY + 6, CX + 7, CY + 6, COL_YELLOW);
}

void drawConcentricBezel() {
  // dark ring is thin, and horizon disc touches it directly
  fillRing(CX, CY, 207, 224, COL_RING_INNER);

  // bright outer ring
  fillRing(CX, CY, 225, 239, COL_RING_OUTER);

  drawCircle(CX, CY, 206, COL_RING_EDGE);
  drawCircle(CX, CY, 224, COL_RING_EDGE);
  drawCircle(CX, CY, 239, COL_WHITE);
}

void drawCompassDot(int x, int y, uint16_t col) {
  fillCircle(x, y, 2, col);
}

void drawCompassTicks(float headingDeg = 0) {
  const int R_OUT  = 216;  // речиси до сивиот обрач
  const int R_IN   = 194;  // подолги црти
  const int R_DOT  = 194;  // точки на средина од цртите

  // 36 црти = на секои 10 степени
  // помеѓу N и E има точно 9 црти: 10,20,30,40,50,60,70,80,90
  for (int deg = 0; deg < 360; deg += 10) {
    float a = (deg - headingDeg - 90) * PI / 180.0;

    int x1 = CX + cos(a) * R_IN;
    int y1 = CY + sin(a) * R_IN;
    int x2 = CX + cos(a) * R_OUT;
    int y2 = CY + sin(a) * R_OUT;

    drawThickLine(x1, y1, x2, y2, COL_WHITE, 3);
  }

  // дискретни точки точно на средина помеѓу секои две црти
  // значи на 5,15,25...
  for (int deg = 5; deg < 360; deg += 10) {
    float a = (deg - headingDeg - 90) * PI / 180.0;

    int x = CX + cos(a) * R_DOT;
    int y = CY + sin(a) * R_DOT;

    fillCircle(x, y, 2, COL_WHITE);
  }
}
void drawCompassLetters(float headingDeg = 0) {
  struct Mark {
    int deg;
    const char* txt;
    uint16_t col;
    int scale;
    int radius;
  };

  Mark marks[] = {
    // smaller N/E/S/W
    {0,   "N",  COL_YELLOW, 1, 158},
    {90,  "E",  COL_YELLOW, 1, 158},
    {180, "S",  COL_YELLOW, 1, 158},
    {270, "W",  COL_YELLOW, 1, 158},

    // degree labels
    {30,  "3",  COL_WHITE, 2, 160},
    {60,  "6",  COL_WHITE, 2, 160},
    {120, "12", COL_WHITE, 2, 160},
    {150, "15", COL_WHITE, 2, 165},
    {210, "21", COL_WHITE, 2, 155},
    {240, "24", COL_WHITE, 2, 150},
    {300, "30", COL_WHITE, 2, 150},
    {330, "33", COL_WHITE, 2, 150}
  };

  for (int i = 0; i < 12; i++) {
    float a = (marks[i].deg - headingDeg - 90) * PI / 180.0;

    int x = CX + cos(a) * marks[i].radius;
    int y = CY + sin(a) * marks[i].radius;

    drawCompassText(marks[i].txt, x, y, marks[i].scale, marks[i].col);
  }
}

void drawCompassAirplane() {
  // static yellow airplane symbol
  uint16_t c = COL_ORANGE;

  // nose / fuselage
 drawThickLine(CX, CY - 194, CX, CY + 65, c, 5);

  // nose sides
  drawThickLine(CX, CY - 130, CX - 28, CY - 40, c, 4);
  drawThickLine(CX, CY - 130, CX + 28, CY - 40, c, 4);

  // wings
  drawThickLine(CX - 28, CY - 40, CX - 88, CY + 10, c, 4);
  drawThickLine(CX + 28, CY - 40, CX + 88, CY + 10, c, 4);
  drawThickLine(CX - 88, CY + 10, CX - 88, CY + 35, c, 4);
  drawThickLine(CX + 88, CY + 10, CX + 88, CY + 35, c, 4);
  drawThickLine(CX - 88, CY + 35, CX - 20, CY + 10, c, 4);
  drawThickLine(CX + 88, CY + 35, CX + 20, CY + 10, c, 4);

  // body lower part
  drawThickLine(CX - 20, CY + 10, CX - 20, CY + 85, c, 4);
  drawThickLine(CX + 20, CY + 10, CX + 20, CY + 85, c, 4);

  // tail
  drawThickLine(CX - 20, CY + 85, CX - 55, CY + 110, c, 4);
  drawThickLine(CX + 20, CY + 85, CX + 55, CY + 110, c, 4);
  drawThickLine(CX - 55, CY + 110, CX - 55, CY + 130, c, 4);
  drawThickLine(CX + 55, CY + 110, CX + 55, CY + 130, c, 4);
  drawThickLine(CX - 55, CY + 130, CX, CY + 108, c, 4);
  drawThickLine(CX + 55, CY + 130, CX, CY + 108, c, 4);
}

void drawScreen_Compass() {
  clearFB(COL_BLACK);

  // thinner gray frame - половина од претходната дебелина
  fillRing(CX, CY, 229, 239, COL_RING_OUTER);
  fillRing(CX, CY, 218, 228, COL_RING_INNER);

  drawCircle(CX, CY, 217, COL_RING_EDGE);
  drawCircle(CX, CY, 228, COL_RING_EDGE);
  drawCircle(CX, CY, 239, COL_WHITE);

  fillCircle(CX, CY, 217, rgb565(42, 50, 52));

  float heading = compassHeadingDeg;

  drawCompassTicks(heading);
  drawCompassLetters(heading);
  drawCompassAirplane();

  gfx->draw16bitRGBBitmap(0, 0, fb, DISPLAY_WIDTH, DISPLAY_HEIGHT);
}

void drawScreen_AttitudeIndicator() {
  
  clearFB(COL_BLACK);

  drawConcentricBezel();

  fillArtificialHorizonDisc();

  drawAircraftSymbol();
  drawRollScaleOnRing();

  gfx->draw16bitRGBBitmap(0, 0, fb, DISPLAY_WIDTH, DISPLAY_HEIGHT);
}

void drawAltimeterNumbers() {
  const int R_NUM = 155;

  for (int n = 0; n <= 9; n++) {
    float deg = n * 36.0;
    float a = (deg - 90) * PI / 180.0;

    int x = CX + cos(a) * R_NUM;
    int y = CY + sin(a) * R_NUM;

    drawSegDigit(n, x - 8, y - 14, 3, COL_WHITE);
  }
}

void drawAltimeterTicks() {
  const int R_OUT = 205;
  const int R_IN_MAJOR = 178;
  const int R_IN_MINOR = 192;

  for (int i = 0; i < 100; i++) {
    float deg = i * 3.6;
    float a = (deg - 90) * PI / 180.0;

    bool major = (i % 10 == 0);
    bool medium = (i % 5 == 0);

    int r1 = major ? R_IN_MAJOR : (medium ? 185 : R_IN_MINOR);
    int r2 = R_OUT;

    int x1 = CX + cos(a) * r1;
    int y1 = CY + sin(a) * r1;
    int x2 = CX + cos(a) * r2;
    int y2 = CY + sin(a) * r2;

    drawThickLine(x1, y1, x2, y2, COL_WHITE, major ? 4 : 2);
  }
}

void drawAltimeterText() {
  // simple static text with lines, framebuffer-safe
  // ALT
// ALT
drawFrameLetter('A', CX - 48, CY - 78, 1, COL_WHITE);

// L
drawThickLine(CX - 10, CY - 78, CX - 10, CY - 50, COL_WHITE, 1);
drawThickLine(CX - 10, CY - 50, CX + 8, CY - 50, COL_WHITE, 1);

// T
drawFrameLetter('T', CX + 28, CY - 78, 1, COL_WHITE);

  // small “x1000 ft” imitation
  drawThickLine(CX - 28, CY - 38, CX + 28, CY - 38, COL_WHITE, 1);
  drawThickLine(CX - 18, CY - 30, CX + 18, CY - 30, COL_WHITE, 1);
}

void drawAltimeterHand(float angleDeg, int length, int width, uint16_t col) {
  float a = (angleDeg - 90) * PI / 180.0;

  int tipX = CX + cos(a) * length;
  int tipY = CY + sin(a) * length;

  float px = -sin(a);
  float py = cos(a);

  int leftX  = CX + px * width;
  int leftY  = CY + py * width;
  int rightX = CX - px * width;
  int rightY = CY - py * width;

  fillTriangle(leftX, leftY, rightX, rightY, tipX, tipY, col);
}

void drawAltimeterHands(int altitudeFt) {

  // LONG thin hand
  // one full rotation = 1000 ft
  float longAngle =
      ((altitudeFt % 1000) / 1000.0f) * 360.0f;

  // SHORT thick hand
  // one full rotation = 10000 ft
  float shortAngle =
      ((altitudeFt % 10000) / 10000.0f) * 360.0f;

  // short thick hand
  drawAltimeterHand(shortAngle, 95, 11, COL_WHITE);

  // long thin hand
  drawAltimeterHand(longAngle, 175, 4, COL_WHITE);

  fillCircle(CX, CY, 14, rgb565(120,120,120));
  fillCircle(CX, CY, 7, COL_WHITE);
}

void drawAltimeterSmallWindow() {
  // small striped reference window at bottom, like aircraft altimeters
  int x0 = CX - 42;
  int y0 = CY + 72;

 fillCircle(CX, CY + 88, 38, rgb565(25, 25, 25));

  for (int i = 0; i < 5; i++) {
    drawThickLine(x0 + i * 16, y0 + 35, x0 + i * 16 + 35, y0, COL_WHITE, 5);
  }
}

void drawScreen_Altimeter() {
  clearFB(COL_BLACK);

  // concentric frame, same style as compass
  fillRing(CX, CY, 229, 239, COL_RING_OUTER);
  fillRing(CX, CY, 218, 228, COL_RING_INNER);

  drawCircle(CX, CY, 217, COL_RING_EDGE);
  drawCircle(CX, CY, 228, COL_RING_EDGE);
  drawCircle(CX, CY, 239, COL_WHITE);

  // dial background
  fillCircle(CX, CY, 217, rgb565(25, 28, 30));
  fillCircle(CX, CY, 105, rgb565(36, 40, 42));

  drawAltimeterTicks();
  drawAltimeterNumbers();
  drawAltimeterText();
  drawAltimeterSmallWindow();

  
  int altitudeDisplay = (int)altitudeFt;

  drawAltimeterHands(altitudeDisplay);

  gfx->draw16bitRGBBitmap(0, 0, fb, DISPLAY_WIDTH, DISPLAY_HEIGHT);

  
}

void drawCurrentScreen() {
  switch (currentScreen) {
    case SCREEN_ATTITUDE:
      drawScreen_AttitudeIndicator();
      break;

    case SCREEN_COMPASS:
      drawScreen_Compass();
      break;

    case SCREEN_ALTIMETER:
      drawScreen_Altimeter();
      break;

   case SCREEN_AIRPLANE:
      drawScreen_AirplaneDemo();
      break;     

    default:
      drawScreen_AttitudeIndicator();
      break;
  }
}  

void init_display() {
 pinMode(BL_PIN, OUTPUT);
ledcAttach(BL_PIN, 5000, 8);
ledcWrite(BL_PIN, brightnessLevel);

  panelBus = new Arduino_SWSPI(
    GFX_NOT_DEFINED, PANEL_CS, PANEL_SCK, PANEL_SDA, GFX_NOT_DEFINED
  );

  rgbpanel = new Arduino_ESP32RGBPanel(
      40,7,15,41,
      46,3,8,18,17,
      14,13,12,11,10,9,
      5,45,48,47,21,
      1,50,10,50,
      1,30,10,30,
      PCLK_NEG,6000000UL
  );

  gfx = new Arduino_RGB_Display(
    DISPLAY_WIDTH, DISPLAY_HEIGHT, rgbpanel, 0, true,
    panelBus, GFX_NOT_DEFINED,
    st7701_type7_init_operations, sizeof(st7701_type7_init_operations)
  );

  gfx->begin(8000000);

  fb = (uint16_t*)ps_malloc(DISPLAY_WIDTH * DISPLAY_HEIGHT * 2);
  if (!fb) {
    Serial.println("Framebuffer allocation failed!");
    while (1);
  }
}

void pcf8574_init() {
  Wire.begin(I2C_SDA, I2C_SCL);

  Wire.beginTransmission(PCF8574_ADDR);
  Wire.write(0xFF);        // all pins high = inputs with pullups
  Wire.endTransmission();
}

uint8_t pcf8574_read() {
  Wire.requestFrom(PCF8574_ADDR, (uint8_t)1);

  if (Wire.available()) {
    return Wire.read();
  }

  return 0xFF;
}

bool isEncoderButtonPressed() {
  uint8_t state = pcf8574_read();

  // P5 low = button pressed
  return !(state & (1 << 5));
}

bool mpu6050_init() {
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(0x6B);      // PWR_MGMT_1
  Wire.write(0x00);      // wake up
  if (Wire.endTransmission() != 0) return false;

  delay(100);

  // accelerometer ±2g
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(0x1C);
  Wire.write(0x00);
  Wire.endTransmission();

  return true;
}

bool mpu6050_readAccel(float &ax, float &ay, float &az) {
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(0x3B); // ACCEL_XOUT_H
  if (Wire.endTransmission(false) != 0) return false;

  Wire.requestFrom(MPU6050_ADDR, (uint8_t)6);
  if (Wire.available() < 6) return false;

  int16_t rawX = (Wire.read() << 8) | Wire.read();
  int16_t rawY = (Wire.read() << 8) | Wire.read();
  int16_t rawZ = (Wire.read() << 8) | Wire.read();

  ax = rawX / 16384.0f;
  ay = rawY / 16384.0f;
  az = rawZ / 16384.0f;

  return true;
}

void updateMPU6050() {
  float ax, ay, az;

  if (!mpu6050_readAccel(ax, ay, az)) return;

  float newRoll  = atan2(ay, az) * 180.0f / PI;
  float newPitch = atan2(-ax, sqrt(ay * ay + az * az)) * 180.0f / PI;

  // smoothing
  rollDeg  = rollDeg  * 0.85f + newRoll  * 0.15f;
  pitchDeg = pitchDeg * 0.85f + newPitch * 0.15f;
}

bool qmc5883l_init() {
  Wire.beginTransmission(QMC5883L_ADDR);
  Wire.write(0x0B);      // SET/RESET period register
  Wire.write(0x01);
  Wire.endTransmission();

  Wire.beginTransmission(QMC5883L_ADDR);
  Wire.write(0x09);      // control register
  Wire.write(0x1D);      // continuous, 200Hz, 2G, 512 OSR
  if (Wire.endTransmission() != 0) return false;

  return true;
}

bool qmc5883l_readHeading(float &headingDeg) {
  Wire.beginTransmission(QMC5883L_ADDR);
  Wire.write(0x00);
  if (Wire.endTransmission(false) != 0) return false;

  Wire.requestFrom(QMC5883L_ADDR, (uint8_t)6);
  if (Wire.available() < 6) return false;

  int16_t x = Wire.read() | (Wire.read() << 8);
  int16_t y = Wire.read() | (Wire.read() << 8);
  int16_t z = Wire.read() | (Wire.read() << 8);

  float heading = atan2((float)y, (float)x) * 180.0f / PI;

  if (heading < 0) heading += 360.0f;
  if (heading >= 360) heading -= 360.0f;

  headingDeg = heading;

  Serial.print("QMC X=");
  Serial.print(x);
  Serial.print(" Y=");
  Serial.print(y);
  Serial.print(" Z=");
  Serial.print(z);
  Serial.print(" Heading=");
  Serial.println(headingDeg);

  return true;
}

void updateQMC5883L() {
  float h;

  if (!qmc5883l_readHeading(h)) {
    Serial.println("QMC5883L read failed");
    return;
  }

  float diff = h - compassHeadingDeg;

  if (diff > 180.0f)  diff -= 360.0f;
  if (diff < -180.0f) diff += 360.0f;

  compassHeadingDeg += diff * 0.15f;

  if (compassHeadingDeg < 0) compassHeadingDeg += 360.0f;
  if (compassHeadingDeg >= 360.0f) compassHeadingDeg -= 360.0f;
}

void updateSimulatedAltitude() {
  unsigned long now = millis();

  if (lastAltitudeUpdate == 0) {
    lastAltitudeUpdate = now;
    return;
  }

  float dt = (now - lastAltitudeUpdate) / 1000.0f;
  lastAltitudeUpdate = now;

  // dead zone за да не “плива” кога е речиси рамно
  float p = pitchDeg;

  if (abs(p) < 2.0f) {
    p = 0;
  }

  // брзина на промена на висина
  // 1 степен pitch ≈ 12 ft/sec
  float climbRateFtPerSec = -p * 12.0f;

  altitudeFt += climbRateFtPerSec * dt;

  // ограничување
  if (altitudeFt < 0) altitudeFt = 0;
  if (altitudeFt > 9999) altitudeFt = 9999;
}

void IRAM_ATTR updateEncoder() {
  int MSB = digitalRead(ENCODER_CLK);
  int LSB = digitalRead(ENCODER_DT);

  int encoded = (MSB << 1) | LSB;
  int sum = (lastEncoded << 2) | encoded;

  if (sum == 0b1101 || sum == 0b0100 || sum == 0b0010 || sum == 0b1011)
    encoderValue++;

  if (sum == 0b1110 || sum == 0b0111 || sum == 0b0001 || sum == 0b1000)
    encoderValue--;

  lastEncoded = encoded;
}

void handleBrightnessEncoder() {
  noInterrupts();
  long currentValue = encoderValue;
  interrupts();

  if (currentValue != lastEncoderValue) {
    int diff = currentValue - lastEncoderValue;

    brightnessLevel += diff * 4;

    if (brightnessLevel < 20) brightnessLevel = 20;
    if (brightnessLevel > 255) brightnessLevel = 255;

    ledcWrite(BL_PIN, brightnessLevel);

    lastEncoderValue = currentValue;

    Serial.print("Brightness: ");
    Serial.println(brightnessLevel);
  }
}

void setup() {
  Serial.begin(115200);

  pinMode(ENCODER_CLK, INPUT_PULLUP);
  pinMode(ENCODER_DT, INPUT_PULLUP);

  attachInterrupt(digitalPinToInterrupt(ENCODER_CLK), updateEncoder, CHANGE);
  attachInterrupt(digitalPinToInterrupt(ENCODER_DT), updateEncoder, CHANGE);

  init_display();

  pcf8574_init();

//  scanI2C();
  
  if (mpu6050_init()) {
  Serial.println("MPU6050 OK");
} else {
  Serial.println("MPU6050 NOT FOUND");
}

if (qmc5883l_init()) {
  Serial.println("QMC5883L OK");
} else {
  Serial.println("QMC5883L NOT FOUND");
}

  drawCurrentScreen();
}

void loop() {

  handleBrightnessEncoder();
  
  static bool lastBtn = false;

  bool btn = isEncoderButtonPressed();

  if (btn && !lastBtn) {
    currentScreen = (ScreenMode)((currentScreen + 1) % SCREEN_COUNT);
    drawCurrentScreen();
    delay(250);
  }

  lastBtn = btn;

  if (currentScreen == SCREEN_ATTITUDE && millis() - lastMPURead > 120) {
    lastMPURead = millis();
    updateMPU6050();
    drawCurrentScreen();
  }
 if (currentScreen == SCREEN_COMPASS && millis() - lastCompassRead > 120) {
  lastCompassRead = millis();
  updateQMC5883L();
  drawCurrentScreen();
}


if (currentScreen == SCREEN_ALTIMETER && millis() - lastMPURead > 120) {
  lastMPURead = millis();

  updateMPU6050();
  updateSimulatedAltitude();

  drawCurrentScreen();
}

if (currentScreen == SCREEN_AIRPLANE && millis() - lastMPURead > 120) {
  lastMPURead = millis();
  updateMPU6050();
  drawCurrentScreen();
}

}

Downloadable files

Code FINAL

code

Code FINAL.zip

Comments

Only logged in users can leave comments

mircemk

0 Followers

•

0 Projects

Intro