From 2fbb6afcf4fdcad4b2e33d6a787cc715a6fe6f0c Mon Sep 17 00:00:00 2001
From: jgromes <jgromes@users.noreply.github.com>
Date: Sat, 5 Jan 2019 10:20:14 +0100
Subject: [PATCH] [XBee] Added multiple attempts when connecting

---
 keywords.txt         |  1 +
 src/TypeDef.h        |  1 +
 src/modules/XBee.cpp | 45 +++++++++++++++++++++++++++++++-------------
 src/modules/XBee.h   |  2 +-
 4 files changed, 35 insertions(+), 14 deletions(-)

diff --git a/keywords.txt b/keywords.txt
index c1b55979..95cb7d28 100644
--- a/keywords.txt
+++ b/keywords.txt
@@ -174,6 +174,7 @@ ERR_CMD_MODE_FAILED	LITERAL1
 ERR_FRAME_MALFORMED	LITERAL1
 ERR_FRAME_INCORRECT_CHECKSUM	LITERAL1
 ERR_FRAME_UNEXPECTED_ID	LITERAL1
+ERR_FRAME_NO_RESPONSE	LITERAL1
 
 ASCII	LITERAL1
 ASCII_EXTENDED	LITERAL1
diff --git a/src/TypeDef.h b/src/TypeDef.h
index 3cff33ed..f50ce999 100644
--- a/src/TypeDef.h
+++ b/src/TypeDef.h
@@ -111,6 +111,7 @@
 #define ERR_FRAME_MALFORMED                   -302
 #define ERR_FRAME_INCORRECT_CHECKSUM          -303
 #define ERR_FRAME_UNEXPECTED_ID               -304
+#define ERR_FRAME_NO_RESPONSE                 -305
 
 // RTTY status codes
 #define ERR_INVALID_RTTY_SHIFT                -401
diff --git a/src/modules/XBee.cpp b/src/modules/XBee.cpp
index 7fcdd897..0a96f720 100644
--- a/src/modules/XBee.cpp
+++ b/src/modules/XBee.cpp
@@ -27,17 +27,35 @@ int16_t XBee::begin(long speed) {
   // empty UART buffer (garbage data)
   _mod->ATemptyBuffer();
   
-  // send test frame (get baudrate setting)
-  uint8_t frameID = _frameID++;
-  sendApiFrame(XBEE_API_FRAME_AT_COMMAND_QUEUE, frameID, "BD");
-  
-  // get response code
-  int16_t state = readApiFrame(frameID, 4);
-  if(state != ERR_NONE) {
-    return(state);
+  // try to find the XBee
+  bool flagFound = false;
+  uint8_t i = 0;
+  while((i < 10) && !flagFound) {
+    // send test frame (get baudrate setting)
+    uint8_t frameID = _frameID++;
+    sendApiFrame(XBEE_API_FRAME_AT_COMMAND_QUEUE, frameID, "BD");
+    int16_t state = readApiFrame(frameID, 4, 2000);
+    
+    if(state == ERR_NONE) {
+      flagFound = true;
+    } else {
+      DEBUG_PRINT_STR("XBee not found! (");
+      DEBUG_PRINT(i + 1);
+      DEBUG_PRINT_STR(" of 10 tries) STATE == ");
+      DEBUG_PRINTLN(state);
+      DEBUG_PRINTLN_STR("Resetting ...");
+      reset();
+    }
   }
   
-  return(state);
+  if(!flagFound) {
+    DEBUG_PRINTLN_STR("No XBee found!");
+    return(ERR_CMD_MODE_FAILED);
+  } else {
+    DEBUG_PRINTLN_STR("Found XBee!");
+  }
+  
+  return(ERR_NONE);
 }
 
 void XBee::reset() {
@@ -352,13 +370,13 @@ void XBee::sendApiFrame(uint8_t type, uint8_t id, uint8_t* data, uint16_t length
   delete[] frame;
 }
 
-int16_t XBee::readApiFrame(uint8_t frameID, uint8_t codePos) {
+int16_t XBee::readApiFrame(uint8_t frameID, uint8_t codePos, uint16_t timeout) {
   // TODO: modemStatus frames may be sent at any time, interfering with frame parsing. Add check to make sure this does not happen.
   
   // get number of bytes in response (must be enough to read the length field
-  uint16_t numBytes = getNumBytes(5000, 3);
+  uint16_t numBytes = getNumBytes(timeout/2, 3);
   if(numBytes == 0) {
-    return(ERR_FRAME_MALFORMED);
+    return(ERR_FRAME_NO_RESPONSE);
   }
   
   // checksum byte is not included in length field
@@ -367,7 +385,8 @@ int16_t XBee::readApiFrame(uint8_t frameID, uint8_t codePos) {
   // wait until all response bytes are available (5s timeout)
   uint32_t start = millis();
   while(_mod->ModuleSerial->available() < (int16_t)numBytes) {
-    if(millis() - start >= 5000) {
+    if(millis() - start >= timeout/2) {
+      Serial.println(_mod->ModuleSerial->available());
       return(ERR_FRAME_MALFORMED);
     }
   }
diff --git a/src/modules/XBee.h b/src/modules/XBee.h
index fad8d235..da2c2560 100644
--- a/src/modules/XBee.h
+++ b/src/modules/XBee.h
@@ -79,7 +79,7 @@ class XBee {
     
     void sendApiFrame(uint8_t type, uint8_t id, const char* data);
     void sendApiFrame(uint8_t type, uint8_t id, uint8_t* data, uint16_t length);
-    int16_t readApiFrame(uint8_t frameID, uint8_t codePos);
+    int16_t readApiFrame(uint8_t frameID, uint8_t codePos, uint16_t timeout = 5000);
     
     uint16_t getNumBytes(uint32_t timeout = 10000, size_t minBytes = 10);
 };