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