diff --git a/firmware/src/CMakeLists.txt b/firmware/src/CMakeLists.txt index e742c74..86ebe41 100644 --- a/firmware/src/CMakeLists.txt +++ b/firmware/src/CMakeLists.txt @@ -3,7 +3,7 @@ pico_sdk_init() include_directories(${CMAKE_CURRENT_LIST_DIR}) add_compile_options(-Wall -Werror -Wfatal-errors -O3) link_libraries(pico_multicore pico_stdlib hardware_i2c hardware_spi - hardware_pio hardware_flash hardware_watchdog + hardware_pio hardware_adc hardware_flash hardware_watchdog tinyusb_device tinyusb_board) function(make_firmware board board_def) diff --git a/firmware/src/air.c b/firmware/src/air.c index ab7712a..6c06f97 100644 --- a/firmware/src/air.c +++ b/firmware/src/air.c @@ -9,8 +9,10 @@ #include #include +#include #include "hardware/gpio.h" +#include "hardware/adc.h" #include "board_defs.h" #include "config.h" @@ -20,10 +22,18 @@ #include "i2c_hub.h" static const uint8_t TOF_LIST[] = TOF_MUX_LIST; -static uint8_t tof_model[sizeof(TOF_LIST)]; -static uint16_t distances[sizeof(TOF_LIST)]; +static uint8_t tof_model[count_of(TOF_LIST)]; +static uint16_t distances[count_of(TOF_LIST)]; -void air_init() +static const uint8_t IR_ABC[] = IR_GROUP_ABC_GPIO; +static const uint8_t IR_SIG[] = IR_SIG_ADC_CHANNEL; +static_assert(count_of(IR_ABC) == 3, "IR should have 3 groups"); +static_assert(count_of(IR_SIG) == 2, "IR should use 2 analog signals"); +static uint16_t ir_raw[6]; +static bool ir_blocked[6]; +#define IR_DEBOUNCE_PERCENT 90 + +static void air_init_tof() { i2c_init(I2C_PORT, I2C_FREQ); gpio_set_function(I2C_SDA, GPIO_FUNC_I2C); @@ -52,7 +62,29 @@ void air_init() } } -size_t air_num() +static void air_init_ir() +{ + for (int i = 0; i < count_of(IR_ABC); i++) { + gpio_init(IR_ABC[i]); + gpio_set_dir(IR_ABC[i], GPIO_OUT); + gpio_put(IR_ABC[i], 0); + } + for (int i = 0; i < count_of(IR_SIG); i++) { + adc_init(); + adc_gpio_init(26 + IR_SIG[i]); + } +} + +void air_init() +{ + if (chu_cfg->ir.enabled == 0) { + air_init_tof(); + } else { + air_init_ir(); + } +} + +size_t air_tof_num() { return sizeof(TOF_LIST); } @@ -72,7 +104,7 @@ static inline uint8_t air_bits(int dist, int offset) return 1 << index; } -uint8_t air_bitmap() +static uint8_t tof_bitmap() { int offset = chu_cfg->tof.offset * 10; int pitch = chu_cfg->tof.pitch * 10; @@ -84,7 +116,21 @@ uint8_t air_bitmap() return bitmap; } -unsigned air_value(uint8_t index) +static uint8_t ir_bitmap() +{ + uint8_t bitmap = 0; + for (int i = 0; i < count_of(ir_blocked); i++) { + bitmap |= ir_blocked[i] << i; + } + return bitmap; +} + +uint8_t air_bitmap() +{ + return chu_cfg->ir.enabled ? ir_bitmap() : tof_bitmap(); +} + +unsigned air_tof_value(uint8_t index) { if (index >= sizeof(TOF_LIST)) { return 0; @@ -103,7 +149,23 @@ unsigned air_value(uint8_t index) return 0; } -void air_update() +uint16_t air_tof_raw(uint8_t index) +{ + if (index >= count_of(TOF_LIST)) { + return 0; + } + return distances[index]; +} + +uint16_t air_ir_raw(uint8_t index) +{ + if (index >= count_of(ir_raw)) { + return 0; + } + return ir_raw[index]; +} + +static void air_update_tof() { for (int i = 0; i < sizeof(TOF_LIST); i++) { i2c_select(I2C_PORT, 1 << TOF_LIST[i]); @@ -115,10 +177,69 @@ void air_update() } } -uint16_t air_raw(uint8_t index) +static void ir_read() { - if (index >= sizeof(TOF_LIST)) { - return 0; + for (int i = 0; i < count_of(IR_ABC); i++) { + gpio_put(IR_ABC[i], 1); + sleep_us(9); + + adc_select_input(IR_SIG[0]); + sleep_us(1); + ir_raw[i * 2] = adc_read(); + + adc_select_input(IR_SIG[1]); + sleep_us(1); + ir_raw[i * 2 + 1] = adc_read(); + + gpio_put(IR_ABC[i], 0); + sleep_us(1); + } +} + +static void ir_judge() +{ + for (int i = 0; i < count_of(ir_raw); i++) { + int offset = chu_cfg->ir.base[i] - ir_raw[i]; + int threshold = chu_cfg->ir.base[i] * chu_cfg->ir.trigger[i] / 100; + + if (ir_blocked[i]) { + threshold = threshold * IR_DEBOUNCE_PERCENT / 100; + } + + ir_blocked[i] = offset >= threshold; + } +} + +static void ir_diagnostic() +{ + if (!chu_runtime.ir_diagnostics) { + return; + } + + static uint64_t last_print = 0; + uint64_t now = time_us_64(); + if (now - last_print > 500000) { + printf("IR: "); + for (int i = 0; i < count_of(ir_raw); i++) { + printf(" %4d", ir_raw[i]); + } + printf("\n"); + last_print = now; + } +} + +static void air_update_ir() +{ + ir_read(); + ir_judge(); + ir_diagnostic(); +} + +void air_update() +{ + if (chu_cfg->ir.enabled) { + air_update_ir(); + } else { + air_update_tof(); } - return distances[index]; } diff --git a/firmware/src/air.h b/firmware/src/air.h index 8f8f618..c94900e 100644 --- a/firmware/src/air.h +++ b/firmware/src/air.h @@ -1,19 +1,20 @@ -/* - * Chu Pico Air Sensor - * WHowe - */ - -#ifndef AIR_H -#define AIR_H - -#include -#include - -void air_init(); -size_t air_num(); -unsigned air_value(uint8_t index); -uint16_t air_raw(uint8_t index); -uint8_t air_bitmap(); -void air_update(); - -#endif +/* + * Chu Pico Air Sensor + * WHowe + */ + +#ifndef AIR_H +#define AIR_H + +#include +#include + +void air_init(); +size_t air_tof_num(); +unsigned air_tof_value(uint8_t index); +uint16_t air_tof_raw(uint8_t index); +uint16_t air_ir_raw(uint8_t index); +uint8_t air_bitmap(); +void air_update(); + +#endif diff --git a/firmware/src/board_defs.h b/firmware/src/board_defs.h index 15c4a32..bfa38a7 100644 --- a/firmware/src/board_defs.h +++ b/firmware/src/board_defs.h @@ -1,23 +1,28 @@ -/* - * Chu Controller Board Definitions - * WHowe - */ - -#if defined BOARD_CHU_PICO - -#define I2C_PORT i2c0 -#define I2C_SDA 16 -#define I2C_SCL 17 -#define I2C_FREQ 620*1000 - -#define I2C_HUB_EN 19 - -#define TOF_MUX_LIST { 1, 2, 0, 4, 5 } - -#define RGB_PIN 2 -#define RGB_ORDER GRB // or RGB - -#define NKRO_KEYMAP "1aqz2swx3dec4frv5gtb6hyn7jum8ki90olp,." -#else - -#endif +/* + * Chu Controller Board Definitions + * WHowe + */ + +#if defined BOARD_CHU_PICO + +#define I2C_PORT i2c0 +#define I2C_SDA 16 +#define I2C_SCL 17 +#define I2C_FREQ 620*1000 + +#define I2C_HUB_EN 19 + +#define TOF_MUX_LIST { 1, 2, 0, 4, 5 } + +#define IR_GROUP_ABC_GPIO { 3, 4, 5 } +#define IR_SIG_ADC_CHANNEL { 0, 1 } + +#define RGB_MAIN_PIN 2 +#define RGB_TOWER_LEFT_PIN 0 +#define RGB_TOWER_RIGHT_PIN 1 +#define RGB_ORDER GRB // or RGB + +#define NKRO_KEYMAP "1aqz2swx3dec4frv5gtb6hyn7jum8ki90olp,." +#else + +#endif diff --git a/firmware/src/commands.c b/firmware/src/commands.c index 072bc59..1bda8e6 100644 --- a/firmware/src/commands.c +++ b/firmware/src/commands.c @@ -44,6 +44,21 @@ static void disp_tof() printf(" Offset: %d, Pitch: %d\n", chu_cfg->tof.offset, chu_cfg->tof.pitch); } +static void disp_ir() +{ + printf("[IR]\n"); + printf(" Enabled: %s\n", chu_cfg->ir.enabled ? "ON" : "OFF"); + printf(" Base:"); + for (int i = 0; i < count_of(chu_cfg->ir.base); i++) { + printf(" %d", chu_cfg->ir.base[i]); + } + printf("\n Trigger:"); + for (int i = 0; i < count_of(chu_cfg->ir.trigger); i++) { + printf(" %d%%", chu_cfg->ir.trigger[i]); + } + printf("\n"); +} + static void disp_sense() { printf("[Sense]\n"); @@ -84,7 +99,7 @@ static void disp_aime() void handle_display(int argc, char *argv[]) { - const char *usage = "Usage: display [colors|style|tof|sense|hid|aime]\n"; + const char *usage = "Usage: display [colors|style|tof|ir|sense|hid|aime]\n"; if (argc > 1) { printf(usage); return; @@ -94,14 +109,15 @@ void handle_display(int argc, char *argv[]) disp_colors(); disp_style(); disp_tof(); + disp_ir(); disp_sense(); disp_hid(); disp_aime(); return; } - const char *choices[] = {"colors", "style", "tof", "sense", "hid", "aime"}; - switch (cli_match_prefix(choices, 6, argv[0])) { + const char *choices[] = {"colors", "style", "tof", "ir", "sense", "hid", "aime"}; + switch (cli_match_prefix(choices, count_of(choices), argv[0])) { case 0: disp_colors(); break; @@ -112,12 +128,15 @@ void handle_display(int argc, char *argv[]) disp_tof(); break; case 3: - disp_sense(); + disp_ir(); break; case 4: - disp_hid(); + disp_sense(); break; case 5: + disp_hid(); + break; + case 6: disp_aime(); break; default: @@ -200,8 +219,8 @@ static void handle_tof(int argc, char *argv[]) if (argc == 0) { printf("TOF: "); - for (int i = air_num(); i > 0; i--) { - printf(" %4d", air_raw(i - 1) / 10); + for (int i = air_tof_num(); i > 0; i--) { + printf(" %4d", air_tof_raw(i - 1) / 10); } printf("\n"); return; @@ -228,6 +247,68 @@ static void handle_tof(int argc, char *argv[]) disp_tof(); } +static void air_diagnostic() +{ + chu_runtime.ir_diagnostics = !chu_runtime.ir_diagnostics; + printf("IR Diagnostics: %s\n", chu_runtime.ir_diagnostics ? "ON" : "OFF"); +} + +static void air_baseline() +{ + printf("IR Baseline:"); + for (int i = 0; i < count_of(chu_cfg->ir.base); i++) { + chu_cfg->ir.base[i] = air_ir_raw(i); + printf(" %4d", chu_cfg->ir.base[i]); + } + config_changed(); + printf("\n"); +} + +static void air_trigger(char *argv[]) +{ + const char *usage = "Usage: ir trigger \n" + " percent: [1..100]\n"; + + if (strncasecmp(argv[0], "trigger", strlen(argv[0])) != 0) { + printf(usage); + return; + } + + int percent = cli_extract_non_neg_int(argv[1], 0); + if ((percent < 1) || (percent > 100)) { + printf(usage); + return; + } + + for (int i = 0; i < count_of(chu_cfg->ir.trigger); i++) { + chu_cfg->ir.trigger[i] = percent; + } + config_changed(); + disp_ir(); +} + +static void handle_ir(int argc, char *argv[]) +{ + const char *usage = "Usage: ir \n" + " ir trigger \n" + " percent: [1..100]\n"; + if (argc == 1) { + const char *commands[] = { "diagnostic", "baseline" }; + int cmd = cli_match_prefix(commands, count_of(commands), argv[0]); + if (cmd == 0) { + air_diagnostic(); + } else if (cmd == 1) { + air_baseline(); + } else { + printf(usage); + } + } else if (argc == 2) { + air_trigger(argv); + } else { + printf(usage); + } +} + static void handle_filter(int argc, char *argv[]) { const char *usage = "Usage: filter [interval]\n" @@ -471,6 +552,7 @@ void commands_init() cli_register("stat", handle_stat, "Display or reset statistics."); cli_register("hid", handle_hid, "Set HID mode."); cli_register("tof", handle_tof, "Set ToF config."); + cli_register("ir", handle_ir, "Set IR config."); cli_register("filter", handle_filter, "Set pre-filter config."); cli_register("sense", handle_sense, "Set sensitivity config."); cli_register("debounce", handle_debounce, "Set debounce config."); diff --git a/firmware/src/config.c b/firmware/src/config.c index 7824b0e..63c0b74 100644 --- a/firmware/src/config.c +++ b/firmware/src/config.c @@ -42,9 +42,14 @@ static chu_cfg_t default_cfg = { .mode = 0, .virtual_aic = 0, }, + .ir = { + .enabled = 0, + .base = { 3800, 3800, 3800, 3800, 3800, 3800 }, + .trigger = { 20, 20, 20, 20, 20, 20 }, + }, }; -chu_runtime_t chu_runtime; +chu_runtime_t chu_runtime = {0}; static void config_loaded() { diff --git a/firmware/src/config.h b/firmware/src/config.h index c8cacfc..c0c0b6b 100644 --- a/firmware/src/config.h +++ b/firmware/src/config.h @@ -42,10 +42,16 @@ typedef struct __attribute__((packed)) { uint8_t mode : 4; uint8_t virtual_aic : 4; } aime; + struct { + bool enabled; + uint16_t base[6]; + uint8_t trigger[6]; + } ir; } chu_cfg_t; typedef struct { bool debug; + bool ir_diagnostics; } chu_runtime_t; extern chu_cfg_t *chu_cfg; diff --git a/firmware/src/main.c b/firmware/src/main.c index bd12e7e..3c3915b 100644 --- a/firmware/src/main.c +++ b/firmware/src/main.c @@ -115,8 +115,8 @@ static void run_lights() if (now - last_hid_time >= 1000000) { const uint32_t colors[] = {0x000000, 0x0000ff, 0xff0000, 0xffff00, 0x00ff00, 0x00ffff, 0xffffff}; - for (int i = 0; i < air_num(); i++) { - int d = air_value(i); + for (int i = 0; i < air_tof_num(); i++) { + int d = air_tof_value(i); rgb_set_color(31 + i, colors[d]); } diff --git a/firmware/src/rgb.c b/firmware/src/rgb.c index d656843..952dd74 100644 --- a/firmware/src/rgb.c +++ b/firmware/src/rgb.c @@ -19,9 +19,8 @@ #include "board_defs.h" #include "config.h" -#define ARRAY_SIZE(a) (sizeof(a) / sizeof((a)[0])) - -static uint32_t rgb_buf[47]; // 16(Keys) + 15(Gaps) + 16(maximum ToF indicators) +static uint32_t buf_main[47]; // 16(Keys) + 15(Gaps) + 16 (ToF/Tower indicators) +static const uint32_t *buf_tower = &buf_main[31]; #define _MAP_LED(x) _MAKE_MAPPER(x) #define _MAKE_MAPPER(x) MAP_LED_##x @@ -92,22 +91,29 @@ static void drive_led() last = now; for (int i = 30; i >= 0; i--) { - pio_sm_put_blocking(pio0, 0, rgb_buf[i] << 8u); + pio_sm_put_blocking(pio0, 0, buf_main[i] << 8u); } - for (int i = 31; i < ARRAY_SIZE(rgb_buf); i++) { - pio_sm_put_blocking(pio0, 0, rgb_buf[i] << 8u); + for (int i = 31; i < count_of(buf_main); i++) { + pio_sm_put_blocking(pio0, 0, buf_main[i] << 8u); + } + + for (int i = 0; i < 6; i++) { + pio_sm_put_blocking(pio0, 0, buf_tower[i] << 8u); + } + for (int i = 0; i < 6; i++) { + pio_sm_put_blocking(pio0, 1, buf_tower[i] << 8u); } } void rgb_set_colors(const uint32_t *colors, unsigned index, size_t num) { - if (index >= ARRAY_SIZE(rgb_buf)) { + if (index >= count_of(buf_main)) { return; } - if (index + num > ARRAY_SIZE(rgb_buf)) { - num = ARRAY_SIZE(rgb_buf) - index; + if (index + num > count_of(buf_main)) { + num = count_of(buf_main) - index; } - memcpy(&rgb_buf[index], colors, num * sizeof(*colors)); + memcpy(&buf_main[index], colors, num * sizeof(*colors)); } static inline uint32_t apply_level(uint32_t color) @@ -125,10 +131,10 @@ static inline uint32_t apply_level(uint32_t color) void rgb_set_color(unsigned index, uint32_t color) { - if (index >= ARRAY_SIZE(rgb_buf)) { + if (index >= count_of(buf_main)) { return; } - rgb_buf[index] = apply_level(color); + buf_main[index] = apply_level(color); } void rgb_key_color(unsigned index, uint32_t color) @@ -136,7 +142,7 @@ void rgb_key_color(unsigned index, uint32_t color) if (index > 16) { return; } - rgb_buf[index * 2] = apply_level(color); + buf_main[index * 2] = apply_level(color); } void rgb_gap_color(unsigned index, uint32_t color) @@ -144,31 +150,31 @@ void rgb_gap_color(unsigned index, uint32_t color) if (index > 15) { return; } - rgb_buf[index * 2 + 1] = apply_level(color); + buf_main[index * 2 + 1] = apply_level(color); } void rgb_set_brg(unsigned index, const uint8_t *brg_array, size_t num) { - if (index >= ARRAY_SIZE(rgb_buf)) { + if (index >= count_of(buf_main)) { return; } - if (index + num > ARRAY_SIZE(rgb_buf)) { - num = ARRAY_SIZE(rgb_buf) - index; + if (index + num > count_of(buf_main)) { + num = count_of(buf_main) - index; } for (int i = 0; i < num; i++) { uint8_t b = brg_array[i * 3 + 0]; uint8_t r = brg_array[i * 3 + 1]; uint8_t g = brg_array[i * 3 + 2]; - rgb_buf[index + i] = apply_level(rgb32(r, g, b, false)); + buf_main[index + i] = apply_level(rgb32(r, g, b, false)); } } void rgb_init() { - uint pio0_offset = pio_add_program(pio0, &ws2812_program); - - gpio_set_drive_strength(RGB_PIN, GPIO_DRIVE_STRENGTH_2MA); - ws2812_program_init(pio0, 0, pio0_offset, RGB_PIN, 800000, false); + uint offset = pio_add_program(pio0, &ws2812_program); + ws2812_program_init(pio0, 0, offset, RGB_MAIN_PIN, 800000, false); + ws2812_program_init(pio0, 1, offset, RGB_TOWER_LEFT_PIN, 800000, false); + ws2812_program_init(pio0, 2, offset, RGB_TOWER_RIGHT_PIN, 800000, false); } void rgb_update() diff --git a/firmware/src/rgb.h b/firmware/src/rgb.h index e12037d..98df8ff 100644 --- a/firmware/src/rgb.h +++ b/firmware/src/rgb.h @@ -1,29 +1,29 @@ -/* - * RGB LED (WS2812) Strip control - * WHowe - */ - -#ifndef RGB_H -#define RGB_H - -#include -#include -#include - -#include "config.h" - -void rgb_init(); -void rgb_update(); - -uint32_t rgb32(uint32_t r, uint32_t g, uint32_t b, bool gamma_fix); -uint32_t rgb32_from_hsv(uint8_t h, uint8_t s, uint8_t v); - -void rgb_set_colors(const uint32_t *colors, unsigned index, size_t num); -void rgb_set_color(unsigned index, uint32_t color); -void rgb_key_color(unsigned index, uint32_t color); -void rgb_gap_color(unsigned index, uint32_t color); - -/* num of the rgb leds, num*3 bytes in the array */ -void rgb_set_brg(unsigned index, const uint8_t *brg_array, size_t num); - -#endif +/* + * RGB LED (WS2812) Strip control + * WHowe + */ + +#ifndef RGB_H +#define RGB_H + +#include +#include +#include + +#include "config.h" + +void rgb_init(); +void rgb_update(); + +uint32_t rgb32(uint32_t r, uint32_t g, uint32_t b, bool gamma_fix); +uint32_t rgb32_from_hsv(uint8_t h, uint8_t s, uint8_t v); + +void rgb_set_colors(const uint32_t *colors, unsigned index, size_t num); +void rgb_set_color(unsigned index, uint32_t color); +void rgb_key_color(unsigned index, uint32_t color); +void rgb_gap_color(unsigned index, uint32_t color); + +/* num of the rgb leds, num*3 bytes in the array */ +void rgb_set_brg(unsigned index, const uint8_t *brg_array, size_t num); + +#endif