Working BLE

README.md

@@ -48,7 +48,7 @@ ESP32 Dev Pinout:
                              U2_TXD / GPIO17 | [ ]               [ ] | GPIO26 / ADC2_9 / DAC2           
                           V_SPI_CS0 /  GPIO5 | [ ]  ___________  [ ] | GPIO25 / ADC2_8 / DAC1          
                     SCK / V_SPI_CLK / GPIO18 | [ ] |           | [ ] | GPIO33 / ADC1_5 / Touch8 / XTAL32 
-            U0_CTS / MSIO / V_SPI_Q / GPIO19 | [ ] |           | [ ] | GPIO32 / ADC1_4 / Touch9 / XTAL32
+            U0_CTS / MISO / V_SPI_Q / GPIO19 | [ ] |           | [ ] | GPIO32 / ADC1_4 / Touch9 / XTAL32
                      SDA / V_SPI_HD / GPIO21 | [ ] |           | [ ] | GPIO35 / ADC1_7                 
                       CLK2 / U0_RXD /  GPIO3 | [ ] |           | [ ] | GPIO34 / ADC1_6 
                       CLK3 / U0_TXD /  GPIO1 | [ ] |           | [ ] | GPIO39 / ADC1_3 / SensVN 
@@ -64,3 +64,14 @@ ESP32 Dev Pinout:
 # REF:
 Pinout and labels - https://docs.m5stack.com/en/core/core2_for_aws
 MP3 Sound generator - https://ttsmp3.com/
+
+GPIO35 - Throttle
+GPIO36 - Yaw
+GPIO32 - Pitch
+GPIO33 - Roll
+
+GPIO27 - CSN  - BLUE
+GPIO19 - CE   - WHITE
+       - SCK  - GREEN
+       - MOSI - YELLOW
+       - MISO - RED

platformio.ini

@@ -18,3 +18,4 @@ board_build.partitions = bert2M_fat12M_16MB.csv
 lib_deps = 
 	m5stack/M5Core2@^0.1.0
 	lorol/LittleFS_esp32@^1.0.6
+	lemmingdev/ESP32-BLE-Gamepad@^0.3.4

src/BLE.cpp

@@ -0,0 +1,51 @@
+#include "BLE.h"
+
+BleGamepad bleGamepad;
+
+BLE::BLE(void) {
+    updated = true;
+}
+
+void BLE::begin(void) {
+    updated = true;
+    ble_begin = true;
+}
+
+
+void BLE::end(void) {
+    ble_begin = false;
+    ble_started = false;
+
+    Serial.printf("BLEGamepad is disconnected");
+    bleGamepad.end();
+}
+
+
+void BLE::update(void) {
+    // Starts to connect when receive this...
+    if(ble_begin) {
+        if(!ble_started) {
+            Serial.printf("Starting BLEGamepad.");
+            bleGamepad.begin();
+            Serial.printf("BLEGamepad OK");
+            ble_started = true;
+        }
+
+
+        if(bleGamepad.isConnected()) {
+            if((uint32_t)(millis() - last_updated) > 30) {
+                int16_t throttle = map(inputs.throttle, 0, 4096, -32767, 32767); // Throttle
+                int16_t yaw = map(inputs.yaw, 0, 4096, -32767, 32767);           // Yaw
+                int16_t pitch = map(inputs.pitch, 0, 4096, -32767, 32767);       // Pitch
+                int16_t roll = map(inputs.roll, 0, 4096, -32767, 32767);         // Roll
+
+                bleGamepad.setAxes(yaw, throttle, roll, pitch, 0, 0, DPAD_CENTERED);
+
+                last_updated = millis();
+                // Serial.printf("Sent\n");
+            } 
+        }
+    }
+}
+
+BLE ble;

src/BLE.h

@@ -0,0 +1,25 @@
+#ifndef _BLE_H
+#define _BLE_H
+
+#include <Arduino.h>
+#include <M5Core2.h>
+#include <BleGamepad.h>
+#include "Inputs.h"
+
+class BLE {
+    public:
+        bool ble_begin = false;
+        bool updated = false;
+        bool ble_started = false;
+
+        BLE(void);
+        void begin(void);
+        void end(void);
+        void update(void); 
+
+    private:
+        uint32_t last_updated = 0;
+};
+
+extern BLE ble;
+#endif

src/BLEGamepad.cpp

@@ -0,0 +1,61 @@
+#include "BLEGamepad.h"
+
+BLEGamepad::BLEGamepad(void) {
+    updated = true;
+}
+
+void BLEGamepad::begin(void) {
+    updated = true;
+}
+
+
+void BLEGamepad::show(TFT_eSprite *m) {
+    if(!ble_begin) {
+        ble_begin = true;
+        ble.begin();
+    }
+
+    if((uint32_t) (millis() - last_updated) > 50) {
+        updated = true;
+    }
+
+    if(updated) {
+        m->fillRect(0, 0, 320, 240, BLACK);
+        m->setTextColor(WHITE, BLACK);
+        m->setTextSize(2);
+        m->setCursor(0,0);
+        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(64,64);
+        m->print("BLE GAMEPAD");
+
+        m->pushSprite(0,0);
+
+        updated = false;
+        last_updated = millis();
+    }
+
+
+    // Key Input Handler
+    if(M5.BtnC.wasPressed()) {
+        if(ble_begin) {
+            ble_begin = false;
+            ble.end();
+        }
+
+        screen.current_screen = SCREEN_MENU;
+        screen.updated = true;
+    }
+
+    // Temporary connect from here...
+    if(M5.BtnA.wasPressed()) {
+    }
+}
+
+BLEGamepad blegamepad;

src/BLEGamepad.h

@@ -0,0 +1,24 @@
+#ifndef _BLEGAMEPAD_H
+#define _BLEGAMEPAD_H
+
+#include <Arduino.h>
+#include <M5Core2.h>
+#include "BLE.h"
+#include "Inputs.h"
+#include "Screen.h"
+
+class BLEGamepad {
+    public:
+        bool ble_begin = false;
+        bool updated = false;
+
+        BLEGamepad(void);
+        void begin(void);
+        void show(TFT_eSprite *m);
+
+    private:
+        uint32_t last_updated = 0;
+};
+
+extern BLEGamepad blegamepad;
+#endif

src/GimbalGraph.cpp

@@ -0,0 +1,5 @@
+#include "GimbalGraph.h"
+
+GimbalGraph::GimbalGraph(void) {
+}
+

src/GimbalGraph.h

@@ -0,0 +1,14 @@
+#ifndef _GIMBAL_GRAPH_H
+#define _GIMBAL_GRAPH_H
+
+#include <Arduino.h>
+
+class GimbalGraph {
+    public:
+        GimbalGraph(void);
+
+    private:
+};
+
+extern GimbalGraph gimbalgraph;
+#endif

src/Inputs.cpp

@@ -0,0 +1,259 @@
+#include "Inputs.h"
+
+Inputs::Inputs(void) {
+}
+
+
+void Inputs::begin(void) {
+    throttle_raw = 0;
+    yaw_raw      = 0;
+    pitch_raw    = 0;
+    roll_raw     = 0;
+
+    // Prepare the storage engine for TYPR
+    for(int i = 0; i < MEDIAN_TOTAL; i++) {
+        throttle_pool[i] = 0;
+        yaw_pool[i]      = 0;
+        pitch_pool[i]    = 0;
+        roll_pool[i]     = 0;
+    }
+}
+
+
+void Inputs::read(void) {
+    throttle_raw = analogRead(THROTTLE_PIN);
+    yaw_raw      = analogRead(YAW_PIN);
+    pitch_raw    = analogRead(PITCH_PIN);
+    roll_raw     = analogRead(ROLL_PIN);
+
+    throttle     = pool_insert(THROTTLE, throttle_raw);
+    yaw          = pool_insert(YAW, yaw_raw);
+    pitch        = pool_insert(PITCH, pitch_raw);
+    roll         = pool_insert(ROLL, roll_raw);
+}
+
+
+/*
+ * median_get:
+ * To get the median from the data depending on the median_type
+ *
+ * E.g.:
+ * retval = median_get(THROTTLE);
+ */
+uint16_t Inputs::median_get(uint8_t median_type) {
+    bool inserted = false;
+
+    uint16_t median_tmp[MEDIAN_TOTAL];
+
+    // Loop through the variable to determine position to insert
+    //  [ 12, 32, 4 ,0, 50, 2, 10, 10, 5, 20, 0 ]
+    /*
+        [ 12 ]
+        [ 12, 32 ]
+        [ 4, 12, 32 ]
+        [ 0, 4, 12, 32 ]
+        [ 0, 4, 12, 32, 50 ]
+        [ 0, 2, 4, 12, 32, 50 ]
+
+     */
+
+    // Initial
+    uint16_t temp_val = 0;
+    uint16_t total_insert = 1;
+
+    // Search on type
+    if(median_type == THROTTLE) {
+        median_tmp[0] = throttle_pool[0];
+    } else if(median_type == YAW) {
+        median_tmp[0] = yaw_pool[0];
+    } else if(median_type == PITCH) {
+        median_tmp[0] = pitch_pool[0];
+    } else if(median_type == ROLL) {
+        median_tmp[0] = roll_pool[0];
+    } 
+
+    // Loop insert and sort
+    for(int raw_count = 1; raw_count < MEDIAN_TOTAL; raw_count++) {
+        inserted = false;
+
+        if(median_type == THROTTLE) {
+            temp_val = throttle_pool[raw_count];
+        } else if(median_type == YAW) {
+            temp_val = yaw_pool[raw_count];
+        } else if(median_type == PITCH) {
+            temp_val = pitch_pool[raw_count];
+        } else if(median_type == ROLL) {
+            temp_val = roll_pool[raw_count];
+        }
+
+        for(int median_count = 0; median_count < total_insert; median_count++) {
+            if(!inserted) {
+
+                if(temp_val < median_tmp[median_count]) {
+                    inserted = true;
+
+                    // Reverse copy and insert
+                    for(int median_reverse = total_insert + 1; median_reverse > median_count; median_reverse--) {
+                        median_tmp[median_reverse] = median_tmp[median_reverse - 1];
+                    }
+
+                    // Insert the detected
+                    median_tmp[median_count] = temp_val;
+
+                    // Increase total insertion
+                    total_insert++;
+                }
+            }
+        }
+
+        // Nothing was inserted... so put at the last.
+        if(!inserted) {
+            median_tmp[total_insert++] = temp_val;
+        }
+    }
+
+    // Return the result
+    // return median_tmp[MEDIAN_POS];
+    // Using RC dead band way....**** NEED TO IMPROvE!!
+    if(median_type == THROTTLE) {
+        if(throttle > median_tmp[MEDIAN_POS]) {
+            if(throttle - median_tmp[MEDIAN_POS] > RC_DEADBAND) {
+                return median_tmp[MEDIAN_POS];
+            } else {
+                if(median_tmp[MEDIAN_POS] - throttle > RC_DEADBAND) {
+                    return median_tmp[MEDIAN_POS];
+                } else {
+                    return throttle;
+                }
+            }
+        }
+    } else if(median_type == YAW) {
+        if(yaw > median_tmp[MEDIAN_POS]) {
+            if(yaw - median_tmp[MEDIAN_POS] > RC_DEADBAND) {
+                return median_tmp[MEDIAN_POS];
+            } else {
+                if(median_tmp[MEDIAN_POS] - yaw > RC_DEADBAND) {
+                    return median_tmp[MEDIAN_POS];
+                } else {
+                    return yaw;
+                }
+            }
+        }
+    } else if(median_type == PITCH) {
+        if(pitch > median_tmp[MEDIAN_POS]) {
+            if(pitch - median_tmp[MEDIAN_POS] > RC_DEADBAND) {
+                return median_tmp[MEDIAN_POS];
+            } else {
+                if(median_tmp[MEDIAN_POS] - pitch > RC_DEADBAND) {
+                    return median_tmp[MEDIAN_POS];
+                } else {
+                    return pitch;
+                }
+            }
+        }
+    } else if(median_type == ROLL) {
+        if(roll > median_tmp[MEDIAN_POS]) {
+            if(roll - median_tmp[MEDIAN_POS] > RC_DEADBAND) {
+                return median_tmp[MEDIAN_POS];
+            } else {
+                if(median_tmp[MEDIAN_POS] - roll > RC_DEADBAND) {
+                    return median_tmp[MEDIAN_POS];
+                } else {
+                    return roll;
+                }
+            }
+        }
+    }
+
+}
+
+
+uint16_t Inputs::throttle_insert(uint16_t val) {
+    // Insert to the array...at the last 
+    for(int i = 1; i < MEDIAN_TOTAL; i++) 
+        // Shift to left
+        throttle_pool[i - 1] = throttle_pool[i];
+    
+    throttle_pool[MEDIAN_TOTAL - 1] = val;
+
+    // Find median....
+    return median_get(THROTTLE);
+}
+
+
+uint16_t Inputs::yaw_insert(uint16_t val) {
+    // Insert to the array...at the last 
+    for(int i = 1; i < MEDIAN_TOTAL; i++) 
+        // Shift to left
+        yaw_pool[i - 1] = yaw_pool[i];
+    
+    yaw_pool[MEDIAN_TOTAL - 1] = val;
+
+    // Find median....
+    return median_get(YAW);
+}
+
+
+uint16_t Inputs::pitch_insert(uint16_t val) {
+    // Insert to the array...at the last 
+    for(int i = 1; i < MEDIAN_TOTAL; i++) 
+        // Shift to left
+        pitch_pool[i - 1] = pitch_pool[i];
+    
+    pitch_pool[MEDIAN_TOTAL - 1] = val;
+
+    // Find median....
+    return median_get(PITCH);
+}
+
+
+uint16_t Inputs::roll_insert(uint16_t val) {
+    // Insert to the array...at the last 
+    for(int i = 1; i < MEDIAN_TOTAL; i++) 
+        // Shift to left
+        roll_pool[i - 1] = roll_pool[i];
+    
+    roll_pool[MEDIAN_TOTAL - 1] = val;
+
+    // Find median....
+    return median_get(ROLL);
+}
+
+
+/*
+ * pool_insert:
+ * To insert the data into the median pool depending on type of the variable
+ *
+ * E.g.:
+ * throttle_value = pool_insert(THROTTLE);
+ */
+uint16_t Inputs::pool_insert(uint8_t pool_type, uint16_t val) {
+    // Insert to the array...at the last of the array
+    for(int i = 1; i < MEDIAN_TOTAL; i++) {
+        // Shift to left
+        if(pool_type == THROTTLE) {
+            throttle_pool[i - 1] = throttle_pool[i];
+        } else if(pool_type == YAW) {
+            yaw_pool[i - 1] = yaw_pool[i];
+        } else if(pool_type == PITCH) {
+            pitch_pool[i - 1] = pitch_pool[i];
+        } else if(pool_type == ROLL) {
+            roll_pool[i - 1] = roll_pool[i];
+        }
+    }
+  
+    // Find median....
+    if(pool_type == THROTTLE) {
+        throttle_pool[MEDIAN_TOTAL - 1] = val;
+    } else if(pool_type == YAW) {
+        yaw_pool[MEDIAN_TOTAL - 1] = val;
+    } else if(pool_type == ROLL) {
+        roll_pool[MEDIAN_TOTAL - 1] = val;
+    } else if(pool_type == PITCH) {
+        pitch_pool[MEDIAN_TOTAL - 1] = val;
+    }
+    return median_get(pool_type);
+}
+
+
+Inputs inputs;

src/Inputs.h

@@ -0,0 +1,56 @@
+#ifndef _INPUTS_H
+#define _INPUTS_H
+
+#include <Arduino.h>
+
+#define THROTTLE_PIN    35
+#define YAW_PIN         36
+#define PITCH_PIN       32
+#define ROLL_PIN        33
+
+#define MEDIAN_TOTAL    11
+#define MEDIAN_POS      MEDIAN_TOTAL/2
+
+// For RC Deadband
+#define RC_DEADBAND     5
+
+enum input_names {
+    THROTTLE = 0,
+    YAW,
+    PITCH,
+    ROLL
+};
+
+class Inputs {
+    public:
+        uint16_t throttle = 0;
+        uint16_t yaw      = 0;
+        uint16_t pitch    = 0;
+        uint16_t roll     = 0;
+
+        Inputs(void);
+        void begin(void);
+        void read(void);
+
+    private:
+        uint16_t throttle_pool[MEDIAN_TOTAL];
+        uint16_t yaw_pool[MEDIAN_TOTAL];
+        uint16_t pitch_pool[MEDIAN_TOTAL];
+        uint16_t roll_pool[MEDIAN_TOTAL];
+        uint16_t tmp_median_store[MEDIAN_TOTAL];
+
+        uint16_t throttle_raw;
+        uint16_t yaw_raw;
+        uint16_t pitch_raw;
+        uint16_t roll_raw;
+
+        uint16_t median_get(uint8_t median_type);
+        uint16_t throttle_insert(uint16_t val);
+        uint16_t yaw_insert(uint16_t val);
+        uint16_t pitch_insert(uint16_t val);
+        uint16_t roll_insert(uint16_t val);
+        uint16_t pool_insert(uint8_t pool_type, uint16_t val);
+};
+
+extern Inputs inputs;
+#endif

src/Screen.cpp

@@ -20,6 +20,9 @@ void Screen::begin(void) {
                      */
     // Initial value
     updated = true;
+
+    stickcalibration.begin();
+
     M5.Lcd.fillScreen(BLACK);
     mainscreen_buffer.createSprite(320, 240);
 }
@@ -36,6 +39,10 @@ void Screen::update(void) {
             menu_reboot();
         } else if(current_screen == SCREEN_MENU_POWEROFF) {
             menu_poweroff();
+        } else if(current_screen == SCREEN_MENU_BLUETOOTH) {
+            blegamepad.show(&mainscreen_buffer);
+        } else if(current_screen == SCREEN_MENU_STICK_CALIBRATION) {
+            stickcalibration.mydebug(&mainscreen_buffer);
         }
 
         last_screen_update = millis();
@@ -171,6 +178,9 @@ void Screen::menu(void) {
             current_screen = SCREEN_MENU_POWEROFF;
         } else if(current_menu == MENU_BLUETOOTH) {
             current_screen = SCREEN_MENU_BLUETOOTH;
+        } else if(current_menu == MENU_STICK_CALIBRATION) {
+            current_screen = SCREEN_MENU_STICK_CALIBRATION;
+            stickcalibration.updated = true;
         }
         updated = true;
     }

src/Screen.h

@@ -3,7 +3,9 @@
 
 #include <Arduino.h>
 #include <M5Core2.h>
+#include "BLEGamepad.h"
 #include "Speak.h"
+#include "StickCalibration.h"
 
 #define REFRESH_TIME 1
 #define INTRO_TIME 100
@@ -19,7 +21,8 @@ enum screen_names {
     SCREEN_MENU,
     SCREEN_MENU_REBOOT,
     SCREEN_MENU_POWEROFF,
-    SCREEN_MENU_BLUETOOTH
+    SCREEN_MENU_BLUETOOTH,
+    SCREEN_MENU_STICK_CALIBRATION
 };
 
 enum menu_items {
@@ -46,6 +49,9 @@ enum selection_choice_yesno {
 
 class Screen {
     public:
+        uint8_t current_screen = SCREEN_INTRO;
+        bool updated = false;
+
         Screen(void);
         void begin(void);
         void intro(void);
@@ -56,9 +62,7 @@ class Screen {
         void update(void);
 
     private:
-        uint8_t current_screen = SCREEN_INTRO;
         uint32_t last_screen_update = 0;
-        bool updated = false;
 };
 
 extern Screen screen;

src/StickCalibration.cpp

@@ -0,0 +1,44 @@
+#include "StickCalibration.h"
+
+StickCalibration::StickCalibration(void) {
+    updated = true;
+}
+
+void StickCalibration::begin(void) {
+    updated = true;
+}
+
+
+void StickCalibration::mydebug(TFT_eSprite *m) {
+    if((uint32_t) (millis() - last_updated) > 50) {
+        updated = true;
+    }
+
+    if(updated) {
+        m->fillRect(0, 0, 320, 240, BLACK);
+        m->setTextColor(WHITE, BLACK);
+        m->setTextSize(2);
+        m->setCursor(0,0);
+        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->pushSprite(0,0);
+
+        updated = false;
+        last_updated = millis();
+    }
+
+
+    // Key Input Handler
+    if(M5.BtnC.wasPressed()) {
+        screen.current_screen = SCREEN_MENU;
+        screen.updated = true;
+    }
+}
+
+StickCalibration stickcalibration;

src/StickCalibration.h

@@ -0,0 +1,21 @@
+#ifndef _STICK_CALIBRATION_H
+#define _STICK_CALIBRATION_H
+
+#include <Arduino.h>
+#include <M5Core2.h>
+#include "Inputs.h"
+#include "Screen.h"
+
+class StickCalibration {
+    public:
+        StickCalibration(void);
+        void begin(void);
+        void mydebug(TFT_eSprite *m);
+        bool updated = false;
+
+    private:
+        uint32_t last_updated = 0;
+};
+
+extern StickCalibration stickcalibration;
+#endif

src/main.cpp

@@ -23,9 +23,11 @@ void setup() {
     
     storage.begin();
     customwifi.begin();
+    inputs.begin();
     web.begin();
     speak.welcome();
 
+
     // Task Creation
     xTaskCreatePinnedToCore(
         taskScreen,
@@ -39,7 +41,7 @@ void setup() {
 
     xTaskCreatePinnedToCore(
         taskWeb,
-        "TaskWeb",           // Name of the process
+        "TaskWeb",              // Name of the process
         4096,                   // This stack size can be checked & adjusted by reading the Stack Highwater
         NULL,
         4,                      // Priority
@@ -57,6 +59,26 @@ void setup() {
         CPU_0
     );
 
+    xTaskCreatePinnedToCore(
+        taskInput,
+        "TaskInput",            // Name of the process
+        8192,                   // This stack size can be checked & adjusted by reading the Stack Highwater
+        NULL,
+        4,                      // Priority
+        NULL,
+        CPU_0
+    );
+
+    xTaskCreatePinnedToCore(
+        taskBLE,
+        "TaskBLE",            // Name of the process
+        8192,                   // This stack size can be checked & adjusted by reading the Stack Highwater
+        NULL,
+        4,                      // Priority
+        NULL,
+        CPU_0
+    );
+
 }
 
 
@@ -101,3 +123,23 @@ void taskWeb(void *pvParameters) {
         vTaskDelay(100);
     }
 }
+
+
+void taskInput(void *pvParameters) {
+    (void) pvParameters;
+    
+    for(;;) {
+        inputs.read();       
+        vTaskDelay(10);
+    }
+}
+
+
+void taskBLE(void *pvParameters) {
+    (void) pvParameters;
+    
+    for(;;) {
+        ble.update();       
+        vTaskDelay(10);
+    }
+}

src/main.h

@@ -6,6 +6,8 @@
 #define CPU_0 0
 
 #include <M5Core2.h>
+#include "Inputs.h"
+#include "BLE.h"
 #include "Speak.h"
 #include "Screen.h"
 #include "Storage.h"
@@ -16,5 +18,7 @@
 void taskScreen(void *pvParameters);
 void taskSpeak(void *pvParameters);
 void taskWeb(void *pvParameters);
+void taskInput(void *pvParameters);
+void taskBLE(void *pvParameters);
 
 #endif