diff options
| -rw-r--r-- | main.cpp | 22 | ||||
| -rw-r--r-- | xbee.h | 11 |
2 files changed, 16 insertions, 17 deletions
@@ -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); } } @@ -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]); |