First Tests

I finally completed enough of the packet code that I finally test to see if it actually works! and Amazingly enough ti works. Packets can now be sent from a remote wireless node to a receiver and passed to the computer. It also works the other way, data from the computer can be sent to a remote node. I recorded a video but I don't have it up yet.

Code after the jump


Arduino Code
packet.h

#define CMD_ERROR           0x00
#define CMD_VERSION         0x01
#define CMD_BATTERYLEVEL    0x03
#define CMD_PING            0x3E
#define CMD_PONG            0xE3
#define CMD_ACK             0x11
#define CMD_SETNODEID       0x21
#define CMD_GETNODEID       0x21
#define CMD_SETNODEID       0x22
#define CMD_GETPLUG         0x23
#define CMD_SETPLUG         0x24
#define CMD_AUTOSENDSTART   0xC1
#define CMD_AUTOSENDSTOP    0xC2
#define CMD_GETVALUE        0xD0
#define CMD_SETVALUE        0xD1
#define CMD_ON              0xE0
#define CMD_OFF             0xE1
#define CMD_LEVEL           0xE2
#define CMD_BYTE            0xF0
#define CMD_STRING          0xF1
#define CMD_INT             0xF2
#define CMD_FLOAT           0xF3
#define CMD_LONG            0xF4

#define DVC_TMP37           0x01
#define DVC_LIGHT           0x02
#define DVC_SHT21           0x03
#define DVC_DOORCONTACT     0x04
#define DVC_WINDOWCONTACT   0x05
#define DVC_MOTION          0x06
#define DVC_2LINEDISPLAY    0x07
#define DVC_POWER30         0x08
#define DVC_POWER100        0x09
#define DVC_SMARTOUTLET     0x10
#define DVC_RGBLIGHT        0x11
#define DVC_SOUND           0x12
#define DVC_SWITCH          0x13
#define DVC_STATUSLED       0x14
#define DVC_BUZZER          0x15

//decoded packet struct
typedef struct {
  uint8_t command;
 //if 255 = ready to send // 0 = resend last
  uint8_t length;
  uint8_t status;
  uint8_t settings; //01010101 //version //type direct or broadcast //ack //ttl //priority //multi
  uint8_t type; //1 single tcp, 2 single udp, 3 multipacket tcp
  uint16_t fromNode;
  uint8_t fromPlug;
  uint16_t toNode; //00 is base //xffff is broadcast
  uint8_t toPlug;
  uint8_t id;
  uint8_t ttl;
  uint8_t from;
  uint8_t payload[48];
  uint16_t checksum;
  uint8_t ptr;
} DataPacketDecoded;


//simple data packet

typedef struct {
  uint8_t ptr;
  uint8_t from;
  uint8_t status;
  uint8_t payload[56];
  uint16_t checksum;
} DataPacket;

//payload type

typedef struct {
  uint8_t length;
  uint8_t data[56];
} Payload;

//not used yet
typedef struct {
  long start;
  uint8_t waiting;
  uint8_t ptr;
} ACKStatus;
/*
typedef struct {
  uint8_t type;
  uint8_t status;
} Port;*/

//shortcuts to important bytes
//keeps from having to decode the whole thing to check simple parts
#define PACKET_LENGTH   0
#define PACKET_SETTINGS 1
#define PACKET_FROM     2
#define PACKET_TO       4
#define PACKET_TTL      4
#define PACKET_ID       6
#define PACKET_COMMAND  7
#define PACKET_PAYLOAD  8

//an enum might be better here
#define STATUS_CLEAR    0
#define STATUS_RECEVING 1
#define STATUS_RECEIVED 2
#define STATUS_WAITING  3
#define STATUS_READY    4
#define STATUS_SENDING  5
#define STATUS_SENT     6
#define STATUS_ACK      7
#define STATUS_SUCCESS  8

#define OFFSET_PACKET 10
#define OFFSET_HEADER 8
#define OFFSET_FOOTER 2

node.pde

#include h> //checksum functions
#include "protocol.h"

#include h>
#include h>

//#include 
//PortI2C myI2C (2);
//LiquidCrystalI2C lcd (myI2C);



#define NODE_ID 0x04

Port leds (1);
Port light (2);
MilliTimer sendTimer;

#define PLUG_1 DVC_STATUSLED
#define PLUG_2 DVC_2LINEDISPLAY
#define PLUG_3 0
#define PLUG_4 0

#define PACKET_BUFFER 16 //can be adujsted based on available space

#define PACKET_TIMEOUT 150

#define PORT_RF12    1
#define PORT_SERIAL  2
#define PORT_SERIAL1 3
#define PORT_SERIAL2 4
#define PORT_SERIAL3 5

#define PORT_COUNT 2
#define PORT_0 PORTTYPE_SERIAL
#define PORT_1 PORTTYPE_RF12

//#define DEBUG 1
//#define ENABLE_SERIAL 1
#define ENABLE_RF12 1
//#define ENABLE_SERIAL123 

#define PACKET_TCP       1
#define PACKET_UDP       2
#define PACKET_BROADCAST 3
//types
//ACK
//Single TCp //requires 2byte packet ID
//Single UDP
//Multi TCP //requires 2byte total ID, 2 byte Packet ID + byte current packet + byte total packets
//multi packet
//packet id
//current packets
//total packets
//
/*
Port ports[] = {{PORTTYPE_SERIAL,STATUS_CLEAR},
                {PORTTYPE_RF12,STATUS_CLEAR}};


*/
//list attached ports
//1 2 3 4 //rf12

#define COLLECT 0x20 // collect mode, i.e. pass incoming without sending acks

DataPacket data[PACKET_BUFFER];

uint16_t checksum;
byte uniqueId = 0;
byte i;
DataPacket incomingPacket; //current incoming packet


DataPacket *sendingPacket;

//probably should break this out by type
boolean sending = false;
boolean receiving = false;
long packetTimer;;

DataPacket *getNewPacket() {
  for(i=0; i < PACKET_BUFFER; i++) {
    if(data[i].status == STATUS_CLEAR){
      
      data[i].from = 0;
      return &data[i];
    } 
  }
  //Serial.println("all full");
  return 0;
}

uint8_t getId() 
{
  return uniqueId++; //loops when it overflows
}

uint16_t compressAddress(uint16_t node, uint8_t port) 
{
  return (node << 4) | (port & 0x0F);
}
//byte m = 1;
#ifdef ENABLE_SERIAL
void send_packet_serial(DataPacket *packet)
{
        #ifdef DEBUG
      Serial.println("SENDING PACKET SERIAL");
      #endif
 
  uint16_t dataChecksum = 0;
  
  for(i=0; i < (packet->payload[PACKET_LENGTH]-OFFSET_FOOTER); i++){
    dataChecksum = _crc16_update(dataChecksum, packet->payload[i]);
    Serial.print(packet->payload[i],BYTE); 
  } 
  
  packet->checksum = dataChecksum;
  Serial.print(highByte(dataChecksum),BYTE);
 // Serial.print(",");
  Serial.print(lowByte(dataChecksum),BYTE);
  packet->status = STATUS_SENT;
 //Serial.println("");
}
#endif
#ifdef ENABLE_RF12
void send_packet_rf12(DataPacket *packet)
{
  if (rf12_canSend()) { //check if radio is free
       #ifdef DEBUG
      Serial.println("SENDING PACKET RF12");
      #endif
    byte header = (packet->payload[PACKET_SETTINGS] == PACKET_TCP) ? RF12_HDR_ACK : 0;
    //Serial.print("Node Header: ");
    //Serial.println(header,DEC);
    byte toNode = get_node(packet,PACKET_TO);
    //Serial.print("Send to Node: ");
    //Serial.println(toNode,DEC);
    
    if (toNode > 0){
      if(toNode > 32){
        toNode = 1;//if port is too high send to basestation to retransmit
      }
      header |= RF12_HDR_DST | toNode;
    }   
    //rf12 does it's own check sum. no need to waste resources doing it twice
    rf12_sendStart(0, packet->payload, packet->payload[PACKET_LENGTH]-OFFSET_FOOTER);
    
    sendLed(1);
    delay(100);
    sendLed(0);
    
    packet->status = STATUS_SUCCESS;
    sending = false; 
  }
}
#endif
void process_command(DataPacket *packet)
{
  uint8_t plug = get_plug(packet,PACKET_TO);
  if(plug == 0){ //system function
    switch (packet->payload[PACKET_COMMAND]) {
      case CMD_ERROR:
        //build_udp_packet(NODE_ID, 1, 0, 0, CMD_PONG, get_payload(packet));
        break;
      case CMD_VERSION:
        //build_udp_packet(NODE_ID, 1, 0, 0, CMD_PONG, get_payload(packet));
        break;
      case CMD_BATTERYLEVEL:
        //build_udp_packet(NODE_ID, 1, 0, 0, CMD_PONG, get_payload(packet));
        break;
      case CMD_PING:
        build_udp_packet(NODE_ID, 1, 0, 0, CMD_PONG, get_payload(packet));
        break;
      case CMD_PONG:
        //build_udp_packet(NODE_ID, 1, 0, 0, CMD_PONG, get_payload(packet));
        break;
      case CMD_ACK:
        //build_udp_packet(NODE_ID, 1, 0, 0, CMD_PONG, get_payload(packet));
        break;
      case CMD_GETNODEID:
        //build_udp_packet(NODE_ID, 1, 0, 0, CMD_PONG, get_payload(packet));
        break;  
      case CMD_SETNODEID:
        //build_udp_packet(NODE_ID, 1, 0, 0, CMD_PONG, get_payload(packet));
        break;  
      case CMD_GETPLUG:
        //build_udp_packet(NODE_ID, 1, 0, 0, CMD_PONG, get_payload(packet));
        break;    
      case CMD_SETPLUG:
        //build_udp_packet(NODE_ID, 1, 0, 0, CMD_PONG, get_payload(packet));
        break;      
    }
  } else if(plug == 2){ //system function
/*  switch (packet->payload[PACKET_COMMAND]) {
    case CMD_STRING:
      Payload *p = get_payload(packet);
      lcd.setCursor(0, 1);
  // print the number of seconds since reset:
      for(int a = 0; a < p->length; a++){
        lcd.print(p->data[a]);
      }
      break;
    }*/
  } 
}

#ifdef ENABLE_RF12
//build rf12 packet
DataPacket* receive_rf12(const void* payload, uint8_t length){
  DataPacket* packet = getNewPacket();
  memcpy(packet->payload,payload, length);
  packet->status = STATUS_RECEIVED;
  packet->from = PORT_RF12;
  return packet;
}
#endif

DataPacket* clonePacket(DataPacket* packet){
  DataPacket* packet2 = getNewPacket();
  memcpy(packet2,packet, sizeof(packet)) ;
  return packet2;
}

void build_incoming_packet(uint8_t byteIn)
{
   //mark which port it is coming from
   if(incomingPacket.ptr == 0){  //Packet length
    
    if(byteIn == 0) { //resend last
      sendingPacket->status = STATUS_SENDING;
      return;
    }
    if(byteIn == 255) { //aknowledge
      sendingPacket->status = STATUS_SUCCESS;
      sending = false; //allow next packet to send
      
      return;
    }
    if (byteIn > 128) {
      Serial.flush();
      Serial.print(0,BYTE);
      return;
    }
    receiving = true;
    packetTimer = millis();
    incomingPacket.payload[PACKET_LENGTH] = byteIn;
    incomingPacket.status = STATUS_RECEVING;
    checksum = 0;
  }
  
  //checksum 
  if(incomingPacket.ptr < (incomingPacket.payload[PACKET_LENGTH] - OFFSET_FOOTER)){ //add to check sum
    incomingPacket.payload[incomingPacket.ptr] = byteIn;
    checksum = _crc16_update(checksum, byteIn);
    incomingPacket.ptr++;
    return;
  } else if(incomingPacket.ptr == (incomingPacket.payload[PACKET_LENGTH] - OFFSET_FOOTER)){ //get first byte of checksum
    incomingPacket.checksum = byteIn;
    incomingPacket.ptr++;
    return;
  } else { //get second byte and check the checksum
    incomingPacket.checksum = (incomingPacket.checksum << 8) | (byteIn & 0xFF);         
    if(checksum != incomingPacket.checksum) {
       DataPacket *p = build_udp_packet(NODE_ID, 1, 0, 0, CMD_ERROR, packet_to_payload(&incomingPacket));
       p->status = STATUS_READY;
      Serial.flush(); 
      Serial.print(1,BYTE); //tell the node to resend last
    } else {
      incomingPacket.status=STATUS_RECEIVED;
      DataPacket *packet = getNewPacket();
      incomingPacket.from = PORT_SERIAL;
      *packet = incomingPacket;
      //Serial.flush();
      Serial.print(255,BYTE); //tell the node the packet was successful
    }
    //reset incomingPacket
    receiving = false;
    incomingPacket.status=STATUS_CLEAR;
    incomingPacket.ptr = 0;
  }
}


//determine where to send packet. how to expand when we have 4 serial ports?
void process_stack(){
  //sending = false; //only send one pack per cycle
  for(i=0;i//check for packets to send

//check for packets to resend if ACKs are late

//check for data packets that can be deleted from the stack 
}



void process_packet(DataPacket *packet){
  switch (packet->status){
    case STATUS_CLEAR:
      #ifdef DEBUG
      //Serial.println("CLEAR");
      #endif
      break;
    case STATUS_RECEVING:
      #ifdef DEBUG
      Serial.println("RECEVING");
      #endif
      break;
    case STATUS_RECEIVED:
      #ifdef DEBUG
      debug_packet(packet);
      #endif 
      
      if(get_node(packet,PACKET_TO) == NODE_ID){
        process_command(packet);
        packet->status = STATUS_CLEAR;
      } else {
        packet->status = STATUS_READY;
      }
      #ifdef DEBUG
      Serial.println("RECEIVED");
      #endif
      break;
    case STATUS_WAITING:
      #ifdef DEBUG
      Serial.println("WAITING");
      #endif
      break;
    case STATUS_READY:
      //check for free port
      if(sending == false){
        packet->status = STATUS_SENDING;
        sendingPacket = packet;
        sending = true;
      }
      //if reset pointer and set to send
      #ifdef DEBUG
      Serial.println("READY");
      #endif
      break;
    case STATUS_SENDING:
      //because of the above, only one packet will be changed to sending
      #if defined(ENABLE_SERIAL) && defined(ENABLE_RF12)
      if(packet->from == 0){ //self generated send to both ports
        packet->from = PORT_RF12;
        DataPacket *packet2 = getNewPacket();
        packet2 = packet;
      }
      #endif
      #ifdef ENABLE_RF12
      if(packet->from != PORT_RF12){
        send_packet_rf12(packet); 
      }
      #endif
      
      #ifdef ENABLE_SERIAL
      if(packet->from != PORT_SERIAL){
         send_packet_serial(packet); 
         packet->status = STATUS_SENT;
      }
      #endif
      
      #ifdef DEBUG
      Serial.println("SENDING");
      #endif
      break;
    case STATUS_SENT:
      //if is a ack packet 
      if(packet->payload[PACKET_SETTINGS] == 1){
        packet->status = STATUS_ACK;
        //start packets ack timer
      } else {
        //packet->status = STATUS_SUCCESS;
      }
      #ifdef DEBUG
      sending = false; //allow next packet to send
      packet->status = STATUS_SUCCESS;
      Serial.println("SENT");
      #endif
      break;
    case STATUS_ACK:
       //if execeded timer resen
       //count resends if exceds max kill packet and add error
       #ifdef DEBUG
       Serial.println("ACK");
       #endif
      break;
    case STATUS_SUCCESS:
      //option to post status to screen
      packet->status = STATUS_CLEAR;
      #ifdef DEBUG
      Serial.println("SUCCESS");
      #endif
      break; 
  }
}

static void sendLed (byte on) {
    leds.mode(OUTPUT);
    leds.digiWrite(on);
}

static void receiveLed (byte on) {
    leds.mode2(OUTPUT);
    leds.digiWrite2(!on); // inverse, because LED is tied to VCC
}
long counter;

void setup()
{
  #if defined(ENABLE_SERIAL) || defined(DEBUG)
  Serial.begin(115200);
  #endif
   
  #ifdef ENABLE_RF12
  rf12_initialize(NODE_ID, RF12_915MHZ, 33);
  #endif
  
     #ifdef DEBUG
       Serial.println("DEBUGGING MODE");
     #endif
  /*
  lcd.begin(16, 2);
  // Print a message to the LCD.
  lcd.print("Hello, Matthew");
  lcd.backlight();*/
  counter = millis();
  sendLed(1);
  receiveLed(1);
  delay(500);
  sendLed(0);
  receiveLed(0);
  
}

void loop() {
/*
  if(counter == 100000) {
    Serial.print(availableMemory(),DEC);
   // DataPacket *p = build_udp_packet(NODE_ID, 1, 0, 0, CMD_STRING, str_to_payload("ha ha ha22222"));
   // p->status = STATUS_READY;
   // counter = 0;
  }
  counter++;*/

  process_stack();
  
  //recieve timer
  
  #ifdef ENABLE_SERIAL
  //and send command to resend last
  int byteIn;
  if (Serial.available()>0) {
    byteIn =  Serial.read();
    build_incoming_packet(byteIn);
  }
  process_stack();
  if(receiving == true) {
    if((millis() - packetTimer) > PACKET_TIMEOUT) {
      //Serial.flush();
      Serial.print(2,BYTE); //tell the node to resend last
      //DataPacket *p = build_udp_packet(NODE_ID, 1, 0, 0, CMD_ERROR, packet_to_payload(&incomingPacket));
      // p->status = STATUS_READY;
      receiving = false;
    }
  }
  #endif
  
  #ifdef ENABLE_RF12
  if (rf12_recvDone() && rf12_crc == 0) {
   // receiveLed(1);
     #ifdef DEBUG
       Serial.println("RF12 Receiving");
     #endif
    
    if ((rf12_hdr & RF12_HDR_ACK) && !(rf12_hdr & RF12_HDR_CTL) && (NODE_ID & COLLECT) == 0) {
      byte addr = rf12_hdr & RF12_HDR_MASK;
      // if request was sent only to us, send ack back as broadcast
      rf12_sendStart(rf12_hdr & RF12_HDR_DST ? RF12_HDR_CTL : RF12_HDR_CTL | RF12_HDR_DST | addr, 0, 0);
    }
    DataPacket *p = receive_rf12((void*)rf12_data,rf12_len);
    //blink led
    receiveLed(1);
    delay(100);
    receiveLed(0);
  }
  #endif
  //delay(5);  
  

  if (sendTimer.poll(5000)){
    light.mode2(INPUT);
    byte l = light.anaRead()/4;
    
    DataPacket *p = build_udp_packet(NODE_ID, 0, 3, 2, CMD_BYTE, byte_to_payload(l));
    //p->from = PORT_SERIAL;
  }
  
  
  delay(5);
}

/*FINISHED*/
Payload *get_payload(DataPacket *packet) 
{
  Payload payload;
  payload.length = packet->payload[PACKET_LENGTH] - OFFSET_PACKET;
  for(i=0; i < payload.length; i++){
    if(packet->payload[PACKET_SETTINGS] < 3) { //single packet TCP or UDP
      payload.data[i] = packet->payload[i+OFFSET_HEADER];   
     
    } else if(packet->payload[PACKET_SETTINGS] == 3) {  //broadcast
        //todo
    }
  }
  return &payload; 
}

Payload *packet_to_payload(DataPacket *packet) 
{
  Payload payload;
  payload.length = packet->payload[PACKET_LENGTH];
  for(i=0; i < (payload.length - OFFSET_FOOTER); i++){
    payload.data[i] = packet->payload[i];   
  }
  payload.data[payload.length - OFFSET_FOOTER] = highByte(packet->checksum);
  payload.data[payload.length - OFFSET_FOOTER + 1] = lowByte(packet->checksum);
  return &payload; 
}

uint16_t get_node(DataPacket *packet, uint8_t location)
{
  uint16_t node;  
  node = packet->payload[location];
  node = node << 4;
  node = node | (packet->payload[location+1] >> 4);
  return node;
}

uint8_t get_plug(DataPacket *packet, uint8_t location)
{
  return (packet->payload[location+1] & 0x0F);
}
 
Payload *str_to_payload(char string[])
{
  Payload payload;
  i=0;
  while(string[i] != 0){
    payload.data[i] = byte(string[i]); 
    i++;  
  }
  payload.length = i;
  //Serial.print("sizeOf: ");
 // Serial.println(i,DEC);
  return &payload; 
}

Payload *long_to_payload(long number)
{
  Payload payload;
  i=(sizeof(number)-1);
  payload.length = i;
  while(i != 0){
    payload.data[i] = number & 0xFF; 
    number = number >> 8;
    i--;  
  }
  return &payload; 
}

Payload *byte_to_payload(byte number)
{
  Payload payload;
  payload.length = 1;
  payload.data[0] = number & 0xFF; 
  return &payload; 
}

Payload *uint16_to_payload(uint16_t number)
{
  Payload payload;
  
   payload.length = 2;
   payload.data[0] = highByte(number);
   payload.data[1] = lowByte(number);
  
  return &payload; 
}

DataPacket *build_tcp_packet(uint16_t fromNode, uint8_t fromPlug, uint16_t toNode, uint8_t toPlug, uint8_t command, Payload *payload)
{
  DataPacket *packet = getNewPacket();
  packet->status = STATUS_READY;
  packet->payload[PACKET_LENGTH] = payload->length + OFFSET_PACKET; //header 8 crc 2
  packet->payload[PACKET_SETTINGS] = 1;
  uint16_t address = compressAddress(fromNode, fromPlug);
  packet->payload[PACKET_FROM] = (address >> 8) & 0xFF;
  packet->payload[PACKET_FROM+1] = address & 0xFF;
  address = compressAddress(toNode, toPlug);
  packet->payload[PACKET_TO ] = (address >> 8) & 0xFF;
  packet->payload[PACKET_TO+1] = address & 0xFF;
  packet->payload[PACKET_ID] = getId();
  packet->payload[PACKET_COMMAND] = command;
  
  for(i=0;i < payload->length;i++){
    packet->payload[PACKET_PAYLOAD+i] = payload->data[i];
  }

  return packet;
}

/*
 * @return DataPacket * Pointer to the New DataPacket
 */

DataPacket *build_udp_packet(uint16_t fromNode, uint8_t fromPlug, uint16_t toNode, uint8_t toPlug, uint8_t command, Payload *payload)
{
  DataPacket *packet = getNewPacket();
  packet->status = STATUS_READY;
  packet->payload[PACKET_LENGTH] = payload->length + OFFSET_PACKET; //header 8 crc 2
  packet->payload[PACKET_SETTINGS] = 2;
  uint16_t address = compressAddress(fromNode, fromPlug);
  packet->payload[PACKET_FROM] = (address >> 8) & 0xFF;
  packet->payload[PACKET_FROM+1] = address & 0xFF;
  address = compressAddress(toNode, toPlug);
  packet->payload[PACKET_TO ] = (address >> 8) & 0xFF;
  packet->payload[PACKET_TO+1] = address & 0xFF;
  packet->payload[PACKET_ID] = getId();
  packet->payload[PACKET_COMMAND] = command;
  
  for(i=0;i < payload->length;i++){
  packet->payload[PACKET_PAYLOAD+i] = payload->data[i];
  }
  
  return packet;
}

DataPacket *build_broadcast_packet(uint16_t fromNode, uint8_t fromPlug, uint8_t ttl, Payload payload)
{
  DataPacket *packet = getNewPacket();
  packet->status = STATUS_WAITING;
  return packet;
}

/**
 * turns a packet into a decoded packet
 */
DataPacketDecoded decode_packet(DataPacket *packet)
{
  DataPacketDecoded decodedPacket;
     
  decodedPacket.ttl = 0;
  
  //Set Packet length
  decodedPacket.length = packet->payload[PACKET_LENGTH];
  
  decodedPacket.status = packet->status;
  //settings 
  decodedPacket.settings = packet->payload[PACKET_SETTINGS];
  decodedPacket.type = packet->payload[PACKET_SETTINGS]; //right now these are the same things
  //From 12bit node id, 4 bit plug id
  decodedPacket.fromNode = packet->payload[PACKET_FROM];
  decodedPacket.fromNode = decodedPacket.fromNode << 4;
  decodedPacket.fromNode = decodedPacket.fromNode | (packet->payload[PACKET_FROM+1] >> 4);
  decodedPacket.fromPlug = packet->payload[PACKET_FROM+1] &0xF;
  //To 12bit node id, 4 bit plug id
  decodedPacket.toNode = packet->payload[PACKET_TO];
  decodedPacket.toNode = decodedPacket.toNode << 4;
  decodedPacket.toNode = decodedPacket.toNode | (packet->payload[PACKET_TO+1] >> 4);
  decodedPacket.toPlug = packet->payload[PACKET_TO+1] &0xF;
 //packet id
  decodedPacket.id = packet->payload[PACKET_ID];
 //Command
  decodedPacket.command = packet->payload[PACKET_COMMAND];
  
  decodedPacket.ptr = packet->payload[PACKET_LENGTH] - OFFSET_PACKET;
  
  
  Payload *payload = get_payload(packet);
   
  for(i=0; i < payload->length; i++){
    if(decodedPacket.type < 3) { //single packet TCP or UDP 
          decodedPacket.payload[i] = payload->data[i];   
    } else if(decodedPacket.type == 3) {  //broadcast
        //todo
    }
  }
  decodedPacket.checksum=packet->checksum;
  return decodedPacket;
}

void debug_packet(DataPacket *packet){
 DataPacketDecoded sendPacketedDecoded = decode_packet(packet);
  Serial.println("Decode Packet");
  Serial.print("    sent: ");
  Serial.println(sendPacketedDecoded.status,DEC);
  Serial.print("  length: ");
  Serial.println(sendPacketedDecoded.length,DEC);
  Serial.print("settings: ");
  Serial.println(sendPacketedDecoded.settings,DEC);
  Serial.print("    type: ");
  Serial.println(sendPacketedDecoded.type,DEC); 
  Serial.print("fromNode: ");
  Serial.println(sendPacketedDecoded.fromNode,DEC);
  Serial.print("fromPlug: ");
  Serial.println(sendPacketedDecoded.fromPlug,DEC);
  Serial.print("  toNode: ");
  Serial.println(sendPacketedDecoded.toNode,DEC);
  Serial.print("  toPlug: ");
  Serial.println(sendPacketedDecoded.toPlug,DEC);
  Serial.print("     ttl: ");
  Serial.println(sendPacketedDecoded.ttl,DEC);
  Serial.print("      id: ");
  Serial.println(sendPacketedDecoded.id,DEC);
  Serial.print(" command: ");
  Serial.println(sendPacketedDecoded.command,DEC);
  Serial.print(" payload: ");
  for(i=0; i < sendPacketedDecoded.ptr; i++){
    Serial.print(sendPacketedDecoded.payload[i],DEC);
    Serial.print(",");
  } 
  Serial.println("");
   Serial.print(" payload: ");
  for(i=0; i < sendPacketedDecoded.ptr; i++){
    Serial.print(sendPacketedDecoded.payload[i],BYTE);
  } 
  Serial.println("");
  Serial.print("checksum: ");
  Serial.println(sendPacketedDecoded.checksum,DEC);
  Serial.print("     ptr: ");
  Serial.println(sendPacketedDecoded.ptr,DEC);
  Serial.print("    from: ");
  Serial.println(sendPacketedDecoded.from,DEC);
}

int availableMemory() {
  int size = 2048; // Use 2048 with ATmega328
  byte *buf;

  while ((buf = (byte *) malloc(--size)) == NULL)
    ;

  free(buf);

  return size;
}

Processing code:

import processing.serial.*;

//shortcuts to important bytes
//keeps from having to decode the whole thing to check simple parts
final int PACKET_LENGTH   = 0;
final int PACKET_SETTINGS = 1;
final int PACKET_FROM     = 2;
final int PACKET_TO       = 4;
final int PACKET_TTL      = 4;
final int PACKET_ID       = 6;
final int PACKET_COMMAND  = 7;
final int PACKET_PAYLOAD  = 8;

final int CMD_PING = 0x3E;
final int CMD_PONG = 0xE3;
final int CMD_STRING = 0xF1;
//an enum might be better here
final int STATUS_CLEAR    = 0;
final int STATUS_RECEVING = 1;
final int STATUS_RECEVIED = 2;
final int STATUS_WAITING  = 3;
final int STATUS_READY    = 4;
final int STATUS_SENDING  = 5;
final int STATUS_SENT     = 6;
final int STATUS_ACK      = 7;
final int STATUS_SUCCESS  = 8;

final int OFFSET_PACKET = 10;
final int OFFSET_HEADER = 8;
final int OFFSET_FOOTER = 2;



class DataPacketDecoded {
  int command, length, status = 0, settings, type,  fromPort, toPort, id, ttl = 0, ptr = 0;
  int fromNode, toNode,  checksum = 0;
  int[] payload = new int[56];

  DataPacketDecoded(){ }
}

//simple data packet

class DataPacket {
  int ptr = 0, status = 0, checksum = 0;
  int[] payload = new int[64];
  
  DataPacket(){ }
} 

class Payload {
  int length;
  int[] data = new int[56];
  
  Payload() { }
} 

//What should master ports be?

final int  NODE_ID = 255;

final int PACKET_BUFFER  = 10;
final int PACKET_TIMEOUT = 150;


//Should be an ArrayList so we can add and remove elements
DataPacket[] data = new DataPacket[PACKET_BUFFER];
DataPacket incomingPacket = new DataPacket(); //current incoming packet

int checksum;
int uniqueId = 0;
int i;
boolean sending = false;
boolean receiving = false;
long packetTimer;

int bgcolor;			     // Background color
int fgcolor;			     // Fill color
Serial myPort;                       // The serial port
PFont myFont;

XMLElement xmlConfig;

//build rf12 packet

void build_incoming_packet(int byteIn)
{
   //mark which port it is coming from
  println("count: "+incomingPacket.ptr + " byte: " + byteIn);
 
  if(incomingPacket.ptr == 0){  //Packet length
    if(byteIn <= 2) {
      println("RESENDING LAST");
      //resend last
      sendingPacket.status = STATUS_SENDING;
      return;
    }
    if(byteIn == 255) {
      println("ACK SUCCESS");
      sendingPacket.status = STATUS_SUCCESS; 
      sending = false; //allow next packet to send
      //aknowledge
      return;
    }
    if (byteIn > 128) {
      myPort.clear();
      myPort.write(0);
      println("BYTE to high");
      return;
    }
    receiving = true;
    packetTimer = millis();
    incomingPacket.payload[PACKET_LENGTH] = byteIn;
    incomingPacket.status = STATUS_RECEVING;
    checksum = 0;
    
    println("length: " + byteIn);
  }
  
  //checksum 
  if(incomingPacket.ptr < (incomingPacket.payload[PACKET_LENGTH] - OFFSET_FOOTER)){ //add to check sum
    incomingPacket.payload[incomingPacket.ptr] = byteIn;
    checksum = _crc16_update(checksum, byteIn);
    incomingPacket.ptr++;
    return;
  } else if(incomingPacket.ptr == (incomingPacket.payload[PACKET_LENGTH] - OFFSET_FOOTER)){ //get first byte of checksum
    incomingPacket.checksum = byteIn;
    incomingPacket.ptr++;
    return;
  } else { //get second byte and check the checksum
    incomingPacket.checksum = (incomingPacket.checksum << 8) | (byteIn & 0xFF);         
    if(checksum != incomingPacket.checksum) {
      println("processing checksum: " + checksum);  
      println("    packet checksum: " + incomingPacket.checksum);   
      println("Failed CRC Check"); 
      myPort.clear();
      myPort.write(int(0));
    } else {
      println("processing checksum: " + checksum);  
      println("    packet checksum: " + incomingPacket.checksum);  
      println("Passed CRC Check");  
      incomingPacket.status=STATUS_RECEVIED;
      DataPacket packet = getNewPacket();
      packet = incomingPacket;
      
      myPort.clear();
      myPort.write(int(255));
      println("SENT ACK");
      //packet
      debug_incoming_packet(packet); 
      // incomingPacket.payload;
      
      //incomingPacket = DataPacket;
    }
    //reset incoming packet
    incomingPacket = new DataPacket();
    receiving = false;
  }
}
//determine where to send packet. how to expand when we have 4 serial ports?

void process_stack(){
  //sending = false; //only send one pack per cycle
  
  for(i=0;i//check for packets to send

//check for packets to resend if ACKs are late
   
//check for data packets that can be deleted from the stack 
}

DataPacket sendingPacket;

void process_packet(DataPacket packet){

  switch (packet.status){
    case STATUS_CLEAR:
      //println("CLEAR");
      break;
    case STATUS_RECEVING:
      println("RECEVING");

      break;
    case STATUS_RECEVIED:
      //if(get_node(packet,PACKET_TO) == NODE_ID){
        //process_command(packet);
        packet.status = STATUS_CLEAR;
     // } else {
     //   packet.status = STATUS_READY; 
     // }
      println("RECEVIED");
      break;
    case STATUS_WAITING:
      println("WAITING");
      break;
    case STATUS_READY:
      //check for free port
      if(sending == false){
        packet.status = STATUS_SENDING;
        sending = true;
      }
      //if reset pointer and set to send
      println("READY");
      break;
    case STATUS_SENDING:
      //because of the above, only one packet will be changed to sending
      //if(get_node(packet,PACKET_FROM) < 30){
        // send_packet_rf12(packet); 
      //} else {
         send_packet_serial(packet); 
         sendingPacket = packet;
     // }
      packet.status = STATUS_SENT;
      println("SENDING");
      break;
    case STATUS_SENT:
      //if is a ack packet 
      if(packet.payload[PACKET_SETTINGS] == 1){
        packet.status = STATUS_ACK;
        //start packets ack timer
      } else {
        //packet.status = STATUS_SUCCESS;
      }
      println("SENT");
      break;
    case STATUS_ACK:
       //if execeded timer resen
       //count resends if exceds max kill packet and add error
       println("ACK");
      break;
    case STATUS_SUCCESS:
      //option to post status to screen
      packet.status = STATUS_CLEAR;
      println("SUCCESS");
      break; 
  }
}

String findXMLid(XMLElement[] xml , int value, String rAttribute){
  String rValue = ""; 
  for(int a = 0; aif(unhex(commands[a].getAttribute("id")) == value){
      return commands[a].getAttribute(rAttribute);
    }
  }
  return rValue;
} 

XMLElement commands[];

XMLElement devices[];

long sendTimer;

void setup() {
  size(600, 400);  // Stage size
  //noStroke();      // No border on the next thing drawn
  myFont = createFont("Consolas", 14);
  textFont(myFont);
  
  for(i=0; i < PACKET_BUFFER; i++) {
    data[i] = new DataPacket();
    data[i].status = STATUS_CLEAR;
  }
  
  xmlConfig = new XMLElement(this, "config.txt");
  devices = xmlConfig.getChildren("devices/device");
  commands = xmlConfig.getChildren("commands/command");
  
  // Print a list of the serial ports, for debugging purposes:
  //println(PFont.list());
  
  String portName = Serial.list()[0];
  myPort = new Serial(this, portName, 115200);
}

boolean arduinoBoot = false;
int count = 0;
void draw() {
  
  if(arduinoBoot == false){
    delay(2500);
    sendTimer = millis();
    arduinoBoot = true;;
  }
  process_stack();

  if((millis() - sendTimer) > 2000){
    //DataPacket packet = build_udp_packet(NODE_ID,0,3,2,CMD_STRING,str_to_payload("Hello "+count));
   // packet.status = STATUS_READY;
    sendTimer=millis();
    count++;
  }


  if(receiving == true) {
    if((millis() - packetTimer) > PACKET_TIMEOUT) {
      myPort.clear();
      myPort.write(0); //tell the node to resend last
      
      incomingPacket = new DataPacket();
      println("bad packet");
      receiving = false;
    }
  }
}

void serialEvent(Serial myPort) {
  // read a byte from the serial port:
  int inByte = myPort.read();
  build_incoming_packet(inByte);
  //print(inByte);
  //print(",");
}

//Done
DataPacket getNewPacket() {
  for(i=0; i < PACKET_BUFFER; i++) {
    if(data[i].status == STATUS_CLEAR){
      return data[i];
    } 
  }
  println("BUFFER FULL");
  return new DataPacket();
}



int get_node(DataPacket packet, int location)
{
  int node;  
  node = packet.payload[location];
  node = node << 4;
  node = node | (packet.payload[location+1] >> 4);
  return node;
}

Payload get_payload(DataPacket packet) 
{
  Payload payload = new Payload();
  payload.length = packet.payload[PACKET_LENGTH] - OFFSET_PACKET;
  for(i=0; i < payload.length; i++){
    if(packet.payload[PACKET_SETTINGS] < 3) { //single packet TCP or UDP
      payload.data[i] = packet.payload[i+OFFSET_HEADER];   
     
    } else if(packet.payload[PACKET_SETTINGS] == 3) {  //broadcast
        //todo
    }
  }
  return payload; 
}
 
Payload str_to_payload(String string)
{
  Payload payload = new Payload();
  //println("Payload Length: "+ string.length());
  for(i=0;iint(string.charAt(i)); 
  }
  payload.length = string.length();
  return payload; 
}

void process_command(DataPacket packet)
{
  switch (packet.payload[PACKET_COMMAND]) {
    case CMD_PING:
      
     // DataPacket reply = build_udp_packet(NODE_ID, 1, 0, 0, CMD_PONG, get_payload(packet));
     // reply.status = STATUS_READY;
      
      //change status to ready to send
      
      break;
    //default:
      //send_packet(COMMAND_ERROR,NULL,0);
   //   break;
    }
}


int _crc16_update(int crc, int a)
{
    int i;
    crc ^= a;
    for (i = 0; i < 8; ++i){
        if ((crc & 1) == 1){
            crc = (crc >> 1) ^ 0xA001;
        }else{
            crc = (crc >> 1);
        }
    }
    return crc & 0xFFFF;
}

int getId() {
  if(uniqueId == 255){
    uniqueId = 0;
  }
  return uniqueId++; //what happens when this wraps?
}

int compressAddress(int node, int port) 
{
  node = node << 4; //make space
  port = port & 0x0F;
  return node | port;
}


DataPacket build_tcp_packet(int fromNode, int fromPlug, int toNode, int toPlug, int command, Payload payload)
{
  DataPacket packet = getNewPacket();
  //*packet.status = STATUS_READY;
  packet.status = STATUS_WAITING;
  packet.payload[PACKET_LENGTH] = payload.length + OFFSET_PACKET; //header 8 crc 2
  packet.payload[PACKET_SETTINGS] = 1;
  int address = compressAddress(fromNode, fromPlug);
  packet.payload[PACKET_FROM] = (address >> 8) & 0xFF;
  packet.payload[PACKET_FROM+1] = address & 0xFF;
  address = compressAddress(toNode, toPlug);
  packet.payload[PACKET_TO ] = (address >> 8) & 0xFF;
  packet.payload[PACKET_TO+1] = address & 0xFF;
  packet.payload[PACKET_ID] = getId();
  packet.payload[PACKET_COMMAND] = command;
  
  for(i=0;i < payload.length;i++){
    packet.payload[PACKET_PAYLOAD+i] = payload.data[i];
  }

  return packet;
}

/*
 * @return DataPacket * Pointer to the New DataPacket
 */

DataPacket build_udp_packet(int fromNode, int fromPlug, int toNode, int toPlug, int command, Payload payload)
{
  DataPacket packet = getNewPacket();
  //*packet.status = STATUS_READY;
  packet.status = STATUS_WAITING;
  packet.payload[PACKET_LENGTH] = payload.length + OFFSET_PACKET; //header 8 crc 2
  packet.payload[PACKET_SETTINGS] = 2;
  int address = compressAddress(fromNode, fromPlug);
  packet.payload[PACKET_FROM] = (address >> 8) & 0xFF;
  packet.payload[PACKET_FROM+1] = address & 0xFF;
  address = compressAddress(toNode, toPlug);
  packet.payload[PACKET_TO ] = (address >> 8) & 0xFF;
  packet.payload[PACKET_TO+1] = address & 0xFF;
  packet.payload[PACKET_ID] = getId();
  packet.payload[PACKET_COMMAND] = command;
  
  for(i=0;i < payload.length;i++){
  packet.payload[PACKET_PAYLOAD+i] = payload.data[i];
  }
  
  return packet;
}

/*
uint8_t build_broadcast_packet(uint16_t fromNode, uint8_t fromPort, uint16_t toNode, uint8_t toPort, uint8_t payload[48], uint8_t length, uint8_t ttl )
{
  byte packetId = getNewPacketId();
  data[packetId].sent = 0;
  return packetId;
}
*/

boolean send_packet_serial(DataPacket packet)
{
  int dataChecksum = 0;
  for(i=0; i < (packet.payload[PACKET_LENGTH]-2); i++){
    dataChecksum = _crc16_update(dataChecksum, packet.payload[i]);
    myPort.write(packet.payload[i] & 0xFF);
    print(int((packet.payload[i] & 0xFF)) + ",");
  } 
  
  print((dataChecksum >>8)&0xFF);
  print(",");
  print(dataChecksum & 0xFF);
  
  println("");
  println("Packet Sent");
  packet.status = 1;
  myPort.write((dataChecksum >> 8) & 0xFF);	
  myPort.write(dataChecksum & 0xFF);

  return true;
}

/**
 * turns a packet into a decoded packet
 */

DataPacketDecoded decode_packet(DataPacket packet)
{
  DataPacketDecoded decodedPacket = new DataPacketDecoded();
       
  //Set Packet length
  decodedPacket.length = packet.payload[PACKET_LENGTH];
  
  decodedPacket.status = packet.status;
  
  //settings 
  decodedPacket.settings = packet.payload[PACKET_SETTINGS];
  decodedPacket.type = packet.payload[PACKET_SETTINGS]; //right now these are the same things

  //From 12bit node id, 4 bit plug id
  decodedPacket.fromNode = packet.payload[PACKET_FROM];
  decodedPacket.fromNode = decodedPacket.fromNode << 4;
  decodedPacket.fromNode = decodedPacket.fromNode | (packet.payload[PACKET_FROM+1] >> 4);
  decodedPacket.fromPort = packet.payload[PACKET_FROM+1] &0xF;
  
  //To 12bit node id, 4 bit plug id
  decodedPacket.toNode = packet.payload[PACKET_TO];
  decodedPacket.toNode = decodedPacket.toNode << 4;
  decodedPacket.toNode = decodedPacket.toNode | (packet.payload[PACKET_TO+1] >> 4);
  decodedPacket.toPort = packet.payload[PACKET_TO+1] &0xF;

 //packet id
  decodedPacket.id = packet.payload[PACKET_ID];

 //Command
  decodedPacket.command = packet.payload[PACKET_COMMAND];
  decodedPacket.checksum = packet.checksum;
  decodedPacket.ptr = packet.payload[PACKET_LENGTH] - 10;
   
  for(i=PACKET_PAYLOAD; i < packet.payload[PACKET_LENGTH]; i++){

    if(decodedPacket.type < 3) { //single packet TCP or UDP
        
          decodedPacket.payload[i-PACKET_PAYLOAD] = packet.payload[i];   
     
    } else if(decodedPacket.type == 3) {  //broadcast
        //todo
    }
  }
  return decodedPacket;
}

void debug_incoming_packet(DataPacket packet) 
{
  background(0);
  //println("show packet info");
  text("Packet Info",5,15);
  int x = 20;
  DataPacketDecoded packetDecoded = decode_packet(packet);
  text("  status: " + packetDecoded.status,10,x+=20);
  text("  length: " + packetDecoded.length,10,x+=20);
  text("settings: " + packetDecoded.settings,10,x+=20);
  text("    type: " + packetDecoded.type,10,x+=20); 
  text("fromNode: " + packetDecoded.fromNode,10,x+=20);
  text("fromPort: " + packetDecoded.fromPort,10,x+=20);
  text("  toNode: " + packetDecoded.toNode,10,x+=20);
  text("  toPort: " + packetDecoded.toPort,10,x+=20);
  text("     ttl: " + packetDecoded.ttl,10,x+=20);
  text("      id: " + packetDecoded.id,10,x+=20);
  text(" command: " + findXMLid(commands, packetDecoded.command,"code"),10,x+=20);
  
  String message = "";
  String message2 = "";
  for(i=0; i < packetDecoded.ptr; i++){
    message += int(packetDecoded.payload[i]);
    message += ",";
    message2 += char(packetDecoded.payload[i]);
  } 
  text(" payload: " + message,10,x+=20);
  text(" payload: " + message2,10,x+=20);
  
  text("checksum: " + packetDecoded.checksum,10,x+=20);
  text("     ptr: " + packetDecoded.ptr,10,x+=20);//*/
}