[Firebug-cvs] fireboard/beta/tools/gps/SiRF sirftest2.c,NONE,1.1
Brought to you by:
doolin
From: David M. D. <do...@us...> - 2005-08-18 21:14:07
|
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; } |