update firmware to @319

1 files changed, 32 insertions, 6 deletions

diff --git a/firmware/debug.c b/firmware/debug.c
index 539a3e2..5e7a950 100644
--- a/firmware/debug.c
+++ b/firmware/debug.c
@@ -1,4 +1,7 @@
 #include <inttypes.h>
+#include <stdarg.h>
+#include <stdio.h>
+#include <stdlib.h>
 #include <avr/io.h>
 
 #include "uart.h"
@@ -8,21 +11,39 @@
 
 #define UART_PORT 0
 
-void debug_init(void) {
+void debug_dummy(char *fmt, ...) {}
+
+void debug_init()
+{
+#ifdef DEBUG
+#warning Compiling with debug-support!
   uart_init(UART_PORT);
   uart_baudrate(UART_PORT, 25, 0, 0, 0);
   uart_stopbits(UART_PORT, 1);
   uart_databits(UART_PORT, 8);
   uart_parity(UART_PORT, 'N');
+
+  static FILE mystdout = FDEV_SETUP_STREAM(debug_putchar, NULL,
+                                              _FDEV_SETUP_WRITE);
+  stdout = &mystdout;
+#endif
 }
 
-void debug_deinit(void) {
-  uart_deinit(UART_PORT);
+int debug_putchar(char c, FILE *stream)
+{
+#ifdef DEBUG
+  if (c == '\n')
+    uart_putchar(UART_PORT, '\r');
+  uart_putchar(UART_PORT, c);
+#endif
+  return 1;
 }
 
-void debug_write(char *buf) {
-  while(*buf)
-    uart_putchar(UART_PORT, *buf++);
+void debug_deinit(void)
+{
+#ifdef DEBUG
+  uart_deinit(UART_PORT);
+#endif
 }
 
 unsigned char debug_AsciiToHex(unsigned char high, unsigned char low)
@@ -52,6 +73,7 @@ unsigned char debug_AsciiToHex(unsigned char high, unsigned char low)
 
 void debug_SendHex(unsigned char hex)
 {
+#ifdef DEBUG
   unsigned char high,low;
   // get highnibble
   high = hex & 0xF0;
@@ -69,4 +91,8 @@ void debug_SendHex(unsigned char hex)
     uart_putchar(UART_PORT, low+48);
   else
     uart_putchar(UART_PORT, low+87);
+
+  //_debug_write("\r\n");
+#endif
 }
+