float rfm_freq_trim = 0.068f;


uint8_t freqbuf[3];
long _freq;
void rfm69_set_frequency(float freqMHz) {
    _freq = (long)(((freqMHz + rfm_freq_trim) * 1000000) / 61.04f); // 32MHz / 2^19 = 61.04 Hz
    freqbuf[0] = (_freq >> 16) & 0xff;
    freqbuf[1] = (_freq >> 8) & 0xff;
    freqbuf[2] = _freq & 0xff;
    
#ifdef HAVE_HWSERIAL0
    Serial.print(F("Setting frequency to: "));
    Serial.print(freqMHz, DEC);
    Serial.print(F("MHz = 0x"));
    Serial.println(_freq, HEX);
    Serial.flush();
    
    _rf69_burst_write(RFM69_REG_07_FRF_MSB, freqbuf, 3);
    
    _rf69_burst_read(RFM69_REG_07_FRF_MSB, freqbuf, 3);
    _freq = (freqbuf[0] << 16) | (freqbuf[1] << 8) | freqbuf[2];
    
    Serial.print(F("Frequency was set to: "));
    Serial.print((float)_freq / 1000000 * 61.04f, DEC);
    Serial.print(F("MHz = 0x"));
    Serial.println(_freq, HEX);
    Serial.flush();
#endif
}