HAL_AVR: implement an efficient buffer write() call for AVR

this saves some cycles in the telemetry code

libraries/AP_HAL_AVR/Console.cpp

@@ -48,5 +48,9 @@ size_t AVRConsoleDriver::write(uint8_t c) {
     return _base_uart->write(c);
 }
 
+size_t AVRConsoleDriver::write(const uint8_t *buffer, size_t size) {
+    return _base_uart->write(buffer, size);
+}
+
 #endif // CONFIG_HAL_BOARD
 

libraries/AP_HAL_AVR/Console.h

@@ -22,6 +22,7 @@ public:
 
     /* Implementations of Print virtual methods */
     size_t write(uint8_t c);
+    size_t write(const uint8_t *buffer, size_t size);
 
 private:
     AP_HAL::UARTDriver* _base_uart;

libraries/AP_HAL_AVR/UARTDriver.cpp

@@ -206,6 +206,61 @@ size_t AVRUARTDriver::write(uint8_t c) {
 	return 1;
 }
 
+/*
+  write size bytes to the write buffer
+ */
+size_t AVRUARTDriver::write(const uint8_t *buffer, size_t size)
+{
+    if (!_open) {
+        return 0;
+    }
+
+    if (!_nonblocking_writes) {
+        /*
+          use the per-byte delay loop in write() above for blocking writes
+         */
+        size_t ret = 0;
+        while (size--) {
+            if (write(*buffer++) != 1) break;
+            ret++;
+        }
+        return ret;
+    }
+
+    int16_t space = txspace();
+    if (space <= 0) {
+        return 0;
+    }
+    if (size > (size_t)space) {
+        // throw away remainder if too much data
+        size = space;
+    }
+    if (_txBuffer->tail > _txBuffer->head) {
+        // perform as single memcpy
+        memcpy(&_txBuffer->bytes[_txBuffer->head], buffer, size);
+        _txBuffer->head = (_txBuffer->head + size) & _txBuffer->mask;
+        // enable the data-ready interrupt, as it may be off if the buffer is empty
+        *_ucsrb |= _portTxBits;
+        return size;
+    }
+
+    // perform as two memcpy calls
+    uint16_t n = (_txBuffer->mask+1) - _txBuffer->head;
+    if (n > size) n = size;
+    memcpy(&_txBuffer->bytes[_txBuffer->head], buffer, n);
+    _txBuffer->head = (_txBuffer->head + n) & _txBuffer->mask;
+    buffer += n;
+    n = size - n;
+    if (n > 0) {
+        memcpy(&_txBuffer->bytes[0], buffer, n);
+        _txBuffer->head = (_txBuffer->head + n) & _txBuffer->mask;
+    }        
+
+    // enable the data-ready interrupt, as it may be off if the buffer is empty
+    *_ucsrb |= _portTxBits;
+    return size;
+}
+
 // Buffer management ///////////////////////////////////////////////////////////
     
 

libraries/AP_HAL_AVR/UARTDriver.h

@@ -47,6 +47,7 @@ public:
 
     /* Implementations of Print virtual methods */
     size_t write(uint8_t c);
+    size_t write(const uint8_t *buffer, size_t size);
 
 	/// Transmit/receive buffer descriptor.
 	///