More working command lines

firmware/src/air.c

@@ -14,6 +14,7 @@
 #include "hardware/gpio.h"
 
 #include "board_defs.h"
+#include "config.h"
 
 #include "gp2y0e.h"
 #include "i2c_hub.h"
@@ -21,10 +22,6 @@
 static const uint8_t TOF_LIST[] = TOF_MUX_LIST;
 static uint16_t distances[sizeof(TOF_LIST)];
 
-const int offset_a = 600;
-const int offset_b = 800;
-const int pitch = 160;
-
 void air_init()
 {
     i2c_init(I2C_PORT, I2C_FREQ);
@@ -50,9 +47,11 @@ size_t air_num()
 
 static inline uint8_t air_bits(int dist, int offset)
 {
-    if (dist < offset) {
+    if ((dist < offset) || (dist == 4095)) {
         return 0;
     }
+
+    int pitch = chu_cfg->tof.pitch * 10;
     int index = (dist - offset) / pitch;
     if (index >= 6) {
         return 0;
@@ -63,10 +62,12 @@ static inline uint8_t air_bits(int dist, int offset)
 
 uint8_t air_bitmap()
 {
+    int offset = chu_cfg->tof.offset * 10;
+    int pitch = chu_cfg->tof.pitch * 10;
     uint8_t bitmap = 0;
     for (int i = 0; i < sizeof(TOF_LIST); i++) {
-        bitmap |= air_bits(distances[i], offset_a);
-        bitmap |= air_bits(distances[i], offset_b);
+        bitmap |= air_bits(distances[i], offset) |
+                  air_bits(distances[i], offset + pitch);
     }
     return bitmap;
 }
@@ -76,13 +77,14 @@ unsigned air_value(uint8_t index)
     if (index >= sizeof(TOF_LIST)) {
         return 0;
     }
-
-    uint8_t bitmap = air_bits(distances[index], offset_a) |
-                     air_bits(distances[index], offset_b);
+    int offset = chu_cfg->tof.offset * 10;
+    int pitch = chu_cfg->tof.pitch * 10;
+    uint8_t bitmap = air_bits(distances[index], offset) |
+                     air_bits(distances[index], offset + pitch);
 
     for (int i = 0; i < 6; i++) {
         if (bitmap & (1 << i)) {
-            return i + 1;
+            return i + 1; // lowest detected position
         }
     }
 

firmware/src/cmd.c

@@ -8,6 +8,7 @@
 #include "pico/stdlib.h"
 
 #include "config.h"
+#include "save.h"
 
 #define SENSE_LIMIT_MAX 8
 #define SENSE_LIMIT_MIN -8
@@ -29,7 +30,7 @@ typedef struct {
 command_t commands[MAX_COMMANDS];
 int num_commands = 0;
 
-void register_command(char *name, cmd_handler_t handler)
+static void register_command(char *name, cmd_handler_t handler)
 {
     if (num_commands < MAX_COMMANDS) {
         strcpy(commands[num_commands].name, name);
@@ -38,7 +39,7 @@ void register_command(char *name, cmd_handler_t handler)
     }
 }
 
-void handle_help(int argc, char *argv[])
+static void handle_help(int argc, char *argv[])
 {
     printf("Available commands:\n");
     for (int i = 0; i < num_commands; i++) {
@@ -72,7 +73,8 @@ static void list_tof()
 static void list_sense()
 {
     printf("[Sense]\n");
-    printf("  Global: %d, debounce: %d\n", chu_cfg->sense.global, chu_cfg->sense.debounce);
+    printf("  Global: %d, debounce (touch, release): %d, %d\n", chu_cfg->sense.global,
+           chu_cfg->sense.debounce_touch, chu_cfg->sense.debounce_release);
     printf("    | 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10| 11| 12| 13| 14| 15|\n");
     printf("    -----------------------------------------------------------------\n");
     printf("  A |");
@@ -143,12 +145,12 @@ void fps_count(int core)
     counter[core] = 0;
 }
 
-void handle_fps(int argc, char *argv[])
+static void handle_fps(int argc, char *argv[])
 {
     printf("FPS: core 0: %d, core 1: %d\n", fps[0], fps[1]);
 }
 
-void handle_hid(int argc, char *argv[])
+static void handle_hid(int argc, char *argv[])
 {
     const char *usage = "Usage: hid <joy|nkro>\n";
     if (argc != 1) {
@@ -172,6 +174,52 @@ void handle_hid(int argc, char *argv[])
     config_changed();
 }
 
+static int extract_non_neg_int(const char *param, int len)
+{
+    if (len == 0) {
+        len = strlen(param);
+    }
+    int result = 0;
+    for (int i = 0; i < len; i++) {
+        if (!isdigit(param[i])) {
+            return -1;
+        }
+        result = result * 10 + param[i] - '0';
+    }
+    return result;
+}
+
+static void handle_tof(int argc, char *argv[])
+{
+    const char *usage = "Usage: tof <offset> [pitch]\n"
+                        "  offset: 40-255\n"
+                        "  pitch: 4-50\n";
+    if ((argc < 1) || (argc > 2)) {
+        printf(usage);
+        return;
+    }
+
+    int offset = chu_cfg->tof.offset;
+    int pitch = chu_cfg->tof.pitch;
+    if (argc >= 1) {
+        offset = extract_non_neg_int(argv[0], 0);
+    }
+    if (argc == 2) {
+        pitch = extract_non_neg_int(argv[1], 0);
+    }
+
+    if ((offset < 40) || (offset > 255) || (pitch < 4) || (pitch > 50)) {
+        printf(usage);
+        return;
+    }
+
+    chu_cfg->tof.offset = offset;
+    chu_cfg->tof.pitch = pitch;
+
+    config_changed();
+    list_tof();
+}
+
 static uint8_t *extract_key(const char *param)
 {
     int len = strlen(param);
@@ -185,22 +233,15 @@ static uint8_t *extract_key(const char *param)
         return NULL;
     }
 
-    int id = 0;
-    for (int i = 0; i < len - 1; i++) {
-        if (!isdigit(param[i])) {
-            return NULL;
-        }
-        id = id * 10 + param[i] - '0';
-    }
-
-    if (id > 15) {
+    int id = extract_non_neg_int(param, len - 1);
+    if ((id < 0) || (id > 15)) {
         return NULL;
     }
 
     return &chu_cfg->sense.keys[id * 2 + offset];
 }
 
-void handle_sense(int argc, char *argv[])
+static void handle_sense(int argc, char *argv[])
 {
     const char *usage = "Usage: sense [key] <+|->\n"
                         "Example:\n"
@@ -214,7 +255,7 @@ void handle_sense(int argc, char *argv[])
     }
 
     int8_t *target = &chu_cfg->sense.global;
-    const char *op = argv[1];
+    const char *op = argv[argc - 1];
     if (argc == 2) {
         target = extract_key(argv[0]);
         if (!target) {
@@ -240,41 +281,58 @@ void handle_sense(int argc, char *argv[])
     list_sense();
 }
 
-void handle_debounce(int argc, char *argv[])
+static void handle_debounce(int argc, char *argv[])
 {
-    const char *usage = "Usage: debounce <0..32>\n";
-    if (argc != 1) {
+    const char *usage = "Usage: debounce <touch> [release]\n"
+                        "  touch, release: 0-255\n";
+    if ((argc < 1) || (argc > 2)) {
         printf(usage);
         return;
     }
 
-    int num = 0;
-    for (int i = 0; i < strlen(argv[0]); i++) {
-        if (!isdigit(argv[0][i])) {
-            printf(usage);
-            return;
+    int touch = chu_cfg->sense.debounce_touch;
+    int release = chu_cfg->sense.debounce_release;
+    if (argc >= 1) {
+        touch = extract_non_neg_int(argv[0], 0);
     }
-        num = num * 10 + argv[0][i] - '0';
+    if (argc == 2) {
+        release = extract_non_neg_int(argv[1], 0);
     }
 
-    if (num > 32) {
+    if ((touch < 0) || (release < 0)) {
         printf(usage);
         return;
     }
 
-    chu_cfg->sense.debounce = num;
+    chu_cfg->sense.debounce_touch = touch;
+    chu_cfg->sense.debounce_release = release;
+
     config_changed();
+    list_sense();
+}
+
+static void handle_save()
+{
+    save_request(true);
+}
+
+static void handle_factory_reset()
+{
+    config_factory_reset();
+    printf("Factory reset done.\n");
 }
 
 void cmd_init()
 {
-    fcntl(0, F_SETFL, O_NONBLOCK);
     register_command("?", handle_help);
     register_command("list", handle_list);
     register_command("fps", handle_fps);
     register_command("hid", handle_hid);
+    register_command("tof", handle_tof);
     register_command("sense", handle_sense);
     register_command("debounce", handle_debounce);
+    register_command("save", handle_save);
+    register_command("factory", config_factory_reset);
 }
 
 static char cmd_buf[256];

firmware/src/config.c

@@ -27,10 +27,11 @@ static chu_cfg_t default_cfg = {
     },
     .tof = {
         .offset = 64,
-        .pitch = 16,
+        .pitch = 18,
     },
     .sense = {
-        .debounce = 4,
+        .debounce_touch = 1,
+        .debounce_release = 8,
      },
     .hid = {
         .joy = 0x0f,
@@ -46,15 +47,11 @@ static void config_loaded()
         chu_cfg->style.level = default_cfg.style.level;
         config_changed();
     }
-    if ((chu_cfg->tof.offset < 400) || (chu_cfg->tof.offset > 2000) ||
-        (chu_cfg->tof.pitch < 50) || (chu_cfg->tof.pitch > 500)) {
+    if ((chu_cfg->tof.offset < 40) ||
+        (chu_cfg->tof.pitch < 4) || (chu_cfg->tof.pitch > 50)) {
         chu_cfg->tof = default_cfg.tof;
         config_changed();
     }
-
-    if (chu_cfg->sense.debounce > 32) {
-        chu_cfg->sense.debounce = default_cfg.sense.debounce;
-    }
 }
 
 void config_changed()

firmware/src/config.h

@@ -29,7 +29,8 @@ typedef struct __attribute__((packed)) {
     } tof;
     struct {
         int8_t global;
-        uint8_t debounce;
+        uint8_t debounce_touch;
+        uint8_t debounce_release;        
         int8_t keys[32];
     } sense;
     struct {

firmware/src/main.c

@@ -60,15 +60,6 @@ void report_usb_hid()
     }
 }
 
-static bool request_core1_pause = false;
-
-static void pause_core1(bool pause)
-{
-    request_core1_pause = pause;
-    if (pause) {
-        sleep_ms(5); /* wait for any IO ops to finish */
-    }
-}
 static void gen_joy_report()
 {
     hid_joy.axis = 0;
@@ -142,12 +133,15 @@ static void run_lights()
     }
 }
 
+static mutex_t core1_io_lock;
 static void core1_loop()
 {
     while (1) {
+        if (mutex_try_enter(&core1_io_lock, NULL)) {
             run_lights();
             rgb_update();
-
+            mutex_exit(&core1_io_lock);
+        }
         slider_update_baseline();
         fps_count(1);
         sleep_ms(1);
@@ -180,7 +174,8 @@ void init()
     stdio_init_all();
 
     config_init();
-    save_init(0xca34cafe, pause_core1);
+    mutex_init(&core1_io_lock);
+    save_init(0xca34cafe, &core1_io_lock);
 
     slider_init();
     air_init();

firmware/src/rgb.c

@@ -56,7 +56,7 @@ static void drive_led()
 {
     static uint64_t last = 0;
     uint64_t now = time_us_64();
-    if (now - last < 5000) { // no faster than 200Hz
+    if (now - last < 4000) { // no faster than 250Hz
         return;
     }
     last = now;

firmware/src/save.c

@@ -18,7 +18,7 @@
 #include "pico/stdio.h"
 
 #include "hardware/flash.h"
-#include "hardware/sync.h"
+#include "pico/multicore.h"
 
 static struct {
     size_t size;
@@ -29,6 +29,8 @@ static int module_num = 0;
 
 static uint32_t my_magic = 0xcafecafe;
 
+#define SAVE_TIMEOUT_US 30000000
+
 #define SAVE_SECTOR_OFFSET (PICO_FLASH_SIZE_BYTES - FLASH_SECTOR_SIZE)
 
 typedef struct __attribute ((packed)) {
@@ -44,15 +46,16 @@ static int data_page = -1;
 static bool requesting_save = false;
 static uint64_t requesting_time = 0;
 
-static io_locker_func io_lock;
+static mutex_t *io_lock;
 
 static void save_program()
 {
     old_data = new_data;
 
     data_page = (data_page + 1) % (FLASH_SECTOR_SIZE / FLASH_PAGE_SIZE);
-    printf("Program Flash %d %8lx\n", data_page, old_data.magic);
-    io_lock(true);
+    printf("\nProgram Flash %d %8lx\n", data_page, old_data.magic);
+    if (mutex_enter_timeout_us(io_lock, 100000)) {
+        sleep_ms(5); /* wait for all io operations to finish */
         uint32_t ints = save_and_disable_interrupts();
         if (data_page == 0) {
             flash_range_erase(SAVE_SECTOR_OFFSET, FLASH_SECTOR_SIZE);
@@ -60,7 +63,10 @@ static void save_program()
         flash_range_program(SAVE_SECTOR_OFFSET + data_page * FLASH_PAGE_SIZE,
                             (uint8_t *)&old_data, FLASH_PAGE_SIZE);
         restore_interrupts(ints);
-    io_lock(false);
+        mutex_exit(io_lock);
+    } else {
+        printf("Program Flash Failed.\n");
+    }
 }
 
 static void load_default()
@@ -103,7 +109,7 @@ static void save_loaded()
     }
 }
 
-void save_init(uint32_t magic, io_locker_func locker)
+void save_init(uint32_t magic, mutex_t *locker)
 {
     my_magic = magic;
     io_lock = locker;
@@ -114,17 +120,13 @@ void save_init(uint32_t magic, io_locker_func locker)
 
 void save_loop()
 {
-    if (requesting_save && (time_us_64() - requesting_time > 1000000)) {
+    if (requesting_save && (time_us_64() - requesting_time > SAVE_TIMEOUT_US)) {
         requesting_save = false;
-        printf("Time to save.\n");
         /* only when data is actually changed */
-        for (int i = 0; i < sizeof(old_data); i++) {
-            if (((uint8_t *)&old_data)[i] != ((uint8_t *)&new_data)[i]) {
-                save_program();
+        if (memcmp(&old_data, &new_data, sizeof(old_data)) == 0) {
             return;
         }
-        }
-        printf("No change.\n");
+        save_program();
     }
 }
 
@@ -142,7 +144,7 @@ void *save_alloc(size_t size, void *def, void (*after_load)())
 void save_request(bool immediately)
 {
     if (!requesting_save) {
-        printf("Save marked.\n");
+        printf("Save requested.\n");
         requesting_save = true;
         new_data.magic = my_magic;
         requesting_time = time_us_64();

firmware/src/save.h

@@ -10,9 +10,11 @@
 #include <stdint.h>
 #include <stdbool.h>
 
+#include "pico/multicore.h"
+
 /* It's safer to lock other I/O ops during saving, so we need a locker */
 typedef void (*io_locker_func)(bool pause);
-void save_init(uint32_t magic, io_locker_func locker);
+void save_init(uint32_t magic, mutex_t *lock);
 
 void save_loop();
 

firmware/src/slider.c

@@ -28,7 +28,6 @@ static uint16_t baseline[36];
 static int16_t error[36];
 static uint16_t readout[36];
 static bool touched[36];
-static uint16_t debounce[36];
 static uint16_t touch[3];
 
 static struct mpr121_sensor mpr121[3];
@@ -139,6 +138,9 @@ int slider_delta(unsigned key)
     return readout[key] - baseline[key];
 }
 
+static uint16_t touch_count[36];
+static uint16_t release_count[36];
+
 bool slider_touched(unsigned key)
 {
     if (key >= 32) {
@@ -151,18 +153,23 @@ bool slider_touched(unsigned key)
     int release_thre = RELEASE_THRESHOLD - bias / 2;
 
     if (touched[key]) {
-        if (delta > touch_thre) {
-            debounce[key] = 0;
+        if (delta > release_thre) {
+            release_count[key] = 0;
+        } else {
+            release_count[key]++;
         }
-        if (debounce[key] > chu_cfg->sense.debounce) {
-            if (delta < release_thre) {
+        if (release_count[key] > chu_cfg->sense.debounce_release) {
+            touch_count[key] = 0;
             touched[key] = false;
         }
-        } else {
-            debounce[key]++;
-        }
     } else if (!touched[key]) {
-        if (delta > touch_thre) {
+        if (delta < touch_thre) {
+            touch_count[key] = 0;
+        } else {
+            touch_count[key]++;
+        }
+        if (touch_count[key] > chu_cfg->sense.debounce_touch) {
+            release_count[key] = 0;
             touched[key] = true;
         }
     }