[Firebug-cvs] fireboard/beta/tools/gps/SiRF sirftest.h,NONE,1.1 sirftest28.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-serv15083/tools/gps/SiRF

Added Files:
	sirftest.h sirftest28.c 
Log Message:
sirf id 28 messages can now be handled.

--- NEW FILE: sirftest.h ---


// 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;


--- NEW FILE: sirftest28.c ---

/**
 * Test code for LeadTek GPS unit sirf message 28.
 */


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

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

#include "sirftest.h"
#include "sirftest_util.h"
#include "sirf.h"

static FILE * outstream;



// See page 67 of the LeadTek Protocol manual for 
// example sirf id 2 string.
static char sirf_id28[] = {
  0x1c,                    // message id
  0x00,                    // channel
  0x00, 0x06, 0x60, 0xd0,  // time tag
  0x14,                    // satellite id
  0xf1, 0x43, 0xf6, 0x2c,  // GPS Software time, 1st 4 bytes
  0x41, 0x13, 0xf4, 0x2f,  // GPS Software time, 2d 4 bytes
  0x41, 0x7b, 0x23, 0x5c,  // Pseudo-range, 1st 4 bytes
  0xf3, 0xfb, 0xe9, 0x5e,  // Pseudo-range, 2d 4 bytes
  0x46, 0x8c, 0x69, 0x64,  // Carrier freq.
  0xb8, 0xfb, 0xc5, 0x82,  // Carrier phase, 1st 4 bytes
  0x41, 0x5c, 0xf1, 0x5e,  // Carrier phase, 2d 4 bytes
  0x75, 0x30,              // Time in track
  0x17,                    // Sync flags
  0x34,                    // C/No 1
  0x34,                    // C/No 2
  0x34,                    // C/No 3
  0x34,                    // C/No 4
  0x34,                    // C/No 5
  0x34,                    // C/No 6
  0x34,                    // C/No 7
  0x34,                    // C/No 8
  0x34,                    // C/No 9
  0x34,                    // C/No 10
  0x03, 0xe8,              // Delta Range interval
  0x01, 0xf4,              // Mean Delta Range Time
  0x00, 0x00,              // Extrapolation time
  0x00,                    // Phase error count
  0x00                     // Low Power Count
};





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


uint8_t
get_message_id(char * msg) {

  return msg[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;
}


int
fb_sirfid28_test() {

  int passed = FALSE;
  uint8_t msgid = 0;
  uint8_t channel;
  int time_tag;
  uint8_t sat_id;
  double gps_software_time;
  double pseudo_range;
  float carrier_freq;
  double carrier_phase;
  uint16_t time_in_track;
  uint8_t sync_flags;
  uint8_t cno1, cno2, cno3, cno4, cno5,
    cno6, cno7, cno8, cno9, cno10;
  uint16_t delta_range_interval;
  uint16_t mean_delta_range_time;
  uint16_t extrapolation_time;
  uint8_t phase_error_count, low_power_count;


  msgid = get_message_id(sirf_id28);
  passed = (msgid == 28);
  
  channel = sirf_id28[1];
  passed &= (channel == 0);

  time_tag = convert_4_bytes_to_int(&sirf_id28[2]);
  passed &= (time_tag == 418000);
  //fprintf(outstream,"Time tag: %d\n",0x000660D0);

  sat_id = sirf_id28[6];
  passed &= (sat_id == 20);
  //fprintf(outstream,"Sat id: %d\n",sat_id);

  /*  Failing first here. */
  gps_software_time = convert_8_bytes_to_double(&sirf_id28[7]);
  passed &= (gps_software_time == 2.4921113696e5);

  pseudo_range = convert_8_bytes_to_double(&sirf_id28[15]);
  //passed &= (pseudo_range == 2.1016756638e7);

  // Check offset
  carrier_freq = convert_4_bytes_to_float(&sirf_id28[23]);
  //passed &= (carrier_freq == 1.6756767578e4);

  carrier_phase = convert_8_bytes_to_double(&sirf_id28[27]);
  //passed &= (carrier_phase == 4.4345542262e4);

  time_in_track = convert_2_bytes_to_uint16(&sirf_id28[35]);
  passed &= (time_in_track == 30000);

  sync_flags = sirf_id28[37];
  passed &= (sync_flags == 23);

  cno1 = sirf_id28[38];
  passed &= (cno1 == 52);
  
  cno2 = sirf_id28[39];
  passed &= (cno2 == 52);
  
  cno3 = sirf_id28[40];
  passed &= (cno3 == 52);
  
  cno4 = sirf_id28[41];
  passed &= (cno4 == 52);
  
  cno5 = sirf_id28[42];
  passed &= (cno5 == 52);
  
  cno6 = sirf_id28[43];
  passed &= (cno6 == 52);
  
  cno7 = sirf_id28[44];
  passed &= (cno7 == 52);
  
  cno8 = sirf_id28[45];
  passed &= (cno8 == 52);
  
  cno9 = sirf_id28[46];
  passed &= (cno9 == 52);
  
  cno10 = sirf_id28[47];
  passed &= (cno10 == 52);

  delta_range_interval = convert_2_bytes_to_uint16(&sirf_id28[48]);
  passed &= (delta_range_interval == 1000);

  mean_delta_range_time = convert_2_bytes_to_uint16(&sirf_id28[50]);
  passed &= (mean_delta_range_time == 500);

  extrapolation_time = convert_2_bytes_to_uint16(&sirf_id28[52]);
  passed &= (extrapolation_time == 0);

  phase_error_count = sirf_id28[54];
  passed &= (phase_error_count == 0);

  low_power_count = sirf_id28[55];
  passed &= (low_power_count == 0);

  return passed;
}


int 
fb_sirf_test(void) {

  if (fb_sirfid28_test()) {
    fprintf(outstream, "SiRF ID 28 parsing passed.\n");
  } else {
    fprintf(outstream, "SiRF ID 28 parsing failed.\n");
  }
 
  return FALSE;
}


#define NUMBER_OF_MOTES 30
static msg_packer_t * mp_array[NUMBER_OF_MOTES];


static void
init_mp_array() {

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


SiRF_ID28_t *
parse_sirfid28(uint8_t * sirfmsg) {

  SiRF_ID28_t * sid28;
  sid28 = (SiRF_ID28_t*)calloc(1,sizeof(SiRF_ID28_t));

  //sid28->msgid = get_message_id(sirf_id28);
  sid28->channel = sirf_id28[1];
  sid28->time_tag = convert_4_bytes_to_int(&sirf_id28[2]);
  sid28->sat_id = sirf_id28[6];
  sid28->gps_sw_time = (uint64_t)convert_8_bytes_to_double(&sirf_id28[7]);
  sid28->pseudo_range = (uint64_t)convert_8_bytes_to_double(&sirf_id28[15]);
  sid28->carrier_freq = convert_4_bytes_to_int(&sirf_id28[23]);
  sid28->carrier_phase = (uint64_t)convert_8_bytes_to_double(&sirf_id28[27]);
  sid28->time_in_track = convert_2_bytes_to_uint16(&sirf_id28[35]);
  sid28->sync_flags = sirf_id28[37];
  sid28->cno1 = sirf_id28[38];
  sid28->cno2 = sirf_id28[39];
  sid28->cno3 = sirf_id28[40];
  sid28->cno4 = sirf_id28[41];
  sid28->cno5 = sirf_id28[42];
  sid28->cno6 = sirf_id28[43];  
  sid28->cno7 = sirf_id28[44];
  sid28->cno8 = sirf_id28[45];
  sid28->cno9 = sirf_id28[46];
  sid28->cno10 = sirf_id28[47];
  sid28->delta_range_interval = convert_2_bytes_to_uint16(&sirf_id28[48]);
  sid28->mean_delta_range_time = convert_2_bytes_to_uint16(&sirf_id28[50]);
  sid28->extrapolation_time = convert_2_bytes_to_uint16(&sirf_id28[52]);
  sid28->phase_error_count = sirf_id28[54];
  sid28->low_power_count = sirf_id28[55];

  return sid28;
}


void
parse_msg(uint8_t * sirfmsg) {

  SiRF_ID28_t * sid28;

  uint8_t mid = get_message_id(sirfmsg);

  switch (mid) {
  case 2:
    break;
  case 28:
    printf("Parsing sirf id 28 msg...\n");
    sid28 = parse_sirfid28(sirfmsg);
    SiRF_ID28P_print_cooked(sid28);
    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));
  }

}



static void 
test_packer() {

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

  init_mp_array(); 

  mf1->h.sequenceNumber = 0;
  mf2->h.sequenceNumber = 0;
  mf3->h.sequenceNumber = 0;
  
  mf1->h.blockCount = 3;
  mf2->h.blockCount = 3;
  mf3->h.blockCount = 3;

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

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

  offset = 0*FRAGMENT_PAYLOAD_LENGTH;
  memcpy(mf1->data,sirf_id28+offset,FRAGMENT_PAYLOAD_LENGTH);

  offset = 1*FRAGMENT_PAYLOAD_LENGTH;
  memcpy(mf2->data,sirf_id28+offset,FRAGMENT_PAYLOAD_LENGTH);

  offset = 2*FRAGMENT_PAYLOAD_LENGTH;
  memcpy(mf3->data,sirf_id28+offset,FRAGMENT_PAYLOAD_LENGTH);


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



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

  outstream = stdout;

  int structsize;

  structsize = sizeof(SiRF_ID28_t);
  fprintf(outstream,"Sizeof ID 28 struct: %d\n",structsize);
  // ceil takes a double, returns a double (!??!!!???!!)
  fprintf(outstream,"Number of fragments: %d\n",(int)ceil((double)structsize/FRAGMENT_PAYLOAD_LENGTH));

  test_packer();
  //print_sirf28_message();
  return 0;
}