From 2fb8dcf8aea35346664e58f77a96779ba54dcd81 Mon Sep 17 00:00:00 2001
From: Kristian Lauszus <lauszus@gmail.com>
Date: Tue, 31 Jul 2012 21:10:25 +0200
Subject: [PATCH] Better print routines

Also added ability to print arrays
---
 RFCOMM.cpp | 27 ++++++++++++++++++++-------
 RFCOMM.h   |  2 ++
 2 files changed, 22 insertions(+), 7 deletions(-)

diff --git a/RFCOMM.cpp b/RFCOMM.cpp
index f10054bd..133eb2f1 100644
--- a/RFCOMM.cpp
+++ b/RFCOMM.cpp
@@ -1509,13 +1509,18 @@ void RFCOMM::print(const char* data) {
     RFCOMM_Command(rfcommbuf,strlen(data)+4);
 }
 void RFCOMM::print(uint8_t data) {
+    print(&data,1);
+}
+void RFCOMM::print(uint8_t* array, uint8_t length) {
     rfcommbuf[0] = rfcommChannelPermanent | 0 | 0 | extendAddress;; // RFCOMM Address
-    rfcommbuf[1] = RFCOMM_UIH; // RFCOMM Control                                                                
-    rfcommbuf[2] = 1 << 1 | 1; // Length = 1
-    rfcommbuf[3] = data;                                                                
-    rfcommbuf[4] = calcFcs(rfcommbuf);                            
+    rfcommbuf[1] = RFCOMM_UIH; // RFCOMM Control
+    rfcommbuf[2] = length << 1 | 1; // Length
+    uint8_t i = 0;
+    for(; i < length; i++)
+        rfcommbuf[i+3] = array[i];
+    rfcommbuf[i+3] = calcFcs(rfcommbuf);
     
-    RFCOMM_Command(rfcommbuf,5);
+    RFCOMM_Command(rfcommbuf,length+4);
 }
 
 void RFCOMM::println(const char* data) {
@@ -1525,9 +1530,17 @@ void RFCOMM::println(const char* data) {
     print(output);
 }
 void RFCOMM::println(uint8_t data) {
-    print(data);
-    print("\r\n");    
+    uint8_t buf[3] = {data, '\r', '\n'};
+    print(buf,3);
 }
+void RFCOMM::println(uint8_t* array, uint8_t length) {
+    uint8_t buf[length+2];
+    memcpy(buf,array,length);
+    buf[length] = '\r';
+    buf[length+1] = '\n';    
+    print(buf,length+2);
+}
+
 uint8_t RFCOMM::read() {
     uint8_t output = rfcommDataBuffer[0];
     for(uint8_t i = 1; i < rfcommAvailable; i++)
diff --git a/RFCOMM.h b/RFCOMM.h
index d0d148ca..6de8338f 100644
--- a/RFCOMM.h
+++ b/RFCOMM.h
@@ -202,8 +202,10 @@ public:
     /* Serial commands currently supported */
     void print(const char* data); // Used to send strings
     void print(uint8_t data); // Used to send single bytes
+    void print(uint8_t* array, uint8_t length); // Used to send arrays
     void println(const char* data); // Include newline and carriage return
     void println(uint8_t data); // Include newline and carriage return
+    void println(uint8_t* array, uint8_t length); // Include newline and carriage return
     uint8_t available() { return rfcommAvailable; }; // Get the bytes waiting to be read
     uint8_t read(); // Used to read the buffer