Toshihisa T
/
S1315FLogger
Skytraq S1315F-RAW-EVK Logger
main.cpp
- Committer:
- tosihisa
- Date:
- 2010-12-19
- Revision:
- 2:7eb11afe02bd
- Parent:
- 1:d1bb695fe3bc
File content as of revision 2:7eb11afe02bd:
/* * S1315F-RAW-EVK Logger * (c) @tosihisa http://twitter.com/tosihisa */ #include "mbed.h" #include "TextLCD.h" #include "SDFileSystem.h" #include "libT/mbed/tserialbuffer.h" #define OUTPUT_RATE_1HZ #define FSNAME "sd" using namespace libT; Serial debug(USBTX,USBRX); InterruptIn PPS(p15); PwmOut PPSLED(LED1); PwmOut LOGLED(LED4); tSerialBuffer gps(p13,p14); DigitalIn LogSW(p22); Ticker timer; TextLCD lcd(p24, p26, p27, p28, p29, p30); // rs, e, d4-d7 SDFileSystem sd(p5, p6, p7, p8, FSNAME); int logSW_state[2] = { 0 , 0 }; int logSW_idx = 0; unsigned long jif = 0; int do_disp = 0; void timer_handler() { logSW_state[logSW_idx] = (LogSW == 0) ? 1 : 0; logSW_idx = (logSW_idx + 1) & 1; jif++; do_disp++; } int pps_count = 0; void pps_rise() { pps_count++; if(pps_count & 1) PPSLED=0.80; else PPSLED=0.0; } typedef struct Skytraq_bin_info_s { int cjobst; unsigned short len; unsigned short recv; unsigned char sum; unsigned char ID; unsigned char body[2048]; void *work; unsigned long KERNEL_ver; unsigned long ODM_ver; unsigned long Revision; } Skytraq_bin_info_t; typedef struct Skytraq_ID_s { unsigned char ID; int (*func)(Skytraq_bin_info_t *info); char *name; } Skytraq_ID_t; int Skytraq_0x80(Skytraq_bin_info_t *info) { int idx = 1; info->KERNEL_ver = 0; info->KERNEL_ver = (info->KERNEL_ver << 8) | info->body[idx]; idx++; info->KERNEL_ver = (info->KERNEL_ver << 8) | info->body[idx]; idx++; info->KERNEL_ver = (info->KERNEL_ver << 8) | info->body[idx]; idx++; info->KERNEL_ver = (info->KERNEL_ver << 8) | info->body[idx]; idx++; info->ODM_ver = 0; info->ODM_ver = (info->ODM_ver << 8) | info->body[idx]; idx++; info->ODM_ver = (info->ODM_ver << 8) | info->body[idx]; idx++; info->ODM_ver = (info->ODM_ver << 8) | info->body[idx]; idx++; info->ODM_ver = (info->ODM_ver << 8) | info->body[idx]; idx++; info->Revision = 0; info->Revision = (info->Revision << 8) | info->body[idx]; idx++; info->Revision = (info->Revision << 8) | info->body[idx]; idx++; info->Revision = (info->Revision << 8) | info->body[idx]; idx++; info->Revision = (info->Revision << 8) | info->body[idx]; idx++; for(idx = 0;idx < 14;idx++){ debug.printf("%02x ",info->body[idx]); } debug.printf("\n"); debug.printf("KERNEL:0x%08x , ODM:0x%08x , Rev:0x%08x\n",info->KERNEL_ver,info->ODM_ver,info->Revision); return 0; } int Skytraq_0x83(Skytraq_bin_info_t *info) { debug.printf("\n"); debug.printf("ACK ID:0x%02x\n",info->body[0]); return 0; } Skytraq_ID_t Skytraq_ID_List[] = { { 0x01 ,NULL,"Input System Restart Force system to restart"}, { 0x02 ,NULL,"Input Query Software version Query revision information of software"}, { 0x03 ,NULL,"Input Query Software CRC Query the CRC of the software"}, { 0x04 ,NULL,"Input Set Factory Defaults Set system to factory default values"}, { 0x05 ,NULL,"Input Configure Serial Port Set up serial port COM, baud rate, data bits, stop bits and parity"}, { 0x06 ,NULL,"Input Reserved Reserved"}, { 0x07 ,NULL,"Input Reserved Reserved"}, { 0x08 ,NULL,"Input Configure NMEA Configure NMEA output message"}, { 0x09 ,NULL,"Input Configure Output Message Format Configure the output message format from GPS receiver" }, { 0x0C ,NULL,"Input Configure Power Mode Set system power mode" }, { 0x0E ,NULL,"Input Configure position update rate Configure the position update rate of GPS system" }, { 0x10 ,NULL,"Input Query position update rate Query the position update rate of GPS system" }, { 0x11 ,NULL,"Input Get Almanac Retrieve almanac data of the GPS receiver" }, { 0x12 ,NULL,"Input Configure binary measurement output rates Configure the output rates of the binary measurement outputs Input GPS Messages" }, { 0x29 ,NULL,"Input Configure Datum Configure Datum of the GPS receiver" }, { 0x2D ,NULL,"Input Query Datum Query datum used by the GPS receiver" }, { 0x30 ,NULL,"Input Get Ephemeris Retrieve ephemeris data of the GPS receiver Output System Messages" }, { 0x31 ,NULL,"Input Set ephemeris Set ephemeris data to the GPS receiver" }, { 0x37 ,NULL,"Input Configure WAAS Configure the enable or disable of WAAS" }, { 0x38 ,NULL,"Input Query WAAS status Query WAAS status of GPS receiver" }, { 0x39 ,NULL,"Input Configure position pinning Enable or disable position pinning of GPS receiver" }, { 0x3A ,NULL,"Input Query position pinning Query position pinning status of the GPS receiver" }, { 0x3B ,NULL,"Input Configure position pinning parameters Set position pinning parameters of GPS receiver" }, { 0x3C ,NULL,"Input Configuration navigation mode Configure the navigation mode of GPS system" }, { 0x3D ,NULL,"Input Query navigation mode Query the navigation mode of GPS receiver" }, { 0x3E ,NULL,"Input Configure 1PPS mode Set 1PPS mode to the GPS receiver" }, { 0x3F ,NULL,"Input Query 1PPS mode Query 1PPS mode of the GPS receiver" }, { 0x80 ,Skytraq_0x80,"Output Software version Software revision of the receiver" }, { 0x81 ,NULL,"Output Software CRC Software CRC of the receiver" }, { 0x82 ,NULL,"Output Reserved Reserved" }, { 0x83 ,Skytraq_0x83,"Output ACK ACK to a successful input message" }, { 0x84 ,NULL,"Output NACK Response to an unsuccessful input message" }, { 0x86 ,NULL,"Output Position update rate Position update rate of GPS system" }, { 0x87 ,NULL,"Output GPS Almanac Data Outputting the GPS Almanac Data of GPS receiver"}, { 0xAE ,NULL,"Output GPS Datum Datum used by the GPS receiver"}, { 0xB1 ,NULL,"Output GPS Ephemeris Data Outputting the GPS Ephemeris Data of GPS receiver"}, { 0xB3 ,NULL,"Output GPS WAAS status WAAS status of the GPS receiver"}, { 0xB4 ,NULL,"Output GPS Position pinning status Position pinning status of the GPS receiver"}, { 0xB5 ,NULL,"Output GPS navigation mode Navigation mode of the GPS receiver"}, { 0xB6 ,NULL,"Output GPS 1PPS mode 1PPS mode of GPS receiver"}, { 0xDC ,NULL,"Output MEAS_TIME Measurement time information"}, { 0xDD ,NULL,"Output RAW_MEAS Raw channel measurements"}, { 0xDE ,NULL,"Output SV_CH_STATUS SV and Channel status information"}, { 0xDF ,NULL,"Output RCV_STATE GPS receiver navigation state"}, { 0xE0 ,NULL,"Output SUBFRAME Subframe buffer data"}, { 0xFF ,NULL,"****UNKNOWN****"} }; unsigned char Skytraq_System_Restart[] = {0x01,0x01}; unsigned char Skytraq_QUERY_SOFTWARE_VERSION[] = {0x02,0x01}; unsigned char Skytraq_set_rate[] = { 0x12, /* ID */ 0x00, /* 00: 1Hz / 01: 2Hz / 02: 4Hz / 03: 5Hz / 04: 10Hz / 05: 20Hz / Others: 20Hz */ 0x01, /* Meas_time Enabling */ 0x01, /* Raw_meas Enabling */ 0x00, /* SV_CH_Staus Enabling */ 0x00, /* RCV_State Enabling */ 0x00, /* Subframe Enabling */ 0x00 /* Attributes */ }; void Skytraq_bin_send(unsigned short LEN,unsigned char *body) { unsigned short i; unsigned char sum; gps.putc(0xA0); gps.putc(0xA1); gps.putc((LEN >> 8) & 0x00ff); gps.putc(LEN & 0x00ff); sum = 0; for(i = 0;i < LEN;i++){ gps.putc((char)(*(body+i))); sum = sum ^ *(body+i); } gps.putc(sum); gps.putc(0x0D); gps.putc(0x0A); } void Skytraq_bin_parse(unsigned char c,Skytraq_bin_info_t *info) { if(info->cjobst == 0){ info->cjobst = (c == 0xA0) ? 1 : 0; } else if(info->cjobst == 1){ info->cjobst = (c == 0xA1) ? 2 : 0; } else if(info->cjobst == 2){ info->len = c; info->cjobst = 3; } else if(info->cjobst == 3){ info->len = (info->len << 8) | c; info->cjobst = 4; info->sum = 0; info->recv = 0; if(info->len >= sizeof(info->body)){ info->cjobst = 0; } } else if(info->cjobst == 4){ if(info->recv == 0){ info->ID = c; } else { info->body[info->recv - 1] = c; } info->sum = info->sum ^ c; info->recv++; if(info->recv >= info->len){ info->cjobst = 5; } } else if(info->cjobst == 5){ //debug.printf("sum[%02x][%02x]\n",info->sum,c); if(info->sum == c){ info->cjobst = 6; } else { info->cjobst = 0; debug.printf("\nSUMERROR[%02x][%02x]\n",info->sum,c); } } else if(info->cjobst == 6){ info->cjobst = (c == 0x0D) ? 7 : 0; } else if(info->cjobst == 7){ info->cjobst = (c == 0x0A) ? 8 : 0; } } char *logDir = "/" FSNAME "/GPSLOG"; unsigned short logNextCount = 0; int UseLogMemory = 0; char *logFile_getName(int addDir) { static char fname[32]; if (addDir) { snprintf(fname,sizeof(fname)-1,"%s/RAW%05d.STQ",logDir,logNextCount); } else { snprintf(fname,sizeof(fname)-1,"RAW%05d.STQ",logNextCount); } return fname; } void logFile_Init() { DIR *d; struct dirent *p; unsigned short countCandidate; char *endptr; int sts; UseLogMemory = 0; sts = mkdir(logDir,0777); debug.printf("mkdir(\"%s\") - %d\n",logDir,sts); d = opendir(logDir); if ( d != NULL ) { while ( (p = readdir(d)) != NULL ) { debug.printf("FILE - %s\x0d\x0a", p->d_name); if (strlen(p->d_name) == (sizeof("RAWxxxxx.stq")-1)) { if ( ((p->d_name[0] == 'R') || (p->d_name[0] == 'r')) && ((p->d_name[1] == 'A') || (p->d_name[1] == 'a')) && ((p->d_name[2] == 'W') || (p->d_name[2] == 'w'))) { } countCandidate = (unsigned short)strtoul(&(p->d_name[3]),&endptr,10); if (strcasecmp(endptr,".STQ") == 0) { if (countCandidate > logNextCount) { logNextCount = countCandidate; } } } } closedir(d); logNextCount++; UseLogMemory = 1; } } unsigned char wbuf[512]; int wcnt = 0; int main() { Skytraq_bin_info_t bin_info; int i; unsigned long recv_bytes = 0; FILE *fp = NULL; unsigned char c; bin_info.cjobst = 0; debug.format(8,Serial::None,1); debug.baud(115200); debug.printf("GPS Logger \"__S1315F__1\" Start (BUILD:[" __DATE__ "/" __TIME__ "])\x0d\x0a"); PPS.rise(pps_rise); lcd.cls(); lcd.locate(0,0); lcd.printf("S1315F Logger"); lcd.locate(0,1); lcd.printf("Please wait"); wait(1.0); logFile_Init(); gps.format(8,Serial::None,1); gps.baud(115200); gps.recvStart(); Skytraq_set_rate[1] = 0x05; #ifdef OUTPUT_RATE_1HZ Skytraq_set_rate[1] = 0x00; #endif lcd.locate(0,1); lcd.printf("S1315F Restart"); Skytraq_bin_send(sizeof(Skytraq_System_Restart),Skytraq_System_Restart); wait(1.0); lcd.locate(0,1); lcd.printf("S1315F Configure"); Skytraq_bin_send(sizeof(Skytraq_set_rate),Skytraq_set_rate); //Skytraq_bin_send(sizeof(Skytraq_QUERY_SOFTWARE_VERSION),Skytraq_QUERY_SOFTWARE_VERSION); timer.attach_us(timer_handler,1000*50); lcd.cls(); while(1) { if(do_disp){ do_disp = 0; lcd.locate(0,0); if(UseLogMemory){ lcd.printf("LOG:%s",logFile_getName(0)); } else { lcd.printf("NO SD Card"); } lcd.locate(0,1); lcd.printf("R:%-10lu",recv_bytes); } if((logSW_state[0] == 0) && (logSW_state[1] == 0)){ if((fp != NULL) && (bin_info.cjobst == 0)){ if(wcnt > 0){ fwrite(wbuf,sizeof(wbuf[0]),wcnt,fp); wcnt = 0; } fclose(fp); fp = NULL; logNextCount++; recv_bytes=0; LOGLED=0; } } else if((logSW_state[0] != 0) && (logSW_state[1] != 0)){ if((fp == NULL) && (bin_info.cjobst == 0)){ if((fp = fopen(logFile_getName(1),"w+b")) != NULL){ recv_bytes=0; LOGLED=0.80; } } } if (gps.readable()) { c = (unsigned char)(gps.getc()); Skytraq_bin_parse(c,&bin_info); recv_bytes++; if(fp != NULL){ wbuf[wcnt] = c; wcnt++; if(wcnt >= sizeof(wbuf)){ fwrite(wbuf,sizeof(wbuf[0]),wcnt,fp); wcnt = 0; } } if(bin_info.cjobst == 8){ for(i = 0;Skytraq_ID_List[i].ID != 0xFF;i++){ if(bin_info.ID == Skytraq_ID_List[i].ID){ break; } } debug.printf("ID=%02x len=%5d [%s]\n",bin_info.ID,bin_info.len,Skytraq_ID_List[i].name); if(Skytraq_ID_List[i].func != NULL){ (*Skytraq_ID_List[i].func)(&bin_info); } bin_info.cjobst = 0; } //debug.printf("[%02x]",gps.getc()); } } }