float AS5600::getAngularSpeed(uint8_t mode) {
  //  return radians, RPM or degrees.
  if (mode == AS5600_MODE_RADIANS)
  {
    return _angular_speed * AS5600_RAW_TO_RADIANS;
  }
  if (mode == AS5600_MODE_RPM)
  {
    return _angular_speed * AS5600_RAW_TO_RPM;
  }
  //  default return degrees
  return _angular_speed * AS5600_RAW_TO_DEGREES;
}

int32_t AS5600::getCumulativePosition() {
  return _position;
}

void AS5600::update() {
  uint32_t now  = micros();
  int16_t angle = readAngle();
  if (_error != AS5600_OK) return;
  // update cummulative position
  //  whole rotation CW?
  //  less than half a circle
  if ((_lastPosition > 2048) && ( angle < (_lastPosition - 2048))) {
    _position = _position + 4096 - _lastPosition + angle;
  }
  //  whole rotation CCW?
  //  less than half a circle
  else if ((angle > 2048) && ( _lastPosition < (angle - 2048))) {
    _position = _position - 4096 - _lastPosition + angle;
  }
  else {
    _position = _position - _lastPosition + angle;
  }
  _lastPosition = angle;

  uint32_t deltaT  = now - _lastMeasurement;
  int      deltaA  = angle - _lastAngle;

  //  assumption is that there is no more than 180° rotation
  //  between two consecutive measurements.
  //  => at least two measurements per rotation (preferred 4).
  if (deltaA >  2048) deltaA -= 4096;
  if (deltaA < -2048) deltaA += 4096;
  _angular_speed = (deltaA * 1e6) / deltaT;

  //  remember last time & angle
  _lastMeasurement = now;
  _lastAngle       = angle;

}