raspberry pi pico wでMicroPython環境でps4コントローラーが使いたい

/*
 * Derived from the btstack hid_host_demo:
 * Copyright (C) 2017 BlueKitchen GmbH
 *
 * Modifications Copyright (C) 2021-2023 Brian Starkey <stark3y@gmail.com>
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 3. Neither the name of the copyright holders nor the names of
 *    contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 * 4. Any redistribution, use, or modification is done solely for
 *    personal benefit and not for any commercial purpose or for
 *    monetary gain.
 *
 * THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH AND CONTRIBUTORS
 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL MATTHIAS
 * RINGWALD OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
 * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 *
 * Please inquire about commercial licensing options at
 * contact@bluekitchen-gmbh.com
 *
 */

#include <BTstackLib.h>
#include <inttypes.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>

#include "pico/stdlib.h"
#include "pico/cyw43_arch.h"
#include "pico/async_context.h"
#include "boards/pico_w.h"

#include "btstack_run_loop.h"
#include "btstack_config.h"
#include "btstack.h"
#include "btstack_hid.h"
#include "classic/sdp_server.h"

// #include "bt_hid.h"

#define MAX_ATTRIBUTE_VALUE_SIZE 512

// SN30 Pro
//static const char * remote_addr_string = "E4:17:D8:EE:73:0E";
// Real DS4
//static const char * remote_addr_string = "00:22:68:DB:D3:66";
// Knockoff DS4
//static const char * remote_addr_string = "A5:15:66:8E:91:3B";
// Brian C Knockoff DS4
static const char * remote_addr_string = "ac:fd:93:fa:ec:be";

static bd_addr_t remote_addr;
static bd_addr_t connected_addr;
static btstack_packet_callback_registration_t hci_event_callback_registration;

// SDP
static uint8_t hid_descriptor_storage[MAX_ATTRIBUTE_VALUE_SIZE];

static uint16_t hid_host_cid = 0;
static bool     hid_host_descriptor_available = false;
static hid_protocol_mode_t hid_host_report_mode = HID_PROTOCOL_MODE_REPORT;

static void packet_handler (uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size);

static void hid_host_setup(void){
	// Initialize L2CAP
	l2cap_init();

	sdp_init();

	// Initialize HID Host
	hid_host_init(hid_descriptor_storage, sizeof(hid_descriptor_storage));
	hid_host_register_packet_handler(packet_handler);

	// Allow sniff mode requests by HID device and support role switch
	gap_set_default_link_policy_settings(LM_LINK_POLICY_ENABLE_SNIFF_MODE | LM_LINK_POLICY_ENABLE_ROLE_SWITCH);

	// try to become master on incoming connections
	hci_set_master_slave_policy(HCI_ROLE_MASTER);

	// register for HCI events
	hci_event_callback_registration.callback = &packet_handler;
	hci_add_event_handler(&hci_event_callback_registration);
}

struct bt_hid_state {
	uint16_t buttons;
	uint8_t lx;
	uint8_t ly;
	uint8_t rx;
	uint8_t ry;
	uint8_t l2;
	uint8_t r2;
	uint8_t hat;
	uint8_t pad;
};

const struct bt_hid_state default_state = {
	.buttons = 0,
	.lx = 0x80,
	.ly = 0x80,
	.rx = 0x80,
	.ry = 0x80,
	.l2 = 0x80,
	.r2 = 0x80,
	.hat = 0x8,
};

struct bt_hid_state latest;

struct __attribute__((packed)) input_report_17 {
	uint8_t report_id;
	uint8_t pad[2];

	uint8_t lx, ly;
	uint8_t rx, ry;
	uint8_t buttons[3];
	uint8_t l2, r2;

	uint16_t timestamp;
	uint16_t temperature;
	uint16_t gyro[3];
	uint16_t accel[3];
	uint8_t pad2[5];
	uint8_t status[2];
	uint8_t pad3;
};

static void hid_host_handle_interrupt_report(const uint8_t *packet, uint16_t packet_len){
	static struct bt_hid_state last_state = { 0 };

	// Only interested in report_id 0x11
	if (packet_len < sizeof(struct input_report_17) + 1) {
		return;
	}

	if ((packet[0] != 0xa1) || (packet[1] != 0x11)) {
		return;
	}

	//printf_hexdump(packet, packet_len);

	struct input_report_17 *report = (struct input_report_17 *)&packet[1];

	// Note: This assumes that we're protected by async_context's
	// single-threaded-ness
	latest = (struct bt_hid_state){
		// Somewhat arbitrary packing of the buttons into a single 16-bit word
		.buttons = ((report->buttons[0] & 0xf0) << 8) | ((report->buttons[2] & 0x3) << 8) | (report->buttons[1]),

		.lx = report->lx,
		.ly = report->ly,
		.rx = report->rx,
		.ry = report->ry,
		.l2 = report->l2,
		.r2 = report->r2,

		.hat = (report->buttons[0] & 0xf),
	};

	// TODO: Parse out battery, touchpad, sixaxis, timestamp, temperature(?!)
	// Sensors will also need calibration

}

void bt_hid_get_latest(struct bt_hid_state *dst)
{
	async_context_t *context = cyw43_arch_async_context();
	async_context_acquire_lock_blocking(context);
	memcpy(dst, &latest, sizeof(*dst));
	async_context_release_lock(context);
}

static void bt_hid_disconnected(bd_addr_t addr)
{
	hid_host_cid = 0;
	hid_host_descriptor_available = false;

	memcpy(&latest, &default_state, sizeof(latest));
}

static void packet_handler (uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size)
{
	UNUSED(channel);
	UNUSED(size);

	uint8_t   event;
	uint8_t   hid_event;
	bd_addr_t event_addr;
	uint8_t   status;
	uint8_t reason;

	if (packet_type != HCI_EVENT_PACKET) {
		return;
	}

	event = hci_event_packet_get_type(packet);
	switch (event) {
	case BTSTACK_EVENT_STATE:
		// On boot, we try a manual connection
		if (btstack_event_state_get_state(packet) == HCI_STATE_WORKING){
			Serial.printf("Starting hid_host_connect (%s)\n", bd_addr_to_str(remote_addr));
			status = hid_host_connect(remote_addr, hid_host_report_mode, &hid_host_cid);
			if (status != ERROR_CODE_SUCCESS){
				Serial.printf("hid_host_connect command failed: 0x%02x\n", status);
			}
		}
		break;
	case HCI_EVENT_CONNECTION_COMPLETE:
		status = hci_event_connection_complete_get_status(packet);
		Serial.printf("Connection complete: %x\n", status);
		break;
	case HCI_EVENT_DISCONNECTION_COMPLETE:
		status = hci_event_disconnection_complete_get_status(packet);
		reason = hci_event_disconnection_complete_get_reason(packet);
		Serial.printf("Disconnection complete: status: %x, reason: %x\n", status, reason);
		break;
	case HCI_EVENT_MAX_SLOTS_CHANGED:
		status = hci_event_max_slots_changed_get_lmp_max_slots(packet);
		Serial.printf("Max slots changed: %x\n", status);
		break;
	case HCI_EVENT_PIN_CODE_REQUEST:
		Serial.printf("Pin code request. Responding '0000'\n");
		hci_event_pin_code_request_get_bd_addr(packet, event_addr);
		gap_pin_code_response(event_addr, "0000");
		break;
	case HCI_EVENT_USER_CONFIRMATION_REQUEST:
		Serial.printf("SSP User Confirmation Request: %d\n", little_endian_read_32(packet, 8));
		break;
	case HCI_EVENT_HID_META:
		hid_event = hci_event_hid_meta_get_subevent_code(packet);
		switch (hid_event) {
		case HID_SUBEVENT_INCOMING_CONNECTION:
			hid_subevent_incoming_connection_get_address(packet, event_addr);
			Serial.printf("Accepting connection from %s\n", bd_addr_to_str(event_addr));
			hid_host_accept_connection(hid_subevent_incoming_connection_get_hid_cid(packet), hid_host_report_mode);
			break;
		case HID_SUBEVENT_CONNECTION_OPENED:
			status = hid_subevent_connection_opened_get_status(packet);
			hid_subevent_connection_opened_get_bd_addr(packet, event_addr);
			if (status != ERROR_CODE_SUCCESS) {
				Serial.printf("Connection to %s failed: 0x%02x\n", bd_addr_to_str(event_addr), status);
				bt_hid_disconnected(event_addr);
				return;
			}
			hid_host_descriptor_available = false;
			hid_host_cid = hid_subevent_connection_opened_get_hid_cid(packet);
			Serial.printf("Connected to %s\n", bd_addr_to_str(event_addr));
			bd_addr_copy(connected_addr, event_addr);
			break;
		case HID_SUBEVENT_DESCRIPTOR_AVAILABLE:
			status = hid_subevent_descriptor_available_get_status(packet);
			if (status == ERROR_CODE_SUCCESS){
				hid_host_descriptor_available = true;

				uint16_t dlen = hid_descriptor_storage_get_descriptor_len(hid_host_cid);
				Serial.printf("HID descriptor available. Len: %d\n", dlen);

				// Send FEATURE 0x05, to switch the controller to "full" report mode
				hid_host_send_get_report(hid_host_cid, HID_REPORT_TYPE_FEATURE, 0x05);
			} else {
				Serial.printf("Couldn't process HID Descriptor, status: %d\n", status);
			}
			break;
		case HID_SUBEVENT_REPORT:
			if (hid_host_descriptor_available){
				hid_host_handle_interrupt_report(hid_subevent_report_get_report(packet), hid_subevent_report_get_report_len(packet));
			} else {
				Serial.printf("No hid host descriptor available\n");
				printf_hexdump(hid_subevent_report_get_report(packet), hid_subevent_report_get_report_len(packet));
			}
			break;
		case HID_SUBEVENT_SET_PROTOCOL_RESPONSE: {
			status = hid_subevent_set_protocol_response_get_handshake_status(packet);
			if (status != HID_HANDSHAKE_PARAM_TYPE_SUCCESSFUL){
				Serial.printf("Protocol handshake error: 0x%02x\n", status);
				break;
			}
			hid_protocol_mode_t proto = (hid_protocol_mode_t)hid_subevent_set_protocol_response_get_protocol_mode(packet);
			switch (proto) {
			case HID_PROTOCOL_MODE_BOOT:
				Serial.printf("Negotiated protocol: BOOT\n");
				break;
			case HID_PROTOCOL_MODE_REPORT:
				Serial.printf("Negotiated protocol: REPORT\n");
				break;
			default:
				Serial.printf("Negotiated unknown protocol: 0x%x\n", proto);
				break;
			}
    }
			break;
		case HID_SUBEVENT_CONNECTION_CLOSED:
			Serial.printf("HID connection closed: %s\n", bd_addr_to_str(connected_addr));
			bt_hid_disconnected(connected_addr);
			break;
		case HID_SUBEVENT_GET_REPORT_RESPONSE:
			{
				status = hid_subevent_get_report_response_get_handshake_status(packet);
				uint16_t dlen =  hid_subevent_get_report_response_get_report_len(packet);
				Serial.printf("GET_REPORT response. status: %d, len: %d\n", status, dlen);
			}
			break;
		default:
			Serial.printf("Unknown HID subevent: 0x%x\n", hid_event);
			break;
		}
		break;
	default:
		//Serial.printf("Unknown HCI event: 0x%x\n", event);
		break;
	}
}

#define BLINK_MS 250
static btstack_timer_source_t blink_timer;
static void blink_handler(btstack_timer_source_t *ts)
{
	static bool on = 0;

	if (hid_host_cid != 0) {
		on = true;
	} else {
		on = !on;
	}

	cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, !!on);

	btstack_run_loop_set_timer(&blink_timer, BLINK_MS);
	btstack_run_loop_add_timer(&blink_timer);
}

void bt_main(void) {
	if (cyw43_arch_init()) {
		Serial.printf("Wi-Fi init failed\n");
		return;
	}

	gap_set_security_level(LEVEL_2);

	blink_timer.process = &blink_handler;
	btstack_run_loop_set_timer(&blink_timer, BLINK_MS);
	btstack_run_loop_add_timer(&blink_timer);

	hid_host_setup();
	sscanf_bd_addr(remote_addr_string, remote_addr);
	bt_hid_disconnected(remote_addr);

	hci_power_control(HCI_POWER_ON);

	btstack_run_loop_execute();
}

// SPDX-License-Identifier: BSD-3-Clause
// Copyright (c) 2023 Brian Starkey <stark3y@gmail.com>

#include <BTstackLib.h>
// #include <stdio.h>
#include <string.h>

#include "hardware/gpio.h"
#include "hardware/pwm.h"
#include "pico/stdlib.h"
// #include "pico/stdio.h"
#include "pico/multicore.h"

// #include "bt_hid.h"

// These magic values are just taken from M0o+, not calibrated for
// the Tiny chassis.
#define PWM_MIN 80
#define PWM_MAX (PWM_MIN + 127)

static inline int8_t clamp8(int16_t value) {
        if (value > 127) {
                return 127;
        } else if (value < -128) {
                return -128;
        }

        return value;
}

struct slice {
	unsigned int slice_num;
	unsigned int pwm_min;
};

struct chassis {
	struct slice slice_l;
	struct slice slice_r;

	int8_t l;
	int8_t r;
};

void init_slice(struct slice *slice, unsigned int slice_num, unsigned int pwm_min, uint8_t pin_a)
{
	slice->slice_num = slice_num;
	slice->pwm_min = pwm_min;
	gpio_set_function(pin_a, GPIO_FUNC_PWM);
	gpio_set_function(pin_a + 1, GPIO_FUNC_PWM);
	pwm_set_wrap(slice->slice_num, slice->pwm_min + 127 + 1);
	pwm_set_chan_level(slice->slice_num, PWM_CHAN_A, 0);
	pwm_set_chan_level(slice->slice_num, PWM_CHAN_B, 0);
	pwm_set_enabled(slice->slice_num, true);
}

void chassis_init(struct chassis *chassis, uint8_t pin_la, uint8_t pin_ra)
{

	init_slice(&chassis->slice_l, pwm_gpio_to_slice_num(pin_la), PWM_MIN, pin_la);
	init_slice(&chassis->slice_r, pwm_gpio_to_slice_num(pin_ra), PWM_MIN, pin_ra);
}

static inline uint8_t abs8(int8_t v) {
	return v < 0 ? -v : v;
}

void slice_set_with_brake(struct slice *slice, int8_t value, bool brake)
{
	uint8_t mag = abs8(value);

	if (value == 0) {
		pwm_set_both_levels(slice->slice_num, brake ? slice->pwm_min + 127 : 0, brake ? slice->pwm_min + 127 : 0);
	} else if (value < 0) {
		pwm_set_both_levels(slice->slice_num, slice->pwm_min + mag, 0);
	} else {
		pwm_set_both_levels(slice->slice_num, 0, slice->pwm_min + mag);
	}
}

void slice_set(struct slice *slice, int8_t value)
{
	slice_set_with_brake(slice, value, false);
}

void chassis_set_raw(struct chassis *chassis, int8_t left, int8_t right)
{
	slice_set(&chassis->slice_l, left);
	slice_set(&chassis->slice_r, right);

	chassis->l = left;
	chassis->r = right;
}

void chassis_set(struct chassis *chassis, int8_t linear, int8_t rot)
{
	// Positive rotation == CCW == right goes faster

	if (linear < -127) {
		linear = -127;
	}

	if (rot < -127) {
		rot = -127;
	}

	int l = linear - rot;
	int r = linear + rot;
	int adj = 0;

	if (l > 127) {
		adj = l - 127;
	} else if (l < -127) {
		adj = l + 127;
	}else if (r > 127) {
		adj = r - 127;
	} else if (r < -127) {
		adj = r + 127;
	}

	l -= adj;
	r -= adj;

	// FIXME: Motor directions should be a parameter
	r = -r;

	chassis_set_raw(chassis, l, r);
}

struct chassis chassis = { 0 };
struct bt_hid_state state;

void setup() {
	// stdio_init_all();
  Serial.begin(9600);
  delay(5000); 

	delay(1000);
	Serial.printf("Hello\n");

	multicore_launch_core1(bt_main);
	// Wait for init (should do a handshake with the fifo here?)
	delay(1000);

	chassis_init(&chassis, 6, 8);
	bt_hid_get_latest(&state);
}

void loop() {
		delay(1000);
		Serial.printf("buttons: %04x, l: %d,%d, r: %d,%d, l2,r2: %d,%d hat: %d\n",
				state.buttons, state.lx, state.ly, state.rx, state.ry,
				state.l2, state.r2, state.hat);

		float speed_scale = 1.0;
		int8_t linear = clamp8(-(state.ly - 128) * speed_scale);
		int8_t rot = clamp8(-(state.rx - 128));
		chassis_set(&chassis, linear, rot);
}