Added ESPNow RC screen

README.md

@@ -132,3 +132,8 @@ curl -s http://192.168.1.1/erase?filename=the_filename
 1. https://randomnerdtutorials.com/esp-now-auto-pairing-esp32-esp8266/ - Auto pairing
 2. https://github.com/Servayejc
 
+
+### TODO
+1. ESPNow RC Controller
+2. ESPNow SBUS (FPV Bridge)
+3. Changing + and - sign

src/ESPNow.cpp

@@ -20,21 +20,20 @@ void ESPNow2::begin(void) {
     WiFi.mode(WIFI_STA);
 
     uint8_t temp_addr[] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF };
-    uint8_t fpvbridge_addr[] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF };
 
-    fpvbridge_addr[0] = EEPROM.read(0);
-    fpvbridge_addr[1] = EEPROM.read(1);
-    fpvbridge_addr[2] = EEPROM.read(2);
-    fpvbridge_addr[3] = EEPROM.read(3);
-    fpvbridge_addr[4] = EEPROM.read(4);
-    fpvbridge_addr[5] = EEPROM.read(5);
+    destination_server_address[0] = EEPROM.read(0);
+    destination_server_address[1] = EEPROM.read(1);
+    destination_server_address[2] = EEPROM.read(2);
+    destination_server_address[3] = EEPROM.read(3);
+    destination_server_address[4] = EEPROM.read(4);
+    destination_server_address[5] = EEPROM.read(5);
 
     espnow_channel = EEPROM.read(6);
     if(espnow_channel > MAX_CHANNEL)
         espnow_channel = MAX_CHANNEL;
 
     Serial.print("FPV Bridge MAC: ");
-    printMAC(fpvbridge_addr);
+    printMAC(destination_server_address);
     Serial.println();
     Serial.print("On Channel: ");
     Serial.println(espnow_channel);
@@ -45,6 +44,74 @@ void ESPNow2::begin(void) {
     pairing_status = PAIR_REQUEST;
 }
 
+
+void ESPNow2::connectBridge(void) {
+    Serial.println("Connecting Brigde...");
+    
+    // Set WiFi Channel
+    esp_now_deinit();                                   // Cleaning Up
+    WiFi.mode(WIFI_STA);
+    esp_wifi_set_promiscuous(true);
+    esp_wifi_set_channel(espnow_channel, WIFI_SECOND_CHAN_NONE);
+    esp_wifi_set_promiscuous(false);
+    WiFi.disconnect();
+
+    if(esp_now_init() != ESP_OK) {
+        Serial.println("Error initializing ESP-NOW");
+        isConnected = false;
+        return;
+    }
+
+    Serial.print("Bridge channel: ");
+    Serial.println(WiFi.channel());
+
+    // Setting up callbacks
+    esp_now_register_send_cb(on_data_sent);
+    esp_now_register_recv_cb(on_data_recv);
+
+    memcpy(peerInfo.peer_addr, destination_server_address, 6);
+    peerInfo.channel = 0;
+    peerInfo.encrypt = false;
+    if(esp_now_add_peer(&peerInfo) != ESP_OK) {
+        Serial.println("Unable to connect bridge");
+        isConnected = false;
+        return;
+    }
+
+    isConnected = true;
+}
+
+
+void ESPNow2::update(void) {
+    if(espnow_begin) {
+        if(!isConnected) {
+            // Not connected...so... make it connect
+            connectBridge();
+        } else {
+            if((uint32_t)(millis() - last_updated) > 1) {
+                data_to_send.message_type = DATA;
+                data_to_send.throttle = map(inputs.throttle, 0, 4096, 1000, 2000);  // Throttle
+                data_to_send.yaw = map(inputs.yaw, 0, 4096, 1000, 2000);            // Yaw
+                data_to_send.pitch = map(inputs.pitch, 0, 4096, 1000, 2000);        // Pitch
+                data_to_send.roll = map(inputs.roll, 0, 4096, 1000, 2000);          // Roll
+
+                // Sending Data
+                esp_now_send(destination_server_address, (uint8_t *) &data_to_send, sizeof(data_to_send));
+
+                last_updated = millis();
+            }
+        }   
+    }
+
+    // Reset the profiler when hits 1 second
+    if((uint32_t)(millis() - last_seconds) > 1000) {
+        sent_counter = sent_counter_raw;
+        sent_counter_raw = 0;
+        last_seconds = millis();
+    }
+}
+
+
 PairingStatus ESPNow2::auto_pairing(void) {
     switch(pairing_status) {
         case PAIR_REQUEST:
@@ -94,6 +161,7 @@ PairingStatus ESPNow2::auto_pairing(void) {
             sprintf(m, "%02X:%02X:%02X:%02X:%02X:%02X", default_server_address[0], default_server_address[1], default_server_address[2], default_server_address[3], default_server_address[4], default_server_address[5]);
             Serial.println(m);
 
+
             millis_previous = millis();
             pairing_status = PAIR_REQUESTED;
 
@@ -125,8 +193,12 @@ PairingStatus ESPNow2::auto_pairing(void) {
 // Handling call backs
 void on_data_sent(const uint8_t *mac_addr, esp_now_send_status_t status) {
     // Remove the serial print to speed up the process. Only uncomment it for debugging purposes.
-    Serial.print("\r\nLast Packet Send Status:\t");
-    Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Delivery Success" : "Delivery Fail");
+    // Serial.print("\r\nLast Packet Send Status:\t");
+    // Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Delivery Success" : "Delivery Fail");
+
+    if(status == ESP_NOW_SEND_SUCCESS) {
+        espnow2.sent_counter_raw++;
+    }
 }
 
 void on_data_recv(const uint8_t * mac_addr, const uint8_t *incoming_data, int len) { 

src/ESPNow2.h

@@ -46,6 +46,8 @@ class ESPNow2 {
         PairingStatus auto_pairing(void);
         void printMAC(const uint8_t * mac_addr);
         void addPeer(const uint8_t * mac_addr, uint8_t channel);
+        void update(void);
+        void connectBridge(void);
 
         uint8_t espnow_channel_request = 1;
         uint8_t default_server_address[6];
@@ -59,6 +61,15 @@ class ESPNow2 {
         uint8_t espnow_channel;
         uint32_t millis_current, millis_previous;
 
+        bool espnow_begin = false;
+        int16_t sent_counter_raw = 0;
+        int16_t sent_counter = 0;
+
+        uint32_t last_seconds = 0;
+        uint32_t last_updated = 0;
+
+        bool isConnected = false;
+
     private:
 };
 

src/ESPNowRC.cpp

@@ -0,0 +1,229 @@
+#include "ESPNowRC.h"
+
+ESPNowRC::ESPNowRC(void) {
+    updated = true;
+}
+
+void ESPNowRC::begin(void) {
+    updated = true;
+}
+
+void ESPNowRC::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 ESPNowRC::show(TFT_eSprite *m) {
+    if(espnow2.espnow_begin == false) {
+        espnow2.espnow_begin = true;
+        Serial.println("ESPNOW Begin");
+    }
+
+    if((uint32_t) (millis() - last_updated) > 50) {
+        updated = true;
+    }
+
+    if((uint32_t) (millis() - last_voltage_read) > 500) {
+        voltage = roundf(M5.Axp.GetBatVoltage() * 10)/10;
+        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->print("P:");
+        m->print(String(espnow2.sent_counter));
+
+        m->setCursor(240, 0);
+        m->printf("CH:%d", espnow2.espnow_channel);
+
+        m->setCursor(74, 0);
+        m->printf("B:%.1fV %.0fmA", voltage, current);
+        // m->printf(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, stickcalibration.throttle_min, stickcalibration.throttle_max, 90, 0);
+        uint16_t left_gimbal_x = map(inputs.yaw, stickcalibration.yaw_min, stickcalibration.yaw_max, 90, 0);
+        m->fillCircle(left_gimbal_x + 69, left_gimbal_y + 150, 5, TFT_YELLOW);
+        uint16_t right_gimbal_y = map(inputs.pitch, stickcalibration.pitch_min, stickcalibration.pitch_max, 0, 90);
+        uint16_t right_gimbal_x = map(inputs.roll, stickcalibration.roll_min, stickcalibration.roll_max, 0, 90);
+        m->fillCircle(right_gimbal_x + 161, right_gimbal_y + 150, 5, TFT_YELLOW);
+
+        // Draw the switches
+        uint32_t last_millis = millis();
+
+        if(!espnow2.data_to_send.aux[0]) {
+            m->drawRect(0, 40, 60, 60, TFT_BLUE);       // Button 1 - OFF
+        } else {
+            m->drawRect(0, 40, 60, 60, TFT_BLUE);       // Button 1 - ON
+            m->fillRect(1, 41, 60, 60, TFT_BLUE);
+        }
+
+        if(!espnow2.data_to_send.aux[1]) {
+            m->drawRect(65, 40, 60, 60, TFT_BLUE);      // Button 2
+        } else {
+            m->drawRect(65, 40, 60, 60, TFT_BLUE);      // Button 2
+            m->fillRect(66, 41, 60, 60, TFT_BLUE);
+        }
+
+        if(!espnow2.data_to_send.aux[2]) {
+            m->drawRect(130, 40, 60, 60, TFT_BLUE);     // Button 3
+        } else {
+            m->drawRect(130, 40, 60, 60, TFT_BLUE);     // Button 3
+            m->fillRect(131, 41, 60, 60, TFT_BLUE);    
+        }
+
+        if(!espnow2.data_to_send.aux[3]) {
+            m->drawRect(195, 40, 60, 60, TFT_BLUE);     // Button 4
+        } else {
+            m->drawRect(195, 40, 60, 60, TFT_BLUE);     // Button 4
+            m->fillRect(196, 41, 60, 60, TFT_BLUE);
+        }
+
+        if(!espnow2.data_to_send.aux[4]) {
+            m->drawRect(260, 40, 60, 60, TFT_BLUE);     // Button 5
+        } else {
+            m->drawRect(260, 40, 60, 60, TFT_BLUE);     // Button 5
+            m->fillRect(261, 41, 60, 60, TFT_BLUE); 
+        }
+
+        m->setCursor(90, 130);
+        m->print("ESPNowRC");
+
+        m->pushSprite(0,0);
+
+        updated = false;
+        last_updated = millis();
+    }
+
+
+    // Key Input Handler
+    if(M5.BtnC.wasPressed()) {
+        if(espnow2.espnow_begin) {
+            espnow2.espnow_begin = false;
+            // ble.end();
+        }
+
+        screen.current_screen = SCREEN_MENU;
+        screen.updated = true;
+    }
+
+    // Temporary connect from here...
+    if(M5.BtnA.wasPressed()) {
+    }
+
+    // Handling inputs
+    Event &e = M5.Buttons.event;
+    coordinate = M5.Touch.getPressPoint();
+
+    // Making sure not retrigger...
+    if((uint32_t)(millis() - touch_time) > 100) {
+        if(e & (E_TOUCH)) {
+            Serial.printf("E_TOUCH X:%d, Y:%d\r\n", e.to.x, e.to.y);
+
+            // Determine touched positions:
+            // Left gimbal:
+            // - Top
+            //   - MinX: 110
+            //   - MinY: 150
+            //   - MaxX: 120
+            //   - MaxY: 180
+            // - Bottom
+            //   - MinX: 110
+            //   - MinY: 200
+            //   - MaxX: 120
+            //   - MaxY: 220
+            // - Left
+            //   - MinX: 80
+            //   - MinY: 180
+            //   - MaxX: 105
+            //   - MaxY: 210
+            // - Right
+            //   - MinX: 110
+            //   - MinY: 150
+            //   - MaxX: 160
+            //   - MaxY: 190
+
+            if(e.to.x >= 110 && e.to.x <= 120 && e.to.y >= 140 && e.to.y <= 180) {
+                // Minus throttle
+                // Serial.printf("Left: TOP\n");
+                inputs.trim_throttle-=20;
+                //// speak.speak_trim_word = TRIM_THROTTLE_INCREASE;
+                //// speak.speak_now_trim = true;
+            } else if(e.to.x >= 110 && e.to.x <= 170 && e.to.y >= 200 && e.to.y <= 220) {
+                // Add throttle
+                // Serial.printf("Left: BOTTOM\n");
+                inputs.trim_throttle+=20;
+                //// speak.speak_trim_word = TRIM_THROTTLE_DECREASE;
+                //// speak.speak_now_trim = true;
+            } else if(e.to.x >= 60 && e.to.x <= 105 && e.to.y >= 180 && e.to.y <= 210) {
+                // Minus Yaw
+                // Serial.printf("Left: LEFT\n");
+                inputs.trim_yaw-=20;
+                //// speak.speak_trim_word = TRIM_YAW_DECREASE;
+                //// speak.speak_now_trim = true;
+            } else if(e.to.x >= 110 && e.to.x <= 160 && e.to.y >= 150 && e.to.y <= 190) {
+                // Add Yaw
+                // Serial.printf("Left: RIGHT\n");
+                inputs.trim_yaw+=20;
+                //// speak.speak_trim_word = TRIM_YAW_INCREASE;
+                //// speak.speak_now_trim = true;
+            } else if(e.to.x >= 250 && e.to.x <= 230 && e.to.y >= 187 && e.to.y <= 197) {
+                // Add Row
+                inputs.trim_roll+=20;
+                //// speak.speak_trim_word = TRIM_ROLL_INCREASE;
+                //// speak.speak_now_trim = true;
+            } else if(e.to.x >= 180 && e.to.x <= 175 && e.to.y >= 187 && e.to.y <= 197) {
+                // Minus Row
+                inputs.trim_roll+=20;
+                //// speak.speak_trim_word = TRIM_ROLL_DECREASE;
+                //// speak.speak_now_trim = true;
+                // TODO: Continue the rest.....
+            } else if(e.to.x >= 0 && e.to.x <= 60 && e.to.y >= 40 && e.to.y <= 100) {
+                espnow2.data_to_send.aux[0] = !espnow2.data_to_send.aux[0];   // Flip Switch 0
+            } else if(e.to.x >= 65 && e.to.x <= 125 && e.to.y >= 40 && e.to.y <= 100) {
+                espnow2.data_to_send.aux[1] = !espnow2.data_to_send.aux[1];   // Flip Switch 1
+            } else if(e.to.x >= 130 && e.to.x <= 190 && e.to.y >= 40 && e.to.y <= 100) {
+                espnow2.data_to_send.aux[2] = !espnow2.data_to_send.aux[2];   // Flip Switch 2
+            } else if(e.to.x >= 195 && e.to.x <= 255 && e.to.y >= 40 && e.to.y <= 100) {
+                espnow2.data_to_send.aux[3] = !espnow2.data_to_send.aux[3];   // Flip Switch 3
+            } else if(e.to.x >= 260 && e.to.x <= 310 && e.to.y >= 40 && e.to.y <= 100) {
+                espnow2.data_to_send.aux[4] = !espnow2.data_to_send.aux[4];   // Flip Switch 4
+            }
+            touch_time = millis();
+        }
+    }
+
+    /***
+    if(e & (E_RELEASE)) {
+        Serial.printf("E_MOVE X:%d, Y:%d\r\n", e & E_MOVE ? e.from.x : e.to.x, e & E_MOVE ? e.from.y : e.to.y);
+    }
+    if(coordinate.x > 0 || coordinate.y > 0)
+        Serial.printf("x:%d, y:%d \r\n", coordinate.x, coordinate.y);   
+     ***/
+}
+
+ESPNowRC espnowrc;

src/ESPNowRC.h

@@ -0,0 +1,36 @@
+#ifndef _ESPNOWRC_H
+#define _ESPNOWRC_H
+
+#include <Arduino.h>
+#include <M5Core2.h>
+#include "ESPNow2.h"
+#include "Inputs.h"
+#include "Screen.h"
+
+class ESPNowRC {
+    public:
+        TouchPoint_t coordinate;
+        uint32_t touch_time = 0;
+        bool updated = false;
+        float voltage = 0.00;
+        float current = 0.00;
+
+        uint32_t last_button1 = 0;
+        uint32_t last_button2 = 0;
+        uint32_t last_button3 = 0;
+        uint32_t last_button4 = 0;
+        uint32_t last_button5 = 0;
+        uint32_t last_button6 = 0;
+
+        ESPNowRC(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 ESPNowRC espnowrc;
+#endif

src/ESPPair.cpp

@@ -6,6 +6,7 @@ ESPPair::ESPPair(void) {
 
 void ESPPair::begin(void) {
     updated = true;
+    channel = espnow2.espnow_channel;
 }
 
 void ESPPair::show(TFT_eSprite *m) {

src/Screen.cpp

@@ -47,6 +47,8 @@ void Screen::update(void) {
             stickcalibration.show(&mainscreen_buffer);
         } else if(current_screen == SCREEN_MENU_STICK_POSITIONS) {
             stickpositions.main(&mainscreen_buffer);
+        } else if(current_screen == SCREEN_MENU_ESPNOWRC) {
+            espnowrc.show(&mainscreen_buffer);
         }
 
         last_screen_update = millis();
@@ -82,20 +84,21 @@ void Screen::intro(void) {
 
 // Main Menu
 void Screen::menu(void) {
-    static uint8_t current_menu = MENU_BLUETOOTH;
+    static uint8_t current_menu = MENU_ESPNOW_RC;
     static uint8_t menu_start_position = 0;
     static uint8_t menu_max_position = MENU_MAX_ITEMS;
 
     static String menu_items_name[] = {
-        "Bluetooth                ",
-        "ESPNow Pairing           ",
-        "Stick Calibration        ",
-        "Stick Positions          ",
-        "Trims                    ",
-        "Save Settings            ",
-        "Reset                    ",
-        "Reboot                   ",
-        "Power Off                "
+        " ESPNow RC                ",
+        " Bluetooth                ",
+        " ESPNow Pairing           ",
+        " Stick Calibration        ",
+        " Stick Positions          ",
+        " Trims                    ",
+        " Save Settings            ",
+        " Reset                    ",
+        " Reboot                   ",
+        " Power Off                "
     };
     // Serial.println((int)sizeof(menu_items_name)/sizeof(menu_items_name[0]));
 
@@ -124,7 +127,7 @@ void Screen::menu(void) {
                 mainscreen_buffer.setTextColor(WHITE, BLACK);
             }
 
-            mainscreen_buffer.setCursor(10, y);
+            mainscreen_buffer.setCursor(0, y);
             mainscreen_buffer.print(menu_items_name[menu_pointer]);
 
             menu_count++;
@@ -135,6 +138,12 @@ void Screen::menu(void) {
         mainscreen_buffer.drawFastHLine(50, 239, 20, TFT_WHITE);
         mainscreen_buffer.drawFastHLine(150, 239, 20, TFT_WHITE);
         mainscreen_buffer.drawFastHLine(250, 239, 20, TFT_WHITE);
+
+        // Just Title
+        mainscreen_buffer.setTextColor(WHITE, RED);
+        mainscreen_buffer.setTextSize(4);
+        mainscreen_buffer.setCursor(0, 183);
+        mainscreen_buffer.printf("FPVController");
         
 
         mainscreen_buffer.pushSprite(0,0);
@@ -186,6 +195,8 @@ void Screen::menu(void) {
             current_screen = SCREEN_MENU_STICK_POSITIONS;
         } else if(current_menu == MENU_ESPNOW_PAIRING) {
             current_screen = SCREEN_MENU_ESPNOW_PAIRING;
+        } else if(current_menu == MENU_ESPNOW_RC) {
+            current_screen = SCREEN_MENU_ESPNOWRC;
         }
         updated = true;
     }

src/Screen.h

@@ -5,6 +5,7 @@
 #include <M5Core2.h>
 #include "BLEGamepad.h"
 #include "ESPPair.h"
+#include "ESPNowRC.h"
 #include "StickCalibration.h"
 #include "StickPositions.h"
 
@@ -25,10 +26,12 @@ enum screen_names {
     SCREEN_MENU_BLUETOOTH,
     SCREEN_MENU_STICK_CALIBRATION,
     SCREEN_MENU_STICK_POSITIONS,
-    SCREEN_MENU_ESPNOW_PAIRING
+    SCREEN_MENU_ESPNOW_PAIRING,
+    SCREEN_MENU_ESPNOWRC
 };
 
 enum menu_items {
+    MENU_ESPNOW_RC,
     MENU_BLUETOOTH,
     MENU_ESPNOW_PAIRING,
     MENU_STICK_CALIBRATION,

src/main.cpp

@@ -30,8 +30,9 @@ void setup() {
     ///////customwifi.begin();
     inputs.begin();
     stickcalibration.begin();
-    esppair.begin();
     espnow2.begin();
+    esppair.begin();
+    espnowrc.begin();
     // tx.begin();
     web.begin();
 
@@ -93,7 +94,17 @@ void setup() {
 
     xTaskCreatePinnedToCore(
         taskBLE,
-        "TaskBLE",            // Name of the process
+        "TaskBLE",              // Name of the process
+        8192,                   // This stack size can be checked & adjusted by reading the Stack Highwater
+        NULL,
+        4,                      // Priority
+        NULL,
+        CPU_0
+    );
+
+    xTaskCreatePinnedToCore(
+        taskESPNow,
+        "TaskESPNow",           // Name of the process
         8192,                   // This stack size can be checked & adjusted by reading the Stack Highwater
         NULL,
         4,                      // Priority
@@ -107,13 +118,23 @@ void loop() {
 }
 
 
+void taskESPNow(void *pvParameters) {
+    (void) pvParameters;
+
+    for(;;) {
+        espnow2.update();       
+        vTaskDelay(1);
+    }
+}
+
+
 void taskScreen(void *pvParameters) {
     (void) pvParameters;
     screen.intro();
 
     for(;;) {
         screen.update();
-        vTaskDelay(2);
+        vTaskDelay(1);
     }
 }
 

src/main.h

@@ -22,6 +22,7 @@
 #include "CustomWiFi.h"
 #include "ESPNow2.h"
 #include "ESPPair.h"
+#include "ESPNowRC.h"
 #include "Web.h"
 
 // Task Handler
@@ -30,6 +31,7 @@ void taskScreen(void *pvParameters);
 void taskWeb(void *pvParameters);
 void taskInput(void *pvParameters);
 void taskBLE(void *pvParameters);
+void taskESPNow(void *pvParameters);
 void taskCharger(void *pvParameters);
 
 // void on_data_sent(const uint8_t *mac_addr, esp_now_send_status_t status);