diff --git a/src/ESPNow.cpp b/src/ESPNow.cpp new file mode 100644 index 0000000..ffb066b --- /dev/null +++ b/src/ESPNow.cpp @@ -0,0 +1,195 @@ +#include "ESPNow2.h" + +// REF: +// https://raw.githubusercontent.com/RuiSantosdotme/Random-Nerd-Tutorials/master/Projects/ESP/ESP_NOW/ESP_NOW_Auto_Pairing/ESP32_Sender.ino + +#define BOARD_ID 1 +#define MAX_CHANNEL 13 + + +ESPNow2::ESPNow2(void) { +} + +void ESPNow2::begin(void) { +/** + 1. If configuration not found then start with peering mode. Other wise read the MAC + 2. Once peering mode done and it will receive the first message from the master node and save the configuration (Mac Address) + 3. Use the MAC address to keep broadcasting to the master node. + **/ + WiFi.mode(WIFI_STA); + EEPROM.begin(EEPROM_SIZE); + + uint8_t default_server_address[] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF }; + uint8_t fpvbridge_addr[] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF }; + // uint8_t temp_addr[] = { 0x08, 0xB6, 0x1F, 0x34, 0x89, 0x98 }; + + 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); + + espnow_channel = EEPROM.read(6); + if(espnow_channel > MAX_CHANNEL) + espnow_channel = MAX_CHANNEL; + + Serial.print("FPV Bridge MAC: "); + printMAC(fpvbridge_addr); + Serial.println(); + Serial.print("On Channel: "); + Serial.println(espnow_channel); + + memcpy(destination_server_address, default_server_address, sizeof(uint8_t[6])); + pairing_status = PAIR_REQUEST; +} + +PairingStatus ESPNow2::auto_pairing(void) { + switch(pairing_status) { + case PAIR_REQUEST: + // Set WiFi Channel + esp_now_deinit(); // Cleaning Up + WiFi.mode(WIFI_STA); + esp_wifi_set_promiscuous(true); + esp_wifi_set_channel(DEFAULT_CHANNEL, WIFI_SECOND_CHAN_NONE); + esp_wifi_set_promiscuous(false); + WiFi.disconnect(); + + ////// ESP_ERROR_CHECK(esp_wifi_set_channel(DEFAULT_CHANNEL, WIFI_SECOND_CHAN_NONE)); + if(esp_now_init() != ESP_OK) { + Serial.println("Error initializing ESP-NOW"); + } + Serial.println("Pairing on channel: "); + Serial.println(WiFi.channel()); + + //// esp_now_set_self_role(ESP_NOW_ROLE_COMBO); + + // Setting up callbacks + esp_now_register_send_cb(on_data_sent); + esp_now_register_recv_cb(on_data_recv); + + Serial.println("After esp_now_register..."); + + // Adding peer... + memcpy(peerInfo.peer_addr, default_server_address, 6); + peerInfo.channel = 0; + peerInfo.encrypt = false; + if(esp_now_add_peer(&peerInfo) != ESP_OK) { + Serial.println("Failed to add peer"); + return PAIR_REQUEST; + } + + // Setting up pairing data for sending to master node + pairing_data.message_type = PAIRING; + pairing_data.channel = espnow_channel; + + // Add peer and broadcasting it + // addPeer(default_server_address, espnow_channel); + // Serial.println("After addPeer..."); + + esp_now_send(destination_server_address, (uint8_t *) &pairing_data, sizeof(pairing_data)); + Serial.print("Sending to: "); + char m[18]; + sprintf(m, "%02X:%02X:%02X:%02X:%02X:%02X", destination_server_address[0], destination_server_address[1], destination_server_address[2], destination_server_address[3], destination_server_address[4], destination_server_address[5]); + Serial.println(m); + + millis_previous = millis(); + pairing_status = PAIR_REQUESTED; + + break; + + case PAIR_REQUESTED: + if(uint32_t(millis() - millis_previous) > 250) { + millis_previous = millis(); + + // Trying next channel + espnow_channel++; + + if(espnow_channel > MAX_CHANNEL) { + espnow_channel = 1; + } + + pairing_status = PAIR_REQUEST; + } + break; + + case PAIR_PAIRED: + // Nothing to do. + break; + } + + return pairing_status; +} + +// 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"); +} + +void on_data_recv(const uint8_t * mac_addr, const uint8_t *incoming_data, int len) { + // Remove the serial print to speed up the process. Only uncomment it for debugging purposes. + Serial.print("Packet received from: "); + espnow2.printMAC(mac_addr); + Serial.println(); + Serial.print("data size = "); + Serial.println(sizeof(incoming_data)); + + uint8_t type = incoming_data[0]; + switch(type) { + case DATA: // received data from server + // Do nothing. As this program interested on sending data but not receiving. + break; + + case PAIRING: // received pairing data from server + memcpy(&espnow2.pairing_data, incoming_data, sizeof(espnow2.pairing_data)); + espnow2.printMAC(mac_addr); + Serial.print("Pairing done for "); + // espnow2.printMAC(espnow2.pairing_data.mac_address); + Serial.print(" on channel " ); + Serial.print(espnow2.pairing_data.channel); // channel used by the server + // espnow2.addPeer(espnow2.pairing_data.mac_address, espnow2.pairing_data.channel); // add the server to the peer list + + // Save the details into EEPROM + EEPROM.write(0, mac_addr[0]); + EEPROM.write(1, mac_addr[1]); + EEPROM.write(2, mac_addr[2]); + EEPROM.write(3, mac_addr[3]); + EEPROM.write(4, mac_addr[4]); + EEPROM.write(5, mac_addr[5]); + EEPROM.write(6, espnow2.pairing_data.channel); + + EEPROM.commit(); + + Serial.println("Saved into EEPROM"); + espnow2.pairing_status = PAIR_PAIRED; // set the pairing status + break; + } +} + +void ESPNow2::printMAC(const uint8_t * mac_addr) { + char macStr[18]; + snprintf( + macStr, sizeof(macStr), "%02x:%02x:%02x:%02x:%02x:%02x", + mac_addr[0], mac_addr[1], mac_addr[2], mac_addr[3], mac_addr[4], mac_addr[5]); + Serial.print(macStr); +} + +void ESPNow2::addPeer(const uint8_t * mac_addr, uint8_t channel) { + return; + esp_now_peer_info_t peer; + ESP_ERROR_CHECK(esp_wifi_set_channel(channel ,WIFI_SECOND_CHAN_NONE)); + esp_now_del_peer(mac_addr); + memset(&peer, 0, sizeof(esp_now_peer_info_t)); + peer.channel = channel; + peer.encrypt = false; + memcpy(peer.peer_addr, mac_addr, sizeof(uint8_t[6])); + if (esp_now_add_peer(&peer) != ESP_OK){ + Serial.println("Failed to add peer"); + return; + } + memcpy(default_server_address, mac_addr, sizeof(uint8_t[6])); +} + +ESPNow2 espnow2; diff --git a/src/ESPNow2.h b/src/ESPNow2.h new file mode 100644 index 0000000..7f7f9ce --- /dev/null +++ b/src/ESPNow2.h @@ -0,0 +1,71 @@ +#ifndef _ESPNOW_H +#define _ESPNOW_H + +#include +#include +#include +#include "esp_wifi.h" +#include +#include "Screen.h" +#include "Storage.h" + + +#define DEFAULT_CHANNEL 1 +#define EEPROM_SIZE 7 + +typedef struct struct_data { + uint8_t message_type; + uint16_t throttle; + uint16_t yaw; + uint16_t roll; + uint16_t pitch; + bool aux[9]; +} struct_data; + +typedef struct struct_pairing { + uint8_t message_type; + uint8_t channel; +} struct_pairing; + +enum PairingStatus { + NOT_PAIRED, + PAIR_REQUEST, + PAIR_REQUESTED, + PAIR_PAIRED +}; + +enum MessageType { + PAIRING, + DATA +}; + +class ESPNow2 { + public: + ESPNow2(void); + void begin(void); + PairingStatus auto_pairing(void); + void printMAC(const uint8_t * mac_addr); + void addPeer(const uint8_t * mac_addr, uint8_t channel); + + uint8_t espnow_channel_request = 1; + uint8_t default_server_address[6]; + uint8_t destination_server_address[6]; + esp_now_peer_info_t peerInfo; + PairingStatus pairing_status = NOT_PAIRED; + struct_data data_to_send; + struct_data data_receive; + struct_pairing pairing_data; + MessageType message_type; + uint8_t espnow_channel; + uint32_t millis_current, millis_previous; + + private: +}; + + +void on_data_sent(const uint8_t *mac_addr, esp_now_send_status_t status); +void on_data_recv(const uint8_t * mac_addr, const uint8_t *incoming_data, int len); + + +extern ESPNow2 espnow2; +#endif diff --git a/src/ESPPair.cpp b/src/ESPPair.cpp new file mode 100644 index 0000000..198b9b6 --- /dev/null +++ b/src/ESPPair.cpp @@ -0,0 +1,126 @@ +#include "ESPPair.h" + +ESPPair::ESPPair(void) { + updated = true; +} + +void ESPPair::begin(void) { + updated = true; +} + +void ESPPair::show(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("ESPNow Configuration"); + + m->setTextColor(YELLOW, BLACK); + m->setCursor(0, 30); + m->print("Channel[1-13]: "); + + m->setCursor(0, 128); + m->print("MAC: "); + m->setTextColor(WHITE, BLACK); + m->setCursor(0, 144); + m->print(WiFi.macAddress()); + + m->setTextSize(5); + uint32_t last_millis = millis(); + if((uint32_t)(last_millis - last_button1) > 200) { + m->drawRect(0, 48, 60, 60, TFT_BLUE); // Button + + m->setTextColor(WHITE, BLACK); + } else { + m->drawRect(0, 48, 60, 60, TFT_BLUE); // Button + + m->fillRect(1, 49, 60, 60, TFT_BLUE); // Button + + m->setTextColor(WHITE, BLUE); + } + m->setCursor(16, 64); + m->print("+"); + + m->setCursor(280, 64); + if((uint32_t)(last_millis - last_button2) > 200) { + m->drawRect(260, 48, 60, 60, TFT_BLUE); // Button - + m->setTextColor(WHITE, BLACK); + } else { + m->drawRect(260, 48, 60, 60, TFT_BLUE); // Button - + m->fillRect(261, 49, 60, 60, TFT_BLUE); // Button - + m->setTextColor(WHITE, BLUE); + } + m->setCursor(280, 64); + m->print("-"); + + m->setCursor(20, 200); + if((uint32_t)(last_millis - last_button3) > 200) { + m->drawRect(0, 180, 320, 60, TFT_BLUE); // Button PAIR + m->setTextColor(WHITE, BLACK); + } else { + m->drawRect(0, 180, 320, 60, TFT_BLUE); // Button PAIR + m->fillRect(1, 181, 320, 60, TFT_BLUE); // Button PAIR + m->setTextColor(WHITE, BLUE); + } + m->setCursor(106, 190); + m->print("PAIR"); + + + m->setTextColor(WHITE, BLACK); + if(channel < 10) + m->setCursor(140, 64); + else + m->setCursor(133, 64); + + m->printf("%d", channel); + + m->pushSprite(0,0); + + updated = false; + last_updated = millis(); + } + + // Key Input Handler + if(M5.BtnC.wasPressed()) { + screen.current_screen = SCREEN_MENU; + screen.updated = true; + } + + // 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); + + if(e.to.x >= 0 && e.to.x <= 60 && e.to.y >= 48 && e.to.y <= 108) { + // Button + + channel++; + last_button1 = millis(); + } else if(e.to.x >= 260 && e.to.x <= 320 && e.to.y >= 48 && e.to.y <= 108) { + // Button - + channel--; + last_button2 = millis(); + } else if(e.to.x >= 0 && e.to.x <= 320 && e.to.y >= 180 && e.to.y <= 240) { + // Button PAIR + espnow2.espnow_channel = channel; + espnow2.auto_pairing(); + last_button3 = millis(); + } + + if(channel > ESPNOW_MAX_CHANNEL) + channel--; + + if(channel < 1) + channel++; + + touch_time = millis(); + } + } +} + +ESPPair esppair; diff --git a/src/ESPPair.h b/src/ESPPair.h new file mode 100644 index 0000000..5b310b5 --- /dev/null +++ b/src/ESPPair.h @@ -0,0 +1,33 @@ +#ifndef _ESP_PAIR_H +#define _ESP_PAIR_H + +#include +#include +#include +#include "ESPNow2.h" +#include "Screen.h" +#include "Storage.h" + +#define ESPNOW_MAX_CHANNEL 13 + +class ESPPair { + public: + ESPPair(void); + void begin(void); + void show(TFT_eSprite *m); + + private: + uint32_t last_updated = 0; + uint32_t last_button1 = 0; + uint32_t last_button2 = 0; + uint32_t last_button3 = 0; + + bool updated = false; + TouchPoint_t coordinate; + uint32_t touch_time = 0; + + uint8_t channel = 1; +}; + +extern ESPPair esppair; +#endif