bool slavesensors_selectaddress(char* addrHigh, char* addrLow) 

{

	serial0_ioff();

	#ifdef DEBUG_CONTEXTSWITCH

	uint32_t startTime = time_millis();

	#endif

	#ifdef DEBUG_SELECTNODE

	serial0_sendString("Switch to node ");

	serial0_sendChar(nodeIndex + 0x30);

	serial0_sendString("\r\n");

	#endif

	_delay_ms(20);

	char tmpBuf[23];

	// If we can get into AT mode

	if(slavesensors_enterAT() == 0) 

	{

		snprintf(tmpBuf, 23, "ATDH %s%c",addrHigh, 0x0D);

		serial0_sendString(tmpBuf);

		if(xbeeIsOk() != 0) 

		{

			error_log(ERROR_XBEETIMEOUT, true);

			return false;

		}

		snprintf(tmpBuf, 23, "ATDL %s%c",addrLow, 0x0D);

		serial0_sendString(tmpBuf);

		if(xbeeIsOk() != 0) 

		{

			error_log(ERROR_XBEETIMEOUT, true);

			return false;

		}

		slavesensors_exitAT();

	}

	_delay_ms(2);

	#ifdef DEBUG_SELECTNODE

	serial0_sendString("Selected ");

	serial0_sendChar(nodeIndex + 0x30);

	serial0_sendString("\r\n");

	#endif

	#ifdef DEBUG_CONTEXTSWITCH

	uint32_t switchTime = time_millis() - startTime;

	char tmpB[32];

	snprintf(tmpB, 32, "CTXSW: %lu ms\r\n", switchTime);

	serial0_sendString(tmpB);

	#endif

	serial0_ion();

	return true;

}

void slavesensors_selectlogger() 

{

	if(loggerAddressLow[0] != 0x00) 

	{

		slavesensors_selectaddress(loggerAddressHigh,loggerAddressLow);

	}	

}

void slavesensors_exitAT() 

{

	// Exit AT

	serial0_sendString("ATCN");

	serial0_sendChar(0x0D);

	if(waitTimeout(TIMEOUT_EXITAT)) 

	{

		return;

	}

	xbeeIsOk();

}

bool waitTimeout(uint32_t timeout) {

	uint32_t scanStart = time_millis();

	uint32_t lastBlink = 0;

	while(!serial0_hasChar())

	{

		if(time_millis() - scanStart > timeout)

		{

			error_log(ERROR_XBEETIMEOUT, true);

			return true;

		}

		if(time_millis() - lastBlink > 50)

		{

			led_spin();

			lastBlink = time_millis();

		}

		wdt_reset();

	}

	return false;

}

// Enter AT mode. Leaves "OK" on the buffer.

int slavesensors_enterAT() 

{

	// Delay guard time

	_delay_ms(2);

	serial0_ioff(); // interrupts MUST be off

	// Enter AT mode

	serial0_sendChar('+'); // Enter AT mode

	serial0_sendChar('+');

	serial0_sendChar('+');

	return xbeeIsOk();

}

int xbeeIsOk() 

{

	if(waitTimeout(TIMEOUT_XBEERESPONSE)) {

		error_log(ERROR_XBEETIMEOUT, true);

		return 1;

	}

	char* tmppntr = serial0_readLine();

	if(strcmp(tmppntr, "OK") == 0)

	{

		return 0;

	}

	else

	{

		error_log(ERROR_SLAVETIMEOUT, true);

		return 1;

	}

}

bool slavesensors_dataReady() 

{

	return dataReady;

}

bool slavesensors_isrequesting() 

{

	return requesting;	

}

void slavesensors_startprocess() 

{

	if(nodeCount == 0)

	{

		return;

	}		

	requesting = true;

	slavesensors_request();		

}

uint32_t beginRequest = 0;

void slavesensors_request() 

{

	slavesensors_selectnode(currentSlave);

	beginRequest = time_millis();

	serial_sendCommand("@"); // Request data!

}

uint8_t numReadingsToExpect = 0; // number of values that the slave is about to send

uint8_t numRetries = 0;

void gotoNextSlaveOrSensor(bool fail) {

	// If we finished all sensors for all slaves

	if(currentSlave >= (nodeCount-1) && currentSlaveSensor >= (numReadingsToExpect-1))

	{

		#ifdef DEBUG_GETSLAVEDATA

		serial0_sendString("We got all data for all slaves!\r\n");

		#endif

		dataReady = true;

		currentSlave = 0;

		currentSlaveSensor = 0;

		requesting = false;

		if(!fail) 

		{

			led_alert();	

		}

	}

	// If we finished up one slave, go to the next

	else if(currentSlaveSensor >= (numReadingsToExpect-1))