Send type with xbee message.

2 files changed, 16 insertions, 17 deletions

diff --git a/main.cpp b/main.cpp
index 199ffcd..1993489 100644
--- a/main.cpp
+++ b/main.cpp
@@ -66,22 +66,16 @@ void threadmain() {
 	while(1) {
 		GPIOB.ODR ^= 1 << 1;
 		
-		uint8_t buf[] = {
-			gyro.x & 0xff,
-			gyro.x >> 8,
-			gyro.y & 0xff,
-			gyro.y >> 8,
-			gyro.z & 0xff,
-			gyro.z >> 8,
-			accel.x & 0xff,
-			accel.x >> 8,
-			accel.y & 0xff,
-			accel.y >> 8,
-			accel.z & 0xff,
-			accel.z >> 8,
+		uint16_t buf[] = {
+			gyro.x,
+			gyro.y,
+			gyro.z,
+			accel.x,
+			accel.y,
+			accel.z,
 		};
 		
-		xbee_send(sizeof(buf), buf);
+		xbee_send(1, sizeof(buf), (uint8_t*)buf);
 		Time::sleep(100);
 	}
 }
diff --git a/xbee.h b/xbee.h
index f99fe55..53f9288 100644
--- a/xbee.h
+++ b/xbee.h
@@ -1,11 +1,11 @@
 #ifndef XBEE_H
 #define XBEE_H
 
-void xbee_send(int len, const uint8_t* buf) {
+void xbee_send(uint16_t type, int len, const uint8_t* buf) {
 	// Start and length.
 	usart_send(0x7e);
-	usart_send(((len + 14) >> 8) & 0xff);
-	usart_send((len + 14) & 0xff);
+	usart_send(((len + 16) >> 8) & 0xff);
+	usart_send((len + 16) & 0xff);
 	
 	// Frame type and ID.
 	usart_send(0x10);
@@ -28,6 +28,11 @@ void xbee_send(int len, const uint8_t* buf) {
 	
 	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]);