AP_HAL_AVR UARTDriver does basic hello world printing

* will import more complex tests next

libraries/AP_HAL/UARTDriver.h

@@ -12,6 +12,26 @@ class AP_HAL::UARTDriver : public AP_HAL::BetterStream {
 public:
     UARTDriver() {}
     virtual void begin(long baud) = 0;
+	/// Extended port open method
+	///
+	/// Allows for both opening with specified buffer sizes, and re-opening
+	/// to adjust a subset of the port's settings.
+	///
+	/// @note	Buffer sizes greater than ::_max_buffer_size will be rounded
+	///			down.
+	///
+	/// @param	baud		Selects the speed that the port will be
+	///						configured to.  If zero, the port speed is left
+	///						unchanged.
+	/// @param rxSpace		Sets the receive buffer size for the port.  If zero
+	///						then the buffer size is left unchanged if the port
+	///						is open, or set to ::_default_rx_buffer_size if it is
+	///						currently closed.
+	/// @param txSpace		Sets the transmit buffer size for the port.  If zero
+	///						then the buffer size is left unchanged if the port
+	///						is open, or set to ::_default_tx_buffer_size if it
+	///						is currently closed.
+	///
     virtual void begin(long baud,
                     unsigned int rxSpace,
                     unsigned int txSpace) = 0;

libraries/AP_HAL_AVR/UARTDriver.cpp

@@ -8,13 +8,12 @@
 // your option) any later version.
 //
 
-//
-// Enhancements to the Arduino Stream class.
-//
-
 #include <limits.h>
-#include <AP_Common.h>
+#include <stdlib.h>
+
 #include <avr/pgmspace.h>
+
+#include <AP_Common.h>
 #include <AP_HAL.h>
 
 #include "UARTDriver.h"
@@ -42,36 +41,239 @@ AVRUARTDriver::AVRUARTDriver(
 	_initialized = true;
 	begin(57600);
 }
-/* */
+/* UARTDriver method implementations */
+
+void AVRUARTDriver::begin(long baud, unsigned int rxSpace, unsigned int txSpace) {
+	uint16_t ubrr;
+	bool use_u2x = true;
+	bool need_allocate = true;
+
+	// if we are currently open...
+	if (_open) {
+		// If the caller wants to preserve the buffer sizing, work out what
+		// it currently is...
+		if (0 == rxSpace)
+			rxSpace = _rxBuffer->mask + 1;
+		if (0 == txSpace)
+			txSpace = _txBuffer->mask + 1;
+
+		if (rxSpace == (_rxBuffer->mask + 1) && 
+			txSpace == (_txBuffer->mask + 1)) {
+			// avoid re-allocating the buffers if possible
+			need_allocate = false;
+			*_ucsrb &= ~(_portEnableBits | _portTxBits);
+		} else {
+			// close the port in its current configuration, clears _open
+			end();
+		}
+	}
+
+	if (need_allocate) {
+		// allocate buffers
+		if (!_allocBuffer(_rxBuffer, rxSpace ? : _default_rx_buffer_size) || !_allocBuffer(_txBuffer, txSpace ?	: _default_tx_buffer_size)) {
+			end();
+			return; // couldn't allocate buffers - fatal
+		}
+	}
+
+	// reset buffer pointers
+	_txBuffer->head = _txBuffer->tail = 0;
+	_rxBuffer->head = _rxBuffer->tail = 0;
+
+	// mark the port as open
+	_open = true;
+
+	// If the user has supplied a new baud rate, compute the new UBRR value.
+	if (baud > 0) {
+#if F_CPU == 16000000UL
+		// hardcoded exception for compatibility with the bootloader shipped
+		// with the Duemilanove and previous boards and the firmware on the 8U2
+		// on the Uno and Mega 2560.
+		if (baud == 57600)
+			use_u2x = false;
+#endif
+
+		if (use_u2x) {
+			*_ucsra = 1 << _u2x;
+			ubrr = (F_CPU / 4 / baud - 1) / 2;
+		} else {
+			*_ucsra = 0;
+			ubrr = (F_CPU / 8 / baud - 1) / 2;
+		}
+
+		*_ubrrh = ubrr >> 8;
+		*_ubrrl = ubrr;
+	}
+
+	*_ucsrb |= _portEnableBits;
+
+}
+
+void AVRUARTDriver::end() {
+	*_ucsrb &= ~(_portEnableBits | _portTxBits);
+
+	_freeBuffer(_rxBuffer);
+	_freeBuffer(_txBuffer);
+	_open = false;
+}
+
+int AVRUARTDriver::available(void) {
+	if (!_open)
+		return (-1);
+	return ((_rxBuffer->head - _rxBuffer->tail) & _rxBuffer->mask);
+}
+
+
+
+int AVRUARTDriver::txspace(void) {
+	if (!_open)
+		return (-1);
+	return ((_txBuffer->mask+1) - ((_txBuffer->head - _txBuffer->tail) & _txBuffer->mask));
+}
+
+int AVRUARTDriver::read(void) {
+	uint8_t c;
+
+	// if the head and tail are equal, the buffer is empty
+	if (!_open || (_rxBuffer->head == _rxBuffer->tail))
+		return (-1);
+
+	// pull character from tail
+	c = _rxBuffer->bytes[_rxBuffer->tail];
+	_rxBuffer->tail = (_rxBuffer->tail + 1) & _rxBuffer->mask;
+
+	return (c);
+}
+
+int AVRUARTDriver::peek(void) {
+
+	// if the head and tail are equal, the buffer is empty
+	if (!_open || (_rxBuffer->head == _rxBuffer->tail))
+		return (-1);
 
-/* BetterStream implementations */
-void AVRUARTDriver::print_P(const prog_char_t *s)
+	// pull character from tail
+	return (_rxBuffer->bytes[_rxBuffer->tail]);
+}
+
+void AVRUARTDriver::flush(void) {
+	// don't reverse this or there may be problems if the RX interrupt
+	// occurs after reading the value of _rxBuffer->head but before writing
+	// the value to _rxBuffer->tail; the previous value of head
+	// may be written to tail, making it appear as if the buffer
+	// don't reverse this or there may be problems if the RX interrupt
+	// occurs after reading the value of head but before writing
+	// the value to tail; the previous value of rx_buffer_head
+	// may be written to tail, making it appear as if the buffer
+	// were full, not empty.
+	_rxBuffer->head = _rxBuffer->tail;
+
+	// don't reverse this or there may be problems if the TX interrupt
+	// occurs after reading the value of _txBuffer->tail but before writing
+	// the value to _txBuffer->head.
+	_txBuffer->tail = _txBuffer->head;
+}
+
+size_t AVRUARTDriver::write(uint8_t c) {
+	uint16_t i;
+
+	if (!_open) // drop bytes if not open
+		return 0;
+
+	// wait for room in the tx buffer
+	i = (_txBuffer->head + 1) & _txBuffer->mask;
+
+	// if the port is set into non-blocking mode, then drop the byte
+	// if there isn't enough room for it in the transmit buffer
+	if (_nonblocking_writes && i == _txBuffer->tail) {
+		return 0;
+	}
+
+	while (i == _txBuffer->tail)
+		;
+
+	// add byte to the buffer
+	_txBuffer->bytes[_txBuffer->head] = c;
+	_txBuffer->head = i;
+
+	// enable the data-ready interrupt, as it may be off if the buffer is empty
+	*_ucsrb |= _portTxBits;
+
+	// return number of bytes written (always 1)
+	return 1;
+}
+
+// Buffer management ///////////////////////////////////////////////////////////
+static inline unsigned int min (unsigned int a, unsigned int b)  {
+    return (a>b)?b:a;
+}
+
+bool AVRUARTDriver::_allocBuffer(Buffer *buffer, unsigned int size)
 {
-        char    c;
+	uint16_t	mask;
+	uint8_t		shift;
 
+	// init buffer state
+	buffer->head = buffer->tail = 0;
+
+	// Compute the power of 2 greater or equal to the requested buffer size
+	// and then a mask to simplify wrapping operations.  Using __builtin_clz
+	// would seem to make sense, but it uses a 256(!) byte table.
+	// Note that we ignore requests for more than BUFFER_MAX space.
+	for (shift = 1; (1U << shift) < min(_max_buffer_size, size); shift++)
+		;
+	mask = (1 << shift) - 1;
+
+	// If the descriptor already has a buffer allocated we need to take
+	// care of it.
+	if (buffer->bytes) {
+
+		// If the allocated buffer is already the correct size then
+		// we have nothing to do
+		if (buffer->mask == mask)
+			return true;
+
+		// Dispose of the old buffer.
+		free(buffer->bytes);
+	}
+	buffer->mask = mask;
+
+	// allocate memory for the buffer - if this fails, we fail.
+	buffer->bytes = (uint8_t *) malloc(buffer->mask + 1);
+
+	return (buffer->bytes != NULL);
+}
+
+void AVRUARTDriver::_freeBuffer(Buffer *buffer)
+{
+	buffer->head = buffer->tail = 0;
+	buffer->mask = 0;
+	if (NULL != buffer->bytes) {
+		free(buffer->bytes);
+		buffer->bytes = NULL;
+	}
+}
+
+// BetterStream method implementations /////////////////////////////////////////
+void AVRUARTDriver::print_P(const prog_char_t *s) {
+        char    c;
         while ('\0' != (c = pgm_read_byte((const prog_char *)s++)))
                 write(c);
 }
 
-void AVRUARTDriver::println_P(const prog_char_t *s)
-{
+void AVRUARTDriver::println_P(const prog_char_t *s) {
         print_P(s);
         println();
 }
 
-void AVRUARTDriver::printf(const char *fmt, ...)
-{
+void AVRUARTDriver::printf(const char *fmt, ...) {
         va_list ap;
-
         va_start(ap, fmt);
         _vprintf(0, fmt, ap);
         va_end(ap);
 }
 
-void AVRUARTDriver::_printf_P(const prog_char *fmt, ...)
-{
+void AVRUARTDriver::_printf_P(const prog_char *fmt, ...) {
         va_list ap;
-
         va_start(ap, fmt);
         _vprintf(1, fmt, ap);
         va_end(ap);

libraries/AP_HAL_AVR/UARTDriver.h

@@ -25,36 +25,22 @@ public:
 		const uint8_t portEnableBits, const uint8_t portTxBits);
 
     /* Implementations of UARTDriver virtual methods */
-    void begin(long b) {}
-	/// Extended port open method
-	///
-	/// Allows for both opening with specified buffer sizes, and re-opening
-	/// to adjust a subset of the port's settings.
-	///
-	/// @note	Buffer sizes greater than ::_max_buffer_size will be rounded
-	///			down.
-	///
-	/// @param	baud		Selects the speed that the port will be
-	///						configured to.  If zero, the port speed is left
-	///						unchanged.
-	/// @param rxSpace		Sets the receive buffer size for the port.  If zero
-	///						then the buffer size is left unchanged if the port
-	///						is open, or set to ::_default_rx_buffer_size if it is
-	///						currently closed.
-	/// @param txSpace		Sets the transmit buffer size for the port.  If zero
-	///						then the buffer size is left unchanged if the port
-	///						is open, or set to ::_default_tx_buffer_size if it
-	///						is currently closed.
-	///
-    void begin(long b, unsigned int rxS, unsigned int txS) {}
-    void end() {}
-    void flush() {}
+    void begin(long b) { begin(b, 0, 0); }
+    void begin(long b, unsigned int rxS, unsigned int txS);
+    void end();
+    void flush();
     bool is_initialized() { return _initialized; }
-    void set_blocking_writes(bool blocking) {}
-    bool tx_pending() { return false; }
+
+	void set_blocking_writes(bool blocking) {
+		_nonblocking_writes = !blocking;
+	}
+
+    bool tx_pending() {
+        return (_txBuffer->head != _txBuffer->tail);
+    }
 
     /* Implementations of BetterStream virtual methods */
-    int txspace() { return 10; }
+    int txspace();
     void print_P(const prog_char_t *s);
     void println_P(const prog_char_t *s);
     void printf(const char *s, ...)
@@ -63,12 +49,12 @@ public:
             __attribute__ ((format(__printf__, 2, 3)));
 
     /* Implementations of Stream virtual methods */
-    int available() { return 0; }
-    int read() { return -1; }
-    int peek() { return -1; }
+    int available();
+    int read();
+    int peek();
 
     /* Implementations of Print virtual methods */
-    size_t write(uint8_t c) { return 0; }
+    size_t write(uint8_t c);
 
 	/// Transmit/receive buffer descriptor.
 	///

libraries/AP_HAL_AVR/examples/APM1/APM1.pde

@@ -4,21 +4,39 @@
 #include <AP_HAL_AVR.h>
 
 const AP_HAL::HAL& hal = AP_HAL_AVR_APM1;
-const AP_HAL::HAL& hal2 = AP_HAL_AVR_APM2;
 
 void loop (void) {
-
+    hal.uart0->println(".");
+    hal.gpio->write(3, 0);
+    hal.scheduler->delay(1000);
+    hal.gpio->write(3, 1);
 }
 
 void setup (void) {
-    hal.uart0->begin(9600);
-    hal2.uart0->begin(9600);
-    hal.uart1->begin(9600);
-    hal2.uart1->begin(9600);
-    hal.uart2->begin(9600);
-    hal2.uart2->begin(9600);
-    hal.uart3->begin(9600);
-    hal2.uart3->begin(9600);
+    hal.gpio->pinMode(1, GPIO_OUTPUT);
+    hal.gpio->pinMode(2, GPIO_OUTPUT);
+    hal.gpio->pinMode(3, GPIO_OUTPUT);
+    hal.gpio->pinMode(13, GPIO_OUTPUT);
+
+    hal.gpio->write(1, 1);
+    hal.gpio->write(2, 1);
+    hal.gpio->write(3, 1);
+    hal.gpio->write(13, 1);
+    
+    hal.scheduler->delay(1000);
+
+    hal.gpio->write(1, 0);
+    hal.gpio->write(2, 0);
+    hal.gpio->write(13, 0);
+    
+    hal.scheduler->delay(1000);
+
+    hal.gpio->write(13, 1);
+
+    hal.uart0->begin(115200);
+    hal.gpio->write(1, 0);
+    hal.uart0->println("Hello World");
+    hal.gpio->write(2, 0);
 }