diff --git a/Makefile b/Makefile index f71453c..bcd1166 100644 --- a/Makefile +++ b/Makefile @@ -3,14 +3,19 @@ OOCD_INTERFACE ?= stlink-v2 OOCD_TARGET ?= stm32f4x BMP_PORT ?= /dev/ttyBmpGdb -COMFLAGS=-g -Os -pedantic -Wall +COMFLAGS=-g -Og -pedantic -Wall -#LIBNAME = opencm3_stm32f4 -#FP_FLAGS ?= -mfloat-abi=hard -mfpu=fpv4-sp-d16 -#ARCH_FLAGS = -mthumb -mcpu=cortex-m4 $(FP_FLAGS) -#DEVDEFS = -DSTM32F4 -DSTM32F4CCM -DSTM32F405RG -D_ROM=1008K -D_RAM=128K -D_CCM=64K -D_CCM_OFF=0x10000000 -D_ROM_OFF=0x08004000 -D_RAM_OFF=0x20000000 -DBOOTLOADER -#LDSCRIPT = linker.ld -DEVICE = STM32F405RG +BOOTLOADER ?= no + +ifeq ($(BOOTLOADER),yes) + LIBNAME = opencm3_stm32f4 + FP_FLAGS ?= -mfloat-abi=hard -mfpu=fpv4-sp-d16 + ARCH_FLAGS = -mthumb -mcpu=cortex-m4 $(FP_FLAGS) + DEVDEFS = -DSTM32F4 -DSTM32F4CCM -DSTM32F405RG -D_ROM=1008K -D_RAM=128K -D_CCM=64K -D_CCM_OFF=0x10000000 -D_ROM_OFF=0x08004000 -D_RAM_OFF=0x20000000 -DBOOTLOADER + LDSCRIPT = linker.ld +else + DEVICE = STM32F405RG +endif CFLAGS=$(COMFLAGS) $(DEVDEFS) -std=c99 -D_POSIX_C_SOURCE=200809L CXXFLAGS=$(COMFLAGS) -std=c++17 diff --git a/encoder.c b/encoder.c index a18790a..584969d 100644 --- a/encoder.c +++ b/encoder.c @@ -2,14 +2,42 @@ #include #include #include -#include #include +#include +#include #include "encoder.h" +#include "systick.h" -static volatile int pos; +#define USE_CORRECTION +#define FIXED_OFFSET 20 +// #define DEBUG_ENCODER_TICKS +#define SPEED_AVG_NUM 4 +#ifdef DEBUG_ENCODER_TICKS +#define LOG_ENCODER_DEBUG_TICKS(...) printf(__VA_ARGS__) +#else +#define LOG_ENCODER_DEBUG_TICKS(...) +#endif + +static volatile int pos = 0; +static int offset = + #ifdef FIXED_OFFSET + FIXED_OFFSET + #else + -1 + #endif + ; +static volatile int speed = 0; static void check(void); +static uint32_t last_tick = 0; +static struct tick_pos { + uint32_t tick; + int pos; +} speed_ticks[SPEED_AVG_NUM + 1] = {0}; +static uint32_t ticks[3] = {0}; + +#define MARKS 96 #define PIN_OPEN GPIO1 #define PIN_OPEN_IRQ NVIC_EXTI1_IRQ #define PIN_CLOSE GPIO2 @@ -24,9 +52,12 @@ void encoder_setup(void) { gpio_mode_setup(PIN_PORT, GPIO_MODE_INPUT, GPIO_PUPD_NONE, PIN_OPEN | PIN_CLOSE); - exti_select_source(PIN_OPEN | PIN_CLOSE, PIN_PORT); - exti_set_trigger(PIN_OPEN | PIN_CLOSE, EXTI_TRIGGER_BOTH); - exti_enable_request(PIN_OPEN | PIN_CLOSE); + exti_select_source(PIN_OPEN, PIN_PORT); + exti_select_source(PIN_CLOSE, PIN_PORT); + exti_set_trigger(PIN_OPEN, EXTI_TRIGGER_BOTH); + exti_set_trigger(PIN_CLOSE, EXTI_TRIGGER_BOTH); + exti_enable_request(PIN_OPEN); + exti_enable_request(PIN_CLOSE); nvic_enable_irq(PIN_OPEN_IRQ); nvic_enable_irq(PIN_CLOSE_IRQ); @@ -39,11 +70,26 @@ int encoder_get() { return pos; } -static int prev = 0; +int encoder_speed() { + return speed; +} + +int encoder_current_speed() { + int dt = ticks_ms(tick - speed_ticks[SPEED_AVG_NUM - 1].tick); + if (dt > 200) { + printf("dt: %d", dt); + return 0; + } + return 1000 + * (speed_ticks[SPEED_AVG_NUM-1].pos - speed_ticks[0].pos) + / (int)ticks_ms(tick - speed_ticks[0].tick); +} + void check(void) { - int pp = pos; - int now = GPIO_IDR(PIN_PORT); - now = (!!(now & PIN_OPEN))<<0 | (!!(now & PIN_CLOSE)) << 1; + static int prev = 0; + int now = GPIO_IDR(PIN_PORT); + int diff = 0; + now = (!!(now & PIN_OPEN))<<0 | (!!(now & PIN_CLOSE)) << 1; #define NEITHER 0 #define JUST_OPEN 1 @@ -52,34 +98,96 @@ void check(void) { #define WAS(x) ((x)<<2) #define IS(x) (x) - switch (WAS(prev) + IS(now)) { - case WAS(NEITHER) + IS(JUST_CLOSE): - case WAS(JUST_OPEN) + IS(NEITHER): - case WAS(JUST_CLOSE) + IS(BOTH): - case WAS(BOTH) + IS(JUST_OPEN): - pos--; - break; - case WAS(NEITHER) + IS(JUST_OPEN): - case WAS(JUST_OPEN) + IS(BOTH): - case WAS(BOTH) + IS(JUST_CLOSE): - case WAS(JUST_CLOSE) + IS(NEITHER): - pos++; - break; - default: - prev = now; - return; + switch (WAS(prev) + IS(now)) { + case WAS(NEITHER) + IS(JUST_CLOSE): + case WAS(JUST_OPEN) + IS(NEITHER): + case WAS(JUST_CLOSE) + IS(BOTH): + case WAS(BOTH) + IS(JUST_OPEN): + diff = -1; + break; + case WAS(NEITHER) + IS(JUST_OPEN): + case WAS(JUST_OPEN) + IS(BOTH): + case WAS(BOTH) + IS(JUST_CLOSE): + case WAS(JUST_CLOSE) + IS(NEITHER): + diff = +1; + break; + default: + prev = now; + return; + } + + speed_ticks[SPEED_AVG_NUM].pos += diff; + pos += diff; + + const int tick_diff = tick - last_tick; + if (now == BOTH) { + ticks[0] = tick_diff; + } else if (prev == BOTH) { + ticks[1] = tick_diff; + } else if (now == NEITHER) { + for (int i = 0; i < SPEED_AVG_NUM; i++) { + speed_ticks[i] = speed_ticks[i+1]; + } + speed_ticks[SPEED_AVG_NUM - 1].tick = tick; + speed = 1000 + * (speed_ticks[SPEED_AVG_NUM-1].pos - speed_ticks[0].pos) + / (int)ticks_ms(speed_ticks[SPEED_AVG_NUM-1].tick - speed_ticks[0].tick); + +#ifdef USE_CORRECTION + ticks[2] = tick_diff; + + LOG_ENCODER_DEBUG_TICKS("%3d %4lu %4lu %4lu (%lu)", pos, ticks[0], ticks[1], ticks[2], offset); + + const uint32_t mn = us_ticks(4500), + mx = us_ticks(7000); + if (ticks[0] >= mn && + ticks[0] <= mx && + ticks[2] >= mn && + ticks[2] <= mx && + ticks[1] >= (ticks[0] + ticks[2]) && + ticks[1] <= (ticks[0] + ticks[2]) * 3 / 2) { + if (offset == -1) { +#ifndef FIXED_OFFSET + pos = 2*diff; + offset = 0; +#else + pos = FIXED_OFFSET; +#endif + } else { + int pp = pos; + pos -= 2*diff + offset; + pos = (pos + (diff > 0 ? MARKS - 1 : 0)) / MARKS * MARKS; + pos += 2*diff + offset; + + if (pp != pos) { + LOG_ENCODER_DEBUG_TICKS("changed pos: %d -> %d", pp, pos); + } else { + LOG_ENCODER_DEBUG_TICKS("match"); } + } + } +#endif + } + + if (pos < -1) { +#ifndef FIXED_OFFSET + offset = (offset - pos + 1) % MARKS; + pos = -1; +#else + pos = FIXED_OFFSET; +#endif + } + + last_tick = tick; + #undef NEITHER #undef JUST_OPEN #undef JUST_CLOSE #undef BOTH #undef WAS #undef IS - prev = now; - if (pos < MIN_POS) - pos = MIN_POS; - if (pos > MAX_POS) - pos = MAX_POS; + + prev = now; } void exti1_isr(void) { @@ -90,3 +198,7 @@ void exti2_isr(void) { exti_reset_request(EXTI2); check(); } +void exti4_isr(void) { + exti_reset_request(EXTI4); + check(); +} diff --git a/encoder.h b/encoder.h index d5fd2c5..1c264c5 100644 --- a/encoder.h +++ b/encoder.h @@ -8,4 +8,6 @@ void encoder_setup(void); int encoder_get(void); +int encoder_current_speed(void); +int encoder_speed(void); #endif diff --git a/main.c b/main.c index 2cb314c..2d498f0 100644 --- a/main.c +++ b/main.c @@ -11,6 +11,7 @@ #include "ringbuffer.h" #include "uart.h" #include "encoder.h" +#include "systick.h" static const uint32_t button_time = 100; @@ -20,15 +21,19 @@ RINGBUFFER_STORAGE(uart_to_usb_buf, 64) RINGBUFFER_STORAGE(comm_in_buf, 64) RINGBUFFER_STORAGE(comm_out_buf, 64) +static void sys_tick_setup(void); + +#if defined(BOOTLOADER) extern uint32_t *_board_dfu_dbl_tap; -static void sys_tick_setup(void); -static void fast_reset(void); static void start_bootloader(void); static void erase_app(void); +#endif + +static void fast_reset(void); static void sys_tick_setup() { - systick_set_reload(168000/2-1); + systick_set_reload(rcc_ahb_frequency / SYSTICK_FREQUENCY - 1); systick_set_clocksource(STK_CSR_CLKSOURCE_AHB); systick_counter_enable(); systick_interrupt_enable(); @@ -39,9 +44,13 @@ void sys_tick_handler() { } void fast_reset() { +#if defined(BOOTLOADER) *_board_dfu_dbl_tap = 0xf02669ef; +#endif scb_reset_system(); } + +#if defined(BOOTLOADER) void start_bootloader() { *_board_dfu_dbl_tap = 0xf01669ef; scb_reset_system(); @@ -50,6 +59,7 @@ void erase_app() { *_board_dfu_dbl_tap = 0xf5e80ab4; scb_reset_system(); } +#endif int main(void) { #if 0 @@ -63,12 +73,8 @@ int main(void) { RINGBUFFER_INIT(comm_in_buf, 64); RINGBUFFER_INIT(comm_out_buf, 64); - sys_tick_setup(); - while(tick < 100) - ; - adc_setup(); uart_setup(); usb_setup(); @@ -81,8 +87,17 @@ int main(void) { uint32_t last_tick = tick; int last_pos = 0; int watchdog = 1; - int watchdog_counter = 0; + uint32_t watchdog_counter = 0; int print_changes = 0; + + while (tick < 5000) { + usbd_poll(g_usbd_dev); + } + + if (!usb_got_reset()) { + fast_reset(); + } + while (1) { /* Handle control messages through the comms CDC */ char buf[64]; @@ -97,7 +112,7 @@ int main(void) { last_tick++; watchdog_counter += no_comms; } - if (watchdog_counter >= 35000) { + if (watchdog_counter >= s_ticks(35)) { fast_reset(); } @@ -113,8 +128,7 @@ int main(void) { buttons_close(button_time); break; case 'R': // Report - printf("pos: %d", encoder_get()); - printf("tick: %lu", tick); + printf("pos: %d %d", encoder_get(), encoder_current_speed()); break; case 'r': // toggle reporting print_changes = !print_changes; @@ -124,27 +138,30 @@ int main(void) { watchdog = !watchdog; printf("watchdog: %d", watchdog); break; + case 'S': // reStart + fast_reset(); + break; +#if defined(BOOTLOADER) case 'F': // Flash start_bootloader(); break; case 'E': // Erase erase_app(); break; - case 'S': // reStart - fast_reset(); - break; +#endif } } int pos = encoder_get(); - if (pos != last_pos && ringbuffer_empty(comm_out_buf)) { - if (print_changes) { - printf("pos: %d", pos); + if (pos != last_pos) { + if (ringbuffer_empty(comm_out_buf)) { + if (print_changes) { + printf("pos: %d %d", pos, encoder_speed()); + } + last_pos = pos; } - last_pos = pos; } - /* Send replies */ if ((buf_len = ringbuffer_peek(comm_out_buf, &buf[0], sizeof buf))) { if (usb_write_cdcacm(ACM_COMM, (void *)buf, buf_len, 1)) { @@ -164,7 +181,7 @@ int main(void) { } usbd_poll(g_usbd_dev); - __asm__("wfi"); + // __asm__("wfi"); } } diff --git a/systick.h b/systick.h new file mode 100644 index 0000000..537d0d8 --- /dev/null +++ b/systick.h @@ -0,0 +1,13 @@ +#ifndef SYSTICK_H +#define SYSTICK_H +#include +#define SYSTICK_FREQUENCY 10000 + +static inline uint32_t us_ticks(uint32_t us) { return us * SYSTICK_FREQUENCY / 1000000; } +static inline uint32_t ms_ticks(uint32_t ms) { return ms * SYSTICK_FREQUENCY / 1000; } +static inline uint32_t s_ticks(uint32_t s) { return s * SYSTICK_FREQUENCY; } +static inline uint32_t ticks_ms(uint32_t ticks) { return ticks * 1000 / SYSTICK_FREQUENCY; } + +extern volatile uint32_t tick; + +#endif diff --git a/usb.c b/usb.c index 41b2db5..1b1c62d 100644 --- a/usb.c +++ b/usb.c @@ -40,6 +40,8 @@ static char const *usb_strings[] = { [3 + ACM_NFC] = "RFID", }; +static int got_reset = 0; + usbd_device *g_usbd_dev; /* The descriptor for our device. We need only one of these. */ @@ -173,6 +175,14 @@ static void cdcacm_set_config(usbd_device *usbd_dev, uint16_t wValue) { cdcacm_control_request); } +void usb_reset_callback(void) { + got_reset = 1; +} + +bool usb_got_reset(void) { + return got_reset; +} + void usb_setup() { rcc_periph_clock_enable(RCC_GPIOA); rcc_periph_clock_enable(RCC_OTGFS); @@ -326,6 +336,7 @@ void usb_reinit() { (sizeof usb_strings) / sizeof(usb_strings[0]), usbd_control_buffer, sizeof(usbd_control_buffer)); + usbd_register_reset_callback(g_usbd_dev, &usb_reset_callback); usbd_register_set_config_callback(g_usbd_dev, cdcacm_set_config); } diff --git a/usb.h b/usb.h index aa3ae49..9170dfe 100644 --- a/usb.h +++ b/usb.h @@ -8,6 +8,7 @@ extern usbd_device *g_usbd_dev; void usb_setup(void); void usb_reinit(void); int usb_write_cdcacm(uint8_t acm, void *data, size_t len, int tries); +bool usb_got_reset(void); #define ACM_NFC 1 #define ACM_COMM 0