private void SetPwmFreq(int freq_hz)
{
_frequency = freq_hz;
var prescaleval = 25000000.0; // 25MHz
prescaleval /= 4096.0; // 12-bit
prescaleval /= ((double)freq_hz * 0.9); //Compensation: https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library/issues/11
prescaleval -= 1.0;
var prescale = (byte)Math.Floor(prescaleval + 0.5);
var oldmode = _device.ReadAddressByte(MODE1);
var newmode = (oldmode & 0x7F) | 0x10; // sleep
_device.WriteAddressWord(MODE1, (byte)newmode); // go to sleep
_device.WriteAddressWord(PRESCALE, prescale);
_device.WriteAddressWord(MODE1, oldmode);
Task.Delay(5).Wait(); // wait for oscillator
_device.WriteAddressByte(MODE1, (byte)(oldmode | 0x80));
}
