Arduino Nano code for OneShot125 at 2KHz

oneshot.c

#define DPIN_ESC_PWM    3

static struct tagTimerState {
  uint8_t cntOverflow;
  uint8_t outThrottleTicks;
} timerState;

ISR(TIMER2_OVF_vect)
{
  // A 30uS pulse indicates a telemetry request, send one every 256 overflows or 8x a second
  if (++timerState.cntOverflow == 0x00)
    OCR2B = 15;
  else
    OCR2B = timerState.outThrottleTicks;
}

static void setThrottle(uint16_t throttle, bool report = false)
{
  // Map 1000-2000 to 125-250uS given 2uS per tick
  throttle = constrain(throttle, 1000, 2000);
  uint8_t out = throttle / 16;
  if (timerState.outThrottleTicks == out)
    return;

  timerState.outThrottleTicks = out;

  if (report)
  {
    Serial.print("Throttle "); 
    Serial.print(throttle, DEC); 
    Serial.print('=');
    Serial.println(out, DEC);
  }
}

static void setupOneshot(void)
{
  // Designed to run on TIMER2 OC2B PD3 Digital3 at 2kHz
  pinMode(DPIN_ESC_PWM, OUTPUT);
  // Arduino sets up TCCR2A=0x01 and TCCR2B=0x04 which is 488.28125Hz Phase Correct PWM 
  // Change it to Fast PWM at clock/32 which will make it 1953.125Hz or 2uS per tick
  TCCR2A = bit(WGM21) | bit(WGM20);
  TCCR2B = bit(CS21) | bit(CS20);

  OCR2B = timerState.outThrottleTicks;
  TCNT2 = 0;
  TIMSK2 |= bit(TOIE2);
  TCCR2A |= bit(COM2B1); // timer enabled
}

void setup()
{
  setThrottle(1000); // min throttle
  setupOneshot();
}