Allow XBeeWithCallbacks::waitFor() to check status responses too

Now, if a frameId is passed, waitFor() will check for status response
while waiting. If one is received that matches the frameId and has a
non-zero status (e.g. an error), waiting stops and the status is
returned.

When sending a TX packet and subsequently listening for a RX reply, this
allows waiting for a reply and the TX status at the same time and stop
waiting if the TX turns out to have failed. This is particularly
convenient since the TX status can sometimes arrive *before* the RX
reply and sometimes *after*, which makes a sequential waitForStatus()
and waitFor() approach not work.

Author: matthijskooijman

XBee.cpp

@@ -1697,11 +1697,19 @@ uint8_t XBeeWithCallbacks::matchStatus(uint8_t frameId) {
 	return 0xff;
 }
 
-bool XBeeWithCallbacks::waitForInternal(uint8_t apiId, void *response, uint16_t timeout, void *func, uintptr_t data) {
+uint8_t XBeeWithCallbacks::waitForInternal(uint8_t apiId, void *response, uint16_t timeout, void *func, uintptr_t data, int16_t frameId) {
 	unsigned long start = millis();
 	do {
 		// Wait for a packet of the right type
 		if (loopTop()) {
+			if (frameId >= 0) {
+				uint8_t status = matchStatus(frameId);
+				// If a status was found, but it was not
+				// a zero success status, stop waiting
+				if (status != 0xff && status != 0)
+					return status;
+			}
+
 			if (getResponse().getApiId() == apiId) {
 				// If the type is right, call the right
 				// conversion function based on the
@@ -1720,95 +1728,95 @@ bool XBeeWithCallbacks::waitForInternal(uint8_t apiId, void *response, uint16_t
 						bool(*f)(ZBTxStatusResponse&,uintptr_t) = (bool(*)(ZBTxStatusResponse&,uintptr_t))func;
 						getResponse().getZBTxStatusResponse(*r);
 						if(!f || f(*r, data))
-							return true;
+							return 0;
 						break;
 					}
 					case ZBRxResponse::API_ID: {
 						ZBRxResponse *r = (ZBRxResponse*)response;
 						bool(*f)(ZBRxResponse&,uintptr_t) = (bool(*)(ZBRxResponse&,uintptr_t))func;
 						getResponse().getZBRxResponse(*r);
 						if(!f || f(*r, data))
-							return true;
+							return 0;
 						break;
 					}
 					case ZBExplicitRxResponse::API_ID: {
 						ZBExplicitRxResponse *r = (ZBExplicitRxResponse*)response;
 						bool(*f)(ZBExplicitRxResponse&,uintptr_t) = (bool(*)(ZBExplicitRxResponse&,uintptr_t))func;
 						getResponse().getZBExplicitRxResponse(*r);
 						if(!f || f(*r, data))
-							return true;
+							return 0;
 						break;
 					}
 					case ZBRxIoSampleResponse::API_ID: {
 						ZBRxIoSampleResponse *r = (ZBRxIoSampleResponse*)response;
 						bool(*f)(ZBRxIoSampleResponse&,uintptr_t) = (bool(*)(ZBRxIoSampleResponse&,uintptr_t))func;
 						getResponse().getZBRxIoSampleResponse(*r);
 						if(!f || f(*r, data))
-							return true;
+							return 0;
 						break;
 					}
 					case TxStatusResponse::API_ID: {
 						TxStatusResponse *r = (TxStatusResponse*)response;
 						bool(*f)(TxStatusResponse&,uintptr_t) = (bool(*)(TxStatusResponse&,uintptr_t))func;
 						getResponse().getTxStatusResponse(*r);
 						if(!f || f(*r, data))
-							return true;
+							return 0;
 						break;
 					}
 					case Rx16Response::API_ID: {
 						Rx16Response *r = (Rx16Response*)response;
 						bool(*f)(Rx16Response&,uintptr_t) = (bool(*)(Rx16Response&,uintptr_t))func;
 						getResponse().getRx16Response(*r);
 						if(!f || f(*r, data))
-							return true;
+							return 0;
 						break;
 					}
 					case Rx64Response::API_ID: {
 						Rx64Response *r = (Rx64Response*)response;
 						bool(*f)(Rx64Response&,uintptr_t) = (bool(*)(Rx64Response&,uintptr_t))func;
 						getResponse().getRx64Response(*r);
 						if(!f || f(*r, data))
-							return true;
+							return 0;
 						break;
 					}
 					case Rx16IoSampleResponse::API_ID: {
 						Rx16IoSampleResponse *r = (Rx16IoSampleResponse*)response;
 						bool(*f)(Rx16IoSampleResponse&,uintptr_t) = (bool(*)(Rx16IoSampleResponse&,uintptr_t))func;
 						getResponse().getRx16IoSampleResponse(*r);
 						if(!f || f(*r, data))
-							return true;
+							return 0;
 						break;
 					}
 					case Rx64IoSampleResponse::API_ID: {
 						Rx64IoSampleResponse *r = (Rx64IoSampleResponse*)response;
 						bool(*f)(Rx64IoSampleResponse&,uintptr_t) = (bool(*)(Rx64IoSampleResponse&,uintptr_t))func;
 						getResponse().getRx64IoSampleResponse(*r);
 						if(!f || f(*r, data))
-							return true;
+							return 0;
 						break;
 					}
 					case ModemStatusResponse::API_ID: {
 						ModemStatusResponse *r = (ModemStatusResponse*)response;
 						bool(*f)(ModemStatusResponse&,uintptr_t) = (bool(*)(ModemStatusResponse&,uintptr_t))func;
 						getResponse().getModemStatusResponse(*r);
 						if(!f || f(*r, data))
-							return true;
+							return 0;
 						break;
 					}
 					case AtCommandResponse::API_ID: {
 						AtCommandResponse *r = (AtCommandResponse*)response;
 						bool(*f)(AtCommandResponse&,uintptr_t) = (bool(*)(AtCommandResponse&,uintptr_t))func;
 						getResponse().getAtCommandResponse(*r);
 						if(!f || f(*r, data))
-							return true;
+							return 0;
 						break;
 					}
 					case RemoteAtCommandResponse::API_ID: {
 						RemoteAtCommandResponse *r = (RemoteAtCommandResponse*)response;
 						bool(*f)(RemoteAtCommandResponse&,uintptr_t) = (bool(*)(RemoteAtCommandResponse&,uintptr_t))func;
 						getResponse().getRemoteAtCommandResponse(*r);
 						if(!f || f(*r, data))
-							return true;
+							return 0;
 						break;
 					}
 				}
@@ -1817,7 +1825,7 @@ bool XBeeWithCallbacks::waitForInternal(uint8_t apiId, void *response, uint16_t
 			loopBottom();
 		}
 	} while (millis() - start < timeout);
-	return false;
+	return XBEE_WAIT_TIMEOUT;
 }
 
 uint8_t XBeeWithCallbacks::waitForStatus(uint8_t frameId, uint16_t timeout) {

XBee.h

@@ -895,9 +895,24 @@ class XBeeWithCallbacks : public XBee {
 	 * of the right type received, passing the response and the data
 	 * parameter passed to this method. If the function returns true
 	 * (or if no function was passed), waiting stops and this method
-	 * returns true. If the function returns false, waiting
+	 * returns 0. If the function returns false, waiting
 	 * continues. After the given timeout passes, this method
-	 * returns false.
+	 * returns XBEE_WAIT_TIMEOUT.
+	 *
+	 * If a valid frameId is passed (e.g. 0-255 inclusive) and a
+	 * status API response frame is received while waiting, that has
+	 * a *non-zero* status, waiting stops and that status is
+	 * received. This is intended for when a TX packet was sent and
+	 * you are waiting for an RX reply, which will most likely never
+	 * arrive when TX failed. However, since the status reply is not
+	 * guaranteed to arrive before the RX reply (a remote module can
+	 * send a reply before the ACK), first calling waitForStatus()
+	 * and then waitFor() can sometimes miss the reply RX packet.
+	 *
+	 * Note that when the intended response is received *before* the
+	 * status reply, the latter will not be processed by this
+	 * method and will be subsequently processed by e.g. loop()
+	 * normally.
 	 *
 	 * While waiting, any other responses received are passed to the
 	 * relevant callbacks, just as if calling loop() continuously
@@ -908,8 +923,8 @@ class XBeeWithCallbacks : public XBee {
 	 * retrieved using getResponse() as normal.
 	 */
 	template <typename Response>
-	bool waitFor(Response& response, uint16_t timeout, bool (*func)(Response&, uintptr_t) = NULL, uintptr_t data = 0) {
-		return waitForInternal(Response::API_ID, &response, timeout, (void*)func, data);
+	uint8_t waitFor(Response& response, uint16_t timeout, bool (*func)(Response&, uintptr_t) = NULL, uintptr_t data = 0, int16_t frameId = -1) {
+		return waitForInternal(Response::API_ID, &response, timeout, (void*)func, data, frameId);
 	}
 
 	/**
@@ -950,7 +965,7 @@ class XBeeWithCallbacks : public XBee {
 	 * function to the corresponding type. This means that the
 	 * void* given must match the api id!
 	 */
-	bool waitForInternal(uint8_t apiId, void *response, uint16_t timeout, void *func, uintptr_t data);
+	uint8_t waitForInternal(uint8_t apiId, void *response, uint16_t timeout, void *func, uintptr_t data, int16_t frameId);
 
 	/**
 	 * Helper that checks if the current response is a status