diff --git a/ADF7021.cpp b/ADF7021.cpp
index c45a02e..c837ba7 100644
--- a/ADF7021.cpp
+++ b/ADF7021.cpp
@@ -142,6 +142,7 @@ void CIO::ifConf()
   uint32_t ADF7021_REG4  = 0;
   uint32_t ADF7021_REG10 = 0;
   uint32_t ADF7021_REG13 = 0;
+  uint32_t AFC_OFFSET = 0;
 
   // Toggle CE pin for ADF7021 reset
   CE_pin(LOW);
@@ -171,17 +172,19 @@ void CIO::ifConf()
     div2 = 1U;
   }
 
-#if defined(ADF7021_ENABLE_AFC)
-  if( div2 == 1U )
-    divider = (m_frequency_rx - 100000 + 1000) / (ADF7021_PFD / 2U);
-  else
-    divider = (m_frequency_rx - 100000 + 2000) / ADF7021_PFD;
-#else
+  if(m_dstarEnable)
+    AFC_OFFSET = AFC_OFFSET_DSTAR;
+  else if(m_dmrEnable)
+    AFC_OFFSET = AFC_OFFSET_DMR;
+  else if(m_ysfEnable)
+    AFC_OFFSET = AFC_OFFSET_YSF;
+  else if(m_p25Enable)
+    AFC_OFFSET = AFC_OFFSET_P25;
+
   if( div2 == 1U )
-    divider = (m_frequency_rx - 100000) / (ADF7021_PFD / 2U);
+    divider = (m_frequency_rx - 100000 + AFC_OFFSET) / (ADF7021_PFD / 2U);
   else
-    divider = (m_frequency_rx - 100000) / ADF7021_PFD;
-#endif
+    divider = (m_frequency_rx - 100000 + (2*AFC_OFFSET)) / ADF7021_PFD;
 
   N_divider = floor(divider);
   divider = (divider - N_divider) * 32768;
@@ -231,7 +234,7 @@ void CIO::ifConf()
     ADF7021_REG4 |= (uint32_t) 0b10                      << 8;
     ADF7021_REG4 |= (uint32_t) ADF7021_DISC_BW_DSTAR     << 10;  // Disc BW
     ADF7021_REG4 |= (uint32_t) ADF7021_POST_BW_DSTAR     << 20;  // Post dem BW
-    ADF7021_REG4 |= (uint32_t) 0b00                      << 30;  // IF filter
+    ADF7021_REG4 |= (uint32_t) 0b10                      << 30;  // IF filter
 
     ADF7021_REG13 = (uint32_t) 0b1101      << 0;   // register 13
     ADF7021_REG13 |= (uint32_t) ADF7021_SLICER_TH_DSTAR  << 4;   // slicer threshold
@@ -341,10 +344,6 @@ void CIO::ifConf()
   AD7021_control_word = 0x000E000F;
   Send_AD7021_control();
 
-  // IF FINE CAL (fine cal, defaults) (6)
-  AD7021_control_word = ADF7021_REG6;
-  Send_AD7021_control();
-
   // AGC (auto, defaults) (9)
   AD7021_control_word = 0x000231E9;
   Send_AD7021_control();
diff --git a/ADF7021.h b/ADF7021.h
index 45a3a21..00aeae2 100644
--- a/ADF7021.h
+++ b/ADF7021.h
@@ -79,15 +79,23 @@ www.analog.com/media/en/technical-documentation/data-sheets/ADF7021.pdf
 
 // AFC configuration (REG 10)
 #if defined(ADF7021_ENABLE_AFC)
-#define ADF7021_REG10_DSTAR      0x0496473A
+#define ADF7021_REG10_DSTAR      0x0C96473A
 #define ADF7021_REG10_DMR        0x049E473A
 #define ADF7021_REG10_YSF        0x049E473A
 #define ADF7021_REG10_P25        0x049E473A
+#define AFC_OFFSET_DSTAR         0
+#define AFC_OFFSET_DMR           1000
+#define AFC_OFFSET_YSF           1000
+#define AFC_OFFSET_P25           1000
 #else
 #define ADF7021_REG10_DSTAR      0x0496472A
 #define ADF7021_REG10_DMR        0x049E472A
 #define ADF7021_REG10_YSF        0x049E472A
 #define ADF7021_REG10_P25        0x049E472A
+#define AFC_OFFSET_DSTAR         0
+#define AFC_OFFSET_DMR           0
+#define AFC_OFFSET_YSF           0
+#define AFC_OFFSET_P25           0
 #endif
 
 // Slicer threshold for 4FSK demodulator (REG 13)
@@ -141,15 +149,23 @@ www.analog.com/media/en/technical-documentation/data-sheets/ADF7021.pdf
 
 // AFC (REG 10)
 #if defined(ADF7021_ENABLE_AFC)
-#define ADF7021_REG10_DSTAR      0x0496355A
+#define ADF7021_REG10_DSTAR      0x0C96355A
 #define ADF7021_REG10_DMR        0x049E355A
 #define ADF7021_REG10_YSF        0x049E355A
 #define ADF7021_REG10_P25        0x049E355A
+#define AFC_OFFSET_DSTAR         0
+#define AFC_OFFSET_DMR           1000
+#define AFC_OFFSET_YSF           1000
+#define AFC_OFFSET_P25           1000
 #else
 #define ADF7021_REG10_DSTAR      0x0496354A
 #define ADF7021_REG10_DMR        0x049E354A
 #define ADF7021_REG10_YSF        0x049E354A
 #define ADF7021_REG10_P25        0x049E354A
+#define AFC_OFFSET_DSTAR         0
+#define AFC_OFFSET_DMR           0
+#define AFC_OFFSET_YSF           0
+#define AFC_OFFSET_P25           0
 #endif
 
 // Slicer threshold for 4FSK demodulator (REG 13)
@@ -203,15 +219,23 @@ www.analog.com/media/en/technical-documentation/data-sheets/ADF7021.pdf
 
 // AFC (REG 10)
 #if defined(ADF7021_ENABLE_AFC)
-#define ADF7021_REG10_DSTAR      0x0496557A
+#define ADF7021_REG10_DSTAR      0x0C96557A
 #define ADF7021_REG10_DMR        0x049E557A
 #define ADF7021_REG10_YSF        0x049E557A
 #define ADF7021_REG10_P25        0x049E557A
+#define AFC_OFFSET_DSTAR         0
+#define AFC_OFFSET_DMR           1000
+#define AFC_OFFSET_YSF           1000
+#define AFC_OFFSET_P25           1000
 #else
 #define ADF7021_REG10_DSTAR      0x0496556A
 #define ADF7021_REG10_DMR        0x049E556A
 #define ADF7021_REG10_YSF        0x049E556A
 #define ADF7021_REG10_P25        0x049E556A
+#define AFC_OFFSET_DSTAR         0
+#define AFC_OFFSET_DMR           0
+#define AFC_OFFSET_YSF           0
+#define AFC_OFFSET_P25           0
 #endif
 
 // Slicer threshold for 4FSK demodulator (REG 13)