From 066925280a63e68901684ff7e60a3f9a53ab9980 Mon Sep 17 00:00:00 2001
From: Englebert <=>
Date: Sun, 13 Mar 2022 15:25:23 +0800
Subject: [PATCH] Added gimbal position in graphical way and also temporary
 voltage reading.

---
 README.md          |  1 +
 src/BLEGamepad.cpp | 53 ++++++++++++++++++++++++++++++++++++++--------
 src/BLEGamepad.h   |  4 ++++
 src/Inputs.h       |  3 +++
 4 files changed, 52 insertions(+), 9 deletions(-)

diff --git a/README.md b/README.md
index ec61824..d32ec6f 100644
--- a/README.md
+++ b/README.md
@@ -62,6 +62,7 @@ ESP32 Dev Pinout:
                                              +-----------------------+
 
 # REF:
+Document on How to Use: https://github.com/m5stack/m5-docs/blob/master/docs/en/api/lcd.md
 Pinout and labels - https://docs.m5stack.com/en/core/core2_for_aws
 MP3 Sound generator - https://ttsmp3.com/
 
diff --git a/src/BLEGamepad.cpp b/src/BLEGamepad.cpp
index 8f51e8b..692f63f 100644
--- a/src/BLEGamepad.cpp
+++ b/src/BLEGamepad.cpp
@@ -8,6 +8,24 @@ void BLEGamepad::begin(void) {
     updated = true;
 }
 
+void BLEGamepad::draw_gimbal(TFT_eSprite *m, uint16_t x, uint16_t y) {
+    // Top
+    m->drawFastHLine(x, y, 90, TFT_DARKGREEN); 
+    // Bottom
+    m->drawFastHLine(x, y + 89, 90, TFT_DARKGREEN);
+    // Left
+    m->drawFastVLine(x, y, 90, TFT_DARKGREEN);
+    // Right
+    m->drawFastVLine(x + 89, y, 90, TFT_DARKGREEN);
+
+    // Middle dotted lines
+    for(uint16_t x1 = x; x1 < x + 90; x1 += 10) {
+        m->drawFastHLine(x1, y + 45, 5, TFT_DARKGREEN);
+    }
+    for(uint16_t y1 = y; y1 < y + 90; y1 += 10) {
+        m->drawFastVLine(x + 45, y1, 5, TFT_DARKGREEN);
+    }
+}
 
 void BLEGamepad::show(TFT_eSprite *m) {
     if(!ble_begin) {
@@ -19,23 +37,40 @@ void BLEGamepad::show(TFT_eSprite *m) {
         updated = true;
     }
 
+
+    if((uint32_t) (millis() - last_voltage_read) > 500) {
+        voltage = M5.Axp.GetBatVoltage();
+        current = M5.Axp.GetBatCurrent();
+
+        last_voltage_read = millis();
+    }
+
     if(updated) {
         m->fillRect(0, 0, 320, 240, BLACK);
         m->setTextColor(WHITE, BLACK);
         m->setTextSize(2);
-        m->setCursor(0,0);
+        m->setCursor(0, 0);
         m->print("Packet(s) Sent: ");
         m->print(String(ble.sent_counter));
-        // m->print(String(inputs.throttle));
-        // m->setCursor(0,16);
-        // m->print(String(inputs.yaw));
-        // m->setCursor(0,32);
-        // m->print(String(inputs.pitch));
-        // m->setCursor(0,48);
-        // m->print(String(inputs.roll));
 
+        m->setCursor(0, 17);
+        m->print("Bat: ");
+        m->print(String(voltage) + "V " + String(current) + "mA");
+
+        // Draw the gimbal box
+        draw_gimbal(m, 69, 150);
+        draw_gimbal(m, 161, 150);
+        // Show the position of the left and right gimbal
+        // Convert the throttle and raw to x and y
+        // Temporary assume 4096 = max
+        uint16_t left_gimbal_y = map(inputs.throttle, 0, 4096, 0, 90);
+        uint16_t left_gimbal_x = map(inputs.yaw, 0, 4096, 0, 90);
+        m->fillCircle(left_gimbal_x + 69, left_gimbal_y + 150, 5, TFT_YELLOW);
+        uint16_t right_gimbal_y = map(inputs.pitch, 0, 4096, 0, 90);
+        uint16_t right_gimbal_x = map(inputs.roll, 0, 4096, 0, 90);
+        m->fillCircle(right_gimbal_x + 161, right_gimbal_y + 150, 5, TFT_YELLOW);
 
-        m->setCursor(64,64);
+        m->setCursor(90, 130);
         m->print("BLE GAMEPAD");
 
         m->pushSprite(0,0);
diff --git a/src/BLEGamepad.h b/src/BLEGamepad.h
index 676e99c..ab54bd1 100644
--- a/src/BLEGamepad.h
+++ b/src/BLEGamepad.h
@@ -11,13 +11,17 @@ class BLEGamepad {
     public:
         bool ble_begin = false;
         bool updated = false;
+        float voltage = 0.00;
+        float current = 0.00;
 
         BLEGamepad(void);
         void begin(void);
         void show(TFT_eSprite *m);
+        void draw_gimbal(TFT_eSprite *m, uint16_t x, uint16_t y);
 
     private:
         uint32_t last_updated = 0;
+        uint32_t last_voltage_read = 0;
 };
 
 extern BLEGamepad blegamepad;
diff --git a/src/Inputs.h b/src/Inputs.h
index b077a6e..1861be6 100644
--- a/src/Inputs.h
+++ b/src/Inputs.h
@@ -2,6 +2,7 @@
 #define _INPUTS_H
 
 #include <Arduino.h>
+#include <M5Core2.h>
 
 #define THROTTLE_PIN    35
 #define YAW_PIN         36
@@ -33,6 +34,8 @@ class Inputs {
         void read(void);
 
     private:
+        uint32_t last_voltage_read = 0;
+
         uint16_t throttle_pool[MEDIAN_TOTAL];
         uint16_t yaw_pool[MEDIAN_TOTAL];
         uint16_t pitch_pool[MEDIAN_TOTAL];