diff --git a/ili9341.c b/ili9341.c
index 7417ef7..4620adc 100644
--- a/ili9341.c
+++ b/ili9341.c
@@ -467,7 +467,7 @@ void ili9341_read_memory(int x, int y, int w, int h, int len, uint16_t *out)
   dmaStreamEnable(dmarx);
   // Wait DMA completion
   dmaWaitCompletion(dmatx);
-#if 1
+#if 0
   int count = 0;
   while ((dmarx)->channel->CNDTR > 0U) {
     chThdSleepMicroseconds(100);
@@ -477,7 +477,7 @@ void ili9341_read_memory(int x, int y, int w, int h, int len, uint16_t *out)
   dmaStreamDisable(dmarx);
 #else
   dmaWaitCompletion(dmarx);
-#endif;
+#endif
   chSysUnlock();
   CS_HIGH;
 
diff --git a/sa_core.c b/sa_core.c
index d2b6107..2e5cc94 100644
--- a/sa_core.c
+++ b/sa_core.c
@@ -709,9 +709,19 @@ float perform(bool break_on_operation, int i, int32_t f, int tracking)
     chThdSleepMicroseconds(250);
   } else if (MODE_OUTPUT(setting_mode) && (setting_modulation == MO_NFM || setting_modulation == MO_WFM )) {
       SI4432_Sel = 1;
-      SI4432_Write_Byte(0x79, modulation_counter);  // Use frequency hopping channel for FM modulation
-      if (modulation_counter == 3)
-        modulation_counter = 0;
+      int offset;
+      if (setting_modulation == MO_NFM ) {
+        offset = modulation_counter ;
+        SI4432_Write_Byte(0x73, (offset & 0xff ));  // Use frequency hopping channel for FM modulation
+        SI4432_Write_Byte(0x74, ((offset >> 8) & 0x03 ));  // Use frequency hopping channel for FM modulation
+      }
+      else {
+        offset = modulation_counter * 100;
+        SI4432_Write_Byte(0x73, (offset & 0xff ));  // Use frequency hopping channel for FM modulation
+        SI4432_Write_Byte(0x74, ((offset >> 8) & 0x03 ));  // Use frequency hopping channel for FM modulation
+      }
+      if (modulation_counter == 2)
+        modulation_counter = -2;
       else
         modulation_counter++;
       chThdSleepMicroseconds(250);