#include #include #include #include #include #include #include #include #include #include #define BUFFER_SIZE 4096 static void myHexDump(const char*buf, size_t buflen, uint8_t indent) { if (buflen == 0) { puts("00000000 (Null Length)"); return; } char newline[indent + 7]; newline[0] = '\n'; memset(&(newline[1]), '\t', indent); strcpy(&(newline[indent + 1]), "%08x "); for (size_t xdo = 0; xdo < buflen; ++xdo) { switch (xdo % 16) { case 0: printf(xdo ? newline : (newline + 1), xdo); break; case 8: printf(" "); break; } printf(" %02x", buf[xdo]); } puts(""); } int cellmoStartGPS(int sck) { struct sockaddr_pn sa; bzero(&sa, sizeof(sa)); sa.spn_family = AF_PHONET; sa.spn_resource = 16; if (4 != sendto(sck, "\0\20\1T", 4, 0, (struct sockaddr*)&sa, sizeof(sa))) return -1; sa.spn_resource = 84; if (28 != sendto(sck, "\0\220\0\1\0\0\0\0\0\0\0\0\t\1\0\20\0\0\0\n\0\0\0\3\0\0\0\0", 28, 0, (struct sockaddr*)&sa, sizeof(sa))) return -2; return 0; } int cellmoStopGPS(int sck) { struct sockaddr_pn sa; bzero(&sa, sizeof(sa)); sa.spn_family = AF_PHONET; sa.spn_resource = 84; if (28 != sendto(sck, "\0\220\1\1\0\0\0\0\0\0\0\5\t\1\0\20\0\0\0\1\0\0\0\3\0\n\0\0", 28, 0, (struct sockaddr*)&sa, sizeof(sa))) return -1; if (12 != sendto(sck, "\0\220\2\0\0\0\0\0\0\0\0\5", 12, 0, (struct sockaddr*)&sa, sizeof(sa))) return -2; return 0; } static int sck; static void sigShutdown(int signum) { printf("Shutdown signal received\n"); cellmoStopGPS(sck); exit(0); } int main() { signal(SIGHUP, sigShutdown); signal(SIGINT, sigShutdown); signal(SIGQUIT, sigShutdown); signal(SIGPIPE, sigShutdown); sck = socket(AF_PHONET, SOCK_DGRAM, 0); assert(sck >= 0); { struct sockaddr_pn sa; bzero(&sa, sizeof(sa)); sa.spn_family = AF_PHONET; sa.spn_resource = 255; assert(!bind(sck, (struct sockaddr*)&sa, sizeof(sa))); } assert(!cellmoStartGPS(sck)); char buf[BUFFER_SIZE]; while (1) { ssize_t buflen = recv(sck, buf, sizeof(buf), 0); assert(buflen > 0); printf("Read %d bytes : ", buflen); if (buflen < 9) continue; uint8_t spcount = buf[8]; printf("Total %d subpackets\n", spcount); uint8_t splen; for (size_t spoffset = 12; spoffset < buflen; spoffset += splen) { char*sp = buf + spoffset; assert(sp[0] == 9); uint8_t sptype = sp[1]; assert(sp[2] == 0); splen = sp[3]; char*spd = sp + 4; switch (sptype) { case 2: // Position: Latitude, longitude, altitude { printf("\tSubpacket: Position\n"); assert(splen >= 18); float lat = 360. * (((float)spd[0] / 0x100) + ((float)spd[1] / 0x10000) + ((float)spd[2] / 0x1000000) + ((float)spd[3] / 0x100000000)); if (lat > 180.) lat -= 360.; printf("\t\tLatitude : %f\n", lat); float lon = 360. * (((float)spd[4] / 0x100) + ((float)spd[5] / 0x10000) + ((float)spd[6] / 0x1000000) + ((float)spd[7] / 0x100000000)); if (lon > 180.) lon -= 360.; printf("\t\tLongitude: %f\n", lon); // 8,9,10,11 = Latitude error ? uint32_t eph = (spd[12] << 24) | (spd[13] << 16) | (spd[14] << 8) | spd[15]; printf("\t\teph: %dcm\n", eph); // 16,17 ? uint16_t altiA = (spd[18] << 8) | spd[19]; uint16_t altiB = (spd[22] << 8) | spd[23]; float alti = (float)(altiA - *((int16_t*)&altiB)) / 2; float epv2 = ((spd[20] << 8) | spd[21]) / 2; printf("\t\tAltitude: %.1fm (accurate to %.1f?)\n", alti, epv2); break; } case 3: // Date and time { printf("\tSubpacket: DateTime\n"); uint16_t date_year = (spd[0] << 8) | spd[1]; uint8_t date_month = spd[2]; uint8_t date_day = spd[3]; printf("\t\tDate: %04d-%02d-%02d\n", date_year, date_month, date_day); // 4 ? uint8_t time_hour = spd[5]; uint8_t time_minute = spd[6]; // 7 ? uint16_t time_ms = (spd[8] << 8) | spd[9]; uint8_t time_second = time_ms / 1000; time_ms %= 1000; uint16_t time_accuracy = (spd[10] << 8) | spd[11]; printf("\t\tTime: %02d:%02d:%02d.%03d (accuracy: %d)\n", time_hour, time_minute, time_second, time_ms, time_accuracy); break; } case 4: // Track, speed, climb { printf("\tSubpacket: Motion\n"); float track = ((spd[0] << 8) | spd[1]) / 100.; float epd = ((spd[2] << 8) | spd[3]) / 100.; printf("\t\tDirection of motion: %.2f degrees (accurate to %.2f degrees?)\n", track, epd); // 4,5? uint16_t speed = (spd[6] << 8) | spd[7]; uint16_t eps = (spd[8] << 8) | spd[9]; printf("\t\tSpeed: %d cm/sec (accurate to %d cm/sec)\n", speed, eps); uint16_t climbA = (spd[10] << 8) | spd[11]; int16_t climb = *((int16_t*)&climbA); uint16_t epc = (spd[12] << 8) | spd[13]; printf("\t\tClimb: %d cm/sec (accurate to %d cm/sec)\n", climb, epc); // 14,15? break; } case 5: // Sat info??? { printf("\tSubpacket: Satellites\n"); uint8_t satVisible = spd[0]; printf("\t\tSatellites Visible: %d\n", satVisible); for (size_t sos = 8; sos < splen; sos += 12) { char*sd = sp + sos; // 0 ? uint8_t PRN = sd[1]; uint8_t inUse = sd[2]; float signalStrength = (float)((sd[3] << 8) | sd[4]) / 100.0; // 5 ? float elevation = ((sd[6] << 8) | sd[7]) / 100.0; float azimuth = ((sd[8] << 8) | sd[9]) / 100.0; // 10,11 ? printf("\t\tSatellite: [%c] PRN=%2d signal=%.02f%% azimuth=%6.2f elevation=%5.2f\n", inUse ? 'x' : '_', PRN, signalStrength, azimuth, elevation); } break; } case 7: // CellInfoGSM { printf("\tSubpacket: CellInfoGSM\n"); uint16_t mcc = (spd[0] << 8) | spd[1]; printf("\t\tMobile Country Code: %d\n", mcc); uint16_t mnc = (spd[2] << 8) | spd[3]; printf("\t\tMobile Network Code: %d\n", mnc); uint16_t lac = (spd[4] << 8) | spd[5]; printf("\t\tLocation Area Code: %d\n", lac); uint16_t cellId = (spd[6] << 8) | spd[7]; printf("\t\tCell ID: %d\n", cellId); break; } case 8: // CellInfoWCDMA { printf("\tSubpacket: CellInfoWCDMA\n"); uint16_t mcc = (spd[0] << 8) | spd[1]; printf("\t\tMobile Country Code: %d\n", mcc); uint16_t mnc = (spd[2] << 8) | spd[3]; printf("\t\tMobile Network Code: %d\n", mnc); uint32_t ucid = (spd[4] << 24) | (spd[5] << 16) | (spd[6] << 8) | spd[7]; printf("\t\tUC ID: %d\n", ucid); break; } default: printf("\tSubpacket: Unknown (type 0x%02x), length %d:\n", sptype, splen); myHexDump(sp, splen, 2); } } } }