englebert
/
S3R8TX
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Working BT Gamepad

master
Englebert 10 months ago
parent
bdb3216bcf
commit
722c704491
5 changed files with 506 additions and 23 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 11
      README.md
  2. BIN
      lib/TFT_eSPI/.TFT_eSPI.h.swp
  3. 8
      platformio.ini
  4. 484
      src/main.cpp
  5. 26
      src/main.h

11
README.md
View File

@ -14,5 +14,14 @@ ARM_SW - GPIO01
AUX_SW - GPIO02
GND - GND
#
### Add the below to the library
// Custom Begin for different pins on I2C
bool ADS1X15::begin(int sda, int scl)
{
// _wire = &Wire1;
_wire->begin(sda, scl);
if ((_address < 0x48) || (_address > 0x4B)) return false;
if (! isConnected()) return false;
return true;
}

BIN
lib/TFT_eSPI/.TFT_eSPI.h.swp
View File

8
platformio.ini
View File

@ -24,10 +24,14 @@ build_flags =
[env:default]
lib_deps =
h2zero/NimBLE-Arduino@^1.4.1
robtillaart/ADS1X15@^0.4.1
lemmingdev/ESP32-BLE-Gamepad@^0.5.2
[env:lilygo-t-display-s3]
platform = espressif32
board = lilygo-t-display-s3
framework = arduino
lib_deps = h2zero/NimBLE-Arduino@^1.4.1
lib_deps =
h2zero/NimBLE-Arduino@^1.4.1
robtillaart/ADS1X15@^0.4.1
lemmingdev/ESP32-BLE-Gamepad@^0.5.2

484
src/main.cpp
View File

@ -24,6 +24,7 @@ const int display_backlight_res = 8;
uint32_t batt_raw = 0;
uint32_t batt_volt = 0;
uint8_t current_background = 0;
bool charge_state = false;
uint8_t current_hack = 0;
uint8_t tone_channel = 1;
@ -36,9 +37,56 @@ uint16_t last_y = 0;
uint8_t touch_type = TP_NONE;
uint16_t batt_max = 4200;
uint16_t batt_min = 3300;
uint16_t batt_min = 3000;
uint16_t batt_charge_width = 0;
int16_t throttle_steps = 0;
int16_t yaw_steps = 0;
int16_t roll_steps = 0;
int16_t pitch_steps = 0;
uint16_t profile_gimbal_rate = 0;
uint16_t profile_gimbal_rate_raw = 0;
int16_t throttle_val = 0;
int16_t yaw_val = 0;
int16_t pitch_val = 0;
int16_t roll_val = 0;
int16_t throttle_bt = 0;
int16_t yaw_bt = 0;
int16_t pitch_bt = 0;
int16_t roll_bt = 0;
int16_t gimbal_left_x = 0; // For Yaw
int16_t gimbal_left_y = 0; // For throttle
int16_t gimbal_right_x = 0; // For Roll
int16_t gimbal_right_y = 0; // For Pitch
// Active calibration
int16_t min_throttle = 32767;
int16_t max_throttle = 0;
int16_t min_yaw = 32767;
int16_t max_yaw = 0;
int16_t min_pitch = 32767;
int16_t max_pitch = 0;
int16_t min_roll = 32767;
int16_t max_roll = 0;
// Gimbal Medians
#define MEDIAN_MAX 5
int16_t med_throttle[MEDIAN_MAX];
int16_t med_yaw[MEDIAN_MAX];
int16_t med_pitch[MEDIAN_MAX];
int16_t med_roll[MEDIAN_MAX];
// Transmission modes
uint8_t transmission_mode = MODE_NONE;
uint16_t ble_counter_raw = 0;
uint16_t ble_counter = 0;
bool toggles[TOGGLE_MAX];
static const int MAX_SATELLITES = 40;
@ -92,6 +140,12 @@ lcd_cmd_t lcd_st7789v[] = {
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);
/****
EspSoftwareSerial::UART SWSerialPort;
@ -136,6 +190,8 @@ void setup() {
pinMode(PIN_BUTTON_1, INPUT_PULLUP);
pinMode(PIN_BUTTON_2, INPUT_PULLUP);
pinMode(SW_ARM, INPUT_PULLDOWN);
pinMode(SW_AUX, INPUT_PULLDOWN);
// pinMode(PIN_LCD_BL, OUTPUT);
// digitalWrite(PIN_LCD_BL, HIGH);
@ -157,6 +213,14 @@ void setup() {
toggles[i] = false;
}
// Initialize medians
for(uint8_t i = 0; i < MEDIAN_MAX; i++) {
med_throttle[i] = 0;
med_yaw[i] = 0;
med_pitch[i] = 0;
med_roll[i] = 0;
}
// Initialize all the uninitialized TinyGPSCustom objects
// for(int i = 0; i < MAX_SATELLITES; ++i) {
@ -166,6 +230,19 @@ void setup() {
// snr[i].begin( gps, "GPGSV", 7 + 4 * i); // offsets 7, 11, 15, 19
// }
// Initialize ADC
if(!ADS.begin(ADS1115_SCL, ADS1115_SDA)) {
Serial.println("ADS1115 Failed to initialize.");
delay(5000);
}
ADS.setWireClock(400000);
ADS.setGain(1); // +- 4.096V
ADS.setDataRate(7); // 7 = Fastest
ADS.setMode(1); // Continuous
Serial.print("Wire Speed: ");
Serial.println(ADS.getWireClock());
// Setting up Tasks
xTaskCreatePinnedToCore(
@ -188,6 +265,27 @@ void setup() {
CPU_1
);
xTaskCreatePinnedToCore(
taskGimbal,
"TaskGimbal", // Name of the process
16384, // This stack size can be checked & adjusted by reading the Stack Highwater
NULL,
10, // Priority
NULL,
CPU_0
);
xTaskCreatePinnedToCore(
taskBLE,
"TaskBLE", // Name of the process
16384, // This stack size can be checked & adjusted by reading the Stack Highwater
NULL,
5, // Priority
NULL,
CPU_0
);
Serial.println("Ready");
}
@ -275,6 +373,197 @@ void touch_processor(void) {
}
**/
void draw_gimbal_huge(uint16_t x, uint16_t y) {
// Top
sprite.drawFastHLine(x, y, 120, TFT_DARKGREEN);
// Bottom
sprite.drawFastHLine(x, y + 119, 120, TFT_DARKGREEN);
// Left
sprite.drawFastVLine(x, y, 120, TFT_DARKGREEN);
// Right
sprite.drawFastVLine(x + 119, y, 120, TFT_DARKGREEN);
// Middle dotted lines
for(uint16_t x1 = x; x1 < x + 120; x1 += 10) {
sprite.drawFastHLine(x1, y + 60, 5, TFT_DARKGREEN);
}
for(uint16_t y1 = y; y1 < y + 120; y1 += 10) {
sprite.drawFastVLine(x + 60, y1, 5, TFT_DARKGREEN);
}
}
void load_rccontroller(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 CONTROLLER ###");
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_DARKGREY);
if(transmission_mode == MODE_NONE) {
sprintf(strbuf, "ESPNOW");
sprite.drawString(strbuf, 125, 10);
sprintf(strbuf, "BLE");
sprite.drawString(strbuf, 125, 26);
} else if(transmission_mode == MODE_ESPNOW) {
sprintf(strbuf, "ESPNOW");
sprite.setTextColor(TFT_SKYBLUE);
sprite.drawString(strbuf, 125, 10);
sprintf(strbuf, "BLE");
sprite.setTextColor(TFT_DARKGREY);
sprite.drawString(strbuf, 125, 26);
} else if(transmission_mode == MODE_BLE) {
sprintf(strbuf, "ESPNOW");
sprite.setTextColor(TFT_DARKGREY);
sprite.drawString(strbuf, 125, 10);
sprintf(strbuf, "BLE");
sprite.setTextColor(TFT_SKYBLUE);
sprite.drawString(strbuf, 125, 26);
}
// DEBUG:
sprite.setTextColor(TFT_YELLOW);
sprintf(strbuf, "%d", throttle_bt);
sprite.drawString(strbuf, 125, 42);
sprintf(strbuf, "%d", yaw_bt);
sprite.drawString(strbuf, 125, 58);
sprintf(strbuf, "%d", pitch_bt);
sprite.drawString(strbuf, 125, 74);
sprintf(strbuf, "%d", roll_bt);
sprite.drawString(strbuf, 125, 90);
sprite.unloadFont();
// Drawing the grids of the gimbal
draw_gimbal_huge(0, 0);
draw_gimbal_huge(200, 0);
gimbal_left_x = map(yaw_val, 1000, 2000, 0, 120);
gimbal_left_y = map(throttle_val, 1000, 2000, 0, 120);
sprite.fillCircle(gimbal_left_x, gimbal_left_y, 3, TFT_WHITE);
gimbal_right_x = map(roll_val, 1000, 2000, 200, 320);
gimbal_right_y = map(pitch_val, 1000, 2000, 0, 120);
sprite.fillCircle(gimbal_right_x, gimbal_right_y, 3, TFT_WHITE);
if(throttle_steps > max_throttle) {
max_throttle = throttle_steps;
} else if(throttle_steps < min_throttle) {
min_throttle = throttle_steps;
}
if(yaw_steps > max_yaw) {
max_yaw = yaw_steps;
} else if(yaw_steps < min_yaw) {
min_yaw = yaw_steps;
}
if(pitch_steps > max_pitch) {
max_pitch = pitch_steps;
} else if(pitch_steps < min_pitch) {
min_pitch = pitch_steps;
}
if(roll_steps > max_roll) {
max_roll = roll_steps;
} else if(roll_steps < min_roll) {
min_roll = roll_steps;
}
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) > 150
) {
if((uint32_t)(millis() - last_toggle) > 500) {
// Temporary
last_toggle = millis();
// Gestures Detection
} else {
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
}
void load_mainscreen(void) {
char strbuf[255];
@ -302,6 +591,25 @@ void load_mainscreen(void) {
else
sprite.pushImage(240, 0, 80, 80, (uint16_t *)button_off);
// Based on the voltage to display the icon
if(charge_state) {
sprite.pushImage(320-BATTERY_CHARGING_WIDTH, 90, BATTERY_CHARGING_WIDTH, BATTERY_CHARGING_HEIGHT, (uint16_t *)battery_charging);
} else {
uint8_t battery_icon_type = (batt_volt - batt_min) * 5 / (batt_max - batt_min);
if(battery_icon_type == 5) {
sprite.pushImage(320-BATTERY_5_WIDTH, 90, BATTERY_5_WIDTH, BATTERY_5_HEIGHT, (uint16_t *)battery_5);
} else if(battery_icon_type == 4) {
sprite.pushImage(320-BATTERY_4_WIDTH, 90, BATTERY_4_WIDTH, BATTERY_4_HEIGHT, (uint16_t *)battery_4);
} else if(battery_icon_type == 3) {
sprite.pushImage(320-BATTERY_3_WIDTH, 90, BATTERY_3_WIDTH, BATTERY_3_HEIGHT, (uint16_t *)battery_3);
} else if(battery_icon_type == 2) {
sprite.pushImage(320-BATTERY_2_WIDTH, 90, BATTERY_2_WIDTH, BATTERY_2_HEIGHT, (uint16_t *)battery_2);
} else {
sprite.pushImage(320-BATTERY_1_WIDTH, 90, BATTERY_1_WIDTH, BATTERY_1_HEIGHT, (uint16_t *)battery_1);
}
}
/****
if((uptime % 6) == 1) {
sprite.pushImage(320-BATTERY_CHARGING_WIDTH, 90, BATTERY_CHARGING_WIDTH, BATTERY_CHARGING_HEIGHT, (uint16_t *)battery_charging);
} else if((uptime % 6) == 2) {
@ -315,6 +623,7 @@ void load_mainscreen(void) {
} else {
sprite.pushImage(320-BATTERY_1_WIDTH, 90, BATTERY_1_WIDTH, BATTERY_1_HEIGHT, (uint16_t *)battery_1);
}
***/
sprite.setTextSize(2);
@ -322,6 +631,9 @@ void load_mainscreen(void) {
sprintf(strbuf, "### MAINSCREEN ###");
sprite.drawString(strbuf, 0, 150);
sprintf(strbuf, "B: %dmV R: %d", batt_volt, profile_gimbal_rate);
sprite.drawString(strbuf, 0, 134);
sprite.pushSprite(0, 0);
// Reading input from the touch screen
@ -345,7 +657,7 @@ void load_mainscreen(void) {
(uint32_t)(millis() - last_touch) > 150
) {
// To fix the toggle too fast issue
if((uint32_t)(millis() - last_toggle) > 500) {
if((uint32_t)(millis() - last_toggle) > 100) {
// Button 1
if(
x > 0 && y > 0 &&
@ -378,13 +690,12 @@ void load_mainscreen(void) {
toggles[3] = !toggles[3];
last_toggle = millis();
}
}
// Gestures Detection
} else {
touch_gestures(x, y);
// Reset state for next cycle to detect
touch_state = false;
}
touch_gestures(x, y);
// Reset state for next cycle to detect
touch_state = false;
}
}
@ -394,7 +705,7 @@ void load_mainscreen(void) {
break;
case TP_SLIDE_RIGHT:
// current_screen++;
current_screen++;
// sound_change_screen = true;
break;
@ -419,7 +730,7 @@ void load_mainscreen(void) {
}
}
}
} // End of Touch Read
}
/***
@ -978,6 +1289,10 @@ void taskDisplay(void *pvParameters) {
load_mainscreen();
break;
case SCREEN_RCCONTROLLER:
load_rccontroller();
break;
/****
case SCREEN_GPS:
screen_gps();
@ -996,6 +1311,140 @@ void taskDisplay(void *pvParameters) {
}
void taskBLE(void *pvParameters) {
(void) pvParameters;
bool ble_begin = false;
bool updated = false;
bool ble_start = false;
uint32_t last_updated = 0;
for(;;) {
vTaskDelay(10);
// Skip if is not BLE mode
if(transmission_mode != MODE_BLE) continue;
// If BLE not started... Start it up...
if(!ble_start) {
Serial.printf("BLEGamepad start.\n\r");
bleGamepad.begin();
Serial.printf("BLEGamepad OK.\r\n");
ble_start = true;
}
// Processing it if BLE connected...
if(bleGamepad.isConnected()) {
throttle_bt = map(throttle_val, 1000, 2000, 0, 32767); // Throttle
yaw_bt = map(yaw_val, 1000, 2000, 0, 32767); // Yaw
pitch_bt = map(pitch_val, 1000, 2000, 0, 32767); // Pitch
roll_bt = map(roll_val, 1000, 2000, 0, 32767); // Roll
bleGamepad.setAxes(yaw_bt, throttle_bt, roll_bt, pitch_bt, 0, 0, DPAD_CENTERED);
/****
uint32_t m = millis();
if(button1) {
if((uint32_t)(m - last_button1) > 100) {
bleGamepad.release(BUTTON_1);
button1 = false;
button1_sent = false;
} else {
// Only sent once and delay for 100ms
if(!button1_sent) {
bleGamepad.press(BUTTON_1);
button1_sent = true;
}
}
}
***/
ble_counter_raw++;
last_updated = millis();
}
}
}
void taskGimbal(void *pvParameters) {
(void) pvParameters;
int16_t tmp_throttle, tmp_yaw, tmp_roll, tmp_pitch;
for(;;) {
vTaskDelay(1);
yaw_steps = ADS.readADC(3);
throttle_steps = ADS.readADC(2);
pitch_steps = ADS.readADC(0);
roll_steps = ADS.readADC(1);
// Median Processor
// Shift to left....
for(uint8_t i = 1; i < MEDIAN_MAX; i++) {
med_throttle[i - 1] = med_throttle[i];
med_yaw[i - 1] = med_yaw[i];
med_pitch[i - 1] = med_pitch[i];
med_roll[i - 1] = med_roll[i];
}
// Insert to the last
med_throttle[MEDIAN_MAX - 1] = throttle_steps;
med_yaw[MEDIAN_MAX - 1] = yaw_steps;
med_pitch[MEDIAN_MAX - 1] = pitch_steps;
med_roll[MEDIAN_MAX - 1] = roll_steps;
// Sorting
throttle_val = get_median(med_throttle, MEDIAN_MAX);
yaw_val = get_median(med_yaw, MEDIAN_MAX);
pitch_val = get_median(med_pitch, MEDIAN_MAX);
roll_val = get_median(med_roll, MEDIAN_MAX);
// throttle_val = map(throttle_steps, min_throttle, max_throttle, 1000, 2000);
throttle_val = map(throttle_steps, min_throttle, max_throttle, 2000, 1000);
yaw_val = map(yaw_steps, min_yaw, max_yaw, 2000, 1000);
pitch_val = map(pitch_steps, min_pitch, max_pitch, 1000, 2000);
roll_val = map(roll_steps, min_roll, max_roll, 1000, 2000);
// Serial.println(throttle_val);
profile_gimbal_rate_raw++;
}
}
int16_t get_median(int16_t dataset[], uint8_t total_data) {
int16_t key, j;
uint8_t pos = total_data >> 1;
int16_t arr[total_data];
/**
for(uint8_t i = 0; i < total_data; i++) {
Serial.print(dataset[i]); Serial.print(" ");
}
Serial.println("");
**/
for(uint8_t i = 0; i < total_data; i++) {
key = dataset[i];
j = i - 1;
while( j >= 0 && dataset[j] > key) {
dataset[j + 1] = dataset[j];
j = j - 1;
}
dataset[j + 1] = key;
}
/**
for(uint8_t i = 0; i < total_data; i++) {
Serial.print(dataset[i]); Serial.print(" ");
}
Serial.println("");
**/
return dataset[pos];
}
void taskSystem(void *pvParameters) {
(void) pvParameters;
@ -1004,13 +1453,19 @@ void taskSystem(void *pvParameters) {
uint32_t last_battery_reading = 0;
uint8_t charge_state_on_count = 0;
bool charge_state = false;
for(;;) {
vTaskDelay(1);
if((uint32_t)(millis() - last_update) > 999) {
uptime++;
// Update counters
profile_gimbal_rate = profile_gimbal_rate_raw;
profile_gimbal_rate_raw = 0;
ble_counter = ble_counter_raw;
ble_counter_raw = 0;
// Serial.println(profile_gimbal_rate);
// Increase charge time if is charging.
if(charge_state) {
charge_time++;
@ -1022,10 +1477,6 @@ void taskSystem(void *pvParameters) {
if((uint32_t)(millis() - last_battery_reading) > 2000) {
// esp_adc_cal_characteristics_t adc_chars;
// esp_adc_cal_value_t val_type = esp_adc_cal_characterize(ADC_UNIT_1, ADC_ATTEN_DB_11, ADC_WIDTH_BIT_12, 1100, &adc_chars);
if(!display_state) {
digitalWrite(PIN_POWER_ON, HIGH);
}
batt_raw = analogRead(PIN_BAT_VOLT);
batt_volt = (batt_raw * (.0016560509 * 1000));
// batt_volt = esp_adc_cal_raw_to_voltage(batt_raw, &adc_chars) * 2; // The partial pressure is one-half
@ -1044,9 +1495,6 @@ void taskSystem(void *pvParameters) {
charge_state_on_count = 0;
}
if(!display_state) {
digitalWrite(PIN_POWER_ON, LOW);
}
last_battery_reading = millis();
}

26
src/main.h
View File

@ -55,6 +55,13 @@
#define FONT_NOTO_SMALL NotoSansBold15
#define FONT_NOTO_BIG NotoSansBold36
// ADS1115
#define ADS1115_SDA 18
#define ADS1115_SCL 17
// Physical Buttons
#define SW_ARM 1
#define SW_AUX 2
#include <esp_adc_cal.h>
#if ESP_IDF_VERSION >= ESP_IDF_VERSION_VAL(5,0,0)
@ -64,10 +71,12 @@
// #include <SoftwareSerial.h>
// #include <TinyGPSPlus.h>
#include <NimBLEDevice.h>
// #include <NimBLEDevice.h>
#include <BleGamepad.h>
#include "TouchLib.h"
#include "Wire.h"
#include "ADS1X15.h"
#include "WiFi.h"
#include "time.h"
#include "img_logo.h"
@ -84,7 +93,7 @@
enum screen_names {
SCREEN_INTRO,
SCREEN_MAINSCREEN,
SCREEN_RCCONTROLLERS,
SCREEN_RCCONTROLLER,
SCREEN_LAST,
};
@ -99,11 +108,20 @@ enum touch_types {
TP_SLIDE_DOWN,
};
// Transmission MODE
enum transmission_names {
MODE_NONE,
MODE_ESPNOW,
MODE_BLE,
};
uint8_t current_screen = SCREEN_INTRO;
void taskSystem(void *pvParameters);
void taskDisplay(void *pvParameters);
void taskGimbal(void *pvParameters);
void taskBLE(void *pvParameters);
// void taskGPS(void *pvParameters);
void screen_intro(void);
@ -117,6 +135,10 @@ void screen_tools_3(void);
void screen_system(void);
// void display_info(void);
void load_mainscreen(void);
void load_rccontroller(void);
void draw_gimbal_huge(uint16_t x, uint16_t y);
int16_t get_median(int16_t dataset[], uint8_t total_data);
void display_on(void);
void display_state_processor(void);

Write Preview
Loading…
Cancel
Save