1 files changed, 51 insertions, 0 deletions
diff --git a/xbee.cpp b/xbee.cpp
new file mode 100644
index 0000000..efee619
--- /dev/null
+++ b/xbee.cpp
@@ -0,0 +1,51 @@
+#include "xbee.h"
+
+#include "usart.h"
+#include "mutex.h"
+
+Mutex xbee_mutex;
+
+void xbee_send(uint16_t type, int len, const uint8_t* buf) {
+	xbee_mutex.lock();
+	
+	// Start and length.
+	usart_send(0x7e);
+	usart_send(((len + 16) >> 8) & 0xff);
+	usart_send((len + 16) & 0xff);
+	
+	// Frame type and ID.
+	usart_send(0x10);
+	usart_send(0x01);
+	
+	// Destination address.
+	usart_send(0x00);
+	usart_send(0x13);
+	usart_send(0xa2);
+	usart_send(0x00);
+	usart_send(0x40);
+	usart_send(0x6f);
+	usart_send(0x19);
+	usart_send(0xf1);
+	
+	usart_send(0xff);
+	usart_send(0xfe);
+	usart_send(0x00);
+	usart_send(0x00);
+	
+	uint8_t chsum = 0x83;
+	
+	usart_send(type & 0xff);
+	chsum -= type & 0xff;
+	usart_send(type >> 8);
+	chsum -= type >> 8;
+	
+	// Payload
+	for(int i = 0; i < len; i++) {
+		usart_send(buf[i]);
+		chsum -= buf[i];
+	}
+	
+	usart_send(chsum);
+	
+	xbee_mutex.unlock();
+}