From 8bcac85213b30464b0ea56822d655c473afe8902 Mon Sep 17 00:00:00 2001 From: Englebert Date: Wed, 1 May 2024 19:58:09 +0800 Subject: [PATCH] Updated buttons to send via ESPNow --- src/main.cpp | 724 +++++++++++++++++++++++++++++++++++++++++++++------ 1 file changed, 648 insertions(+), 76 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index e85215b..57d1196 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -82,10 +82,40 @@ int16_t med_roll[MEDIAN_MAX]; // Transmission modes -uint8_t transmission_mode = MODE_NONE; +// uint8_t transmission_mode = MODE_NONE; +uint8_t transmission_mode = MODE_ESPNOW; uint16_t ble_counter_raw = 0; uint16_t ble_counter = 0; +// ESPNOW variables +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; + +int16_t sent_counter_raw = 0; +int16_t sent_counter = 0; + +int16_t ruby_pitch = 0; +int16_t ruby_roll = 0; + +uint32_t last_seconds = 0; +uint32_t last_updated = 0; + +bool isConnected = false; +bool rubybutton_state[9]; +bool ruby_mode = false; +uint32_t last_rubybutton[9]; + +bool aux_state[9]; + bool toggles[TOGGLE_MAX]; @@ -138,14 +168,14 @@ lcd_cmd_t lcd_st7789v[] = { }; #endif -NimBLEAdvertising *pAdvertising; +//// NimBLEAdvertising *pAdvertising; // For the ADS1115 to read all the gimbals voltage levels ADS1115 ADS(0x48, &Wire); // BLE Gamepad use // 49 BleGamepad(std::string deviceName = "ESP32 BLE Gamepad", std::string deviceManufacturer = "Espressif", uint8_t batteryLevel = 100); -BleGamepad bleGamepad("S3R8TX", "Espressif", 100); +BleGamepad bleGamepad("S3R8TX-02", "Espressif", 50); /**** EspSoftwareSerial::UART SWSerialPort; @@ -172,6 +202,8 @@ uint32_t discharged_time_grandtotal = 0; uint32_t flight_time = 0; uint32_t flight_time_total = 0; +bool armed = false; + void setup() { Serial.begin(115200); @@ -254,7 +286,44 @@ void setup() { Serial.print("Wire Speed: "); Serial.println(ADS.getWireClock()); - + // Initialize ESPNow Protocol + WiFi.mode(WIFI_STA); + uint8_t temp_addr[] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF }; + destination_server_address[0] = EEPROM.read(1); + destination_server_address[1] = EEPROM.read(2); + destination_server_address[2] = EEPROM.read(3); + destination_server_address[3] = EEPROM.read(4); + destination_server_address[4] = EEPROM.read(5); + destination_server_address[5] = EEPROM.read(6); + + espnow_channel = EEPROM.read(7); + if(espnow_channel > ESPNOW_MAX_CHANNEL) + espnow_channel = ESPNOW_MAX_CHANNEL; + + // Channel 0 only for syncing purposes. + if(espnow_channel == 0) + espnow_channel = 1; + + // Showing Info + Serial.print("Bridge MAC: "); + printMAC(destination_server_address); + Serial.print(" On Channel: "); + Serial.println(espnow_channel); + + memcpy(default_server_address, temp_addr, sizeof(uint8_t[6])); + pairing_status = PAIR_REQUEST; + + // Initialize the auxilary buttons + for(uint8_t i = 0; i < 9; i++) { + aux_state[i] = false; + } + + // Initialize virtual ruby buttons + for(uint8_t i = 0; i < 9; i++) { + last_rubybutton[i] = 0; + rubybutton_state[i] = false; + } + // Setting up Tasks xTaskCreatePinnedToCore( taskSystem, @@ -311,6 +380,15 @@ void setup() { } +void 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 init_storage(void) { /*** if(!FFat.begin()){ @@ -324,6 +402,30 @@ void init_storage(void) { ***/ // Serial.printf("Total space: %10u\n", FFat.totalBytes()); // Serial.printf("Free space: %10u\n", FFat.freeBytes()); + /*** + if(!LITTLEFS.begin(FORMAT_LITTLEFS_IF_FAILED)) { + Serial.println("LITTLEFS mount failed"); + if(!LITTLEFS.format()) { + Serial.println(F("Unable to FORMAT!")); + } + } + ***/ + EEPROM.begin(EEPROM_SIZE); + + // If reading the first byte != 127 initialize the whole block... + // Byte 0: Flag to determine the storage state. + uint8_t val = EEPROM.read(0); + if(val != 127) { + Serial.println("Formatting EEPROM..."); + // Formatting the block... (Erasing the 10 blocks) + for(uint16_t i = 0; i < 10; i++) { + EEPROM.write(i, 0x00); + EEPROM.commit(); + } + + EEPROM.write(0, 127); + EEPROM.commit(); + } } @@ -554,8 +656,19 @@ void load_rccontroller(void) { x > 120 && y > 85 && x < 200 && y < 170 ) { + // Temporary disable... transmission_mode = MODE_BLE; last_toggle = millis(); + } else if( + x > 0 && y > 140 && + x < 30 && y < 170 + ) { + current_screen--; + } else if( + x > 290 && y > 140 && + x < 320 && y < 170 + ) { + current_screen++; } } @@ -566,50 +679,210 @@ void load_rccontroller(void) { } } - if(!touch_state) { - switch(touch_type) { - case TP_LONGPRESS: - break; - - case TP_SLIDE_RIGHT: - current_screen++; - // sound_change_screen = true; - break; - - case TP_SLIDE_LEFT: - current_screen--; - // sound_change_screen = true; - break; - - case TP_SLIDE_UP: - // current_hack--; - /*** - if(transmission_mode > MODE_NONE) - transmission_mode--; - if(transmission_mode < MODE_NONE) { - transmission_mode = MODE_NONE; - } - ***/ - break; +// if(!touch_state) { +// switch(touch_type) { +// case TP_LONGPRESS: +// break; +// +// case TP_SLIDE_RIGHT: +// current_screen++; +// // sound_change_screen = true; +// break; +// +// case TP_SLIDE_LEFT: +// current_screen--; +// // sound_change_screen = true; +// break; +// +// case TP_SLIDE_UP: +// // current_hack--; +// /*** +// if(transmission_mode > MODE_NONE) +// transmission_mode--; +// if(transmission_mode < MODE_NONE) { +// transmission_mode = MODE_NONE; +// } +// ***/ +// break; +// +// case TP_SLIDE_DOWN: +// // current_hack++; +// /**** +// transmission_mode++; +// if(transmission_mode > MODE_BLE) { +// transmission_mode = MODE_BLE; +// } +// ***/ +// break; +// +// case TP_NONE: +// break; +// +// default: +// break; +// } +// } + + } // End of Touch Read +} + + +void load_rcsettings(void) { + char strbuf[255]; + + sprite.setTextDatum(TL_DATUM); + sprite.fillSprite(TFT_BLACK); + sprite.setSwapBytes(false); + + sprite.setTextSize(2); + sprite.setTextColor(TFT_ORANGE); + sprintf(strbuf, "### RC SETTINGS ###"); + sprite.drawString(strbuf, 0, 150); + + sprintf(strbuf, "B: %dmV R: %d BT: %d", batt_volt, profile_gimbal_rate, ble_counter); + sprite.drawString(strbuf, 0, 134); + + sprite.loadFont(FONT_NOTO_SMALL); + sprite.setTextColor(TFT_WHITE); + sprintf(strbuf, "ESPNOW"); + sprite.drawString(strbuf, 80, 14); - case TP_SLIDE_DOWN: - // current_hack++; - /**** - transmission_mode++; - if(transmission_mode > MODE_BLE) { - transmission_mode = MODE_BLE; + sprite.unloadFont(); + + // Show the channel + sprite.loadFont(FONT_NOTO_BIG); + sprite.setTextColor(TFT_WHITE); + sprintf(strbuf, "%d", espnow_channel); + sprite.drawString(strbuf, 150, 8); + + sprite.fillRoundRect(0, 60, 320, 50, 10, 0x090A); + sprintf(strbuf, "UPDATE VEHICLE"); + sprite.drawString(strbuf, 8, 70); + + sprite.fillRoundRect(0, 0, 50, 50, 10, 0x090A); + sprintf(strbuf, "-"); + sprite.drawString(strbuf, 14, 10); + sprite.fillRoundRect(270, 0, 50, 50, 10, 0x090A); + sprintf(strbuf, "+"); + sprite.drawString(strbuf, 284, 10); + + + sprite.unloadFont(); + + sprite.pushSprite(0, 0); + + + // Reading input from the touch screen + if(touch.read()) { + TP_Point t = touch.getPoint(0); + + // Since the watch design is horizontally, the touch x position needs to invert + int x = map(t.y, 0, 320, 320, 0); + int y = t.x; + + display_touch_count++; + + if(!touch_state) { + last_touch = millis(); + last_x = x; + last_y = y; + + touch_state = true; + } else { + if( + (uint32_t)(millis() - last_touch) > 250 + ) { + if((uint32_t)(millis() - last_toggle) > 500) { + if( + x > 0 && y > 0 && + x < 50 && y < 50 + ) { + espnow_channel--; + last_toggle = millis(); + } else if( + x > 270 && y > 0 && + x < 320 && y < 50 + ) { + espnow_channel++; + last_toggle = millis(); + } else if( + x > 0 && y > 60 && + x < 320 && y < 110) { + auto_pairing(); + last_toggle = millis(); + + // Switch Screen Locations + } else if( + x > 0 && y > 140 && + x < 30 && y < 170 + ) { + current_screen--; + + // Switch Screen Locations + } else if( + x > 290 && y > 140 && + x < 320 && y < 170 + ) { + current_screen++; } - ***/ - break; - - case TP_NONE: - break; - - default: - break; + + if(espnow_channel > ESPNOW_MAX_CHANNEL) + espnow_channel--; + + if(espnow_channel < 1) + espnow_channel++; + } + + touch_gestures(x, y); + // Reset state for next cycle to detect + touch_state = false; } } +// if(!touch_state) { +// switch(touch_type) { +// case TP_LONGPRESS: +// break; +// +// case TP_SLIDE_RIGHT: +// current_screen++; +// // sound_change_screen = true; +// break; +// +// case TP_SLIDE_LEFT: +// current_screen--; +// // sound_change_screen = true; +// break; +// +// case TP_SLIDE_UP: +// // current_hack--; +// /*** +// if(transmission_mode > MODE_NONE) +// transmission_mode--; +// if(transmission_mode < MODE_NONE) { +// transmission_mode = MODE_NONE; +// } +// ***/ +// break; +// +// case TP_SLIDE_DOWN: +// // current_hack++; +// /**** +// transmission_mode++; +// if(transmission_mode > MODE_BLE) { +// transmission_mode = MODE_BLE; +// } +// ***/ +// break; +// +// case TP_NONE: +// break; +// +// default: +// break; +// } +// } + } // End of Touch Read } @@ -680,7 +953,7 @@ void load_mainscreen(void) { sprite.setTextSize(2); sprite.setTextColor(TFT_ORANGE); - sprintf(strbuf, "### MAINSCREEN ###"); + sprintf(strbuf, "F: %ds U: %ds", flight_time, uptime); sprite.drawString(strbuf, 0, 150); sprintf(strbuf, "B: %dmV R: %d", batt_volt, profile_gimbal_rate); @@ -689,8 +962,10 @@ void load_mainscreen(void) { // Top Left Physical Switch if(toggles[0]) { sprintf(strbuf, "ARMED"); + if(!armed) armed = true; } else { sprintf(strbuf, "DISARMED"); + if(armed) armed = false; } sprite.drawString(strbuf, 0, 118); @@ -724,6 +999,7 @@ void load_mainscreen(void) { x < 80 && y < 80 ) { toggles[0] = !toggles[0]; + aux_state[0] = toggles[0]; last_toggle = millis(); // Button 2 @@ -732,6 +1008,7 @@ void load_mainscreen(void) { x < 160 && y < 80 ) { toggles[1] = !toggles[1]; + aux_state[1] = toggles[1]; last_toggle = millis(); // Button 3 @@ -740,6 +1017,7 @@ void load_mainscreen(void) { x < 240 && y < 80 ) { toggles[2] = !toggles[2]; + aux_state[2] = toggles[2]; last_toggle = millis(); // Button 4 @@ -748,7 +1026,22 @@ void load_mainscreen(void) { x < 320 && y < 80 ) { toggles[3] = !toggles[3]; + aux_state[3] = toggles[3]; last_toggle = millis(); + + // Switch Screen Locations + } else if( + x > 0 && y > 140 && + x < 30 && y < 170 + ) { + current_screen--; + + // Switch Screen Locations + } else if( + x > 290 && y > 140 && + x < 320 && y < 170 + ) { + current_screen++; } } @@ -759,37 +1052,36 @@ void load_mainscreen(void) { } } - if(!touch_state) { - switch(touch_type) { - case TP_LONGPRESS: - break; - - case TP_SLIDE_RIGHT: - current_screen++; - // sound_change_screen = true; - break; - - case TP_SLIDE_LEFT: - // current_screen--; - // sound_change_screen = true; - break; - - case TP_SLIDE_UP: - // current_hack--; - break; - - case TP_SLIDE_DOWN: - // current_hack++; - break; - - case TP_NONE: - break; - - default: - break; - } - } - +// if(!touch_state && armed == false) { +// switch(touch_type) { +// case TP_LONGPRESS: +// break; +// +// case TP_SLIDE_RIGHT: +// current_screen++; +// // sound_change_screen = true; +// break; +// +// case TP_SLIDE_LEFT: +// // current_screen--; +// // sound_change_screen = true; +// break; +// +// case TP_SLIDE_UP: +// // current_hack--; +// break; +// +// case TP_SLIDE_DOWN: +// // current_hack++; +// break; +// +// case TP_NONE: +// break; +// +// default: +// break; +// } +// } } // End of Touch Read } @@ -1346,13 +1638,20 @@ void taskDisplay(void *pvParameters) { break; case SCREEN_MAINSCREEN: + ruby_mode = false; load_mainscreen(); break; case SCREEN_RCCONTROLLER: + ruby_mode = false; load_rccontroller(); break; + case SCREEN_SETTINGS: + ruby_mode = true; + load_rcsettings(); + break; + /**** case SCREEN_GPS: screen_gps(); @@ -1374,12 +1673,281 @@ void taskDisplay(void *pvParameters) { void taskESPNow(void *pvParameters) { (void) pvParameters; + uint32_t last_updated = 0; + for(;;) { - vTaskDelay(10); + vTaskDelay(1); + + if(transmission_mode == MODE_ESPNOW) { + if(!isConnected) { + // Not connected...so... make it connect + connect_bridge(); + } else { + if((uint32_t)(millis() - last_updated) > 1) { + data_to_send.message_type = DATA; + + if(!ruby_mode) { + // Gimbal mode... + data_to_send.throttle = throttle_val; + data_to_send.yaw = yaw_val; + data_to_send.pitch = pitch_val; + data_to_send.roll = roll_val; + } else { + // Switch mode... + // Pitch Up: UP + // Pitch Down: DOWN + // Roll Left: BACK + // Roll Right: MENU/OK + ruby_pitch = pitch_val; + ruby_roll = roll_val; + + // UP + if(ruby_pitch > 1750) { + rubybutton_state[2] = 0; + rubybutton_state[3] = 1; + + // DOWN + } else if(ruby_pitch < 1250) { + rubybutton_state[2] = 1; + rubybutton_state[3] = 0; + + // RIGHT + } else if(ruby_roll > 1750) { + rubybutton_state[1] = 0; + rubybutton_state[0] = 1; + + // LEFT + } else if(ruby_roll < 1250) { + rubybutton_state[1] = 1; + rubybutton_state[0] = 0; + + // RESET UP DOWN + } else if(ruby_pitch > 1250 && ruby_pitch < 1750) { + rubybutton_state[2] = 0; + rubybutton_state[3] = 0; + + // RESET LEFT RIGHT + } else if(ruby_roll > 1250 && ruby_roll < 1750) { + rubybutton_state[1] = 0; + rubybutton_state[0] = 0; + } + } + + // Converting Ruby Button state to send. + data_to_send.rubybutton = ( + rubybutton_state[0] | + rubybutton_state[1] << 1 | + rubybutton_state[2] << 2 | + rubybutton_state[3] << 3 | + rubybutton_state[4] << 4 | + rubybutton_state[5] << 5 | + rubybutton_state[6] << 6 | + rubybutton_state[7] << 7 | + rubybutton_state[8] << 8 + ); + + // Convert the state into one variable + data_to_send.aux = ( + aux_state[0] | + aux_state[1] << 1 | + aux_state[2] << 2 | + aux_state[3] << 3 | + aux_state[4] << 4 | + aux_state[5] << 5 | + aux_state[6] << 6 | + aux_state[7] << 7 | + aux_state[8] << 8 | + aux_state[9] << 9 | + aux_state[10] << 10 | + aux_state[11] << 11 + ); + + // Sending Data + esp_now_send(destination_server_address, (uint8_t *) &data_to_send, sizeof(data_to_send)); + + last_updated = millis(); + } + + // Unpress the buttons (virtually) + for(uint8_t i = 0; i < 9; i++) { + if(rubybutton_state[i]) { + if((uint32_t)(millis() - last_rubybutton[i]) > 100) { + rubybutton_state[i] = false; + } + } + } + } + } + } // End of for...loop +} + + +PairingStatus 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(ESPNOW_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(default_server_address, (uint8_t *) &pairing_data, sizeof(pairing_data)); + Serial.print("Sending to: "); + char m[18]; + 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; + + break; + + case PAIR_REQUESTED: + if(uint32_t(millis() - millis_previous) > 250) { + millis_previous = millis(); + + // Trying next channel + espnow_channel++; + + if(espnow_channel > ESPNOW_MAX_CHANNEL) { + espnow_channel = 1; + } + + pairing_status = PAIR_REQUEST; + } + break; + + case PAIR_PAIRED: + // Nothing to do. + break; } + + return pairing_status; } +void connect_bridge(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; + } + + // 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; + } + + Serial.print("Bridge channel: "); + Serial.println(WiFi.channel()); + isConnected = true; +} + + +// 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"); + + if(status == ESP_NOW_SEND_SUCCESS) { + sent_counter_raw++; + } +} + +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: "); + 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(&pairing_data, incoming_data, sizeof(pairing_data)); + printMAC(mac_addr); + Serial.print("Pairing done for "); + Serial.print(" on channel " ); + Serial.print(pairing_data.channel); // channel used by the + + // Save the details into EEPROM + EEPROM.write(1, mac_addr[0]); + EEPROM.write(2, mac_addr[1]); + EEPROM.write(3, mac_addr[2]); + EEPROM.write(4, mac_addr[3]); + EEPROM.write(5, mac_addr[4]); + EEPROM.write(6, mac_addr[5]); + EEPROM.write(7, pairing_data.channel); + + EEPROM.commit(); + pairing_status = PAIR_PAIRED; // set the pairing status + break; + } +} + + + + /*** Template for task functions void taskESPNow(void *pvParameters) { (void) pvParameters; @@ -1595,6 +2163,10 @@ void taskSystem(void *pvParameters) { charge_time++; } + if(armed) { + flight_time++; + } + last_update = millis(); }