A function to set Arduino Timer 5 using the FastIO macros included with Marlin Firmware

timer_fun.cpp

/**
 * Set Timer 5 PWM frequency in Hz, from 3.8Hz up to ~16MHz
 * with a minimum resolution of 100 steps.
 *
 * DC values -1.0 to 1.0. Negative duty cycle inverts the pulse.
 */
uint16_t set_pwm_frequency_hz(const float &hz, const float dca, const float dcb, const float dcc) {
  float count = 0;
  if (hz > 0 && (dca || dcb || dcc)) {
    count = float(F_CPU) / hz;            // 1x prescaler, TOP for 16MHz base freq.
    uint16_t prescaler;                   // Range of 30.5Hz (65535) 64.5KHz (>31)

         if (count >= 255. * 256.) { prescaler = 1024; SET_CS(5, PRESCALER_1024); }
    else if (count >= 255. * 64.)  { prescaler = 256;  SET_CS(5,  PRESCALER_256); }
    else if (count >= 255. * 8.)   { prescaler = 64;   SET_CS(5,   PRESCALER_64); }
    else if (count >= 255.)        { prescaler = 8;    SET_CS(5,    PRESCALER_8); }
    else                           { prescaler = 1;    SET_CS(5,    PRESCALER_1); }

    count /= float(prescaler);
    const float pwm_top = round(count);   // Get the rounded count

    ICR5 = (uint16_t)pwm_top - 1;         // Subtract 1 for TOP
    OCR5A = pwm_top * FABS(dca);          // Update and scale DCs
    OCR5B = pwm_top * FABS(dcb);
    OCR5C = pwm_top * FABS(dcc);
    _SET_COM(5, A, dca ? (dca < 0 ? COM_SET_CLEAR : COM_CLEAR_SET) : COM_NORMAL); // Set compare modes
    _SET_COM(5, B, dcb ? (dcb < 0 ? COM_SET_CLEAR : COM_CLEAR_SET) : COM_NORMAL);
    _SET_COM(5, C, dcc ? (dcc < 0 ? COM_SET_CLEAR : COM_CLEAR_SET) : COM_NORMAL);

    SET_WGM(5, FAST_PWM_ICRn);            // Fast PWM with ICR5 as TOP

    //SERIAL_ECHOLNPGM("Timer 5 Settings:");
    //SERIAL_ECHOLNPAIR("  Prescaler=", prescaler);
    //SERIAL_ECHOLNPAIR("        TOP=", ICR5);
    //SERIAL_ECHOLNPAIR("      OCR5A=", OCR5A);
    //SERIAL_ECHOLNPAIR("      OCR5B=", OCR5B);
    //SERIAL_ECHOLNPAIR("      OCR5C=", OCR5C);
  }
  else {
    // Restore the default for Timer 5
    SET_WGM(5, PWM_PC_8);                 // PWM 8-bit (Phase Correct)
    SET_COMS(5, NORMAL, NORMAL, NORMAL);  // Do nothing
    SET_CS(5, PRESCALER_64);              // 16MHz / 64 = 250KHz
    OCR5A = OCR5B = OCR5C = 0;
  }
  return round(count);
}