motor.c

static void
motor_timer_cb(TimerHandle_t timer) {
  struct rpm_state rotations_left;
  xQueuePeek(motor_rpm_q_handle_left, &rotations_left, (TickType_t)1);
  struct rpm_state rotations_right;
  xQueuePeek(motor_rpm_q_handle_right, &rotations_right, (TickType_t)1);

  if (rotations_right.avg > 0 && rotations_right.cur > 0) {
    rotations_right.avg = (rotations_right.avg + rotations_right.cur) / 2;
  }
  else {
    rotations_right.avg = rotations_right.cur > 0 ? rotations_right.cur : 1;
  }

  if (rotations_left.avg > 0 && rotations_left.cur > 0) {
    rotations_left.avg = (rotations_left.avg + rotations_left.cur) / 2;
  }
  else {
    rotations_left.avg = rotations_left.cur > 0 ? rotations_left.cur : 1;
  }

  rotations_left.cur = 0;
  rotations_right.cur = 0;

  xQueueOverwrite(motor_rpm_q_handle_left, &rotations_left);
  xQueueOverwrite(motor_rpm_q_handle_right, &rotations_right);
}

static void
motor_detect(uint gpio, uint32_t events) {
  struct rpm_state rotations_right = {.avg = 0.0, .cur = 0};
  struct rpm_state rotations_left = {.avg = 0.0, .cur = 0};
  gpio_acknowledge_irq(gpio, events);
  xQueuePeekFromISR(motor_rpm_q_handle_right, &rotations_right);
  xQueuePeekFromISR(motor_rpm_q_handle_left, &rotations_left);
  if (GPIO_RISE(events) || GPIO_FALL(events)) {
    if (gpio == 16) {
      rotations_right.cur++;
    }
    if (gpio == 2) {
      rotations_left.cur++;
    }
  }

  xQueueOverwriteFromISR(motor_rpm_q_handle_right, &rotations_right, pdFALSE);
  xQueueOverwriteFromISR(motor_rpm_q_handle_left, &rotations_left, pdFALSE);
}