jessikitty
84020281ee
Nothing in the sketch calls a uni_ function - bd_addr_t, sscanf_bd_addr, gap_connect and the gatt_client calls are all BTstack.
406 lines
14 KiB
Arduino
406 lines
14 KiB
Arduino
/*
|
|
* ps4_lego_onebrain.ino
|
|
*
|
|
* One ESP32. PS4 controller over Bluetooth Classic (Bluepad32/BTstack) AND two
|
|
* LEGO Powered Up hubs over BLE, driven by a hand-rolled LWP3 GATT client that
|
|
* runs on the same BTstack instance.
|
|
*
|
|
* Board package: esp32-bluepad32 4.1.0 (NOT the plain esp32 package)
|
|
* Libraries: none. No Legoino, no NimBLE - they would be a second host
|
|
* stack and that is exactly what we are avoiding.
|
|
*
|
|
* BEFORE THIS WILL COMPILE:
|
|
* The board package ships the BTstack headers but does not put them on the
|
|
* include path. You must add a platform.local.txt to the package folder
|
|
* first - see docs/BUILD.md. Without it you get:
|
|
* fatal error: btstack.h: No such file or directory
|
|
*
|
|
* THREADING - read this before changing anything:
|
|
* BTstack is not thread-safe. loop() runs in the Arduino task, BTstack runs
|
|
* in its own task. The only BTstack call allowed from loop() is
|
|
* btstack_run_loop_execute_on_main_thread(). Everything LEGO-related here
|
|
* therefore runs inside a repeating BTstack timer (legoTickHandler), which
|
|
* executes on the BTstack thread. loop() only ever writes plain ints into
|
|
* gDesired; the timer reads them.
|
|
*
|
|
* BOOT ORDER:
|
|
* Gamepad discovery starts disabled. We connect both hubs first, then turn
|
|
* discovery on. This stops Bluepad32's inquiry/scan from colliding with our
|
|
* LE create-connection.
|
|
*
|
|
* Created by: Jess Rogerson (yelling commands at Claude.AI)
|
|
*/
|
|
|
|
#include <Bluepad32.h>
|
|
#include <btstack.h>
|
|
|
|
// =========================================================== configuration
|
|
|
|
// Hub BLE addresses, lower case. Find them with the scanner in the two-board
|
|
// repo, or with any BLE app on your phone - do not guess.
|
|
static const char *HUB_ADDR_STR[2] = {
|
|
"90:84:2b:61:e6:8c", // hub 0 - tracks
|
|
"90:84:2b:61:f2:d7", // hub 1 - upper body
|
|
};
|
|
|
|
// LEGO hubs use a public address (90:84:2B is LEGO's OUI). If the connect
|
|
// never completes, try BD_ADDR_TYPE_LE_RANDOM here.
|
|
#define HUB_ADDR_TYPE BD_ADDR_TYPE_LE_PUBLIC
|
|
|
|
static const uint8_t PORT_A = 0x00;
|
|
static const uint8_t PORT_B = 0x01;
|
|
static const uint8_t PORT_LED = 0x32;
|
|
|
|
static const int DEADZONE = 40; // raw stick counts, Bluepad32 range is +/-512
|
|
static const int TRACK_MAX = 100; // LWP3 power range is -100..100
|
|
static const int HEAD_MAX = 60;
|
|
|
|
static const uint32_t TICK_MS = 25; // BTstack timer period
|
|
static const uint32_t MOTOR_MIN_GAP_MS = 60; // per-port command throttle
|
|
static const uint32_t RECONNECT_GAP_MS = 3000;
|
|
|
|
static const int STATUS_LED_PIN = 2;
|
|
|
|
// LWP3 UUIDs, big-endian byte order as BTstack expects.
|
|
static const uint8_t LWP3_SERVICE_UUID[16] = {
|
|
0x00, 0x00, 0x16, 0x23, 0x12, 0x12, 0xef, 0xde,
|
|
0x16, 0x23, 0x78, 0x5f, 0xea, 0xbc, 0xd1, 0x23};
|
|
static const uint8_t LWP3_CHAR_UUID[16] = {
|
|
0x00, 0x00, 0x16, 0x24, 0x12, 0x12, 0xef, 0xde,
|
|
0x16, 0x23, 0x78, 0x5f, 0xea, 0xbc, 0xd1, 0x23};
|
|
|
|
// ============================================== shared between the 2 tasks
|
|
|
|
// Written by loop() (Arduino task), read by the BTstack timer. 32-bit aligned
|
|
// int stores on the ESP32 are single instructions, so a torn read is not
|
|
// possible; the worst case is one tick of stale data, which we do not care
|
|
// about at 40 Hz.
|
|
struct DesiredState {
|
|
volatile int leftTrack;
|
|
volatile int rightTrack;
|
|
volatile int head;
|
|
};
|
|
static DesiredState gDesired = {0, 0, 0};
|
|
|
|
static volatile bool gHubsReady = false;
|
|
|
|
// ============================================================ hub plumbing
|
|
|
|
enum HubState {
|
|
HUB_IDLE,
|
|
HUB_CONNECTING,
|
|
HUB_W4_SERVICE,
|
|
HUB_W4_CHARACTERISTIC,
|
|
HUB_READY,
|
|
};
|
|
|
|
struct Hub {
|
|
bd_addr_t addr;
|
|
HubState state;
|
|
hci_con_handle_t conHandle;
|
|
gatt_client_service_t service;
|
|
uint16_t valueHandle;
|
|
uint32_t retryAt;
|
|
};
|
|
|
|
static Hub gHubs[2];
|
|
static int gActive = -1; // hub currently mid-discovery, -1 if none
|
|
|
|
struct PortThrottle {
|
|
int lastPower;
|
|
uint32_t lastSentAt;
|
|
};
|
|
static PortThrottle gThrottle[3] = {{999, 0}, {999, 0}, {999, 0}};
|
|
|
|
static btstack_packet_callback_registration_t gHciRegistration;
|
|
static btstack_context_callback_registration_t gBootstrapRegistration;
|
|
static btstack_timer_source_t gLegoTimer;
|
|
|
|
// ============================================================== gamepad io
|
|
|
|
ControllerPtr myControllers[BP32_MAX_CONTROLLERS];
|
|
static bool gDiscoveryEnabled = false;
|
|
|
|
void onConnectedController(ControllerPtr ctl) {
|
|
for (int i = 0; i < BP32_MAX_CONTROLLERS; i++) {
|
|
if (myControllers[i] == nullptr) {
|
|
myControllers[i] = ctl;
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
|
|
void onDisconnectedController(ControllerPtr ctl) {
|
|
for (int i = 0; i < BP32_MAX_CONTROLLERS; i++) {
|
|
if (myControllers[i] == ctl) {
|
|
myControllers[i] = nullptr;
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Deadzone, then rescale so the remaining travel still reaches full power.
|
|
static int stickToPower(int raw, int maxPower) {
|
|
if (raw > -DEADZONE && raw < DEADZONE) return 0;
|
|
int sign = (raw < 0) ? -1 : 1;
|
|
long mag = labs((long)raw) - DEADZONE;
|
|
long scaled = (mag * maxPower) / (512L - DEADZONE);
|
|
if (scaled > maxPower) scaled = maxPower;
|
|
return sign * (int)scaled;
|
|
}
|
|
|
|
// ======================================= LWP3 - all of this on BTstack task
|
|
|
|
// Port Output Command / WriteDirectModeData:
|
|
// 08 00 81 <port> 11 51 00 <power>
|
|
// len, hubID, msgType, port, startup+completion, subcmd, mode, payload
|
|
static void lwp3SendPower(Hub &h, uint8_t port, int8_t power) {
|
|
if (h.state != HUB_READY) return;
|
|
uint8_t msg[8] = {0x08, 0x00, 0x81, port, 0x11, 0x51, 0x00, (uint8_t)power};
|
|
gatt_client_write_value_of_characteristic_without_response(
|
|
h.conHandle, h.valueHandle, sizeof(msg), msg);
|
|
}
|
|
|
|
static void lwp3SetLed(Hub &h, uint8_t colour) {
|
|
if (h.state != HUB_READY) return;
|
|
uint8_t msg[8] = {0x08, 0x00, 0x81, PORT_LED, 0x11, 0x51, 0x00, colour};
|
|
gatt_client_write_value_of_characteristic_without_response(
|
|
h.conHandle, h.valueHandle, sizeof(msg), msg);
|
|
}
|
|
|
|
static void driveMotor(Hub &h, uint8_t port, int power, PortThrottle &t) {
|
|
if (h.state != HUB_READY) return;
|
|
|
|
uint32_t now = btstack_run_loop_get_time_ms();
|
|
bool stopping = (power == 0 && t.lastPower != 0);
|
|
if (!stopping) {
|
|
if (power == t.lastPower) return;
|
|
if ((now - t.lastSentAt) < MOTOR_MIN_GAP_MS) return;
|
|
}
|
|
|
|
lwp3SendPower(h, port, (int8_t)power);
|
|
t.lastPower = power;
|
|
t.lastSentAt = now;
|
|
}
|
|
|
|
static void gattPacketHandler(uint8_t packet_type, uint16_t channel,
|
|
uint8_t *packet, uint16_t size) {
|
|
if (gActive < 0) return;
|
|
Hub &h = gHubs[gActive];
|
|
gatt_client_characteristic_t ch;
|
|
|
|
switch (hci_event_packet_get_type(packet)) {
|
|
case GATT_EVENT_SERVICE_QUERY_RESULT:
|
|
gatt_event_service_query_result_get_service(packet, &h.service);
|
|
break;
|
|
|
|
case GATT_EVENT_CHARACTERISTIC_QUERY_RESULT:
|
|
gatt_event_characteristic_query_result_get_characteristic(packet, &ch);
|
|
h.valueHandle = ch.value_handle;
|
|
break;
|
|
|
|
case GATT_EVENT_QUERY_COMPLETE:
|
|
if (h.state == HUB_W4_SERVICE) {
|
|
if (h.service.start_group_handle == 0) {
|
|
// Not a LEGO hub, or it never answered.
|
|
gap_disconnect(h.conHandle);
|
|
break;
|
|
}
|
|
h.state = HUB_W4_CHARACTERISTIC;
|
|
gatt_client_discover_characteristics_for_service_by_uuid128(
|
|
&gattPacketHandler, h.conHandle, &h.service, LWP3_CHAR_UUID);
|
|
} else if (h.state == HUB_W4_CHARACTERISTIC) {
|
|
if (h.valueHandle == 0) {
|
|
gap_disconnect(h.conHandle);
|
|
break;
|
|
}
|
|
h.state = HUB_READY;
|
|
gActive = -1;
|
|
lwp3SetLed(h, 0x06); // green
|
|
}
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
static void onLeConnected(hci_con_handle_t handle) {
|
|
if (gActive < 0) return;
|
|
Hub &h = gHubs[gActive];
|
|
if (h.state != HUB_CONNECTING) return; // not ours - probably a BLE gamepad
|
|
|
|
h.conHandle = handle;
|
|
h.state = HUB_W4_SERVICE;
|
|
memset(&h.service, 0, sizeof(h.service));
|
|
h.valueHandle = 0;
|
|
gatt_client_discover_primary_services_by_uuid128(
|
|
&gattPacketHandler, handle, LWP3_SERVICE_UUID);
|
|
}
|
|
|
|
static void onDisconnected(hci_con_handle_t handle) {
|
|
for (int i = 0; i < 2; i++) {
|
|
if (gHubs[i].state == HUB_IDLE) continue;
|
|
if (gHubs[i].conHandle != handle) continue;
|
|
|
|
gHubs[i].state = HUB_IDLE;
|
|
gHubs[i].conHandle = HCI_CON_HANDLE_INVALID;
|
|
gHubs[i].valueHandle = 0;
|
|
gHubs[i].retryAt = btstack_run_loop_get_time_ms() + RECONNECT_GAP_MS;
|
|
gHubsReady = false;
|
|
if (gActive == i) gActive = -1;
|
|
for (int p = 0; p < 3; p++) gThrottle[p].lastPower = 999;
|
|
}
|
|
}
|
|
|
|
static void hciPacketHandler(uint8_t packet_type, uint16_t channel,
|
|
uint8_t *packet, uint16_t size) {
|
|
if (packet_type != HCI_EVENT_PACKET) return;
|
|
|
|
switch (hci_event_packet_get_type(packet)) {
|
|
#ifdef HCI_EVENT_META_GAP
|
|
case HCI_EVENT_META_GAP:
|
|
if (hci_event_gap_meta_get_subevent_code(packet) !=
|
|
GAP_SUBEVENT_LE_CONNECTION_COMPLETE)
|
|
break;
|
|
onLeConnected(
|
|
gap_subevent_le_connection_complete_get_connection_handle(packet));
|
|
break;
|
|
#endif
|
|
case HCI_EVENT_LE_META:
|
|
if (hci_event_le_meta_get_subevent_code(packet) !=
|
|
HCI_SUBEVENT_LE_CONNECTION_COMPLETE)
|
|
break;
|
|
onLeConnected(
|
|
hci_subevent_le_connection_complete_get_connection_handle(packet));
|
|
break;
|
|
|
|
case HCI_EVENT_DISCONNECTION_COMPLETE:
|
|
onDisconnected(
|
|
hci_event_disconnection_complete_get_connection_handle(packet));
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
// One hub at a time. The controller only allows a single outstanding LE
|
|
// create-connection, and doing them in sequence keeps this simple.
|
|
static void serviceHubConnections() {
|
|
if (gActive >= 0) return;
|
|
|
|
uint32_t now = btstack_run_loop_get_time_ms();
|
|
for (int i = 0; i < 2; i++) {
|
|
if (gHubs[i].state != HUB_IDLE) continue;
|
|
if (now < gHubs[i].retryAt) continue;
|
|
|
|
gActive = i;
|
|
gHubs[i].state = HUB_CONNECTING;
|
|
gHubs[i].retryAt = now + RECONNECT_GAP_MS;
|
|
gap_connect(gHubs[i].addr, HUB_ADDR_TYPE);
|
|
return;
|
|
}
|
|
}
|
|
|
|
static void legoTick() {
|
|
serviceHubConnections();
|
|
|
|
// A connect attempt that never completes leaves us stuck on gActive.
|
|
if (gActive >= 0 && gHubs[gActive].state == HUB_CONNECTING &&
|
|
btstack_run_loop_get_time_ms() > gHubs[gActive].retryAt) {
|
|
gap_connect_cancel();
|
|
gHubs[gActive].state = HUB_IDLE;
|
|
gHubs[gActive].retryAt = btstack_run_loop_get_time_ms() + RECONNECT_GAP_MS;
|
|
gActive = -1;
|
|
return;
|
|
}
|
|
|
|
gHubsReady = (gHubs[0].state == HUB_READY && gHubs[1].state == HUB_READY);
|
|
|
|
driveMotor(gHubs[0], PORT_A, gDesired.leftTrack, gThrottle[0]);
|
|
driveMotor(gHubs[0], PORT_B, gDesired.rightTrack, gThrottle[1]);
|
|
driveMotor(gHubs[1], PORT_A, gDesired.head, gThrottle[2]);
|
|
}
|
|
|
|
static void legoTickHandler(btstack_timer_source_t *ts) {
|
|
legoTick();
|
|
btstack_run_loop_set_timer(ts, TICK_MS);
|
|
btstack_run_loop_add_timer(ts);
|
|
}
|
|
|
|
// Runs once, on the BTstack thread, scheduled from setup().
|
|
static void legoBootstrap(void *context) {
|
|
(void)context;
|
|
|
|
gatt_client_init();
|
|
|
|
gHciRegistration.callback = &hciPacketHandler;
|
|
hci_add_event_handler(&gHciRegistration);
|
|
|
|
gLegoTimer.process = &legoTickHandler;
|
|
btstack_run_loop_set_timer(&gLegoTimer, TICK_MS);
|
|
btstack_run_loop_add_timer(&gLegoTimer);
|
|
}
|
|
|
|
// =================================================================== setup
|
|
|
|
void setup() {
|
|
Serial.begin(115200);
|
|
pinMode(STATUS_LED_PIN, OUTPUT);
|
|
digitalWrite(STATUS_LED_PIN, LOW);
|
|
|
|
for (int i = 0; i < 2; i++) {
|
|
sscanf_bd_addr(HUB_ADDR_STR[i], gHubs[i].addr);
|
|
gHubs[i].state = HUB_IDLE;
|
|
gHubs[i].conHandle = HCI_CON_HANDLE_INVALID;
|
|
gHubs[i].valueHandle = 0;
|
|
gHubs[i].retryAt = 0;
|
|
}
|
|
|
|
BP32.setup(&onConnectedController, &onDisconnectedController);
|
|
BP32.enableVirtualDevice(false);
|
|
|
|
// Hubs first. Gamepad discovery gets switched on once they are up.
|
|
BP32.enableNewBluetoothConnections(false);
|
|
|
|
gBootstrapRegistration.callback = &legoBootstrap;
|
|
gBootstrapRegistration.context = NULL;
|
|
btstack_run_loop_execute_on_main_thread(&gBootstrapRegistration);
|
|
|
|
Serial.println("Connecting hubs, then opening for the gamepad");
|
|
}
|
|
|
|
void loop() {
|
|
BP32.update();
|
|
|
|
if (gHubsReady && !gDiscoveryEnabled) {
|
|
BP32.enableNewBluetoothConnections(true);
|
|
gDiscoveryEnabled = true;
|
|
Serial.println("Hubs up - put the PS4 pad in pairing mode (SHARE + PS)");
|
|
}
|
|
|
|
ControllerPtr gp = nullptr;
|
|
for (int i = 0; i < BP32_MAX_CONTROLLERS; i++) {
|
|
if (myControllers[i] && myControllers[i]->isConnected() &&
|
|
myControllers[i]->isGamepad()) {
|
|
gp = myControllers[i];
|
|
break;
|
|
}
|
|
}
|
|
|
|
digitalWrite(STATUS_LED_PIN, (gp && gHubsReady) ? HIGH : LOW);
|
|
|
|
if (gp == nullptr) {
|
|
gDesired.leftTrack = 0;
|
|
gDesired.rightTrack = 0;
|
|
gDesired.head = 0;
|
|
} else {
|
|
gDesired.leftTrack = stickToPower(-gp->axisY(), TRACK_MAX);
|
|
gDesired.rightTrack = stickToPower(-gp->axisRY(), TRACK_MAX);
|
|
gDesired.head = stickToPower(gp->axisX(), HEAD_MAX);
|
|
}
|
|
|
|
vTaskDelay(1);
|
|
}
|