From 6efd6ba205454ae82021d4522de831c1ea5b7c39 Mon Sep 17 00:00:00 2001 From: whowechina Date: Sun, 29 Oct 2023 13:08:57 +0800 Subject: [PATCH] Some cleanups --- firmware/src/CMakeLists.txt | 2 +- firmware/src/aime.c | 126 +++++++++++++++++++++++++----------- firmware/src/aime.h | 3 + firmware/src/cli.c | 2 +- firmware/src/commands.c | 10 +-- firmware/src/gp2y0e.h | 2 +- firmware/src/main.c | 4 +- firmware/src/mpr121.c | 18 +++--- firmware/src/pn532.c | 57 +++++++++++++--- firmware/src/pn532.h | 5 ++ firmware/src/vl53l0x.c | 112 ++++++++++++++++---------------- firmware/src/vl53l0x.h | 5 -- 12 files changed, 220 insertions(+), 126 deletions(-) diff --git a/firmware/src/CMakeLists.txt b/firmware/src/CMakeLists.txt index 049cb4d..88b5b02 100644 --- a/firmware/src/CMakeLists.txt +++ b/firmware/src/CMakeLists.txt @@ -13,7 +13,7 @@ function(make_firmware board board_def) pico_generate_pio_header(${board} ${CMAKE_CURRENT_LIST_DIR}/ws2812.pio) - target_compile_options(${board} PRIVATE -Wfatal-errors -O3) + target_compile_options(${board} PRIVATE -Wall -Werror -Wfatal-errors -O3) target_include_directories(${board} PRIVATE ${CMAKE_CURRENT_LIST_DIR}) target_include_directories(${board} PRIVATE ${BTSTACK_ROOT}/src diff --git a/firmware/src/aime.c b/firmware/src/aime.c index 0415e32..c4a933f 100644 --- a/firmware/src/aime.c +++ b/firmware/src/aime.c @@ -76,7 +76,7 @@ const char *hw_version[] = { "TN32MSEC003S H/W Ver3.0", "837-15396" }; const char *led_info[] = { "15084\xFF\x10\x00\x12", "000-00000\xFF\x11\x40" }; static int baudrate_mode = 0; -static void aime_set_baudrate(int mode) +void aime_set_baudrate(int mode) { baudrate_mode = (mode == 0) ? 0 : 1; } @@ -91,9 +91,7 @@ void aime_init(int interface) pn532_config_sam(); } -typedef struct __attribute__((packed)) { - -} felica_req_t; +static uint8_t mifare_keys[2][6]; // 'KeyA' and 'KeyB' static union __attribute__((packed)) { struct { @@ -116,6 +114,10 @@ static union __attribute__((packed)) { uint8_t cmd; uint8_t payload_len; union { + struct { + uint8_t uid[4]; + uint8_t block_id; + } mifare; struct { uint8_t idm[8]; uint8_t len; @@ -136,8 +138,6 @@ struct { uint64_t time; } req_ctx; -static uint8_t key_sets[2][6]; // 'KeyA' and 'KeyB' - static void build_response(int payload_len) { response.len = payload_len + 6; @@ -170,7 +170,7 @@ static void send_response() tud_cdc_n_write_flush(aime_interface); } -static void simple_response(uint8_t status) +static void send_simple_response(uint8_t status) { build_response(0); response.status = status; @@ -179,8 +179,8 @@ static void simple_response(uint8_t status) static void cmd_to_normal_mode() { - simple_response(pn532_firmware_ver() ? STATUS_NFCRW_FIRMWARE_UP_TO_DATE - : STATUS_NFCRW_INIT_ERROR); + send_simple_response(pn532_firmware_ver() ? STATUS_NFCRW_FIRMWARE_UP_TO_DATE + : STATUS_NFCRW_INIT_ERROR); } static void cmd_fake_version(const char *version[]) @@ -191,25 +191,16 @@ static void cmd_fake_version(const char *version[]) send_response(); } -static void cmd_get_hw_version() +static void cmd_key_set(uint8_t key[6]) { - build_response(2); - response.payload[0] = 0x01; - response.payload[1] = 0x00; - send_response(); -} - -static void cmd_key_set(int type) -{ - memcpy(key_sets[type], request.payload, 6); - build_response(0); - send_response(); + memcpy(key, request.payload, 6); + send_simple_response(STATUS_OK); } static void cmd_set_polling(bool enabled) { pn532_set_rf_field(0, enabled ? 1 : 0); - simple_response(STATUS_OK); + send_simple_response(STATUS_OK); } static void cmd_detect_card() @@ -218,26 +209,31 @@ static void cmd_detect_card() uint8_t count; uint8_t type; uint8_t id_len; - uint8_t idm[8]; - uint8_t pmm[8]; - uint8_t syscode[2]; + union { + struct { + uint8_t idm[8]; + uint8_t pmm[8]; + uint8_t syscode[2]; + }; + uint8_t uid[6]; + }; } card_info_t; card_info_t *card = (card_info_t *) response.payload; int len = sizeof(card->idm); - if (pn532_poll_mifare(card->idm, &len)) { + if (pn532_poll_mifare(card->uid, &len)) { build_response(len > 4 ? 10 : 7); card->count = 1; card->type = 0x10; card->id_len = len; - printf("Card Mifare %d\n", card->id_len); + printf("Detected Mifare %d\n", card->id_len); } else if (pn532_poll_felica(card->idm, card->pmm, card->syscode)) { build_response(19); card->count = 1; card->type = 0x20; card->id_len = 16; - printf("Card Felica - "); + printf("Detected Felica -"); for (int i = 0; i < 8; i++) { printf(" %02x", card->idm[i]); } @@ -255,6 +251,36 @@ static void cmd_detect_card() send_response(); } +static void cmd_card_select() +{ + printf("CARD SELECT %d\n", request.payload_len); + send_simple_response(STATUS_OK); +} + +static void cmd_mifare_auth(int type) +{ + printf("MIFARE AUTH\n"); + pn532_mifare_auth(request.mifare.uid, request.mifare.block_id, type, mifare_keys[type]); + send_simple_response(STATUS_OK); +} + +static void cmd_mifare_read() +{ + printf("MIFARE READ %02x %02x %02x %02x %02x\n", request.mifare.block_id, + request.mifare.uid[0], request.mifare.uid[1], request.mifare.uid[2], + request.mifare.uid[3]); + build_response(16); + memset(response.payload, 0, 16); + pn532_mifare_read(request.mifare.block_id, response.payload); + send_response(); +} + +static void cmd_mifare_halt() +{ + printf("MIFARE HALT\n"); + send_simple_response(STATUS_OK); +} + typedef struct __attribute__((packed)) { uint8_t len; uint8_t code; @@ -327,7 +353,7 @@ static int cmd_felica_read() if (block_id == 0x8082) { memcpy(read_resp->block_data[i], felica_resp->idm, 8); } -// pn532_felica_read_wo_encrypt(svc_code, block_id, read_resp->block_data[i]); + pn532_felica_read_wo_encrypt(svc_code, block_id, read_resp->block_data[i]); } read_resp->rw_status[0] = 0x00; @@ -348,20 +374,30 @@ static int cmd_felica_write() } uint16_t svc_code = req_data[9] | (req_data[10] << 8); - + uint8_t *block = req_data + 11; uint8_t block_num = block[0]; felica_resp_t *felica_resp = (felica_resp_t *) response.payload; uint8_t *rw_status = felica_resp->data; + printf("svc code: %04x\n", svc_code); + printf("blocks:"); + for (int i = 0; i < block_num; i++) { + uint16_t block_id = (block[i * 2 + 1] << 8) | block[i * 2 + 2]; + printf(" %04x", block_id); + } + printf("\n"); + + uint8_t *block_data = block + 1 + block_num * 2; + rw_status[0] = 0x00; rw_status[1] = 0x00; for (int i = 0; i < block_num; i++) { + printf("writing %02x %02x\n", block_data[i * 16], block_data[i * 16 + 15]); uint16_t block_id = (block[i * 2 + 1] << 8) | block[i * 2 + 2]; - int result = 0; - //pn532_felica_write_wo_encrypt(svc_code, block_id, read_resp->block_data[i]); + int result = pn532_felica_write_wo_encrypt(svc_code, block_id, block_data + i * 16); if (result < 0) { rw_status[0] = 0x01; rw_status[1] = 0x01; @@ -389,7 +425,7 @@ static void cmd_felica() card_id_t card; if (!pn532_poll_felica(card.idm, card.pmm, card.syscode)) { - simple_response(STATUS_FELICA_ERROR); + send_simple_response(STATUS_FELICA_ERROR); } uint8_t felica_code = request.felica.code; @@ -420,7 +456,7 @@ static void cmd_felica() } if (datalen < 0) { - simple_response(STATUS_FELICA_ERROR); + send_simple_response(STATUS_FELICA_ERROR); return; } @@ -466,11 +502,11 @@ static void aime_handle_frame() break; case CMD_MIFARE_KEY_SET_A: printf("CMD_MIFARE_KEY_SET_A\n"); - cmd_key_set(0); + cmd_key_set(mifare_keys[0]); break; case CMD_MIFARE_KEY_SET_B: printf("CMD_MIFARE_KEY_SET_B\n"); - cmd_key_set(1); + cmd_key_set(mifare_keys[1]); break; case CMD_START_POLLING: @@ -487,9 +523,23 @@ static void aime_handle_frame() break; case CMD_CARD_SELECT: + cmd_card_select(); + break; + + case CMD_MIFARE_AUTHORIZE_A: + cmd_mifare_auth(0); + break; + case CMD_MIFARE_AUTHORIZE_B: + cmd_mifare_auth(1); + break; + case CMD_MIFARE_READ: + cmd_mifare_read(); + break; + case CMD_CARD_HALT: + cmd_mifare_halt(); break; case CMD_EXT_BOARD_INFO: @@ -501,7 +551,7 @@ static void aime_handle_frame() case CMD_SEND_HEX_DATA: case CMD_EXT_TO_NORMAL_MODE: - simple_response(STATUS_OK); + send_simple_response(STATUS_OK); break; default: @@ -510,7 +560,7 @@ static void aime_handle_frame() printf(" %02x", request.raw[i]); } printf("]\n"); - simple_response(STATUS_OK); + send_simple_response(STATUS_OK); break; } } diff --git a/firmware/src/aime.h b/firmware/src/aime.h index 2cb13ff..e1073f5 100644 --- a/firmware/src/aime.h +++ b/firmware/src/aime.h @@ -10,4 +10,7 @@ void aime_init(int interface); void aime_update(); uint32_t aime_led_color(); +// mode 0 or 1 +void aime_set_baudrate(int mode); + #endif diff --git a/firmware/src/cli.c b/firmware/src/cli.c index 3ba1ffc..3b38a51 100644 --- a/firmware/src/cli.c +++ b/firmware/src/cli.c @@ -94,7 +94,7 @@ int cli_extract_non_neg_int(const char *param, int len) } int result = 0; for (int i = 0; i < len; i++) { - if (!isdigit(param[i])) { + if (!isdigit((uint8_t)param[i])) { return -1; } result = result * 10 + param[i] - '0'; diff --git a/firmware/src/commands.c b/firmware/src/commands.c index cce4d04..6f784f9 100644 --- a/firmware/src/commands.c +++ b/firmware/src/commands.c @@ -23,10 +23,10 @@ static void disp_colors() { printf("[Colors]\n"); - printf(" Key upper: %06x, lower: %06x, both: %06x, off: %06x\n", + printf(" Key upper: %06lx, lower: %06lx, both: %06lx, off: %06lx\n", chu_cfg->colors.key_on_upper, chu_cfg->colors.key_on_lower, chu_cfg->colors.key_on_both, chu_cfg->colors.key_off); - printf(" Gap: %06x\n", chu_cfg->colors.gap); + printf(" Gap: %06lx\n", chu_cfg->colors.gap); } static void disp_style() @@ -263,7 +263,7 @@ static void handle_filter(int argc, char *argv[]) disp_sense(); } -static uint8_t *extract_key(const char *param) +static int8_t *extract_key(const char *param) { int len = strlen(param); @@ -306,7 +306,7 @@ static void handle_sense(int argc, char *argv[]) " >sense +\n" " >sense -\n" " >sense 1A +\n" - " >sense 13B -\n"; + " >sense 13B -\n" " >sense * 0\n"; if ((argc < 1) || (argc > 2)) { printf(usage); @@ -327,7 +327,7 @@ static void handle_sense(int argc, char *argv[]) sense_do_op(&chu_cfg->sense.keys[i], op[0]); } } else { - uint8_t *key = extract_key(argv[0]); + int8_t *key = extract_key(argv[0]); if (!key) { printf(usage); return; diff --git a/firmware/src/gp2y0e.h b/firmware/src/gp2y0e.h index 81c2111..d36b8e7 100644 --- a/firmware/src/gp2y0e.h +++ b/firmware/src/gp2y0e.h @@ -11,7 +11,7 @@ #define GP2Y0E_DEF_ADDR 0x40 -static inline uint16_t gp2y0e_write(i2c_inst_t *port, uint8_t addr, uint8_t val) +static inline void gp2y0e_write(i2c_inst_t *port, uint8_t addr, uint8_t val) { uint8_t cmd[] = {addr, val}; i2c_write_blocking_until(port, GP2Y0E_DEF_ADDR, cmd, 2, false, time_us_64() + 1000); diff --git a/firmware/src/main.c b/firmware/src/main.c index 8422565..b4a15a6 100644 --- a/firmware/src/main.c +++ b/firmware/src/main.c @@ -79,7 +79,7 @@ static void gen_joy_report() } const uint8_t keycode_table[128][2] = { HID_ASCII_TO_KEYCODE }; -const char keymap[38 + 1] = NKRO_KEYMAP; // 32 keys, 6 air keys, 1 terminator +const uint8_t keymap[38 + 1] = NKRO_KEYMAP; // 32 keys, 6 air keys, 1 terminator static void gen_nkro_report() { for (int i = 0; i < 32; i++) { @@ -137,7 +137,7 @@ static void run_lights() uint8_t b = aime_color; uint32_t blink = now / 100000; - aime_color = (now & 1) ? rgb32(r, g, b, false) : 0; + aime_color = (blink & 1) ? rgb32(r, g, b, false) : 0; rgb_set_color(34, aime_color); rgb_set_color(35, aime_color); diff --git a/firmware/src/mpr121.c b/firmware/src/mpr121.c index 090aece..db51167 100644 --- a/firmware/src/mpr121.c +++ b/firmware/src/mpr121.c @@ -133,19 +133,19 @@ void mpr121_init(uint8_t i2c_addr) #define ABS(x) ((x) < 0 ? -(x) : (x)) -static void mpr121_read_many(uint8_t addr, uint8_t reg, uint8_t *buf, size_t n) +static void mpr121_read_many(uint8_t addr, uint8_t reg, uint8_t *buf, int num) { i2c_write_blocking_until(I2C_PORT, addr, ®, 1, true, time_us_64() + IO_TIMEOUT_US); - i2c_read_blocking_until(I2C_PORT, addr, buf, n, false, - time_us_64() + IO_TIMEOUT_US * n / 2); + i2c_read_blocking_until(I2C_PORT, addr, buf, num, false, + time_us_64() + IO_TIMEOUT_US * num / 2); } -static void mpr121_read_many16(uint8_t addr, uint8_t reg, uint16_t *buf, size_t n) +static void mpr121_read_many16(uint8_t addr, uint8_t reg, uint16_t *buf, int num) { - uint8_t vals[n * 2]; - mpr121_read_many(addr, reg, vals, n * 2); - for (int i = 0; i < n; i++) { + uint8_t vals[num * 2]; + mpr121_read_many(addr, reg, vals, num * 2); + for (int i = 0; i < num; i++) { buf[i] = (vals[i * 2 + 1] << 8) | vals[i * 2]; } } @@ -153,7 +153,7 @@ static void mpr121_read_many16(uint8_t addr, uint8_t reg, uint16_t *buf, size_t uint16_t mpr121_touched(uint8_t addr) { uint16_t touched; - mpr121_read_many16(addr, MPR121_TOUCH_STATUS_REG, &touched, 2); + mpr121_read_many16(addr, MPR121_TOUCH_STATUS_REG, &touched, 1); return touched; } @@ -169,7 +169,7 @@ static uint8_t mpr121_stop(uint8_t addr) return ecr; } -static uint8_t mpr121_resume(uint8_t addr, uint8_t ecr) +static void mpr121_resume(uint8_t addr, uint8_t ecr) { write_reg(addr, MPR121_ELECTRODE_CONFIG_REG, ecr); } diff --git a/firmware/src/pn532.c b/firmware/src/pn532.c index 95905a1..b41e077 100644 --- a/firmware/src/pn532.c +++ b/firmware/src/pn532.c @@ -103,7 +103,7 @@ static bool read_ack() return false; } - int result = read_frame(resp, 6); + read_frame(resp, 6); const uint8_t expect_ack[] = {0, 0, 0xff, 0, 0xff, 0}; if (memcmp(resp, expect_ack, 6) != 0) { @@ -131,7 +131,7 @@ int pn532_write_data(const uint8_t *data, uint8_t len) frame[5 + len] = ~checksum; frame[6 + len] = PN532_POSTAMBLE; - int result = write_frame(frame, 7 + len); + write_frame(frame, 7 + len); return read_ack(); } @@ -140,7 +140,7 @@ int pn532_read_data(uint8_t *data, uint8_t len) { uint8_t resp[len + 7]; - int result = read_frame(resp, len + 7); + read_frame(resp, len + 7); if (resp[0] != PN532_PREAMBLE || resp[1] != PN532_STARTCODE1 || @@ -264,7 +264,7 @@ uint32_t pn532_firmware_ver() bool pn532_config_sam() { uint8_t param[] = {0x01, 0x14, 0x01}; - int result = pn532_write_command(0x14, param, 3); + pn532_write_command(0x14, param, 3); return pn532_read_response(0x14, NULL, 0) == 0; } @@ -273,7 +273,7 @@ bool pn532_config_sam() bool pn532_set_rf_field(uint8_t auto_rf, uint8_t on_off) { uint8_t param[] = { 1, auto_rf | on_off }; - int result = pn532_write_command(0x32, param, 2); + pn532_write_command(0x32, param, 2); return pn532_read_response(0x32, NULL, 0) >= 0; } @@ -337,6 +337,47 @@ bool pn532_poll_felica(uint8_t uid[8], uint8_t pmm[8], uint8_t syscode[2]) return true; } +bool pn532_mifare_auth(const uint8_t uid[4], uint8_t block_id, uint8_t key_id, const uint8_t *key) +{ + uint8_t param[] = { 1, key_id ? 1 : 0, block_id, + key[0], key[1], key[2], key[3], key[4], key[5], + uid[0], uid[1], uid[2], uid[3] }; + int ret = pn532_write_command(0x40, param, sizeof(param)); + if (ret < 0) { + printf("Failed mifare auth command\n"); + return false; + } + int result = pn532_read_response(0x40, readbuf, sizeof(readbuf)); + if (readbuf[0] != 0) { + printf("PN532 Mifare AUTH failed %d %02x\n", result, readbuf[0]); + return false; + } + + return true; +} + +bool pn532_mifare_read(uint8_t block_id, uint8_t block_data[16]) +{ + uint8_t param[] = { 1, 0x30, block_id }; + + int ret = pn532_write_command(0x40, param, sizeof(param)); + if (ret < 0) { + printf("Failed mifare read command\n"); + return false; + } + + int result = pn532_read_response(0x40, readbuf, sizeof(readbuf)); + + if (readbuf[0] != 0 || result != 17) { + printf("PN532 Mifare READ failed %d %02x\n", result, readbuf[0]); + return false; + } + + memmove(block_data, readbuf + 1, 16); + + return true; +} + int pn532_felica_command(uint8_t cmd, const uint8_t *param, uint8_t param_len, uint8_t *outbuf) { int cmd_len = param_len + 11; @@ -357,7 +398,7 @@ int pn532_felica_command(uint8_t cmd, const uint8_t *param, uint8_t param_len, u int result = pn532_read_response(0x40, readbuf, sizeof(readbuf)); int outlen = readbuf[1] - 1; - if (readbuf[0] & 0x3f != 0 || result - 2 != outlen) { + if ((readbuf[0] & 0x3f) != 0 || result - 2 != outlen) { return -1; } @@ -375,7 +416,7 @@ bool pn532_felica_read_wo_encrypt(uint16_t svc_code, uint16_t block_id, uint8_t int result = pn532_felica_command(0x06, param, sizeof(param), readbuf); if (result != 12 + 16 || readbuf[9] != 0 || readbuf[10] != 0) { - printf("PN532 Felica READ read response failed %d %02x %02x\n", + printf("PN532 Felica READ read failed %d %02x %02x\n", result, readbuf[9], readbuf[10]); for (int i = 0; i < result; i++) { printf(" %02x", readbuf[i]); @@ -397,8 +438,8 @@ bool pn532_felica_write_wo_encrypt(uint16_t svc_code, uint16_t block_id, const u int result = pn532_felica_command(0x08, param, sizeof(param), readbuf); - printf("PN532 Felica Write response %d\n", result); if (result < 0) { + printf("PN532 Felica WRITE failed %d\n", result); return false; } diff --git a/firmware/src/pn532.h b/firmware/src/pn532.h index c262e47..641f95a 100644 --- a/firmware/src/pn532.h +++ b/firmware/src/pn532.h @@ -18,6 +18,11 @@ bool pn532_set_rf_field(uint8_t auto_rf, uint8_t on_off); bool pn532_poll_mifare(uint8_t *uid, int *len); bool pn532_poll_felica(uint8_t uid[8], uint8_t pmm[8], uint8_t syscode[2]); + +bool pn532_mifare_auth(const uint8_t uid[4], uint8_t block_id, uint8_t key_id, const uint8_t *key); +bool pn532_mifare_read(uint8_t block_id, uint8_t block_data[16]); + bool pn532_felica_read_wo_encrypt(uint16_t svc_code, uint16_t block_id, uint8_t block_data[16]); +bool pn532_felica_write_wo_encrypt(uint16_t svc_code, uint16_t block_id, const uint8_t block_data[16]); #endif diff --git a/firmware/src/vl53l0x.c b/firmware/src/vl53l0x.c index 4647f8c..757fc89 100644 --- a/firmware/src/vl53l0x.c +++ b/firmware/src/vl53l0x.c @@ -331,6 +331,59 @@ float getSignalRateLimit() return (float)read_reg16(FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT) / (1 << 7); } +// Decode sequence step timeout in MCLKs from register value +// based on VL53L0X_decode_timeout() +// Note: the original function returned a uint32_t, but the return value is +// always stored in a uint16_t. +uint16_t decodeTimeout(uint16_t reg_val) +{ + // format: "(LSByte * 2^MSByte) + 1" + return (uint16_t)((reg_val & 0x00FF) << + (uint16_t)((reg_val & 0xFF00) >> 8)) + 1; +} + +// Encode sequence step timeout register value from timeout in MCLKs +// based on VL53L0X_encode_timeout() +uint16_t encodeTimeout(uint32_t timeout_mclks) +{ + // format: "(LSByte * 2^MSByte) + 1" + + uint32_t ls_byte = 0; + uint16_t ms_byte = 0; + + if (timeout_mclks > 0) { + ls_byte = timeout_mclks - 1; + + while ((ls_byte & 0xFFFFFF00) > 0) { + ls_byte >>= 1; + ms_byte++; + } + + return (ms_byte << 8) | (ls_byte & 0xFF); + } + else { + return 0; + } +} + +// Convert sequence step timeout from MCLKs to microseconds with given VCSEL period in PCLKs +// based on VL53L0X_calc_timeout_us() +uint32_t timeoutMclksToMicroseconds(uint16_t timeout_period_mclks, uint8_t vcsel_period_pclks) +{ + uint32_t macro_period_ns = calcMacroPeriod(vcsel_period_pclks); + + return ((timeout_period_mclks * macro_period_ns) + 500) / 1000; +} + +// Convert sequence step timeout from microseconds to MCLKs with given VCSEL period in PCLKs +// based on VL53L0X_calc_timeout_mclks() +uint32_t timeoutMicrosecondsToMclks(uint32_t timeout_period_us, uint8_t vcsel_period_pclks) +{ + uint32_t macro_period_ns = calcMacroPeriod(vcsel_period_pclks); + + return (((timeout_period_us * 1000) + (macro_period_ns / 2)) / macro_period_ns); +} + // Set the measurement timing budget in microseconds, which is the time allowed // for one measurement; the ST API and this library take care of splitting the // timing budget among the sub-steps in the ranging sequence. A longer timing @@ -403,7 +456,7 @@ bool setMeasurementTimingBudget(uint32_t budget_us) uint32_t final_range_timeout_mclks = timeoutMicrosecondsToMclks(final_range_timeout_us, - timeouts.final_range_vcsel_period_pclks); + timeouts.final_range_vcsel_period_pclks); if (enables.pre_range) { @@ -720,7 +773,7 @@ void vl53l0x_stop_continuous() // single-shot range measurement) uint16_t readRangeContinuousMillimeters() { - if (read_reg(RESULT_INTERRUPT_STATUS) & 0x07 == 0) { + if ((read_reg(RESULT_INTERRUPT_STATUS) & 0x07) == 0) { return 65535; } @@ -854,66 +907,13 @@ void getSequenceStepTimeouts(SequenceStepEnables const * enables, SequenceStepTi timeouts->final_range_vcsel_period_pclks); } -// Decode sequence step timeout in MCLKs from register value -// based on VL53L0X_decode_timeout() -// Note: the original function returned a uint32_t, but the return value is -// always stored in a uint16_t. -uint16_t decodeTimeout(uint16_t reg_val) -{ - // format: "(LSByte * 2^MSByte) + 1" - return (uint16_t)((reg_val & 0x00FF) << - (uint16_t)((reg_val & 0xFF00) >> 8)) + 1; -} - -// Encode sequence step timeout register value from timeout in MCLKs -// based on VL53L0X_encode_timeout() -uint16_t encodeTimeout(uint32_t timeout_mclks) -{ - // format: "(LSByte * 2^MSByte) + 1" - - uint32_t ls_byte = 0; - uint16_t ms_byte = 0; - - if (timeout_mclks > 0) { - ls_byte = timeout_mclks - 1; - - while ((ls_byte & 0xFFFFFF00) > 0) { - ls_byte >>= 1; - ms_byte++; - } - - return (ms_byte << 8) | (ls_byte & 0xFF); - } - else { - return 0; - } -} - -// Convert sequence step timeout from MCLKs to microseconds with given VCSEL period in PCLKs -// based on VL53L0X_calc_timeout_us() -uint32_t timeoutMclksToMicroseconds(uint16_t timeout_period_mclks, uint8_t vcsel_period_pclks) -{ - uint32_t macro_period_ns = calcMacroPeriod(vcsel_period_pclks); - - return ((timeout_period_mclks * macro_period_ns) + 500) / 1000; -} - -// Convert sequence step timeout from microseconds to MCLKs with given VCSEL period in PCLKs -// based on VL53L0X_calc_timeout_mclks() -uint32_t timeoutMicrosecondsToMclks(uint32_t timeout_period_us, uint8_t vcsel_period_pclks) -{ - uint32_t macro_period_ns = calcMacroPeriod(vcsel_period_pclks); - - return (((timeout_period_us * 1000) + (macro_period_ns / 2)) / macro_period_ns); -} - // based on VL53L0X_perform_single_ref_calibration() bool performSingleRefCalibration(uint8_t vhv_init_byte) { write_reg(SYSRANGE_START, 0x01 | vhv_init_byte); // VL53L0X_REG_SYSRANGE_MODE_START_STOP uint32_t start = time_us_32(); - while (read_reg(RESULT_INTERRUPT_STATUS) & 0x07 == 0) { + while ((read_reg(RESULT_INTERRUPT_STATUS) & 0x07) == 0) { if (time_us_32() - start > IO_TIMEOUT_US) { return false; } diff --git a/firmware/src/vl53l0x.h b/firmware/src/vl53l0x.h index d26d591..bf710da 100644 --- a/firmware/src/vl53l0x.h +++ b/firmware/src/vl53l0x.h @@ -154,9 +154,4 @@ void getSequenceStepTimeouts(SequenceStepEnables const * enables, SequenceStepTi bool performSingleRefCalibration(uint8_t vhv_init_byte); -static uint16_t decodeTimeout(uint16_t value); -static uint16_t encodeTimeout(uint32_t timeout_mclks); -static uint32_t timeoutMclksToMicroseconds(uint16_t timeout_period_mclks, uint8_t vcsel_period_pclks); -static uint32_t timeoutMicrosecondsToMclks(uint32_t timeout_period_us, uint8_t vcsel_period_pclks); - #endif \ No newline at end of file