diff --git a/Production/Firmware/diva_pico.uf2 b/Production/Firmware/diva_pico.uf2 index 53278e0..966a362 100644 Binary files a/Production/Firmware/diva_pico.uf2 and b/Production/Firmware/diva_pico.uf2 differ diff --git a/firmware/src/CMakeLists.txt b/firmware/src/CMakeLists.txt index 5114b79..4078b1a 100644 --- a/firmware/src/CMakeLists.txt +++ b/firmware/src/CMakeLists.txt @@ -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) diff --git a/firmware/src/commands.c b/firmware/src/commands.c index 599601a..72b3868 100644 --- a/firmware/src/commands.c +++ b/firmware/src/commands.c @@ -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() diff --git a/firmware/src/gesture.c b/firmware/src/gesture.c new file mode 100644 index 0000000..8aac65c --- /dev/null +++ b/firmware/src/gesture.c @@ -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 + */ + +#include "gesture.h" + +#include +#include +#include +#include + +#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(); +} diff --git a/firmware/src/gesture.h b/firmware/src/gesture.h new file mode 100644 index 0000000..0f6ed6a --- /dev/null +++ b/firmware/src/gesture.h @@ -0,0 +1,18 @@ +/* + * Extract gesture information from raw electrode touch bitmask. + * + * WHowe + */ + +#ifndef GESTURE_H +#define GESTURE_H + +#include +#include + +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 diff --git a/firmware/src/main.c b/firmware/src/main.c index 7a78bf1..c98dadc 100644 --- a/firmware/src/main.c +++ b/firmware/src/main.c @@ -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(); diff --git a/firmware/src/slide.c b/firmware/src/slide.c deleted file mode 100644 index 4c9e628..0000000 --- a/firmware/src/slide.c +++ /dev/null @@ -1,598 +0,0 @@ -#include "slide.h" - -#include -#include -#include - -#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; - } -} diff --git a/firmware/src/slide.h b/firmware/src/slide.h deleted file mode 100644 index 19ae90e..0000000 --- a/firmware/src/slide.h +++ /dev/null @@ -1,33 +0,0 @@ -/* - * Slider gesture extraction for PS4 HID mapping - */ - -#ifndef SLIDE_H -#define SLIDE_H - -#include -#include - -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