[FSK4] Added correction method

examples/FSK4/FSK4_Transmit/FSK4_Transmit.ino

@@ -99,6 +99,23 @@ void setup() {
     Serial.println(state);
     while(true);
   }
+
+  // sometimes, it may be needed to set some manual corrections
+  // this can be done for tone frequencies,
+  // as well as tone lengths
+  /*
+    // set frequency shift offsets to -120, 60, 0 and 60 Hz and decrease tone length to 95%
+    int offsets[4] = { -120, -60, 0, 60 };
+    Serial.print(F("[FSK4] Setting corrections ... "));
+    state = fsk4.setCorrection(offsets, 0.95);
+    if(state == RADIOLIB_ERR_NONE) {
+      Serial.println(F("success!"));
+    } else {
+      Serial.print(F("failed, code "));
+      Serial.println(state);
+      while(true);
+    }
+  */
 }
 
 void loop() {

examples/FSK4/FSK4_Transmit_AFSK/FSK4_Transmit_AFSK.ino

@@ -1,7 +1,7 @@
 /*
    RadioLib FSK4 Transmit AFSK Example
 
-   This example sends an example FSK-4 'Horus Binary' message 
+   This example sends an example FSK-4 'Horus Binary' message
    using SX1278's FSK modem. The data is modulated as AFSK.
 
    This signal can be demodulated using an FM demodulator (SDR or otherwise),
@@ -92,6 +92,23 @@ void setup() {
     Serial.println(state);
     while(true);
   }
+
+  // sometimes, it may be needed to set some manual corrections
+  // this can be done for tone frequencies,
+  // as well as tone lengths
+  /*
+    // set audio tone offsets to -10, 20, 0 and 5 Hz and decrease tone length to 95%
+    int offsets[4] = { -10, 20, 0, 5 };
+    Serial.print(F("[FSK4] Setting corrections ... "));
+    state = fsk4.setCorrection(offsets, 0.95);
+    if(state == RADIOLIB_ERR_NONE) {
+      Serial.println(F("success!"));
+    } else {
+      Serial.print(F("failed, code "));
+      Serial.println(state);
+      while(true);
+    }
+  */
 }
 
 void loop() {

src/protocols/FSK4/FSK4.cpp

@@ -23,31 +23,14 @@ int16_t FSK4Client::begin(float base, uint32_t shift, uint16_t rate) {
   // calculate duration of 1 bit
   _bitDuration = (uint32_t)1000000/rate;
 
-  // calculate module carrier frequency resolution
-  uint32_t step = round(_phy->getFreqStep());
-
-  // check minimum shift value
-  if(shift < step / 2) {
-    return(RADIOLIB_ERR_INVALID_RTTY_SHIFT);
-  }
-
-  // round shift to multiples of frequency step size
-  if(shift % step < (step / 2)) {
-    _shift = shift / step;
-  } else {
-    _shift = (shift / step) + 1;
-  }
+  // calculate carrier shift
+  _shift = getRawShift(shift);
 
   // Write resultant tones into arrays for quick lookup when modulating.
-  _tones[0] = 0;
-  _tones[1] = _shift;
-  _tones[2] = _shift*2;
-  _tones[3] = _shift*3;
-
-  _tonesHz[0] = 0;
-  _tonesHz[1] = _shiftHz;
-  _tonesHz[2] = _shiftHz*2;
-  _tonesHz[3] = _shiftHz*3;
+  for(uint8_t i = 0; i < 4; i++) {
+    _tones[i] = _shift*i;
+    _tonesHz[i] = _shiftHz*i;
+  }
 
   // calculate 24-bit frequency
   _base = (base * 1000000.0) / _phy->getFreqStep();
@@ -61,6 +44,15 @@ void FSK4Client::idle() {
   tone(0);
 }
 
+int16_t FSK4Client::setCorrection(int16_t offsets[], float length) {
+  for(uint8_t i = 0; i < 4; i++) {
+    _tones[i] += getRawShift(offsets[i]);
+    _tonesHz[i] += offsets[i];
+  }
+  _bitDuration *= length;
+  return(RADIOLIB_ERR_NONE);
+}
+
 size_t FSK4Client::write(uint8_t* buff, size_t len) {
   size_t n = 0;
   for(size_t i = 0; i < len; i++) {
@@ -113,4 +105,23 @@ int16_t FSK4Client::standby() {
   return(_phy->standby());
 }
 
+int32_t FSK4Client::getRawShift(int32_t shift) {
+  // calculate module carrier frequency resolution
+  int32_t step = round(_phy->getFreqStep());
+
+  // check minimum shift value
+  if(abs(shift) < step / 2) {
+    return(0);
+  }
+
+  // round shift to multiples of frequency step size
+  if(abs(shift) % step < (step / 2)) {
+    return(shift / step);
+  }
+  if(shift < 0) {
+    return((shift / step) - 1);
+  }
+  return((shift / step) + 1);
+}
+
 #endif

src/protocols/FSK4/FSK4.h

@@ -42,7 +42,6 @@ class FSK4Client {
 
       \param rate Baud rate to be used during transmission.
 
-
       \returns \ref status_codes
     */
     int16_t begin(float base, uint32_t shift, uint16_t rate);
@@ -52,9 +51,43 @@ class FSK4Client {
     */
     void idle();
 
+    /*!
+      \brief Set correction coefficients for frequencies and tone length.
+
+      \param offsets Four positive or negative correction offsets for audio frequencies in Hz.
+
+      \param length Tone length modifier, defaults to 1.0.
+
+      \returns \ref status_codes
+    */
+    int16_t setCorrection(int16_t offsets[4], float length = 1.0f);
+
+    /*!
+      \brief Transmit binary data.
+
+      \param buff Buffer to transmit.
+
+      \param len Number of bytes to transmit.
+
+      \returns Number of transmitted bytes.
+    */
     size_t write(uint8_t* buff, size_t len);
+
+    /*!
+      \brief Transmit a single byte.
+
+      \param b Byte to transmit.
+
+      \returns Number of transmitted bytes.
+    */
     size_t write(uint8_t b);
 
+    /*!
+      \brief Stop transmitting.
+
+      \returns \ref status_codes
+    */
+    int16_t standby();
 
 #if !defined(RADIOLIB_GODMODE)
   private:
@@ -73,7 +106,7 @@ class FSK4Client {
     void tone(uint8_t i);
 
     int16_t transmitDirect(uint32_t freq = 0, uint32_t freqHz = 0);
-    int16_t standby();
+    int32_t getRawShift(int32_t shift);
 };
 
 #endif