[Firebug-cvs] fireboard/beta/tools/gps/SiRF sirftest2.c,NONE,1.1

[David M. D. <do...@us...>]

Update of /cvsroot/firebug/fireboard/beta/tools/gps/SiRF
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv14796

Added Files:
	sirftest2.c 
Log Message:
sirf id 28 messages can now be handled.

--- NEW FILE: sirftest2.c ---
/**
 * Test code for LeadTek GPS unit sirf message 2.
 */



#include <stdio.h>
#include <string.h>
#include <stdlib.h>


#ifndef FALSE
#define FALSE 0
#endif
#ifndef TRUE
#define TRUE !FALSE
#endif

#include "sirftest_util.h"

#include "sirf.h"

static FILE * outstream;



// See page 40 of the LeadTek Protocol manual for 
// example sirf id 2 string.
static char sirf_id2[] = {
  0x02,                    // message id
  0xff, 0xd6, 0xf7, 0x8c,  // x position
  0xff, 0xbe, 0x53, 0x6e,  // y position
  0x00, 0x3a, 0xc0, 0x04,  // z position
  0x00, 0x00,              // x velocity
  0x00, 0x03,              // y velocity
  0x00, 0x01,              // z velocity
  0x04,                    // mode 1
  0x0a,                    // (H)DOP
  0x00,                    // mode 2
  0x03, 0x6b,              // GPS Week
  0x03, 0x97, 0x80, 0xe3,  // GPS TOW
  0x06,                    // Sats in fix
  0x12,                    // ch 1
  0x19,                    // ch 2
  0x0e,                    // ch 3
  0x16,                    // ch 4
  0x0f,                    // ch 5 
  0x04,                    // ch 6
  0x00,                    // ch 7
  0x00,                    // ch 8
  0x00,                    // ch 9 
  0x00,                    // ch 10 
  0x00,                    // ch 11
  0x00                     // ch 12
};




int 
test_sirf_id2_parsing(void) {

  SiRF_ID2_1 sid2_1 = {{0}};

  sid2_1.header.seqno = 13;
  sid2_1.header.am_type = 150;
  sid2_1.xpos = convert_4_bytes_to_int(&sirf_id2[1]);
  sid2_1.ypos = convert_4_bytes_to_int(&sirf_id2[4]);
  sid2_1.zpos = convert_4_bytes_to_int(&sirf_id2[9]);
  sid2_1.xvel = convert_2_bytes_to_float(&sirf_id2[13]);
  sid2_1.yvel = convert_2_bytes_to_float(&sirf_id2[15]);
  sid2_1.zvel = convert_2_bytes_to_float(&sirf_id2[17]);
  sid2_1.mode1 = sirf_id2[19];
  sid2_1.dop = sirf_id2[20];
  sid2_1.mode2 = sirf_id2[21];

  //  memcpy(&sid2_1 + 4,&sirf_id2[1],23);

  //SiRF_ID2_1_print_cooked (&sid2_1);
  
  return 0;
}


void
print_struct_sizes(void) {

  printf("sirf id 2,1: %d\n",sizeof(SiRF_ID2_1));
  printf("sirf id 2,2: %d\n",sizeof(SiRF_ID2_2));
  printf("sirf id 28,1: %d\n",sizeof(SiRF_ID28_1));
  printf("sirf id 28,2: %d\n",sizeof(SiRF_ID28_2));
  printf("sirf id 28,3: %d\n",sizeof(SiRF_ID28_3));

}

void
print_message_sizes(void) {
  printf("Size of sirf id 2 string: %d\n", sizeof(sirf_id2));
}


uint8_t
get_message_id(char * msg) {

  return msg[0];
}


void
print_sirf2_message(void) {

  uint8_t msgid = 0;
  int32_t xpos = 0;
  int32_t ypos = 0;
  int32_t zpos = 0;

  float xvel = 0;
  float yvel = 0;
  float zvel = 0;


  msgid = get_message_id(sirf_id2);

  xpos = convert_4_bytes_to_int(&sirf_id2[1]);
  ypos = convert_4_bytes_to_int(&sirf_id2[5]);
  zpos = convert_4_bytes_to_int(&sirf_id2[9]);

  printf("Message ID: %d:\n",msgid);
  printf("X Pos: %ld\n", xpos);
  printf("Y Pos: %ld\n", ypos);
  printf("Z Pos: %ld\n", zpos);

  xvel = convert_2_bytes_to_float(&sirf_id2[13]);
  yvel = convert_2_bytes_to_float(&sirf_id2[15]);
  zvel = convert_2_bytes_to_float(&sirf_id2[17]);

  printf("X Vel: %f\n", xvel/8.0);
  printf("Y Vel: %f\n", yvel/8.0);
  printf("Z Vel: %f\n", zvel/8.0);

  printf("Mode 1: %d\n",sirf_id2[19]);
  printf("(H)DOP: %f\n",(float)sirf_id2[20]/5.0);
  printf("Mode 2: %d\n",sirf_id2[21]);

  printf("GPS Week: %d\n",convert_2_bytes_to_uint16(&sirf_id2[22]));
  //printf("GPS TOW: %f\n",(float)convert_4_bytes_to_int(&sirf_id2[24])/100.0);
}

unsigned char 
leadtek_crc(char * message, int size) {

  unsigned char cs = 0;
  int index = 1;

  while (message[index] != '*') {

    cs ^= message[index++];
  }

  return cs;
}

unsigned char 
sirf_crc(char * msg, uint8_t size) {

  unsigned char cs = 0;
  uint8_t i;

  for (i=0; i<size; i++) {
    cs = cs ^ msg[i];
  }
  return cs;
}





/**
 * This function tests for correct extraction of data
 * from the raw sirf message.
 */
int
fb_sirfid2_test() {

  int passed = FALSE;

  uint8_t msgid = 0;
  int32_t xpos = 0;
  int32_t ypos = 0;
  int32_t zpos = 0;

  float xvel = 0;
  float yvel = 0;
  float zvel = 0;

  uint8_t mode1, mode2;
  float dop;
  uint16_t gps_week;
  uint32_t gps_tow;
  uint8_t sats_in_fix;
  uint8_t ch1, ch2, ch3, ch4, ch5, ch6,
    ch7, ch8, ch9, ch10, ch11, ch12;


  msgid = get_message_id(sirf_id2);
  passed = (msgid == 2);

  xpos = convert_4_bytes_to_int(&sirf_id2[1]);
  passed &= (xpos == -2689140);

  ypos = convert_4_bytes_to_int(&sirf_id2[5]);
  passed &= (ypos == -4304018);

  zpos = convert_4_bytes_to_int(&sirf_id2[9]);
  passed &= (zpos == 3850244);

  xvel = convert_2_bytes_to_float(&sirf_id2[13]);
  passed &= (xvel == 0.0);

  yvel = convert_2_bytes_to_float(&sirf_id2[15])/8.0;
  passed &= (yvel == 0.375);

  zvel = convert_2_bytes_to_float(&sirf_id2[17])/8.0;
  passed &= (zvel == 0.125);

  mode1 = sirf_id2[19];
  passed &= (mode1 == 4);

  dop = (float)sirf_id2[20]/5.0;
  passed &= (dop == 2.0);
 
  mode2 = sirf_id2[21];
  passed &= (mode2 == 0);

  gps_week = convert_2_bytes_to_uint16(&sirf_id2[22]);
  passed &= (gps_week == 875);
  
  gps_tow = convert_4_bytes_to_int(&sirf_id2[24]);
  passed &= (gps_tow == 60260579);

  sats_in_fix = sirf_id2[28];
  passed &= (sats_in_fix == 6);

  ch1 = sirf_id2[29];
  passed &= (ch1 == 18);

  ch2 = sirf_id2[30];
  passed &= (ch2 == 25);

  ch3 = sirf_id2[31];
  passed &= (ch3 == 14);

  ch4 = sirf_id2[32];
  passed &= (ch4 == 22);

  ch5 = sirf_id2[33];
  passed &= (ch5 == 15);

  ch6 = sirf_id2[34];
  passed &= (ch6 == 4);

  ch7 = sirf_id2[35];
  passed &= (ch7 == 0);

  ch8 = sirf_id2[36];
  passed &= (ch8 == 0);

  ch9 = sirf_id2[37];
  passed &= (ch9 == 0);

  ch10 = sirf_id2[38];
  passed &= (ch10 == 0);

  ch11 = sirf_id2[39];
  passed &= (ch11 == 0);

  ch12 = sirf_id2[40];
  passed &= (ch12 == 0);

  /*
  fprintf(outstream,"GPS Week: %d\n",gps_week);
  fprintf(outstream,"GPS TOW: %d\n", gps_tow);
  fprintf(outstream,"SVs in fix: %d\n", sats_in_fix);


  fprintf(outstream,
	  "Channels 1-6:  %d %d %d %d %d %d\n"
	  "Channels 7-12: %d %d %d %d %d %d\n",
	  ch1, ch2, ch3, ch4, ch5, ch6,
	  ch7, ch8, ch9, ch10, ch11, ch12);

  */

  return passed;

}







int 
fb_sirf_test(void) {

  if (fb_sirfid2_test()) {
    fprintf(outstream, "SiRF ID 2 parsing passed.\n");
  } else {
    fprintf(outstream, "SiRF ID 2 parsing failed.\n");
  }

  return FALSE;
}


void
temporary(void) {

  print_message_sizes();
  print_struct_sizes();
}



// Change to msg * later
SiRF_ID2_t *
parse_sirfid2 (uint8_t * msg) {

  SiRF_ID2_t * sid2;
  sid2 = (SiRF_ID2_t*)calloc(1,sizeof(SiRF_ID2_t));

  printf("zpos: %i\n",convert_4_bytes_to_int(&msg[9]));

  sid2->xpos = convert_4_bytes_to_int(&msg[1]);
  sid2->ypos = convert_4_bytes_to_int(&msg[5]);
  sid2->zpos = convert_4_bytes_to_int(&msg[9]);
  sid2->xvel = convert_2_bytes_to_uint16(&msg[13]);
  sid2->yvel = convert_2_bytes_to_uint16(&msg[15]);
  sid2->zvel = convert_2_bytes_to_uint16(&msg[17]);
  sid2->mode1 = msg[19];
  sid2->dop = msg[20];
  sid2->mode2 = msg[21];
  sid2->gps_week = convert_2_bytes_to_uint16(&msg[22]); 
  sid2->gps_tow = convert_4_bytes_to_int(&msg[24]);
  sid2->sv_in_fix = msg[28];
  sid2->ch1 = msg[29];
  sid2->ch2 = msg[30];
  sid2->ch3 = msg[31];
  sid2->ch4 = msg[32];
  sid2->ch5 = msg[33];
  sid2->ch6 = msg[34];
  sid2->ch7 = msg[35];
  sid2->ch8 = msg[36];
  sid2->ch9 = msg[37];
  sid2->ch10 = msg[38];
  sid2->ch11 = msg[39];
  sid2->ch12 = msg[40];

  return sid2;
}


// Header used in packets that make up a long message.
//
typedef struct fragmentHeader {
  uint8_t mote_id;
  uint8_t sequenceNumber;
  uint8_t blockNumber;
  uint8_t blockCount;
} FragmentHeader_t;

#ifndef TOSH_DATA_LENGTH
#define TOSH_DATA_LENGTH 29
#endif

#define FRAGMENT_PAYLOAD_LENGTH (TOSH_DATA_LENGTH - sizeof(FragmentHeader_t))

// Packet to transport variable length (and long) messages in a fixed
// length radio message.
typedef struct messageFragment {
  FragmentHeader_t h;
  uint8_t data[FRAGMENT_PAYLOAD_LENGTH];
} MessageFragment_t;


// Struct pointer for msg parsing code.
typedef void * (*message_parser)(uint8_t * raw_msg);

struct msg_packer {

  uint8_t number_of_blocks; 
  uint8_t mote_id;
  uint8_t blocknumber;
  uint8_t seqno;
  uint8_t blocks_received_flags;
  uint8_t msg[255];  
};

typedef struct msg_packer msg_packer_t;

#define NUMBER_OF_MOTES 30
msg_packer_t * mp_array[NUMBER_OF_MOTES];


void
init_mp_array() {

  int i;
  for (i=0; i< NUMBER_OF_MOTES; i++) {
    mp_array[i] = calloc(1,sizeof(msg_packer_t));
  }
}

void
parse_msg(uint8_t * sirfmsg) {

  SiRF_ID2_t * sid2;

  uint8_t mid = get_message_id(sirfmsg);

  switch (mid) {
  case 2:
    printf("Parsing sirf id 2 msg...\n");
    sid2 = parse_sirfid2(sirfmsg);
    SiRF_ID2P_print_cooked(sid2);
    break;
  case 28:
    //parse_sirfid28(msg);
    break;
  default:
    ;
  }
    
}


void
pack_struct(msg_packer_t * mp, uint8_t * raw_msg) {

  int offset;

  offset = mp->blocknumber*FRAGMENT_PAYLOAD_LENGTH;
  //printf("offset: %d\n",offset);
  //printf("zpos: %i\n",convert_4_bytes_to_int(&raw_msg[9]));
  memcpy(&mp->msg[offset],raw_msg,FRAGMENT_PAYLOAD_LENGTH); 
  //printf("zpos: %i\n",convert_4_bytes_to_int(&mp->msg[9]));
}



/** This is really quick and dirty code,
 * should work ok, but will probably
 * start to drop a lot of packets as
 * the number of motes increases and
 * as the number of hops from mote to
 * base station increases.
 *
 * The idea is that when packets come in,
 * we have to keep track of what we have
 * versus what we need to have a complete
 * message.
 */
void
unpack_message(void * raw_msg) {

  MessageFragment_t * mf = (MessageFragment_t*)raw_msg;
  uint8_t number_of_blocks = mf->h.blockCount; 
  uint8_t mote_id = mf->h.mote_id;
  uint8_t blocknumber = mf->h.blockNumber;
  uint8_t seqno = mf->h.sequenceNumber;
  msg_packer_t * mp;

  
  //printf("mf->blockcount: %d\n", mf->h.blockCount);
  //printf("mf->mote_id: %d\n", mf->h.mote_id);
  //printf("mf->seqno: %d\n", mf->h.sequenceNumber);
  printf("mf->blocknumber: %d\n", mf->h.blockNumber);
  

  mp = mp_array[mote_id];
  mp->blocknumber = blocknumber;

  //printf("mp->seqno: %d\n",mp->seqno);

  // Out of sequence messages are discarded.
  if (mp->seqno > seqno) {
    printf("older\n");
    return;
  } else if (mp->seqno == seqno) {
    printf("same\n");
    //stuff the msg frag into the struct.
    pack_struct(mp,mf->data);
  } else {  // 
    // Current struct is stale, refill.
    printf("newer\n");
    memset(mp, 0x0, sizeof(msg_packer_t));    
    //stuff the msg frag into the struct.
    pack_struct(mp,mf->data);
  }

  //printf("IDID: %d\n",mp->msg[0]);

  mp->blocks_received_flags |= 1 << blocknumber;

  //printf("Blocks received: %d\n",mp->blocks_received_flags);
  
  if ((number_of_blocks == 1 && mp->blocks_received_flags == 1) ||
      (number_of_blocks == 2 && mp->blocks_received_flags == 3) ||
      (number_of_blocks == 3 && mp->blocks_received_flags == 7) ||
      (number_of_blocks == 4 && mp->blocks_received_flags == 15)) {
    parse_msg(mp->msg);
    //memset(mp, 0x0, sizeof(msg_packer_t));
  }

}



void 
test_packer() {

  MessageFragment_t * mf1;
  MessageFragment_t * mf2;
  
  mf1 = (MessageFragment_t *)calloc(1,sizeof(MessageFragment_t));
  mf2 = (MessageFragment_t *)calloc(1,sizeof(MessageFragment_t));

  init_mp_array(); 

  mf1->h.sequenceNumber = 0;
  mf2->h.sequenceNumber = 0;
  
  mf1->h.blockCount = 2;
  mf2->h.blockCount = 2;

  mf1->h.blockNumber = 0;
  mf2->h.blockNumber = 1;

  mf1->h.mote_id = 13;
  mf2->h.mote_id = 13;

  //printf("sirf msg: %s\n",sirf_id2);

  //printf("%d\n",sirf_id2[0]);

  printf("zvel: %d:\n",convert_2_bytes_to_uint16(&sirf_id2[17]));


  memcpy(mf1->data,sirf_id2,FRAGMENT_PAYLOAD_LENGTH);
  memcpy(mf2->data,sirf_id2+FRAGMENT_PAYLOAD_LENGTH,FRAGMENT_PAYLOAD_LENGTH);


  printf("yvel: %d:\n",convert_2_bytes_to_uint16(&mf1->data[15]));


  unpack_message((void*)mf2);
  unpack_message((void*)mf1);
}



int
main(int argc, char ** argv) {

  //uint64_t foo = 0xf143f62c4113f42f;
  outstream = stdout;
  //fb_sirf_test();

  //print_sirf2_message();

  //temporary();
  //printf("Foo: %f\n", foo);


  test_sirf_id2_parsing();
  test_packer();
  print_sirf2_message();
  return 0;
}