diff --git a/src/rp2040/modbus.c b/src/rp2040/modbus.c
index 06764b9..1d64a37 100644
--- a/src/rp2040/modbus.c
+++ b/src/rp2040/modbus.c
@@ -10,6 +10,7 @@
 #endif  // BUILD_TESTS
 
 #include "modbus.h"
+#include "hardware/irq.h"
 
 uint16_t crc16_table[256];
 
@@ -41,16 +42,44 @@ uint16_t modbus_crc16(const uint8_t *data, uint8_t size) {
   return crc16;
 }
 
+uint8_t modbus_write = 0;
+
+void modbus_send_chars(void) {
+  while(modbus_write < modbus_length && uart_is_writable(MODBUS_UART)) {
+    uart_putc(MODBUS_UART, modbus_command[modbus_write++]);
+  }
+  uart_set_irq_enables(MODBUS_UART, false, modbus_write < modbus_length);
+}
+
 void modbus_transmit(void) {
   gpio_put(MODBUS_DIR_PIN, 1);
   uint16_t crc16 = modbus_crc16((const uint8_t *)modbus_command, modbus_length);
   uart_tx_wait_blocking(MODBUS_UART);
   modbus_command[modbus_length++] = crc16 & 0xFF;
   modbus_command[modbus_length++] = crc16 >> 8;
-  uart_write_blocking(MODBUS_UART, (const uint8_t *)modbus_command, modbus_length);
+  modbus_write = 0;
+  modbus_send_chars();
   modbus_outstanding = 1;
 }
 
+int modbus_check_receive(void) {
+  vfd.cycle++;
+  // Ready to process data
+  if (modbus_pause <= 0)
+    return 1;
+  // Still sending
+  if (uart_get_hw(MODBUS_UART)->fr & UART_UARTFR_BUSY_BITS)
+    return 0;
+  // Receiving data into UART FIFO
+  gpio_put(MODBUS_DIR_PIN, 0);
+  modbus_pause--;
+  return 0;
+}
+
+void modbus_uart_tx(void) {
+  modbus_send_chars();
+}
+
 void modbus_init(void) {
   precompute_crc16();
   gpio_set_function(MODBUS_TX_PIN, GPIO_FUNC_UART);
@@ -62,6 +91,7 @@ void modbus_init(void) {
   vfd.req_freq_x10 = 0;
   memset(&vfd.stats, 0, sizeof(vfd.stats));
   modbus_cur_bitrate = 0;
+  irq_set_exclusive_handler(MODBUS_UART_IRQ, modbus_uart_tx);
 }
 
 int modbus_rtu_encode(uint8_t address, uint8_t function_code, uint16_t v1, uint16_t v2, const uint8_t *data, uint8_t size)
@@ -80,7 +110,7 @@ int modbus_rtu_encode(uint8_t address, uint8_t function_code, uint16_t v1, uint1
       size--;
     }
   }
-  modbus_pause = 200;
+  modbus_pause = vfd_config.type == MODBUS_TYPE_WEIKEN ? 100 : 50;
   return wrptr - modbus_command;
 }
 
@@ -101,11 +131,13 @@ static inline void modbus_encode_twoints(uint8_t address, uint8_t command, uint1
 void modbus_read_holding_registers(uint8_t address, uint16_t reg_to_read, uint16_t num_regs)
 {
   modbus_encode_twoints(address, 3, reg_to_read, num_regs);
+  modbus_transmit();
 }
 
 void modbus_write_holding_register(uint8_t address, uint16_t reg_to_write, uint16_t value)
 {
   modbus_encode_twoints(address, 6, reg_to_write, value);
+  modbus_transmit();
 }
 
 int modbus_check_config(void)
diff --git a/src/rp2040/modbus.h b/src/rp2040/modbus.h
index 682b311..90e9f3d 100644
--- a/src/rp2040/modbus.h
+++ b/src/rp2040/modbus.h
@@ -4,6 +4,7 @@
 #include <stdint.h>
 
 #define MODBUS_UART uart1
+#define MODBUS_UART_IRQ UART1_IRQ
 #define MODBUS_TX_PIN 8
 #define MODBUS_RX_PIN 9
 #define MODBUS_DIR_PIN 10
@@ -79,6 +80,7 @@ extern struct vfd_status vfd;
 extern uint16_t modbus_crc16(const uint8_t *data, uint8_t size);
 extern void modbus_transmit(void);
 extern int modbus_check_config(void);
+extern int modbus_check_receive(void);
 extern int modbus_get_data(void);
 
 extern void modbus_read_holding_registers(uint8_t address, uint16_t reg_to_read, uint16_t num_regs);
diff --git a/src/rp2040/modbus_fuling.c b/src/rp2040/modbus_fuling.c
index 5d4ea44..4df6e56 100644
--- a/src/rp2040/modbus_fuling.c
+++ b/src/rp2040/modbus_fuling.c
@@ -50,8 +50,18 @@
 #define FULING_DZB_CMD_MODBUS_FAULT_LOCKED 7
 #define FULING_DZB_CMD_MODBUS_FAULT_EEPROM_BUSY 8
 
-static void modbus_fuling_receive(void)
+typedef enum FulingReceived {
+  FRCV_Nothing,
+  FRCV_Status,
+  FRCV_Frequency,
+} FulingReceived_t;
+
+#define RECEIVED_NOTHING 0
+#define RECEIVED_STATUS 1
+
+static int modbus_fuling_receive(void)
 {
+  FulingReceived_t got_anything = FRCV_Nothing;
   int len = modbus_get_data();
   if (len) {
     uint16_t crc16 = modbus_crc16(modbus_command, len - 2);
@@ -70,11 +80,13 @@ static void modbus_fuling_receive(void)
           vfd.status_run = modbus_command[4] == FULING_DZB_CMD_STATE_RUN_FWD || modbus_command[4] == FULING_DZB_CMD_STATE_RUN_REV;
           vfd.status_reverse = modbus_command[4] == FULING_DZB_CMD_STATE_RUN_REV;
           vfd.status_running = vfd.status_run;
+          got_anything = FRCV_Status;
           break;
         case 4:
           vfd.last_set_freq_update = vfd.cycle;
           vfd.set_freq_x10 = modbus_command[3] * 256U + modbus_command[4];
           vfd.act_freq_x10 = modbus_command[5] * 256U + modbus_command[6];
+          got_anything = FRCV_Frequency;
           modbus_printf("Freqs %d %d (%d)\n", (int)vfd.set_freq_x10, (int)vfd.act_freq_x10, (int)(vfd.req_freq_x10));
           break;
         default:
@@ -100,58 +112,46 @@ static void modbus_fuling_receive(void)
     ++vfd.stats.unanswered;
   }
   modbus_outstanding = 0;
+  return got_anything;
 }
 
 float modbus_loop_fuling(float frequency)
 {
   if (!modbus_check_config())
     return MODBUS_RESULT_NOT_CONFIGURED;
-  int refresh_delay = 1000; // delay between polls for the same value
+  int refresh_delay = 500; // delay between polls for the same value
   int freshness_limit = 10000; // No updates after this time means that the data are stale
-  vfd.cycle++;
-  vfd.command_run = frequency != 0;
-  vfd.command_reverse = frequency < 0;
   vfd.req_freq_x10 = abs((int16_t)(frequency * 10));
-  if (modbus_pause > 0) {
-    if ((uart_get_hw(MODBUS_UART)->fr & UART_UARTFR_BUSY_BITS)) {
-      goto do_pause;
-    }
-    gpio_put(MODBUS_DIR_PIN, 0);
-    modbus_pause--;
-    goto do_pause;
-  }
-  
-  modbus_fuling_receive();
-  if (vfd.cycle - vfd.last_status_update > refresh_delay) {
-    modbus_read_holding_registers(vfd_config.address, FULING_DZB_CMD_STATE, 1);
-    modbus_transmit();
-  } else if (vfd.cycle - vfd.last_set_freq_update > refresh_delay) {
-    modbus_read_holding_registers(vfd_config.address, FULING_DZB_CMD_STATUS_SET_FREQ, 2);
-    modbus_transmit();
-  } else {
-    // This assumes top frequency of 800 Hz (4-pole motor with max RPM of 24000).
-    // Long term, it should either ask the inverter (P0.04 or 4) or get it from the configuration.
-    // 5 / 4 because of the rate range of -10000..10000 and the frequency being in 0.1 Hz
-    // units.
-    int rate = (vfd.req_freq_x10 * 5 + 2) / 4;
-    // Value received is not always exactly the value sent.
-    if (abs((rate * 4 + 2) / 5 - vfd.set_freq_x10) > 1) {
-      modbus_write_holding_register(vfd_config.address, FULING_DZB_CMD_SPEED, rate);
-      modbus_transmit();
-    } else if (vfd.command_run != vfd.status_run || vfd.command_reverse != vfd.status_reverse) {
-      if (vfd.status_run && !vfd.command_run) {
-        modbus_write_holding_register(vfd_config.address, FULING_DZB_CMD_CONTROL, FULING_DZB_CMD_CONTROL_STOP);
-      } else {
-        if (vfd.command_reverse) {
-          modbus_write_holding_register(vfd_config.address, FULING_DZB_CMD_CONTROL, FULING_DZB_CMD_CONTROL_RUN_REV);
+  if (modbus_check_receive()) {
+    modbus_fuling_receive();
+    if (vfd.cycle - vfd.last_status_update > refresh_delay) {
+      modbus_read_holding_registers(vfd_config.address, FULING_DZB_CMD_STATE, 1);
+    } else if (vfd.cycle - vfd.last_set_freq_update > refresh_delay) {
+      modbus_read_holding_registers(vfd_config.address, FULING_DZB_CMD_STATUS_SET_FREQ, 2);
+    } else {
+      vfd.command_run = frequency != 0;
+      vfd.command_reverse = frequency < 0;
+      // This assumes top frequency of 800 Hz (4-pole motor with max RPM of 24000).
+      // Long term, it should either ask the inverter (P0.04 or 4) or get it from the configuration.
+      // 5 / 4 because of the rate range of -10000..10000 and the frequency being in 0.1 Hz
+      // units.
+      int rate = (vfd.req_freq_x10 * 5 + 2) / 4;
+      // Value received is not always exactly the value sent.
+      if (abs((rate * 4 + 2) / 5 - vfd.set_freq_x10) > 1) {
+        modbus_write_holding_register(vfd_config.address, FULING_DZB_CMD_SPEED, rate);
+      } else if (vfd.command_run != vfd.status_run || vfd.command_reverse != vfd.status_reverse) {
+        if (vfd.status_run && !vfd.command_run) {
+          modbus_write_holding_register(vfd_config.address, FULING_DZB_CMD_CONTROL, FULING_DZB_CMD_CONTROL_STOP);
         } else {
-          modbus_write_holding_register(vfd_config.address, FULING_DZB_CMD_CONTROL, FULING_DZB_CMD_CONTROL_RUN_FWD);
+          if (vfd.command_reverse) {
+            modbus_write_holding_register(vfd_config.address, FULING_DZB_CMD_CONTROL, FULING_DZB_CMD_CONTROL_RUN_REV);
+          } else {
+            modbus_write_holding_register(vfd_config.address, FULING_DZB_CMD_CONTROL, FULING_DZB_CMD_CONTROL_RUN_FWD);
+          }
         }
       }
-      modbus_transmit();
     }
   }
-do_pause:
   vfd.stats.got_status = (vfd.cycle - vfd.last_status_update < freshness_limit);
   vfd.stats.got_set_frequency = (vfd.cycle - vfd.last_set_freq_update < freshness_limit);
   vfd.stats.got_act_frequency = vfd.stats.got_set_frequency; // a single update does both
diff --git a/src/rp2040/modbus_huanyang.c b/src/rp2040/modbus_huanyang.c
index 51eaa0e..bd08ce7 100644
--- a/src/rp2040/modbus_huanyang.c
+++ b/src/rp2040/modbus_huanyang.c
@@ -126,47 +126,38 @@ float modbus_loop_huanyang(float frequency) {
     return MODBUS_RESULT_NOT_CONFIGURED;
   int refresh_delay = 1000; // delay between polls for the same value
   int freshness_limit = 10000; // No updates after this time means that the data are stale
-  vfd.cycle++;
-  vfd.command_run = frequency != 0;
-  vfd.command_reverse = frequency < 0;
   vfd.req_freq_x100 = abs((int16_t)(frequency * 100));
-  if (modbus_pause > 0) {
-    if ((uart_get_hw(MODBUS_UART)->fr & UART_UARTFR_BUSY_BITS)) {
-      goto do_pause;
-    }
-    gpio_put(MODBUS_DIR_PIN, 0);
-    modbus_pause--;
-    goto do_pause;
-  }
-  
-  modbus_huanyang_receive();
-  if (vfd.cycle - vfd.last_status_update > refresh_delay) {
-    huanyang_poll_control_status();
-    modbus_transmit();
-  } else if (vfd.cycle - vfd.last_set_freq_update > refresh_delay) {
-    huanyang_read_status(0);
-    modbus_transmit();
-  } else if (vfd.cycle - vfd.last_act_freq_update > refresh_delay) {
-    huanyang_read_status(1);
-    modbus_transmit();
-  } else {
-    if (vfd.req_freq_x100 != vfd.set_freq_x100) {
-      huanyang_setfreq(vfd.req_freq_x100);
+  if (modbus_check_receive()) {
+    modbus_huanyang_receive();
+    if (vfd.cycle - vfd.last_status_update > refresh_delay) {
+      huanyang_poll_control_status();
+      modbus_transmit();
+    } else if (vfd.cycle - vfd.last_set_freq_update > refresh_delay) {
+      huanyang_read_status(0);
+      modbus_transmit();
+    } else if (vfd.cycle - vfd.last_act_freq_update > refresh_delay) {
+      huanyang_read_status(1);
       modbus_transmit();
-    } else if (vfd.command_run != vfd.status_run || vfd.command_reverse != vfd.status_reverse) {
-      if (vfd.status_run && !vfd.command_run) {
-        huanyang_stop();
-      } else {
-        if (vfd.command_reverse) {
-          huanyang_start_reverse();
+    } else {
+      vfd.command_run = frequency != 0;
+      vfd.command_reverse = frequency < 0;
+      if (vfd.req_freq_x100 != vfd.set_freq_x100) {
+        huanyang_setfreq(vfd.req_freq_x100);
+        modbus_transmit();
+      } else if (vfd.command_run != vfd.status_run || vfd.command_reverse != vfd.status_reverse) {
+        if (vfd.status_run && !vfd.command_run) {
+          huanyang_stop();
         } else {
-          huanyang_start_forward();
+          if (vfd.command_reverse) {
+            huanyang_start_reverse();
+          } else {
+            huanyang_start_forward();
+          }
         }
+        modbus_transmit();
       }
-      modbus_transmit();
     }
   }
-do_pause:
   vfd.stats.got_status = (vfd.cycle - vfd.last_status_update < freshness_limit);
   vfd.stats.got_set_frequency = (vfd.cycle - vfd.last_set_freq_update < freshness_limit);
   vfd.stats.got_act_frequency = (vfd.cycle - vfd.last_act_freq_update < freshness_limit);
diff --git a/src/rp2040/modbus_weiken.c b/src/rp2040/modbus_weiken.c
index 53d1f59..785ba99 100644
--- a/src/rp2040/modbus_weiken.c
+++ b/src/rp2040/modbus_weiken.c
@@ -100,48 +100,33 @@ float modbus_loop_weiken(float frequency)
     return MODBUS_RESULT_NOT_CONFIGURED;
   int refresh_delay = 1000; // delay between polls for the same value
   int freshness_limit = 10000; // No updates after this time means that the data are stale
-  vfd.cycle++;
-  vfd.command_run = frequency != 0;
-  vfd.command_reverse = frequency < 0;
   vfd.req_freq_x10 = abs((int16_t)(frequency * 10));
-  if (modbus_pause > 0) {
-    if ((uart_get_hw(MODBUS_UART)->fr & UART_UARTFR_BUSY_BITS)) {
-      goto do_pause;
-    }
-    gpio_put(MODBUS_DIR_PIN, 0);
-    modbus_pause--;
-    goto do_pause;
-  }
-  
-  modbus_weiken_receive();
-  if (vfd.cycle - vfd.last_status_update > refresh_delay) {
-    if (!vfd.command_run) {
-      modbus_write_holding_register(vfd_config.address, WEIKEN_CMD_CONTROL, WEIKEN_CMD_CONTROL_STOP);
-    } else {
-      if (vfd.command_reverse) {
-        modbus_write_holding_register(vfd_config.address, WEIKEN_CMD_CONTROL, WEIKEN_CMD_CONTROL_RUN_REV);
+  if (modbus_check_receive()) {
+    modbus_weiken_receive();
+    if (vfd.cycle - vfd.last_status_update > refresh_delay) {
+      if (!frequency) {
+        modbus_write_holding_register(vfd_config.address, WEIKEN_CMD_CONTROL, WEIKEN_CMD_CONTROL_STOP);
       } else {
-        modbus_write_holding_register(vfd_config.address, WEIKEN_CMD_CONTROL, WEIKEN_CMD_CONTROL_RUN_FWD);
+        if (frequency < 0) {
+          modbus_write_holding_register(vfd_config.address, WEIKEN_CMD_CONTROL, WEIKEN_CMD_CONTROL_RUN_REV);
+        } else {
+          modbus_write_holding_register(vfd_config.address, WEIKEN_CMD_CONTROL, WEIKEN_CMD_CONTROL_RUN_FWD);
+        }
+      }
+    } else if (vfd.cycle - vfd.last_set_freq_update > refresh_delay) {
+      modbus_read_holding_registers(vfd_config.address, WEIKEN_CMD_STATUS_SET_FREQ, 2);
+    } else {
+      // This assumes top frequency of 400 Hz (2-pole motor with max RPM of 24000).
+      // Scales the -4000 to 4000 range to -10000 to 10000 expected by the inverter.
+      int rate = (vfd.req_freq_x10 * 5 + 1) / 2;
+      if (rate < -10000) rate = -10000;
+      if (rate > 10000) rate = 10000;
+      // Value received is not always exactly the value sent.
+      if (abs((rate * 2 + 2) / 5 - vfd.set_freq_x10) > 1) {
+        modbus_write_holding_register(vfd_config.address, WEIKEN_CMD_SPEED, rate);
       }
-    }
-    modbus_transmit();
-  } else if (vfd.cycle - vfd.last_set_freq_update > refresh_delay) {
-    modbus_read_holding_registers(vfd_config.address, WEIKEN_CMD_STATUS_SET_FREQ, 2);
-    modbus_transmit();
-  } else {
-    // This assumes top frequency of 400 Hz (2-pole motor with max RPM of 24000).
-    // Scales the -4000 to 4000 range to -10000 to 10000 expected by the inverter.
-    int rate = (vfd.req_freq_x10 * 5 + 1) / 2;
-    if (rate < -10000) rate = -10000;
-    if (rate > 10000) rate = 10000;
-    // Value received is not always exactly the value sent.
-    if (abs((rate * 2 + 2) / 5 - vfd.set_freq_x10) > 1) {
-      modbus_write_holding_register(vfd_config.address, WEIKEN_CMD_SPEED, rate);
-      modbus_transmit();
-    } else if (vfd.command_run != vfd.status_run || vfd.command_reverse != vfd.status_reverse) {
     }
   }
-do_pause:
   vfd.stats.got_status = (vfd.cycle - vfd.last_status_update < freshness_limit);
   vfd.stats.got_set_frequency = (vfd.cycle - vfd.last_set_freq_update < freshness_limit);
   vfd.stats.got_act_frequency = vfd.stats.got_set_frequency; // a single update does both