Bootloader, speed calculation, better position detection

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
+BOOTLOADER ?= no
-#FP_FLAGS   ?= -mfloat-abi=hard -mfpu=fpv4-sp-d16
+
-#ARCH_FLAGS  = -mthumb -mcpu=cortex-m4 $(FP_FLAGS)
+ifeq ($(BOOTLOADER),yes)
-#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
+  LIBNAME     = opencm3_stm32f4
-#LDSCRIPT = linker.ld
+  FP_FLAGS   ?= -mfloat-abi=hard -mfpu=fpv4-sp-d16
-DEVICE = STM32F405RG
+  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
 

encoder.c

@@ -2,14 +2,42 @@
 #include <libopencm3/stm32/exti.h>
 #include <libopencm3/stm32/gpio.h>
 #include <libopencm3/stm32/rcc.h>
-#include <stdint.h>
 #include <stddef.h>
+#include <stdint.h>
+#include <stdio.h>
 #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_select_source(PIN_OPEN, PIN_PORT);
-        exti_set_trigger(PIN_OPEN | PIN_CLOSE, EXTI_TRIGGER_BOTH);
+        exti_select_source(PIN_CLOSE, PIN_PORT);
-        exti_enable_request(PIN_OPEN | PIN_CLOSE);
+        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;
+  static int prev = 0;
-        int now = GPIO_IDR(PIN_PORT);
+  int now = GPIO_IDR(PIN_PORT);
-        now = (!!(now & PIN_OPEN))<<0 | (!!(now & PIN_CLOSE)) << 1;
+  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)) {
+  switch (WAS(prev) + IS(now)) {
-        case WAS(NEITHER)    + IS(JUST_CLOSE):
+  case WAS(NEITHER)    + IS(JUST_CLOSE):
-        case WAS(JUST_OPEN)  + IS(NEITHER):
+  case WAS(JUST_OPEN)  + IS(NEITHER):
-        case WAS(JUST_CLOSE) + IS(BOTH):
+  case WAS(JUST_CLOSE) + IS(BOTH):
-        case WAS(BOTH)       + IS(JUST_OPEN):
+  case WAS(BOTH)       + IS(JUST_OPEN):
-                pos--;
+    diff = -1;
-                break;
+    break;
-        case WAS(NEITHER)    + IS(JUST_OPEN):
+  case WAS(NEITHER)    + IS(JUST_OPEN):
-        case WAS(JUST_OPEN)  + IS(BOTH):
+  case WAS(JUST_OPEN)  + IS(BOTH):
-        case WAS(BOTH)       + IS(JUST_CLOSE):
+  case WAS(BOTH)       + IS(JUST_CLOSE):
-        case WAS(JUST_CLOSE) + IS(NEITHER):
+  case WAS(JUST_CLOSE) + IS(NEITHER):
-                pos++;
+    diff = +1;
-                break;
+    break;
-        default:
+  default:
-                prev = now;
+    prev = now;
-                return;
+    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)
+  prev = now;
-                pos = MIN_POS;
-        if (pos > MAX_POS)
-                pos = MAX_POS;
 }
 
 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();
+}

encoder.h

@@ -8,4 +8,6 @@
 
 void encoder_setup(void);
 int encoder_get(void);
+int encoder_current_speed(void);
+int encoder_speed(void);
 #endif

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("pos: %d %d", encoder_get(), encoder_current_speed());
-          printf("tick: %lu", tick);
           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
+#endif
-          fast_reset();
-          break;
       }
     }
 
     int pos = encoder_get();
-    if (pos != last_pos && ringbuffer_empty(comm_out_buf)) {
+    if (pos != last_pos) {
-      if (print_changes) {
+      if (ringbuffer_empty(comm_out_buf)) {
-        printf("pos: %d", pos);
+        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");
   }
 }
 

systick.h

@@ -0,0 +1,13 @@
+#ifndef SYSTICK_H
+#define SYSTICK_H
+#include <stdint.h>
+#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

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);
 }
 

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