mirror of
https://github.com/whowechina/diva_pico.git
synced 2026-10-08 22:50:10 +03:00
PS4 support, but no auth
This commit is contained in:
whowechina
Binary file not shown.
@@ -5,7 +5,7 @@ function(make_firmware board board_def)
|
||||
pico_sdk_init()
|
||||
add_executable(${board}
|
||||
main.c slider.c rgb.c button.c savedata.c config.c commands.c
|
||||
cli.c mpr121.c hebtn.c lzfx.c usb_descriptors.c ps4_feat.c slide.c)
|
||||
cli.c mpr121.c hebtn.c lzfx.c usb_descriptors.c ps4_feat.c gesture.c)
|
||||
target_compile_definitions(${board} PUBLIC ${board_def})
|
||||
pico_enable_stdio_usb(${board} 1)
|
||||
pico_enable_stdio_uart(${board} 0)
|
||||
|
||||
@@ -13,7 +13,7 @@
|
||||
#include "savedata.h"
|
||||
#include "cli.h"
|
||||
|
||||
#include "slide.h"
|
||||
#include "gesture.h"
|
||||
|
||||
#include "usb_descriptors.h"
|
||||
|
||||
@@ -401,7 +401,7 @@ static void handle_debug(int argc, char *argv[])
|
||||
break;
|
||||
case 1:
|
||||
diva_runtime.debug.slide_cluster ^= true;
|
||||
slide_set_debug_cluster(diva_runtime.debug.slide_cluster);
|
||||
gesture_set_debug_cluster(diva_runtime.debug.slide_cluster);
|
||||
break;
|
||||
default:
|
||||
printf(usage);
|
||||
@@ -414,7 +414,7 @@ void cli_ctrl_c_cb(void)
|
||||
diva_runtime.debug.sensor = false;
|
||||
diva_runtime.debug.slide_cluster = false;
|
||||
hebtn_debug(false);
|
||||
slide_set_debug_cluster(false);
|
||||
gesture_set_debug_cluster(false);
|
||||
}
|
||||
|
||||
static void handle_save()
|
||||
|
||||
@@ -0,0 +1,477 @@
|
||||
/*
|
||||
* Extract gesture information from raw electrode touch bitmask.
|
||||
*
|
||||
* I'm using a region clustering + tracking approach.
|
||||
* Regions are contiguous (or gap-bridged) sets of active electrodes in one frame.
|
||||
* Clusters are living, persistent objects that are driven by regions.
|
||||
* Gestures are recognized from cluster movement and reported as joystick axes.
|
||||
*
|
||||
* WHowe <github.com/whowechina>
|
||||
*/
|
||||
|
||||
#include "gesture.h"
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <stdbool.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "pico/stdlib.h"
|
||||
|
||||
#define ELEC_COUNT 32
|
||||
|
||||
#define MAX_REGIONS 12 // Per-frame extracted regions.
|
||||
#define MAX_CLUSTERS 12 // Living clusters tracked across frames.
|
||||
|
||||
#define REGION_GAP_SIZE 1 // Maximum gap size (in electrodes) allowed inside one region.
|
||||
#define MATCH_MAX_DIST_Q4 56 // Max cluster-to-region match distance in Q4 units.
|
||||
|
||||
#define DEBOUNCE_SET_FRAMES 4
|
||||
#define DEBOUNCE_RESET_FRAMES 50
|
||||
|
||||
#define SEQ_MIN_STEPS 3 // Required consecutive +/-1 index moves to confirm direction.
|
||||
#define SEQ_TIMEOUT_FRAMES 200 // Max frames allowed between sequential steps.
|
||||
#define DEATH_HOLD_FRAMES 100 // Cluster death delay.
|
||||
|
||||
#define AXIS_CENTER 0x80
|
||||
#define AXIS_OFFSET 100 // Signed offset from center when steering left/right.
|
||||
|
||||
// A region is a contiguous (or gap-bridged) set of active electrodes in one frame.
|
||||
// Q4 units are used for more precise center position and distance calculation.
|
||||
typedef struct {
|
||||
int start;
|
||||
int end;
|
||||
int width;
|
||||
int center_q4;
|
||||
} region_t;
|
||||
|
||||
// A cluster is a living, persistent object that follows a region across frames.
|
||||
typedef struct {
|
||||
bool alive;
|
||||
int id;
|
||||
int miss;
|
||||
int width;
|
||||
int slot;
|
||||
int center_q4;
|
||||
int prev_center_q4;
|
||||
int last_idx;
|
||||
int seq_age;
|
||||
int seq_len;
|
||||
int seq_dir;
|
||||
int dir;
|
||||
} cluster_t;
|
||||
|
||||
static cluster_t clusters[MAX_CLUSTERS];
|
||||
static int next_id;
|
||||
|
||||
static uint32_t db_state;
|
||||
static int db_cnt[ELEC_COUNT];
|
||||
|
||||
static bool debug_cluster;
|
||||
static int latch_frames = 100;
|
||||
|
||||
static int hold_left_dir;
|
||||
static int hold_right_dir;
|
||||
static int hold_left_frames;
|
||||
static int hold_right_frames;
|
||||
|
||||
static uint32_t debounce_mask(uint32_t raw)
|
||||
{
|
||||
for (int i = 0; i < ELEC_COUNT; i++) {
|
||||
uint32_t bit = (raw >> i) & 1u;
|
||||
bool state = ((db_state >> i) & 1u) != 0u;
|
||||
int threshold = state ? DEBOUNCE_RESET_FRAMES : DEBOUNCE_SET_FRAMES;
|
||||
if (bit != state) {
|
||||
if (db_cnt[i] < threshold) {
|
||||
db_cnt[i]++;
|
||||
}
|
||||
if (db_cnt[i] >= threshold) {
|
||||
db_state ^= (1u << i);
|
||||
db_cnt[i] = 0;
|
||||
}
|
||||
} else {
|
||||
db_cnt[i] = 0;
|
||||
}
|
||||
}
|
||||
return db_state;
|
||||
}
|
||||
|
||||
static int extract_regions(uint32_t mask, region_t *regions, int max_regions)
|
||||
{
|
||||
int count = 0;
|
||||
int i = 0;
|
||||
|
||||
while (i < ELEC_COUNT) {
|
||||
if (((mask >> i) & 1u) == 0u) {
|
||||
i++;
|
||||
continue;
|
||||
}
|
||||
|
||||
int start = i;
|
||||
int end = i;
|
||||
i++;
|
||||
|
||||
while (i < ELEC_COUNT) {
|
||||
if (((mask >> i) & 1u) != 0u) {
|
||||
end = i;
|
||||
i++;
|
||||
} else {
|
||||
int j = i;
|
||||
while ((j < ELEC_COUNT) && (((mask >> j) & 1u) == 0u)) {
|
||||
j++;
|
||||
}
|
||||
int gap = j - i;
|
||||
if ((gap <= REGION_GAP_SIZE) && (j < ELEC_COUNT)) {
|
||||
i = j;
|
||||
} else {
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (count < max_regions) {
|
||||
regions[count].start = start;
|
||||
regions[count].end = end;
|
||||
regions[count].width = end - start + 1;
|
||||
regions[count].center_q4 = (start + end) * 8;
|
||||
count++;
|
||||
}
|
||||
}
|
||||
|
||||
return count;
|
||||
}
|
||||
|
||||
static int alloc_cluster(void)
|
||||
{
|
||||
for (int i = 0; i < MAX_CLUSTERS; i++) {
|
||||
if (!clusters[i].alive) {
|
||||
return i;
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
static int q4_to_idx(int q4)
|
||||
{
|
||||
if (q4 <= 0) {
|
||||
return 0;
|
||||
}
|
||||
if (q4 >= 31 * 16) {
|
||||
return 31;
|
||||
}
|
||||
return (q4 + 8) / 16;
|
||||
}
|
||||
|
||||
static int sgn16(int x)
|
||||
{
|
||||
return (x > 0) - (x < 0);
|
||||
}
|
||||
|
||||
static void update_seq(cluster_t *c)
|
||||
{
|
||||
int idx = q4_to_idx(c->center_q4);
|
||||
if (idx == c->last_idx) {
|
||||
if (c->seq_age < 0x7FFFFFFF) {
|
||||
c->seq_age++;
|
||||
}
|
||||
if (c->seq_age > SEQ_TIMEOUT_FRAMES) {
|
||||
c->dir = 0;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
int diff = idx - c->last_idx;
|
||||
int ad = abs(diff);
|
||||
int sd = sgn16(diff);
|
||||
|
||||
if (ad == 1 && c->seq_age <= SEQ_TIMEOUT_FRAMES) {
|
||||
if (sd == c->seq_dir) {
|
||||
if (c->seq_len < 0x7FFFFFFF) {
|
||||
c->seq_len++;
|
||||
}
|
||||
} else {
|
||||
c->seq_dir = sd;
|
||||
c->seq_len = 1;
|
||||
}
|
||||
} else {
|
||||
c->seq_dir = 0;
|
||||
c->seq_len = 0;
|
||||
}
|
||||
|
||||
c->last_idx = idx;
|
||||
c->seq_age = 0;
|
||||
|
||||
if (c->seq_len >= SEQ_MIN_STEPS) {
|
||||
c->dir = c->seq_dir;
|
||||
}
|
||||
}
|
||||
|
||||
static void assign_slot_if_needed(cluster_t *c)
|
||||
{
|
||||
if (c->slot != -1 || c->dir == 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
bool left_used = false;
|
||||
bool right_used = false;
|
||||
for (int i = 0; i < MAX_CLUSTERS; i++) {
|
||||
if (!clusters[i].alive) {
|
||||
continue;
|
||||
}
|
||||
if (clusters[i].slot == 0) {
|
||||
left_used = true;
|
||||
} else if (clusters[i].slot == 1) {
|
||||
right_used = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (c->center_q4 < (16 * 16)) {
|
||||
if (!left_used) {
|
||||
c->slot = 0;
|
||||
} else if (!right_used) {
|
||||
c->slot = 1;
|
||||
}
|
||||
} else {
|
||||
if (!right_used) {
|
||||
c->slot = 1;
|
||||
} else if (!left_used) {
|
||||
c->slot = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void match_and_update(const region_t *regions, int region_count)
|
||||
{
|
||||
bool region_used[MAX_REGIONS] = {0};
|
||||
bool cluster_used[MAX_CLUSTERS] = {0};
|
||||
|
||||
while (true) {
|
||||
int best = 0x7FFFFFFF;
|
||||
int best_cluster = -1;
|
||||
int best_region = -1;
|
||||
|
||||
for (int i = 0; i < MAX_CLUSTERS; i++) {
|
||||
if (!clusters[i].alive || cluster_used[i]) {
|
||||
continue;
|
||||
}
|
||||
for (int j = 0; j < region_count; j++) {
|
||||
if (region_used[j]) {
|
||||
continue;
|
||||
}
|
||||
int d = abs(clusters[i].center_q4 - regions[j].center_q4);
|
||||
if (d < best) {
|
||||
best = d;
|
||||
best_cluster = i;
|
||||
best_region = j;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (best_cluster < 0 || best_region < 0 || best > MATCH_MAX_DIST_Q4) {
|
||||
break;
|
||||
}
|
||||
|
||||
cluster_t *c = &clusters[best_cluster];
|
||||
const region_t *reg = ®ions[best_region];
|
||||
|
||||
c->miss = 0;
|
||||
c->prev_center_q4 = c->center_q4;
|
||||
c->center_q4 = reg->center_q4;
|
||||
c->width = reg->width;
|
||||
update_seq(c);
|
||||
assign_slot_if_needed(c);
|
||||
|
||||
cluster_used[best_cluster] = true;
|
||||
region_used[best_region] = true;
|
||||
}
|
||||
|
||||
for (int i = 0; i < MAX_CLUSTERS; i++) {
|
||||
if (!clusters[i].alive || cluster_used[i]) {
|
||||
continue;
|
||||
}
|
||||
clusters[i].miss++;
|
||||
if (clusters[i].miss > DEATH_HOLD_FRAMES) {
|
||||
memset(&clusters[i], 0, sizeof(clusters[i]));
|
||||
}
|
||||
}
|
||||
|
||||
for (int j = 0; j < region_count; j++) {
|
||||
if (region_used[j]) {
|
||||
continue;
|
||||
}
|
||||
int ci = alloc_cluster();
|
||||
if (ci < 0) {
|
||||
break;
|
||||
}
|
||||
cluster_t *c = &clusters[ci];
|
||||
memset(c, 0, sizeof(*c));
|
||||
c->alive = true;
|
||||
c->id = next_id++;
|
||||
c->slot = -1;
|
||||
c->center_q4 = regions[j].center_q4;
|
||||
c->prev_center_q4 = regions[j].center_q4;
|
||||
c->width = regions[j].width;
|
||||
c->last_idx = q4_to_idx(regions[j].center_q4);
|
||||
}
|
||||
}
|
||||
|
||||
static void choose_dirs(int *left_dir, int *right_dir)
|
||||
{
|
||||
int l = 0;
|
||||
int r = 0;
|
||||
int lscore = 0;
|
||||
int rscore = 0;
|
||||
|
||||
int moving_count = 0;
|
||||
for (int i = 0; i < MAX_CLUSTERS; i++) {
|
||||
if (clusters[i].alive && clusters[i].dir != 0) {
|
||||
moving_count++;
|
||||
}
|
||||
}
|
||||
|
||||
for (int i = 0; i < MAX_CLUSTERS; i++) {
|
||||
if (!clusters[i].alive || clusters[i].dir == 0) {
|
||||
continue;
|
||||
}
|
||||
if (clusters[i].slot == 0 && clusters[i].seq_len >= lscore) {
|
||||
lscore = clusters[i].seq_len;
|
||||
l = clusters[i].dir;
|
||||
} else if (clusters[i].slot == 1 && clusters[i].seq_len >= rscore) {
|
||||
rscore = clusters[i].seq_len;
|
||||
r = clusters[i].dir;
|
||||
}
|
||||
}
|
||||
|
||||
if (moving_count >= 3) {
|
||||
bool has_neg = false;
|
||||
bool has_pos = false;
|
||||
for (int i = 0; i < MAX_CLUSTERS; i++) {
|
||||
if (!clusters[i].alive || clusters[i].dir == 0) {
|
||||
continue;
|
||||
}
|
||||
if (clusters[i].dir < 0) {
|
||||
has_neg = true;
|
||||
} else if (clusters[i].dir > 0) {
|
||||
has_pos = true;
|
||||
}
|
||||
}
|
||||
if (has_neg && has_pos) {
|
||||
if (l == 0) {
|
||||
l = -1;
|
||||
}
|
||||
if (r == 0) {
|
||||
r = 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
*left_dir = l;
|
||||
*right_dir = r;
|
||||
}
|
||||
|
||||
static void hold_axis(int in_l, int in_r, int *out_l, int *out_r)
|
||||
{
|
||||
if (in_l != 0) {
|
||||
hold_left_dir = in_l;
|
||||
hold_left_frames = latch_frames;
|
||||
} else if (hold_left_frames > 0) {
|
||||
hold_left_frames--;
|
||||
} else {
|
||||
hold_left_dir = 0;
|
||||
}
|
||||
|
||||
if (in_r != 0) {
|
||||
hold_right_dir = in_r;
|
||||
hold_right_frames = latch_frames;
|
||||
} else if (hold_right_frames > 0) {
|
||||
hold_right_frames--;
|
||||
} else {
|
||||
hold_right_dir = 0;
|
||||
}
|
||||
|
||||
*out_l = hold_left_dir;
|
||||
*out_r = hold_right_dir;
|
||||
}
|
||||
|
||||
static void debug_print(void)
|
||||
{
|
||||
if (!debug_cluster) {
|
||||
return;
|
||||
}
|
||||
|
||||
bool printed = false;
|
||||
for (int i = 0; i < MAX_CLUSTERS; i++) {
|
||||
if (!clusters[i].alive) {
|
||||
continue;
|
||||
}
|
||||
const char *tag = "*";
|
||||
if (clusters[i].dir > 0) {
|
||||
tag = ">>";
|
||||
} else if (clusters[i].dir < 0) {
|
||||
tag = "<<";
|
||||
}
|
||||
if (printed) {
|
||||
printf(" ");
|
||||
}
|
||||
printed = true;
|
||||
printf("{%d %d %d %s %u s%u}",
|
||||
clusters[i].id,
|
||||
q4_to_idx(clusters[i].center_q4),
|
||||
clusters[i].width,
|
||||
tag,
|
||||
(unsigned)(DEATH_HOLD_FRAMES - ((clusters[i].miss > DEATH_HOLD_FRAMES) ? DEATH_HOLD_FRAMES : clusters[i].miss)),
|
||||
(unsigned)((clusters[i].slot < 0) ? 9 : clusters[i].slot));
|
||||
}
|
||||
if (printed) {
|
||||
printf("\n");
|
||||
}
|
||||
}
|
||||
|
||||
void gesture_reset(void)
|
||||
{
|
||||
memset(clusters, 0, sizeof(clusters));
|
||||
next_id = 1;
|
||||
|
||||
db_state = 0;
|
||||
memset(db_cnt, 0, sizeof(db_cnt));
|
||||
|
||||
hold_left_dir = 0;
|
||||
hold_right_dir = 0;
|
||||
hold_left_frames = 0;
|
||||
hold_right_frames = 0;
|
||||
}
|
||||
|
||||
void gesture_set_latch(int frames)
|
||||
{
|
||||
latch_frames = (frames == 0) ? 1 : frames;
|
||||
}
|
||||
|
||||
void gesture_set_debug_cluster(bool enable)
|
||||
{
|
||||
debug_cluster = enable;
|
||||
}
|
||||
|
||||
static int axis_from_dir(int dir)
|
||||
{
|
||||
return AXIS_CENTER + dir * AXIS_OFFSET;
|
||||
}
|
||||
|
||||
void gesture_process(uint32_t mask, uint8_t *axis_a, uint8_t *axis_b)
|
||||
{
|
||||
uint32_t filtered_mask = debounce_mask(mask);
|
||||
|
||||
region_t regions[MAX_REGIONS];
|
||||
int region_num = extract_regions(filtered_mask, regions, count_of(regions));
|
||||
|
||||
match_and_update(regions, region_num);
|
||||
|
||||
int l = 0;
|
||||
int r = 0;
|
||||
choose_dirs(&l, &r);
|
||||
hold_axis(l, r, &l, &r);
|
||||
|
||||
*axis_a = axis_from_dir(l);
|
||||
*axis_b = axis_from_dir(r);
|
||||
|
||||
debug_print();
|
||||
}
|
||||
@@ -0,0 +1,18 @@
|
||||
/*
|
||||
* Extract gesture information from raw electrode touch bitmask.
|
||||
*
|
||||
* WHowe <github.com/whowechina>
|
||||
*/
|
||||
|
||||
#ifndef GESTURE_H
|
||||
#define GESTURE_H
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
void gesture_reset(void);
|
||||
void gesture_set_latch(int frames);
|
||||
void gesture_set_debug_cluster(bool enable);
|
||||
void gesture_process(uint32_t mask, uint8_t *axis_a, uint8_t *axis_b);
|
||||
|
||||
#endif
|
||||
+4
-8
@@ -35,7 +35,7 @@
|
||||
#include "hebtn.h"
|
||||
#include "lzfx.h"
|
||||
#include "ps4_feat.h"
|
||||
#include "slide.h"
|
||||
#include "gesture.h"
|
||||
|
||||
struct __attribute__((packed)) {
|
||||
uint16_t buttons; // 16 buttons; see JoystickButtons_t for bit mapping
|
||||
@@ -151,8 +151,6 @@ static void gen_ns_report()
|
||||
static void gen_ps4_report()
|
||||
{
|
||||
uint16_t ps4_buttons = mapped_buttons;
|
||||
slide_result_t slide_result;
|
||||
|
||||
hid_ps4.left_y = 0x80;
|
||||
hid_ps4.right_y = 0x80;
|
||||
hid_ps4.hat_buttons = (ps4_buttons << 4);
|
||||
@@ -161,9 +159,7 @@ static void gen_ps4_report()
|
||||
hid_ps4.trigger_l = 0;
|
||||
hid_ps4.trigger_r = 0;
|
||||
|
||||
slide_process(touch_mask_raw(), &slide_result);
|
||||
hid_ps4.left_x = slide_result.left_x;
|
||||
hid_ps4.right_x = slide_result.right_x;
|
||||
gesture_process(touch_mask_raw(), &hid_ps4.left_x, &hid_ps4.right_x);
|
||||
}
|
||||
|
||||
static void gen_hid_report()
|
||||
@@ -326,8 +322,8 @@ void init()
|
||||
diva_runtime.hid_ps4 = (diva_cfg->hid.joy_map == 3);
|
||||
hid_use_ps4(diva_runtime.hid_ps4);
|
||||
|
||||
slide_reset();
|
||||
slide_set_latch(100);
|
||||
gesture_reset();
|
||||
gesture_set_latch(100);
|
||||
|
||||
board_init();
|
||||
tusb_init();
|
||||
|
||||
@@ -1,598 +0,0 @@
|
||||
#include "slide.h"
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
#define SLIDE_MIN_CLUSTER_WIDTH 1
|
||||
#define SLIDE_MAX_CLUSTERS 8
|
||||
#define SLIDE_CLUSTER_GAP 4
|
||||
#define SLIDE_TRACK_COUNT 2
|
||||
#define SLIDE_TRACK_HOLD_FRAMES 40
|
||||
#define SLIDE_TRIGGER_RANGE_Q4 32
|
||||
#define SLIDE_STOP_REBASE_FRAMES 192
|
||||
#define SLIDE_TOUCH_HOLD_FRAMES 64
|
||||
|
||||
#define SLIDE_DEBOUNCE_SET 10
|
||||
#define SLIDE_DEBOUNCE_RESET 50
|
||||
|
||||
#define SLIDE_NEUTRAL_AXIS 0x80
|
||||
#define SLIDE_AXIS_DELTA 0x50
|
||||
|
||||
typedef struct {
|
||||
uint8_t start;
|
||||
uint8_t end;
|
||||
uint8_t width;
|
||||
int16_t center_q4;
|
||||
} cluster_t;
|
||||
|
||||
typedef struct {
|
||||
bool active;
|
||||
uint8_t miss;
|
||||
uint8_t age;
|
||||
uint8_t width;
|
||||
uint8_t still_frames;
|
||||
int8_t step_dir;
|
||||
int16_t anchor_q4;
|
||||
int16_t pos_q4;
|
||||
int16_t last_pos_q4;
|
||||
int16_t min_q4;
|
||||
int16_t max_q4;
|
||||
} track_t;
|
||||
|
||||
typedef struct {
|
||||
uint8_t count;
|
||||
uint8_t pos[SLIDE_MAX_CLUSTERS];
|
||||
uint8_t len[SLIDE_MAX_CLUSTERS];
|
||||
int8_t dir[SLIDE_MAX_CLUSTERS];
|
||||
} debug_snapshot_t;
|
||||
|
||||
static track_t tracks[SLIDE_TRACK_COUNT];
|
||||
|
||||
static uint32_t debounce_state;
|
||||
static uint8_t debounce_cnt[32];
|
||||
|
||||
static slide_gesture_t stable_gesture;
|
||||
static uint16_t hold_frames;
|
||||
static uint16_t latch_frames = 100;
|
||||
|
||||
static bool debug_cluster;
|
||||
static debug_snapshot_t debug_last;
|
||||
|
||||
static uint8_t axis_from_dir(int8_t dir)
|
||||
{
|
||||
if (dir < 0) {
|
||||
return (uint8_t)(SLIDE_NEUTRAL_AXIS - SLIDE_AXIS_DELTA);
|
||||
}
|
||||
if (dir > 0) {
|
||||
return (uint8_t)(SLIDE_NEUTRAL_AXIS + SLIDE_AXIS_DELTA);
|
||||
}
|
||||
return SLIDE_NEUTRAL_AXIS;
|
||||
}
|
||||
|
||||
static uint32_t debounce_mask(uint32_t raw)
|
||||
{
|
||||
for (uint8_t i = 0; i < 32; i++) {
|
||||
uint32_t bit = (raw >> i) & 1u;
|
||||
bool state = (debounce_state >> i) & 1u;
|
||||
if (!state) {
|
||||
if (bit) {
|
||||
if (debounce_cnt[i] < SLIDE_DEBOUNCE_SET) {
|
||||
debounce_cnt[i]++;
|
||||
}
|
||||
if (debounce_cnt[i] >= SLIDE_DEBOUNCE_SET) {
|
||||
debounce_state |= (1u << i);
|
||||
}
|
||||
} else {
|
||||
debounce_cnt[i] = 0;
|
||||
}
|
||||
} else {
|
||||
if (!bit) {
|
||||
if (debounce_cnt[i] < SLIDE_DEBOUNCE_RESET) {
|
||||
debounce_cnt[i]++;
|
||||
}
|
||||
if (debounce_cnt[i] >= SLIDE_DEBOUNCE_RESET) {
|
||||
debounce_state &= ~(1u << i);
|
||||
}
|
||||
} else {
|
||||
debounce_cnt[i] = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
return debounce_state;
|
||||
}
|
||||
|
||||
static int16_t abs16(int16_t x)
|
||||
{
|
||||
return (x < 0) ? (int16_t)(-x) : x;
|
||||
}
|
||||
|
||||
static int8_t dir_from_displacement(int16_t delta_q4)
|
||||
{
|
||||
if (delta_q4 >= SLIDE_TRIGGER_RANGE_Q4) {
|
||||
return 1;
|
||||
}
|
||||
if (delta_q4 <= -SLIDE_TRIGGER_RANGE_Q4) {
|
||||
return -1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int8_t track_dir(const track_t *t)
|
||||
{
|
||||
if (!t->active) {
|
||||
return 0;
|
||||
}
|
||||
return dir_from_displacement((int16_t)(t->pos_q4 - t->anchor_q4));
|
||||
}
|
||||
|
||||
static uint16_t track_ttl(const track_t *t)
|
||||
{
|
||||
if (!t->active) {
|
||||
return 0;
|
||||
}
|
||||
return (t->miss >= SLIDE_TRACK_HOLD_FRAMES) ? 0u : (uint16_t)(SLIDE_TRACK_HOLD_FRAMES - t->miss);
|
||||
}
|
||||
|
||||
static uint8_t q4_to_idx(int16_t q4)
|
||||
{
|
||||
if (q4 <= 0) {
|
||||
return 0;
|
||||
}
|
||||
if (q4 >= (31 * 16)) {
|
||||
return 31;
|
||||
}
|
||||
return (uint8_t)((q4 + 8) / 16);
|
||||
}
|
||||
|
||||
static bool track_triggered(const track_t *t)
|
||||
{
|
||||
if (!t->active) {
|
||||
return false;
|
||||
}
|
||||
return (t->max_q4 - t->min_q4) >= SLIDE_TRIGGER_RANGE_Q4;
|
||||
}
|
||||
|
||||
static uint8_t extract_clusters(uint32_t mask, cluster_t *clusters, uint8_t max_clusters)
|
||||
{
|
||||
uint8_t count = 0;
|
||||
uint8_t i = 0;
|
||||
|
||||
while (i < 32) {
|
||||
if (((mask >> i) & 1u) == 0u) {
|
||||
i++;
|
||||
continue;
|
||||
}
|
||||
|
||||
uint8_t start = i;
|
||||
uint8_t end = i;
|
||||
i++;
|
||||
|
||||
/* extend cluster, bridging gaps <= SLIDE_CLUSTER_GAP */
|
||||
while (i < 32) {
|
||||
if (((mask >> i) & 1u) != 0u) {
|
||||
end = i;
|
||||
i++;
|
||||
} else {
|
||||
/* measure gap length */
|
||||
uint8_t j = i;
|
||||
while ((j < 32) && (((mask >> j) & 1u) == 0u)) {
|
||||
j++;
|
||||
}
|
||||
uint8_t gap = (uint8_t)(j - i);
|
||||
if ((gap <= SLIDE_CLUSTER_GAP) && (j < 32)) {
|
||||
/* bridge the gap */
|
||||
i = j;
|
||||
} else {
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t width = (uint8_t)(end - start + 1);
|
||||
|
||||
if (width < SLIDE_MIN_CLUSTER_WIDTH) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (count < max_clusters) {
|
||||
clusters[count].start = start;
|
||||
clusters[count].end = end;
|
||||
clusters[count].width = width;
|
||||
clusters[count].center_q4 = (int16_t)((start + end) * 8);
|
||||
count++;
|
||||
}
|
||||
}
|
||||
|
||||
return count;
|
||||
}
|
||||
|
||||
static void sort_clusters(cluster_t *clusters, uint8_t count)
|
||||
{
|
||||
for (uint8_t i = 0; i < count; i++) {
|
||||
for (uint8_t j = (uint8_t)(i + 1); j < count; j++) {
|
||||
if (clusters[j].center_q4 < clusters[i].center_q4) {
|
||||
cluster_t t = clusters[i];
|
||||
clusters[i] = clusters[j];
|
||||
clusters[j] = t;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void update_track(track_t *t, bool matched, const cluster_t *c)
|
||||
{
|
||||
if (matched) {
|
||||
int16_t pos_q4 = c->center_q4;
|
||||
|
||||
if (!t->active) {
|
||||
t->active = true;
|
||||
t->miss = 0;
|
||||
t->age = 0;
|
||||
t->width = c->width;
|
||||
t->still_frames = 0;
|
||||
t->step_dir = 0;
|
||||
t->anchor_q4 = pos_q4;
|
||||
t->pos_q4 = pos_q4;
|
||||
t->last_pos_q4 = pos_q4;
|
||||
t->min_q4 = pos_q4;
|
||||
t->max_q4 = pos_q4;
|
||||
return;
|
||||
}
|
||||
|
||||
int16_t delta_q4 = (int16_t)(pos_q4 - t->pos_q4);
|
||||
int8_t step_dir = (delta_q4 > 0) ? 1 : ((delta_q4 < 0) ? -1 : 0);
|
||||
|
||||
t->miss = 0;
|
||||
t->age++;
|
||||
t->last_pos_q4 = t->pos_q4;
|
||||
t->pos_q4 = pos_q4;
|
||||
t->width = c->width;
|
||||
|
||||
if (step_dir != 0) {
|
||||
if ((t->step_dir != 0) && (step_dir != t->step_dir)) {
|
||||
t->anchor_q4 = t->last_pos_q4;
|
||||
}
|
||||
t->step_dir = step_dir;
|
||||
t->still_frames = 0;
|
||||
} else {
|
||||
if (t->still_frames < 0xFF) {
|
||||
t->still_frames++;
|
||||
}
|
||||
if (t->still_frames >= SLIDE_STOP_REBASE_FRAMES) {
|
||||
t->anchor_q4 = t->pos_q4;
|
||||
t->step_dir = 0;
|
||||
}
|
||||
}
|
||||
|
||||
if (pos_q4 < t->min_q4) {
|
||||
t->min_q4 = pos_q4;
|
||||
}
|
||||
if (pos_q4 > t->max_q4) {
|
||||
t->max_q4 = pos_q4;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
if (!t->active) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (t->miss < 0xFF) {
|
||||
t->miss++;
|
||||
}
|
||||
|
||||
if (t->miss > SLIDE_TRACK_HOLD_FRAMES) {
|
||||
memset(t, 0, sizeof(*t));
|
||||
} else {
|
||||
t->last_pos_q4 = t->pos_q4;
|
||||
if (t->still_frames < 0xFF) {
|
||||
t->still_frames++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static slide_gesture_t classify_gesture(void)
|
||||
{
|
||||
track_t *a = &tracks[0];
|
||||
track_t *b = &tracks[1];
|
||||
|
||||
if (a->active && b->active && (a->pos_q4 > b->pos_q4)) {
|
||||
track_t tmp = *a;
|
||||
*a = *b;
|
||||
*b = tmp;
|
||||
}
|
||||
|
||||
bool a_ready = a->active && (a->age > 0) && track_triggered(a);
|
||||
bool b_ready = b->active && (b->age > 0) && track_triggered(b);
|
||||
|
||||
if (a_ready && b_ready) {
|
||||
int8_t da = track_dir(a);
|
||||
int8_t db = track_dir(b);
|
||||
|
||||
if (da == db && da < 0) {
|
||||
return SLIDE_GESTURE_DUAL_LEFT;
|
||||
}
|
||||
if (da == db && da > 0) {
|
||||
return SLIDE_GESTURE_DUAL_RIGHT;
|
||||
}
|
||||
if (da > 0 && db < 0) {
|
||||
return SLIDE_GESTURE_DUAL_CONVERGE;
|
||||
}
|
||||
if (da < 0 && db > 0) {
|
||||
return SLIDE_GESTURE_DUAL_DIVERGE;
|
||||
}
|
||||
|
||||
return SLIDE_GESTURE_NONE;
|
||||
}
|
||||
|
||||
if (a_ready || b_ready) {
|
||||
track_t *t = a_ready ? a : b;
|
||||
int8_t d = track_dir(t);
|
||||
if (d < 0) {
|
||||
return SLIDE_GESTURE_SINGLE_LEFT;
|
||||
}
|
||||
if (d > 0) {
|
||||
return SLIDE_GESTURE_SINGLE_RIGHT;
|
||||
}
|
||||
}
|
||||
|
||||
return SLIDE_GESTURE_NONE;
|
||||
}
|
||||
|
||||
static void apply_latch(slide_gesture_t instant, bool touching)
|
||||
{
|
||||
if (instant != SLIDE_GESTURE_NONE) {
|
||||
stable_gesture = instant;
|
||||
hold_frames = latch_frames;
|
||||
return;
|
||||
}
|
||||
|
||||
if (touching && hold_frames > SLIDE_TOUCH_HOLD_FRAMES) {
|
||||
hold_frames = SLIDE_TOUCH_HOLD_FRAMES;
|
||||
}
|
||||
|
||||
if (hold_frames > 0) {
|
||||
hold_frames--;
|
||||
} else {
|
||||
stable_gesture = SLIDE_GESTURE_NONE;
|
||||
}
|
||||
}
|
||||
|
||||
static bool snapshot_equals(const debug_snapshot_t *a, const debug_snapshot_t *b)
|
||||
{
|
||||
if (a->count != b->count) {
|
||||
return false;
|
||||
}
|
||||
for (uint8_t i = 0; i < a->count; i++) {
|
||||
if (a->pos[i] != b->pos[i] || a->len[i] != b->len[i] || a->dir[i] != b->dir[i]) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool snapshot_contains(const debug_snapshot_t *s, uint8_t pos, uint8_t len)
|
||||
{
|
||||
for (uint8_t i = 0; i < s->count; i++) {
|
||||
if (s->pos[i] == pos && s->len[i] == len) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
static int8_t nearest_track_info(int16_t center_q4, uint16_t *ttl, bool *matched)
|
||||
{
|
||||
int16_t best = 0x7FFF;
|
||||
int8_t best_dir = 0;
|
||||
uint16_t best_ttl = 0;
|
||||
bool best_match = false;
|
||||
|
||||
for (uint8_t i = 0; i < SLIDE_TRACK_COUNT; i++) {
|
||||
if (!tracks[i].active) {
|
||||
continue;
|
||||
}
|
||||
int16_t d = abs16((int16_t)(tracks[i].pos_q4 - center_q4));
|
||||
if (d < best) {
|
||||
best = d;
|
||||
best_dir = track_dir(&tracks[i]);
|
||||
best_ttl = track_ttl(&tracks[i]);
|
||||
best_match = (d <= 24);
|
||||
}
|
||||
}
|
||||
|
||||
if (ttl) {
|
||||
*ttl = best_ttl;
|
||||
}
|
||||
if (matched) {
|
||||
*matched = best_match;
|
||||
}
|
||||
return best_dir;
|
||||
}
|
||||
|
||||
static void debug_print_clusters(const cluster_t *clusters, uint8_t count)
|
||||
{
|
||||
if (!debug_cluster) {
|
||||
memset(&debug_last, 0, sizeof(debug_last));
|
||||
return;
|
||||
}
|
||||
|
||||
debug_snapshot_t now = {0};
|
||||
uint16_t ttl[SLIDE_MAX_CLUSTERS] = {0};
|
||||
|
||||
for (uint8_t i = 0; (i < count) && (i < SLIDE_MAX_CLUSTERS); i++) {
|
||||
bool matched = false;
|
||||
int8_t dir = nearest_track_info(clusters[i].center_q4, &ttl[i], &matched);
|
||||
now.pos[now.count] = q4_to_idx(clusters[i].center_q4);
|
||||
now.len[now.count] = clusters[i].width;
|
||||
now.dir[now.count] = matched ? dir : 0;
|
||||
now.count++;
|
||||
}
|
||||
|
||||
bool changed = !snapshot_equals(&now, &debug_last);
|
||||
bool has_dead = false;
|
||||
for (uint8_t i = 0; i < debug_last.count; i++) {
|
||||
if (!snapshot_contains(&now, debug_last.pos[i], debug_last.len[i])) {
|
||||
has_dead = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (!changed && !has_dead) {
|
||||
return;
|
||||
}
|
||||
|
||||
bool printed = false;
|
||||
for (uint8_t i = 0; i < now.count; i++) {
|
||||
const char *tag = "*";
|
||||
if (now.dir[i] > 0) {
|
||||
tag = ">>";
|
||||
} else if (now.dir[i] < 0) {
|
||||
tag = "<<";
|
||||
}
|
||||
if (printed) {
|
||||
printf(" ");
|
||||
}
|
||||
printed = true;
|
||||
printf("(%u %u %s %u)", now.pos[i], now.len[i], tag, (unsigned)ttl[i]);
|
||||
}
|
||||
|
||||
for (uint8_t i = 0; i < debug_last.count; i++) {
|
||||
if (snapshot_contains(&now, debug_last.pos[i], debug_last.len[i])) {
|
||||
continue;
|
||||
}
|
||||
if (printed) {
|
||||
printf(" ");
|
||||
}
|
||||
printed = true;
|
||||
printf("(%u %u X %u)", debug_last.pos[i], debug_last.len[i], 0u);
|
||||
}
|
||||
|
||||
printf("\n");
|
||||
debug_last = now;
|
||||
}
|
||||
|
||||
void slide_reset(void)
|
||||
{
|
||||
memset(tracks, 0, sizeof(tracks));
|
||||
debounce_state = 0;
|
||||
memset(debounce_cnt, 0, sizeof(debounce_cnt));
|
||||
memset(&debug_last, 0, sizeof(debug_last));
|
||||
stable_gesture = SLIDE_GESTURE_NONE;
|
||||
hold_frames = 0;
|
||||
}
|
||||
|
||||
void slide_set_latch(uint16_t frames)
|
||||
{
|
||||
latch_frames = (frames == 0) ? 1 : frames;
|
||||
}
|
||||
|
||||
void slide_set_debug_cluster(bool enable)
|
||||
{
|
||||
debug_cluster = enable;
|
||||
if (!enable) {
|
||||
memset(&debug_last, 0, sizeof(debug_last));
|
||||
}
|
||||
}
|
||||
|
||||
void slide_process(uint32_t mask, slide_result_t *out)
|
||||
{
|
||||
cluster_t clusters[SLIDE_MAX_CLUSTERS];
|
||||
bool used[SLIDE_MAX_CLUSTERS] = {0};
|
||||
int8_t idx0 = -1;
|
||||
int8_t idx1 = -1;
|
||||
|
||||
uint8_t count = extract_clusters(debounce_mask(mask), clusters, SLIDE_MAX_CLUSTERS);
|
||||
sort_clusters(clusters, count);
|
||||
|
||||
if (tracks[0].active && count > 0) {
|
||||
int16_t best = 0x7FFF;
|
||||
int8_t best_idx = -1;
|
||||
for (uint8_t i = 0; i < count; i++) {
|
||||
int16_t d = abs16((int16_t)(clusters[i].center_q4 - tracks[0].pos_q4));
|
||||
if (d < best) {
|
||||
best = d;
|
||||
best_idx = (int8_t)i;
|
||||
}
|
||||
}
|
||||
if (best_idx >= 0) {
|
||||
idx0 = best_idx;
|
||||
used[(uint8_t)best_idx] = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (tracks[1].active && count > 0) {
|
||||
int16_t best = 0x7FFF;
|
||||
int8_t best_idx = -1;
|
||||
for (uint8_t i = 0; i < count; i++) {
|
||||
if (used[i]) {
|
||||
continue;
|
||||
}
|
||||
int16_t d = abs16((int16_t)(clusters[i].center_q4 - tracks[1].pos_q4));
|
||||
if (d < best) {
|
||||
best = d;
|
||||
best_idx = (int8_t)i;
|
||||
}
|
||||
}
|
||||
if (best_idx >= 0) {
|
||||
idx1 = best_idx;
|
||||
used[(uint8_t)best_idx] = true;
|
||||
}
|
||||
}
|
||||
|
||||
for (uint8_t i = 0; i < count; i++) {
|
||||
if (used[i]) {
|
||||
continue;
|
||||
}
|
||||
if (idx0 < 0) {
|
||||
idx0 = (int8_t)i;
|
||||
used[i] = true;
|
||||
continue;
|
||||
}
|
||||
if (idx1 < 0) {
|
||||
idx1 = (int8_t)i;
|
||||
used[i] = true;
|
||||
continue;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
update_track(&tracks[0], idx0 >= 0, (idx0 >= 0) ? &clusters[(uint8_t)idx0] : NULL);
|
||||
update_track(&tracks[1], idx1 >= 0, (idx1 >= 0) ? &clusters[(uint8_t)idx1] : NULL);
|
||||
|
||||
apply_latch(classify_gesture(), count > 0);
|
||||
debug_print_clusters(clusters, count);
|
||||
|
||||
out->gesture = stable_gesture;
|
||||
out->left_x = SLIDE_NEUTRAL_AXIS;
|
||||
out->right_x = SLIDE_NEUTRAL_AXIS;
|
||||
out->cluster_count = count;
|
||||
|
||||
switch (stable_gesture) {
|
||||
case SLIDE_GESTURE_SINGLE_LEFT:
|
||||
out->left_x = axis_from_dir(-1);
|
||||
break;
|
||||
case SLIDE_GESTURE_SINGLE_RIGHT:
|
||||
out->left_x = axis_from_dir(1);
|
||||
break;
|
||||
case SLIDE_GESTURE_DUAL_LEFT:
|
||||
out->left_x = axis_from_dir(-1);
|
||||
out->right_x = axis_from_dir(-1);
|
||||
break;
|
||||
case SLIDE_GESTURE_DUAL_RIGHT:
|
||||
out->left_x = axis_from_dir(1);
|
||||
out->right_x = axis_from_dir(1);
|
||||
break;
|
||||
case SLIDE_GESTURE_DUAL_CONVERGE:
|
||||
out->left_x = axis_from_dir(1);
|
||||
out->right_x = axis_from_dir(-1);
|
||||
break;
|
||||
case SLIDE_GESTURE_DUAL_DIVERGE:
|
||||
out->left_x = axis_from_dir(-1);
|
||||
out->right_x = axis_from_dir(1);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
@@ -1,33 +0,0 @@
|
||||
/*
|
||||
* Slider gesture extraction for PS4 HID mapping
|
||||
*/
|
||||
|
||||
#ifndef SLIDE_H
|
||||
#define SLIDE_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
typedef enum {
|
||||
SLIDE_GESTURE_NONE = 0,
|
||||
SLIDE_GESTURE_SINGLE_LEFT,
|
||||
SLIDE_GESTURE_SINGLE_RIGHT,
|
||||
SLIDE_GESTURE_DUAL_LEFT,
|
||||
SLIDE_GESTURE_DUAL_RIGHT,
|
||||
SLIDE_GESTURE_DUAL_CONVERGE,
|
||||
SLIDE_GESTURE_DUAL_DIVERGE,
|
||||
} slide_gesture_t;
|
||||
|
||||
typedef struct {
|
||||
slide_gesture_t gesture;
|
||||
uint8_t left_x;
|
||||
uint8_t right_x;
|
||||
uint8_t cluster_count;
|
||||
} slide_result_t;
|
||||
|
||||
void slide_reset(void);
|
||||
void slide_set_latch(uint16_t frames);
|
||||
void slide_set_debug_cluster(bool enable);
|
||||
void slide_process(uint32_t mask, slide_result_t *out);
|
||||
|
||||
#endif
|
||||
Reference in New Issue
Block a user