AP_HAL_AVR: Console more complete, but still not 100%

libraries/AP_HAL_AVR/Console.cpp

@@ -12,13 +12,14 @@ AVRConsoleDriver::AVRConsoleDriver() :
 {}
 
 // ConsoleDriver method implementations ///////////////////////////////////////
-//
 void AVRConsoleDriver::init(void* base_uart) {
     _base_uart = (AP_HAL::UARTDriver*) base_uart;
 }
 
 
 void AVRConsoleDriver::backend_open() {
+    _txbuf.allocate(128);
+    _rxbuf.allocate(16);
     _user_backend = true;
 }
 
@@ -27,22 +28,29 @@ void AVRConsoleDriver::backend_close() {
 }
 
 int AVRConsoleDriver::backend_read(uint8_t *data, int len) {
-    return 0;
+    for (int i = 0; i < len; i++) {
+        int b = _txbuf.pop();
+        if (b != -1) {
+            data[i] = (uint8_t) b;
+        } else {
+            return i;
+        }
+    }
+    return len;
 }
 
 int AVRConsoleDriver::backend_write(const uint8_t *data, int len) {
-    return 0;
-}
-// Print method implementations /////////////////////////////////////////
-
-size_t AVRConsoleDriver::write(uint8_t c) {
-    if (_user_backend) {
-        return 1;
-    } else {
-        return _base_uart->write(c);
+    for (int i = 0; i < len; i++) {
+        bool valid = _rxbuf.push(data[i]);
+        if (!valid) {
+            return i;
+        }
     }
+    return len;
 }
 
+// AVRConsoleDriver private method implementations ////////////////////////////
+
 // BetterStream method implementations /////////////////////////////////////////
 void AVRConsoleDriver::print_P(const prog_char_t *s) {
         char    c;
@@ -72,6 +80,7 @@ void AVRConsoleDriver::_printf_P(const prog_char *fmt, ...) {
 // Stream method implementations /////////////////////////////////////////
 int AVRConsoleDriver::available(void) {
     if (_user_backend) {
+        // XXX TODO
         return INT_MAX;
     } else {
         return _base_uart->available();
@@ -80,6 +89,7 @@ int AVRConsoleDriver::available(void) {
 
 int AVRConsoleDriver::txspace(void) {
     if (_user_backend) {
+        // XXX TODO
         return INT_MAX;
     } else {
         return _base_uart->txspace();
@@ -88,7 +98,7 @@ int AVRConsoleDriver::txspace(void) {
 
 int AVRConsoleDriver::read() {
     if (_user_backend) {
-        return -1;
+        return _rxbuf.pop();  
     } else {
         return _base_uart->read();
     }
@@ -96,9 +106,66 @@ int AVRConsoleDriver::read() {
 
 int AVRConsoleDriver::peek() {
     if (_user_backend) {
+        // XXX TODO
         return -1;
     } else {
         return _base_uart->peek();
     }
 }
 
+// Print method implementations /////////////////////////////////////////
+
+size_t AVRConsoleDriver::write(uint8_t c) {
+    if (_user_backend) {
+        return (_txbuf.push(c) ? 1 : 0);
+    } else {
+        return _base_uart->write(c);
+    }
+}
+
+
+/**
+ * AVRConsoleDriver::Buffer implementation.
+ * A synchronous nonblocking ring buffer, based on the AVRUARTDriver::Buffer
+ */
+
+bool AVRConsoleDriver::Buffer::allocate(int size) {
+    _head = 0;
+    _tail = 0;
+    uint8_t shift;
+    /* Hardcoded max size of 1024. sue me. */
+    for ( shift = 1;
+          ( 1 << shift ) < 1024 && ( 1 << shift) < size;
+          shift++
+        ) ;
+    uint16_t tmpmask  = (1 << shift) - 1;
+
+    if ( _bytes != NULL ) {
+        if ( _mask == tmpmask ) {
+            return true;
+        }
+        free(_bytes);
+    }
+    _mask = tmpmask;
+    _bytes = (uint8_t*) malloc(_mask+1);
+    return (_bytes != NULL);
+}
+
+bool AVRConsoleDriver::Buffer::push(uint8_t b) {
+    uint16_t next = (_head + 1) & _mask;
+    if ( next == _tail ) {
+        return false;
+    }
+    _bytes[_head] = b;
+    _head = next;
+}
+
+int AVRConsoleDriver::Buffer::pop() {
+    if ( _tail == _head ) {
+        return -1;
+    }
+    uint8_t b = _bytes[_tail];
+    _tail = ( _tail + 1 ) & _mask;
+    return (int) b;
+}
+

libraries/AP_HAL_AVR/Console.h

@@ -2,6 +2,8 @@
 #ifndef __AP_HAL_AVR_CONSOLE_DRIVER_H__
 #define __AP_HAL_AVR_CONSOLE_DRIVER_H__
 
+#include <stdlib.h>
+
 #include <AP_Common.h>
 #include <AP_HAL.h>
 #include "AP_HAL_AVR_Namespace.h"
@@ -32,6 +34,20 @@ public:
     /* Implementations of Print virtual methods */
     size_t write(uint8_t c);
 private:
+    struct Buffer {
+        /* public methods:*/
+        bool allocate(int size);
+        bool push(uint8_t b);
+        int pop();
+    private:
+        uint16_t _head, _tail; /* Head and tail indicies */
+        uint16_t _mask;       /* Buffer size mask for index wrap */
+        uint8_t *_bytes;      /* Pointer to allocated buffer */
+    };
+
+    Buffer _txbuf;
+    Buffer _rxbuf;
+
     AP_HAL::UARTDriver* _base_uart;
     bool _user_backend;
 };

libraries/AP_HAL_AVR/examples/Console/Console.pde

@@ -12,6 +12,62 @@
 
 const AP_HAL::HAL& hal = AP_HAL_AVR_APM1;
 
+
+void stream_loopback(AP_HAL::Stream* s, uint32_t time) {
+    uint32_t end = hal.scheduler->millis() + time;
+    for(;;) {
+        if (hal.scheduler->millis() >= end && time != 0) {
+            return;
+        }
+        if (s->available() > 0) {
+            int c;
+            c = s->read();
+            if (-1 != c) {
+                s->write((uint8_t)c);
+            }
+        }
+    }
+}
+
+void stream_console_loopback(AP_HAL::Stream* s, AP_HAL::ConsoleDriver* c,
+        uint32_t time) {
+    uint32_t end = hal.scheduler->millis() + time;
+    for(;;) {
+        if (hal.scheduler->millis() >= end && time != 0) {
+            return;
+        }
+
+        /* Read the uart, write to the console backend. */
+        if (s->available() > 0) {
+            int b;
+            b = s->read();
+            if (-1 != b) {
+                uint8_t tmp[1];
+                tmp[0] = (uint8_t) b;
+                c->backend_write(tmp, 1);
+            }
+        }
+        /* Loop back the console upon itself. */
+        {
+            int b;
+            b = c->read();
+            if (-1 != b) {
+                c->write((uint8_t)b);
+            }
+
+        }
+   
+        /* Read the console backend, print to the uart. */
+        { 
+            uint8_t tmp[1];
+            int readback = c->backend_read(tmp, 1);
+            if (readback > 0) {
+                s->write(tmp[0]);
+            }
+
+        }
+    }
+}
 void setup(void)
 {
         //
@@ -30,20 +86,32 @@ void setup(void)
     hal.console->println_P(PSTR("progmem"));
     hal.console->printf("printf %d %u %#x %p %f %S\n", -1000, 1000, 1000, 1000, 1.2345, PSTR("progmem"));
     hal.console->printf_P(PSTR("printf_P %d %u %#x %p %f %S\n"), -1000, 1000, 1000, 1000, 1.2345, PSTR("progmem"));
-    hal.console->println("done");
+
+    hal.console->println("loopback for 10sec:");
+    stream_loopback(hal.console, 10000);
+    hal.console->println("loopback done");
+
+    hal.console->println("opening backend:");
+    
+    hal.console->backend_open();
+
+    const char hello[] = "hello world\r\n";
+    hal.console->backend_write((const uint8_t*)hello, strlen(hello));
+
+    hal.console->println("loopback for 10sec:");
+    stream_console_loopback(hal.uart0, hal.console, 10000);
+    hal.console->println("loopback done");
+
+    hal.console->backend_close();
+    hal.console->println("closed backend.");
+
+    hal.console->println("done.");
+    for(;;);
+
 }
 
-void loop(void)
-{
-    int c;
-    //
-    // Perform a simple loopback operation.
-    //
-    c = hal.console->read();
-    if (-1 != c)
-        hal.console->write(c);
-}
 
+void loop(void){}
 
 extern "C" {
 int main (void) {