diff --git a/libraries/AP_HAL_PX4/UARTDriver.cpp b/libraries/AP_HAL_PX4/UARTDriver.cpp
index 47eb2b6678..6163878633 100644
--- a/libraries/AP_HAL_PX4/UARTDriver.cpp
+++ b/libraries/AP_HAL_PX4/UARTDriver.cpp
@@ -38,6 +38,12 @@ void PX4UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
 				_devpath, strerror(errno));
 			return;
 		}
+
+        // always set it non-blocking for the low level IO
+        unsigned v;
+        v = fcntl(_fd, F_GETFL, 0);
+        fcntl(_fd, F_SETFL, v | O_NONBLOCK);
+
 		_initialised = true;
         if (rxS == 0) {
             rxS = 128;
@@ -101,14 +107,7 @@ void PX4UARTDriver::flush() {}
 bool PX4UARTDriver::is_initialized() { return true; }
 void PX4UARTDriver::set_blocking_writes(bool blocking) 
 {
-	unsigned v;
-    v = fcntl(_fd, F_GETFL, 0);
-    if (blocking) {
-        v &= ~O_NONBLOCK;
-    } else {
-        v |= O_NONBLOCK;
-    }
-    fcntl(_fd, F_SETFL, v);
+    _nonblocking_writes = !blocking;
 }
 bool PX4UARTDriver::tx_pending() { return false; }
 
@@ -218,9 +217,16 @@ int16_t PX4UARTDriver::read()
  */
 size_t PX4UARTDriver::write(uint8_t c) 
 { 
-    if (txspace() == 0) {
+    if (!_initialised) {
         return 0;
     }
+    uint16_t _head;
+    while (BUF_SPACE(_writebuf) == 0) {
+        if (_nonblocking_writes) {
+            return 0;
+        }
+        hal.scheduler->delay(1);
+    }
     _writebuf[_writebuf_tail] = c;
     BUF_ADVANCETAIL(_writebuf, 1);
     return 1;
diff --git a/libraries/AP_HAL_PX4/UARTDriver.h b/libraries/AP_HAL_PX4/UARTDriver.h
index 5f45ccdf6e..66780a1a06 100644
--- a/libraries/AP_HAL_PX4/UARTDriver.h
+++ b/libraries/AP_HAL_PX4/UARTDriver.h
@@ -48,6 +48,8 @@ private:
     int _fd;
     void _vdprintf(int fd, const char *fmt, va_list ap);
 
+    bool _nonblocking_writes;
+
     // we use in-task ring buffers to reduce the system call cost
     // of ::read() and ::write() in the main loop
     uint8_t *_readbuf;