From 8adb47a48ee15d220a17f44da55f269f173244a8 Mon Sep 17 00:00:00 2001 From: Jordan Miller Date: Sun, 22 Mar 2009 23:26:17 -0500 Subject: [PATCH] Added Wali's Change to gant.c --- gant.c | 422 +++++++++++++++++++++++++++++++++++++++++++++++++-------- 1 file changed, 366 insertions(+), 56 deletions(-) diff --git a/gant.c b/gant.c index 0cc41bd..e4f6248 100644 --- a/gant.c +++ b/gant.c @@ -1,5 +1,6 @@ -// copyright 2008 paul@ant.sbrk.co.uk. released under GPLv3 -// vers 0.4t +// copyright 2008-2009 paul@ant.sbrk.co.uk. released under GPLv3 +// copyright 2009-2009 Wali +// vers 0.6t #include #include #include @@ -15,6 +16,10 @@ #include "antdefs.h" +char *releasetime = "Jan 24 2009, 16:10:12"; +uint majorrelease = 0; +uint minorrelease = 6; + double round(double); int gottype; @@ -50,22 +55,29 @@ uint mydev = 0; uint peerdev; uint myid; uint devid; -ulong myauth1; -ulong myauth2; +uint myauth1; +uint myauth2; char authdata[32]; uint pairing; uint isa50; uint isa405; uint waitauth; int nphase0; +char modelname[256]; +ushort part = 0; +ushort ver = 0; +uint unitid = 0; + //char *getversion = "440dffff00000000fe00000000000000"; //char *getgpsver = "440dffff0000000006000200ff000000"; char *acks[] = { "fe00000000000000", // get version - 255, 248, 253 "0e02000000000000", // device short name (fr405a) - 525 -//"1c00020000000000", // no data +// "1c00020000000000", // no data "0a0002000e000000", // unit id - 38 + "0a00020002000000", // send position + "0a00020005000000", // send time "0a000200ad020000", // 4 byte something? 0x10270000 = 10000 dec - 1523 "0a000200c6010000", // 3 x 4 ints? - 994 "0a00020035020000", // guessing this is # trackpoints per run - 1066 @@ -93,6 +105,31 @@ int blsize = 0; int bused = 0; int lseq = -1; +/* round a float as garmin does it! */ +/* shoot me for writing this! */ +char * +ground(double d) +{ + int neg = 0; + static char res[30]; + ulong ival; + ulong l; /* hope it doesn't overflow */ + + if (d < 0) { + neg = 1; + d = -d; + } + ival = floor(d); + d -= ival; + l = floor(d*100000000); + if (l % 10 >= 5) + l = l/10+1; + else + l = l/10; + sprintf(res,"%s%ld.%07ld", neg?"-":"", ival, l); + return res; +} + char * timestamp(void) { @@ -104,7 +141,7 @@ timestamp(void) tmp = gmtime(&tv.tv_sec); sprintf(time, "%02d:%02d:%02d.%02d", - tmp->tm_hour, tmp->tm_min, tmp->tm_sec, tv.tv_usec/10000); + tmp->tm_hour, tmp->tm_min, tmp->tm_sec, (int)tv.tv_usec/10000); return time; } @@ -121,6 +158,54 @@ randno(void) return r; } +void +print_tcx_header(FILE *tcxfile) +{ + fprintf(tcxfile, "\n"); + fprintf(tcxfile, "\n\n"); + fprintf(tcxfile, " \n"); + return; +} + +void +print_tcx_footer(FILE *tcxfile) +{ + fprintf(tcxfile, " \n"); + fprintf(tcxfile, " \n"); + fprintf(tcxfile, " \n"); + fprintf(tcxfile, " %s\n", modelname); + fprintf(tcxfile, " %u\n", unitid); + fprintf(tcxfile, " %u\n", part); + fprintf(tcxfile, " \n"); + fprintf(tcxfile, " %u\n", ver/100); + fprintf(tcxfile, " %u\n", ver - ver/100*100); + fprintf(tcxfile, " 0\n"); + fprintf(tcxfile, " 0\n"); + fprintf(tcxfile, " \n"); + fprintf(tcxfile, " \n"); + fprintf(tcxfile, " \n"); + fprintf(tcxfile, " \n\n"); + fprintf(tcxfile, " \n"); + fprintf(tcxfile, " Garmin ANT for Linux\n"); + fprintf(tcxfile, " \n"); + fprintf(tcxfile, " \n"); + fprintf(tcxfile, " %u\n", majorrelease); + fprintf(tcxfile, " %u\n", minorrelease); + fprintf(tcxfile, " 0\n"); + fprintf(tcxfile, " 0\n"); + fprintf(tcxfile, " \n"); + fprintf(tcxfile, " Release\n"); + fprintf(tcxfile, " \n", releasetime); + fprintf(tcxfile, " make\n"); + fprintf(tcxfile, " \n"); + fprintf(tcxfile, " EN\n"); + fprintf(tcxfile, " 006-A0XXX-00\n"); + fprintf(tcxfile, " \n\n"); + fprintf(tcxfile, "\n"); + return; +} + + #pragma pack(1) struct ack_msg { uchar code; @@ -136,10 +221,10 @@ struct auth_msg { uchar phase; uchar u1; uint id; - ulong auth1; - ulong auth2; - ulong fill1; - ulong fill2; + uint auth1; + uint auth2; + uint fill1; + uint fill2; }; struct pair_msg { @@ -155,12 +240,19 @@ struct pair_msg { #define ACKSIZE 8 // above structure must be this size #define AUTHSIZE 24 // ditto #define PAIRSIZE 16 +#define MAXLAPS 256 // max of saving laps data before output with trackpoint data +#define MAXTRACK 256 // max number of tracks to be saved per download decode(ushort bloblen, ushort pkttype, ushort pktlen, int dsize, uchar *data) { int i; int j; int hr; + int hr_av; + int hr_max; + int cal; + float tsec; + float max_speed; int cad; int u1, u2; int doff = 20; @@ -170,14 +262,27 @@ decode(ushort bloblen, ushort pkttype, ushort pktlen, int dsize, uchar *data) float alt; float dist; uint tv; + uint tv_previous = 0; time_t ttv; char tbuf[100]; struct tm *tmp; double lat, lon; - ushort part; - ushort ver; - uint unitid; uint nruns; + uint tv_lap; + static uchar lapbuf[MAXLAPS][48]; + static ushort lap = 0; + static ushort lastlap = 0; + static ushort track = 0; + static short previoustrack_id = -1; + static short track_id = -1; + static short firsttrack_id = -1; + static short firstlap_id = -1; + static ushort firstlap_id_track[MAXTRACK]; + static uchar sporttyp_track[MAXTRACK]; + static FILE *tcxfile = NULL; + static ushort track_pause = 0; + + printf("decode %d %d %d %d\n", bloblen, pkttype, pktlen, dsize); switch (pkttype) { case 255: @@ -205,11 +310,23 @@ decode(ushort bloblen, ushort pkttype, ushort pktlen, int dsize, uchar *data) printf("%d Devname %s\n", pkttype, devname); break; case 12: - case 990: - printf("%d shorts?", pkttype); - for (i = 0; i < pktlen; i += 4) + printf("%d xfer complete", pkttype); + for (i = 0; i < pktlen; i += 2) printf(" %u", data[doff+i] + data[doff+i+1]*256); printf("\n"); + switch (data[doff] + data[doff+1]*256) { + case 6: + // last file completed, add footer and close file + print_tcx_footer(tcxfile); + fclose(tcxfile); + break; + case 117: + break; + case 450: + break; + default: + break; + } break; case 38: unitid = data[doff] + data[doff+1]*256 + @@ -229,13 +346,162 @@ decode(ushort bloblen, ushort pkttype, ushort pktlen, int dsize, uchar *data) data[doff+i+2]*256*256 + data[doff+i+3]*256*256*256); printf("\n"); break; + case 14: + printf("%d time: ", pkttype); + printf("%02u-%02u-%u %02u:%02u:%02u\n", data[doff], data[doff+1], data[doff+2] + data[doff+3]*256, + data[doff+4], data[doff+6], data[doff+7]); + break; + case 17: + printf("%d position ? ", pkttype); + for (i = 0; i < pktlen; i += 4) + printf(" %u", data[doff+i] + data[doff+i+1]*256 + + data[doff+i+2]*256*256 + data[doff+i+3]*256*256*256); + printf("\n"); + break; + case 99: + printf("%d trackindex %u\n", pkttype, data[doff] + data[doff+1]*256); + printf("%d shorts?", pkttype); + for (i = 0; i < pktlen; i += 2) + printf(" %u", data[doff+i] + data[doff+i+1]*256); + printf("\n"); + track_id = data[doff] + data[doff+1]*256; + break; + case 990: + printf("%d track %u lap %u-%u sport %u\n", pkttype, + data[doff] + data[doff+1]*256, data[doff+2] + data[doff+3]*256, + data[doff+4] + data[doff+5]*256, data[doff+6]); + printf("%d shorts?", pkttype); + for (i = 0; i < pktlen; i += 2) + printf(" %u", data[doff+i] + data[doff+i+1]*256); + printf("\n"); + if (firstlap_id == -1) firstlap_id = data[doff+2] + data[doff+3]*256; + if (firsttrack_id == -1) firsttrack_id = data[doff] + data[doff+1]*256; + track = (data[doff] + data[doff+1]*256) - firsttrack_id; + if (track < MAXTRACK) { + firstlap_id_track[track] = data[doff+2] + data[doff+3]*256; + sporttyp_track[track] = data[doff+6]; + } else { + printf("Error: track and lap data temporary array out of range %u!\n", track); + } + break; case 1510: printf("%d waypoints", pkttype); for (i = 0; i < 4 && i < pktlen; i += 4) printf(" %u", data[doff+i] + data[doff+i+1]*256 + data[doff+i+2]*256*256 + data[doff+i+3]*256*256*256); printf("\n"); + // if trackpoints are split into more than one message 1510, do not add xml head again + if (previoustrack_id != track_id) { + // close previous file if it is not the first track to be downloaded + if (previoustrack_id > -1) { + // add xml footer and close file, the next file will be open further down + print_tcx_footer(tcxfile); + fclose(tcxfile); + } + // use first lap starttime as filename + lap = firstlap_id_track[track_id-firsttrack_id] - firstlap_id; + if (dbg) printf("lap %u track_id %u firsttrack_id %u firstlap_id %u\n", lap, track_id, firsttrack_id, firstlap_id); + tv_lap = lapbuf[lap][4] + lapbuf[lap][5]*256 + + lapbuf[lap][6]*256*256 + lapbuf[lap][7]*256*256*256; + ttv = tv_lap + 631065600; // garmin epoch offset + strftime(tbuf, sizeof tbuf, "%d.%m.%Y %H%M%S.TCX", localtime(&ttv)); + // open file and start with header of xml file + tcxfile = fopen(tbuf, "wt"); + print_tcx_header(tcxfile); + } for (i = 4; i < pktlen; i += 24) { + tv = (data[doff+i+8] + data[doff+i+9]*256 + + data[doff+i+10]*256*256 + data[doff+i+11]*256*256*256); + tv_lap = lapbuf[lap][4] + lapbuf[lap][5]*256 + + lapbuf[lap][6]*256*256 + lapbuf[lap][7]*256*256*256; + if ((tv > tv_lap || (tv == tv_lap && lap == (firstlap_id_track[track_id-firsttrack_id] - firstlap_id))) && lap <= lastlap) { + ttv = tv_lap + 631065600; // garmin epoch offset + strftime(tbuf, sizeof tbuf, "%Y-%m-%dT%H:%M:%SZ", gmtime(&ttv)); + tsec = (lapbuf[lap][8] + lapbuf[lap][9]*256 + + lapbuf[lap][10]*256*256 + lapbuf[lap][11]*256*256*256); + memcpy((void *)&dist, &lapbuf[lap][12], 4); + memcpy((void *)&max_speed, &lapbuf[lap][16], 4); + cal = lapbuf[lap][36] + lapbuf[lap][37]*256; + hr_av = lapbuf[lap][38]; + hr_max = lapbuf[lap][39]; + cad = lapbuf[lap][41]; + if (lap == firstlap_id_track[track_id-firsttrack_id] - firstlap_id) { + fprintf(tcxfile, " \n"); + fprintf(tcxfile, " %s\n", tbuf); + } else { + fprintf(tcxfile, " \n"); + fprintf(tcxfile, " \n"); + } + fprintf(tcxfile, " \n", tbuf); + fprintf(tcxfile, " %s\n", ground(tsec/100)); + fprintf(tcxfile, " %s\n", ground(dist)); + fprintf(tcxfile, " %s\n", ground(max_speed)); + fprintf(tcxfile, " %d\n", cal); + if (hr_av > 0) { + fprintf(tcxfile, " \n"); + fprintf(tcxfile, " %d\n", hr_av); + fprintf(tcxfile, " \n"); + } + if (hr_max > 0) { + fprintf(tcxfile, " \n"); + fprintf(tcxfile, " %d\n", hr_max); + fprintf(tcxfile, " \n"); + } + fprintf(tcxfile, " "); + switch (lapbuf[lap][40]) { + case 0: fprintf(tcxfile, "Active"); break; + case 1: fprintf(tcxfile, "Rest"); break; + default: fprintf(tcxfile, "unknown value: %d", lapbuf[lap][40]); + } + fprintf(tcxfile, "\n"); + // for bike the average cadence of this lap is here + if (sporttyp_track[track_id-firsttrack_id] == 1) { + if (cad != 255) { + fprintf(tcxfile, " %d\n", cad); + } + } + fprintf(tcxfile, " "); + switch(lapbuf[lap][42]) { + case 4: fprintf(tcxfile, "Heartrate"); break; + case 3: fprintf(tcxfile, "Time"); break; + case 2: fprintf(tcxfile, "Location"); break; + case 1: fprintf(tcxfile, "Distance"); break; + case 0: fprintf(tcxfile, "Manual"); break; + default: fprintf(tcxfile, "unknown value: %d", lapbuf[lap][42]); + } + fprintf(tcxfile, "\n"); + // I prefere the average run cadence here than at the end of this lap according windows ANTagent + if (sporttyp_track[track_id-firsttrack_id] == 0) { + if (cad != 255) { + fprintf(tcxfile, " \n"); + fprintf(tcxfile, " \n"); + fprintf(tcxfile, " %d\n", cad); + fprintf(tcxfile, " \n"); + fprintf(tcxfile, " \n"); + } + } + fprintf(tcxfile, " \n"); + lap++; + // if the previous trackpoint has same second as lap time display the trackpoint again + if (dbg) printf("i %u tv %d tv_lap %d tv_previous %d\n", i, tv, tv_lap, tv_previous); + if (tv_previous == tv_lap) { + i -= 24; + tv = tv_previous; + } + track_pause = 0; + } // end of if (tv >= tv_lap && lap <= lastlap) + ttv = tv+631065600; // garmin epoch offset + tmp = gmtime(&ttv); + strftime(tbuf, sizeof tbuf, "%Y-%m-%dT%H:%M:%SZ", tmp); // format for printing + memcpy((void *)&alt, data+doff+i+12, 4); + memcpy((void *)&dist, data+doff+i+16, 4); lat = (data[doff+i] + data[doff+i+1]*256 + data[doff+i+2]*256*256 + data[doff+i+3]*256*256*256)*180.0/0x80000000; lon = (data[doff+i+4] + data[doff+i+5]*256 + @@ -244,43 +510,85 @@ decode(ushort bloblen, ushort pkttype, ushort pktlen, int dsize, uchar *data) cad = data[doff+i+21]; u1 = data[doff+i+22]; u2 = data[doff+i+23]; - tv = (data[doff+i+8] + data[doff+i+9]*256 + - data[doff+i+10]*256*256 + data[doff+i+11]*256*256*256); - ttv = tv+631065600; // garmin epoch offset - tmp = gmtime(&ttv); - strftime(tbuf, sizeof tbuf, "%Y-%m-%dT%H:%M:%SZ", tmp); // format for printing - memcpy((void *)&alt, data+doff+i+12, 4); - memcpy((void *)&dist, data+doff+i+16, 4); - if (dbg) printf("lat %.10g lon %.10g hr %d cad %d u1 %d u2 %d tv %d %s alt %f dist %f\n", lat, lon, - hr, cad, u1, u2, tv, tbuf, alt, dist); - printf(" \n"); - printf(" \n",tbuf); - printf(" \n"); - printf(" %d.%07d\n", - (int)lat, (int)(round(10000000*fabs(lat-(int)lat)))); - printf(" %d.%07d\n", - (int)lon, (int)(round(10000000*fabs(lon-(int)lon)))); - printf(" \n"); - printf(" %.7f\n", alt+0.000000005); - printf(" %.7f\n", dist+0.000000005); + if (dbg) printf("lat %.10g lon %.10g hr %d cad %d u1 %d u2 %d tv %d %s alt %f dist %f %02x %02x%02x%02x%02x\n", lat, lon, + hr, cad, u1, u2, tv, tbuf, alt, dist, data[doff+i+3], data[doff+i+16], data[doff+i+17], data[doff+i+18], data[doff+i+19]); + // track pause only if following trackpoint is aswell 'timemarker' with utopic distance + if (track_pause && dist > (float)40000000) { + fprintf(tcxfile, " \n"); + fprintf(tcxfile, " \n"); + } + fprintf(tcxfile, " \n"); + fprintf(tcxfile, " \n",tbuf); + if (lat < 90) { + fprintf(tcxfile, " \n"); + fprintf(tcxfile, " %s\n", + ground(lat)); + fprintf(tcxfile, " %s\n", + ground(lon)); + fprintf(tcxfile, " \n"); + fprintf(tcxfile, " %s\n", ground(alt)); + } + // last trackpoint has utopic distance, 40000km should be enough, hack? + if (dist < (float)40000000) { + fprintf(tcxfile, " %s\n", ground(dist)); + } if (hr > 0) { - printf(" \n"); - printf(" %d\n", hr); - printf(" \n"); + fprintf(tcxfile, " \n"); + fprintf(tcxfile, " %d\n", hr); + fprintf(tcxfile, " \n"); } - printf(" %s\n", - u1 ? "Present" : "Absent"); - printf(" \n"); - if (cad != 255) { - printf(" \n"); - printf(" %d\n", cad); - printf(" \n"); - } else - printf(" \n"); - printf(" \n"); - printf(" \n"); + // for bikes the cadence is written here and for the footpod in , why garmin? + if (sporttyp_track[track_id-firsttrack_id] == 1) { + if (cad != 255) { + fprintf(tcxfile, " %d\n", cad); + } + } + if (dist < (float)40000000) { + fprintf(tcxfile, " %s\n", u1 ? "Present" : "Absent"); + fprintf(tcxfile, " \n"); + fprintf(tcxfile, " \n"); + } else { + fprintf(tcxfile, "Footpod\""); + if (cad != 255) { + fprintf(tcxfile, ">\n"); + fprintf(tcxfile, " %d\n", cad); + fprintf(tcxfile, " \n"); + } else { + fprintf(tcxfile, "/>\n"); + } + } + fprintf(tcxfile, " \n"); + track_pause = 0; + } + fprintf(tcxfile, " \n"); + // maybe if we recieve utopic position and distance this tells pause in the run (stop and go) if not begin or end of lap + if (dist > (float)40000000 && track_pause == 0) { + track_pause = 1; + if (dbg) printf("track pause (stop and go)\n"); + } else { + track_pause = 0; + } + tv_previous = tv; + } // end of for (i = 4; i < pktlen; i += 24) + previoustrack_id = track_id; + break; + case 149: + printf("%d Lap data id: %u %u\n", pkttype, + data[doff] + data[doff+1]*256, data[doff+2] + data[doff+3]*256); + if (lap < MAXLAPS) { + memcpy((void *)&lapbuf[lap][0], data+doff, 48); + lastlap = lap; + lap++; } break; + case 247: + memset(modelname, 0, sizeof modelname); + memcpy(modelname, data+doff+88, dsize-88); + printf("%d Device name %s\n", pkttype, modelname); + break; default: printf("don't know how to decode packet type %d\n", pkttype); for (i = doff; i < dsize && i < doff+pktlen; i++) @@ -527,12 +835,12 @@ chevent(uchar chan, uchar event) if (dbg) printf("burst\n"); break; case EVENT_RX_FAKE_BURST: - if (dbg) printf("rxfake burst pairing %d blast %d waitauth %d\n", - pairing, blast, waitauth); + if (dbg) printf("rxfake burst pairing %d blast %ld waitauth %d\n", + pairing, (long)blast, waitauth); blsize = *(int *)(cbuf+4); memcpy(&blast, cbuf+8, 4); if (dbg) { - printf("fake burst %d %lx ", blsize, blast); + printf("fake burst %d %lx ", blsize, (long)blast); for (i = 0; i < blsize && i < 64; i++) printf("%02x", blast[i]); printf("\n"); @@ -616,7 +924,7 @@ chevent(uchar chan, uchar event) bzero(pair.devname, sizeof pair.devname); //if (peerdev <= 9999999) // only allow 7 digits //sprintf(pair.devname, "%u", peerdev); - sprintf(pair.devname, fname); + strcpy(pair.devname, fname); //else // fprintf(stderr, "pair dev name too large %08x \"%d\"\n", peerdev, peerdev); pair.u1 = strlen(pair.devname); @@ -873,12 +1181,13 @@ main(int ac, char *av[]) ANT_RequestMessage(chan, MESG_CHANNEL_STATUS_ID); //informative ANT_SetNetworkKeya(net, ANTSPT_KEY); ANT_AssignChannel(chan, chtype, net); - ANT_SetChannelId(chan, devno, devtype, manid); - ANT_RequestMessage(chan, MESG_CAPABILITIES_ID); //informative - ANT_SetChannelRFFreq(chan, freq); + // Wali: changed order of the following seq. according windows ANT_SetChannelPeriod(chan, period); ANT_SetChannelSearchTimeout(chan, srchto); + ANT_RequestMessage(chan, MESG_CAPABILITIES_ID); //informative + ANT_SetChannelRFFreq(chan, freq); ANT_SetSearchWaveform(chan, waveform); + ANT_SetChannelId(chan, devno, devtype, manid); ANT_OpenChannel(chan); ANT_RequestMessage(chan, MESG_CHANNEL_STATUS_ID); //informative @@ -886,3 +1195,4 @@ main(int ac, char *av[]) for(;;) sleep(10); } + -- 2.39.5