void Encoder::UpdateRotorAngle(int dir)
{
   if (encMode == SINGLE)
   {
      int16_t numPulses = GetPulseTimeFiltered();

      angle += (int16_t)(dir * numPulses * anglePerPulse);
   }
   else if (encMode == RESOLVER)
   {
      GetAngleResolver();
   }
   else
   {
      static uint16_t lastAngle = 0;
      uint16_t angleDiff;

      if (encMode == SPI)
      {
         angle = GetAngleSPI(); //Gets 12-bit
         angle <<= 4;
         uint16_t diffPos = angle - lastAngle;
         uint16_t diffNeg = lastAngle - angle;
         angleDiff = MIN(diffNeg, diffPos);
      }
      else
      {
         uint32_t cntVal = timer_get_counter(REV_CNT_TIMER);
         cntVal *= TWO_PI;
         cntVal /= pulsesPerTurn * 4;
         angle = (uint16_t)cntVal;
         angleDiff = (angle - lastAngle) & 0xFFFF;
         if ((TIM_CR1(REV_CNT_TIMER) & TIM_CR1_DIR_DOWN))
            angleDiff = (lastAngle - angle) & 0xFFFF;
      }

      lastAngle = angle;
      turnsSinceLastSample += angleDiff;
      //Param::SetDig(Param::tm_meas, angle);
   }
}
void Encoder::Update(int16_t slipAngle)
{
   uint32_t numPulses = GetPulseTimeFiltered();

   angle += (int16_t)(numPulses * angle_per_pulse) + slipAngle;
}