Colin Maxfield / Mbed 2 deprecated All_Combined_Real2

asdf

Dependencies:   SDFileSystem mbed

Fork of All_Combined_Real by JanPaul Bergeson

xbee.cpp@5:6532c5a5f576, 2016-03-23 (annotated)

Committer:
colingm
Date:
Wed Mar 23 14:32:41 2016 +0000
Revision:
5:6532c5a5f576
asdf

Who changed what in which revision?

UserRevisionLine numberNew contents of line
colingm 5:6532c5a5f576 1 #include <string.h>
colingm 5:6532c5a5f576 2 #include "xbee.h"
colingm 5:6532c5a5f576 3
colingm 5:6532c5a5f576 4
colingm 5:6532c5a5f576 5 int XBee::Receive(unsigned char *inBuff, int len, unsigned char *outBuff){
colingm 5:6532c5a5f576 6 int unescapeLen = 0;
colingm 5:6532c5a5f576 7 unsigned char checksum = 0;
colingm 5:6532c5a5f576 8 unsigned char LSB = 0;
colingm 5:6532c5a5f576 9
colingm 5:6532c5a5f576 10 if (inBuff[0] != 0x7E)
colingm 5:6532c5a5f576 11 return 0;
colingm 5:6532c5a5f576 12
colingm 5:6532c5a5f576 13 if (len < 10)
colingm 5:6532c5a5f576 14 return 0;
colingm 5:6532c5a5f576 15
colingm 5:6532c5a5f576 16 unescapeLen = unescape(inBuff, len, outBuff);
colingm 5:6532c5a5f576 17
colingm 5:6532c5a5f576 18 // Check we have at least the amount of bytes indicated by LSB
colingm 5:6532c5a5f576 19 LSB = outBuff[2];
colingm 5:6532c5a5f576 20 if (LSB > (unescapeLen - 4))
colingm 5:6532c5a5f576 21 return 0;
colingm 5:6532c5a5f576 22
colingm 5:6532c5a5f576 23 // Calculate our checksum
colingm 5:6532c5a5f576 24 // (char will overflow, no need to AND for lower bytes)
colingm 5:6532c5a5f576 25 for (int i=3; i<LSB+4; i++){
colingm 5:6532c5a5f576 26 checksum += outBuff[i];
colingm 5:6532c5a5f576 27 }
colingm 5:6532c5a5f576 28
colingm 5:6532c5a5f576 29 if (checksum != 0xFF)
colingm 5:6532c5a5f576 30 return 0;
colingm 5:6532c5a5f576 31
colingm 5:6532c5a5f576 32 return LSB+4;
colingm 5:6532c5a5f576 33 }
colingm 5:6532c5a5f576 34
colingm 5:6532c5a5f576 35
colingm 5:6532c5a5f576 36 int XBee::Send(unsigned char *msg, int len, unsigned char *outBuff, const uint64_t addr){
colingm 5:6532c5a5f576 37 unsigned char buf[len + 15];
colingm 5:6532c5a5f576 38 int escapedLen = 0;
colingm 5:6532c5a5f576 39 unsigned char checksum = 0;
colingm 5:6532c5a5f576 40
colingm 5:6532c5a5f576 41 buf[0] = 0x7E;
colingm 5:6532c5a5f576 42 buf[1] = (len + 11) >> 8;
colingm 5:6532c5a5f576 43 buf[2] = len + 11;
colingm 5:6532c5a5f576 44 buf[3] = 0x00; // transmit request
colingm 5:6532c5a5f576 45 buf[4] = 0x00; // Frame ID
colingm 5:6532c5a5f576 46 buf[5] = addr >> 56;
colingm 5:6532c5a5f576 47 buf[6] = addr >> 48;
colingm 5:6532c5a5f576 48 buf[7] = addr >> 40;
colingm 5:6532c5a5f576 49 buf[8] = addr >> 32;
colingm 5:6532c5a5f576 50 buf[9] = addr >> 24;
colingm 5:6532c5a5f576 51 buf[10] = addr >> 16;
colingm 5:6532c5a5f576 52 buf[11] = addr >> 8;
colingm 5:6532c5a5f576 53 buf[12] = addr;
colingm 5:6532c5a5f576 54 buf[13] = 0x01; // Disable acknowledge
colingm 5:6532c5a5f576 55 memcpy(&buf[14], msg, len);
colingm 5:6532c5a5f576 56
colingm 5:6532c5a5f576 57 for (int i=3;i<len+14;i++){
colingm 5:6532c5a5f576 58 checksum += buf[i];
colingm 5:6532c5a5f576 59 }
colingm 5:6532c5a5f576 60
colingm 5:6532c5a5f576 61 buf[len+14] = 0xFF - checksum;
colingm 5:6532c5a5f576 62 escapedLen = escape(buf, len+15, outBuff);
colingm 5:6532c5a5f576 63
colingm 5:6532c5a5f576 64 return escapedLen;
colingm 5:6532c5a5f576 65 }
colingm 5:6532c5a5f576 66
colingm 5:6532c5a5f576 67 int XBee::escape(unsigned char *input, int inLen, unsigned char *output){
colingm 5:6532c5a5f576 68 int pos = 1;
colingm 5:6532c5a5f576 69
colingm 5:6532c5a5f576 70 output[0] = input[0];
colingm 5:6532c5a5f576 71 for (int i=1; i<inLen; i++){
colingm 5:6532c5a5f576 72 switch(input[i]){
colingm 5:6532c5a5f576 73 case 0x7D:
colingm 5:6532c5a5f576 74 case 0x7E:
colingm 5:6532c5a5f576 75 case 0x11:
colingm 5:6532c5a5f576 76 case 0x13:
colingm 5:6532c5a5f576 77 output[pos++] = 0x7D;
colingm 5:6532c5a5f576 78 output[pos++] = input[i] ^ 0x20;
colingm 5:6532c5a5f576 79 break;
colingm 5:6532c5a5f576 80 default:
colingm 5:6532c5a5f576 81 output[pos++] = input[i];
colingm 5:6532c5a5f576 82 break;
colingm 5:6532c5a5f576 83 }
colingm 5:6532c5a5f576 84 }
colingm 5:6532c5a5f576 85
colingm 5:6532c5a5f576 86 return pos;
colingm 5:6532c5a5f576 87 }
colingm 5:6532c5a5f576 88
colingm 5:6532c5a5f576 89 int XBee::unescape(unsigned char *input, int inLen, unsigned char *output){
colingm 5:6532c5a5f576 90 int pos = 1;
colingm 5:6532c5a5f576 91 bool skip = false;
colingm 5:6532c5a5f576 92 unsigned char curr = 0;
colingm 5:6532c5a5f576 93
colingm 5:6532c5a5f576 94 output[0] = input[0];
colingm 5:6532c5a5f576 95 for (int i=1; i<inLen; i++) {
colingm 5:6532c5a5f576 96 if (skip){
colingm 5:6532c5a5f576 97 skip = false;
colingm 5:6532c5a5f576 98 continue;
colingm 5:6532c5a5f576 99 }
colingm 5:6532c5a5f576 100
colingm 5:6532c5a5f576 101 if (input[i] == 0x7D){
colingm 5:6532c5a5f576 102 curr = input[i+1] ^ 0x20;
colingm 5:6532c5a5f576 103 skip = true;
colingm 5:6532c5a5f576 104 }else{
colingm 5:6532c5a5f576 105 curr = input[i];
colingm 5:6532c5a5f576 106 }
colingm 5:6532c5a5f576 107
colingm 5:6532c5a5f576 108 output[pos] = curr;
colingm 5:6532c5a5f576 109 pos++;
colingm 5:6532c5a5f576 110 }
colingm 5:6532c5a5f576 111
colingm 5:6532c5a5f576 112 return pos;
colingm 5:6532c5a5f576 113 }