main.cpp

00001 
00002 /*
00003  * S1315F-RAW-EVK Logger
00004  * (c) @tosihisa http://twitter.com/tosihisa
00005  */
00006 
00007 #include "mbed.h"
00008 #include "TextLCD.h"
00009 #include "SDFileSystem.h"
00010 #include "libT/mbed/tserialbuffer.h"
00011 
00012 #define OUTPUT_RATE_1HZ
00013 
00014 #define FSNAME "sd"
00015 
00016 using namespace libT;
00017 
00018 Serial debug(USBTX,USBRX);
00019 InterruptIn PPS(p15);
00020 PwmOut PPSLED(LED1);
00021 PwmOut LOGLED(LED4);
00022 tSerialBuffer gps(p13,p14);
00023 DigitalIn LogSW(p22);
00024 Ticker timer;
00025 
00026 TextLCD lcd(p24, p26, p27, p28, p29, p30); // rs, e, d4-d7
00027 SDFileSystem sd(p5, p6, p7, p8, FSNAME);
00028 
00029 int logSW_state[2] = { 0 , 0 };
00030 int logSW_idx = 0;
00031 unsigned long jif = 0;
00032 int do_disp = 0;
00033 void timer_handler()
00034 {
00035     logSW_state[logSW_idx] = (LogSW == 0) ? 1 : 0;
00036     logSW_idx = (logSW_idx + 1) & 1;
00037     jif++;
00038     do_disp++;
00039 }
00040 
00041 int pps_count = 0;
00042 void pps_rise()
00043 {
00044     pps_count++;
00045     if(pps_count & 1)
00046         PPSLED=0.80;
00047     else
00048         PPSLED=0.0;
00049 }
00050 
00051 typedef struct Skytraq_bin_info_s {
00052     int cjobst;
00053     unsigned short len;
00054     unsigned short recv;
00055     unsigned char sum;
00056     unsigned char ID;
00057     unsigned char body[2048];
00058     void *work;
00059 
00060     unsigned long KERNEL_ver;
00061     unsigned long ODM_ver;
00062     unsigned long Revision;
00063 } Skytraq_bin_info_t;
00064 
00065 typedef struct Skytraq_ID_s {
00066     unsigned char ID;
00067     int (*func)(Skytraq_bin_info_t *info);
00068     char *name;
00069 } Skytraq_ID_t;
00070 
00071 int Skytraq_0x80(Skytraq_bin_info_t *info)
00072 {
00073     int idx = 1;
00074 
00075     info->KERNEL_ver = 0;
00076     info->KERNEL_ver = (info->KERNEL_ver << 8) | info->body[idx]; idx++;
00077     info->KERNEL_ver = (info->KERNEL_ver << 8) | info->body[idx]; idx++;
00078     info->KERNEL_ver = (info->KERNEL_ver << 8) | info->body[idx]; idx++;
00079     info->KERNEL_ver = (info->KERNEL_ver << 8) | info->body[idx]; idx++;
00080 
00081     info->ODM_ver = 0;
00082     info->ODM_ver = (info->ODM_ver << 8) | info->body[idx]; idx++;
00083     info->ODM_ver = (info->ODM_ver << 8) | info->body[idx]; idx++;
00084     info->ODM_ver = (info->ODM_ver << 8) | info->body[idx]; idx++;
00085     info->ODM_ver = (info->ODM_ver << 8) | info->body[idx]; idx++;
00086 
00087     info->Revision = 0;
00088     info->Revision = (info->Revision << 8) | info->body[idx]; idx++;
00089     info->Revision = (info->Revision << 8) | info->body[idx]; idx++;
00090     info->Revision = (info->Revision << 8) | info->body[idx]; idx++;
00091     info->Revision = (info->Revision << 8) | info->body[idx]; idx++;
00092 
00093     for(idx = 0;idx < 14;idx++){
00094         debug.printf("%02x ",info->body[idx]);
00095     }
00096     debug.printf("\n");
00097     debug.printf("KERNEL:0x%08x , ODM:0x%08x , Rev:0x%08x\n",info->KERNEL_ver,info->ODM_ver,info->Revision);
00098 
00099     return 0;
00100 }
00101 
00102 int Skytraq_0x83(Skytraq_bin_info_t *info)
00103 {
00104     debug.printf("\n");
00105     debug.printf("ACK ID:0x%02x\n",info->body[0]);
00106     return 0;
00107 }
00108 
00109 Skytraq_ID_t Skytraq_ID_List[] = {
00110     { 0x01 ,NULL,"Input System Restart Force system to restart"},
00111     { 0x02 ,NULL,"Input Query Software version Query revision information of software"},
00112     { 0x03 ,NULL,"Input Query Software CRC Query the CRC of the software"},
00113     { 0x04 ,NULL,"Input Set Factory Defaults Set system to factory default values"},
00114     { 0x05 ,NULL,"Input Configure Serial Port Set up serial port COM, baud rate, data bits, stop bits and parity"},
00115     { 0x06 ,NULL,"Input Reserved Reserved"},
00116     { 0x07 ,NULL,"Input Reserved Reserved"},
00117     { 0x08 ,NULL,"Input Configure NMEA Configure NMEA output message"},
00118     { 0x09 ,NULL,"Input Configure Output Message Format Configure the output message format from GPS receiver" },
00119     { 0x0C ,NULL,"Input Configure Power Mode Set system power mode" },
00120     { 0x0E ,NULL,"Input Configure position update rate Configure the position update rate of GPS system" },
00121     { 0x10 ,NULL,"Input Query position update rate Query the position update rate of GPS system" },
00122     { 0x11 ,NULL,"Input Get Almanac Retrieve almanac data of the GPS receiver" },
00123     { 0x12 ,NULL,"Input Configure binary measurement output rates Configure the output rates of the binary measurement outputs Input GPS Messages" },
00124     { 0x29 ,NULL,"Input Configure Datum Configure Datum of the GPS receiver" },
00125     { 0x2D ,NULL,"Input Query Datum Query datum used by the GPS receiver" },
00126     { 0x30 ,NULL,"Input Get Ephemeris Retrieve ephemeris data of the GPS receiver Output System Messages" },
00127     { 0x31 ,NULL,"Input Set ephemeris Set ephemeris data to the GPS receiver" },
00128     { 0x37 ,NULL,"Input Configure WAAS Configure the enable or disable of WAAS" },
00129     { 0x38 ,NULL,"Input Query WAAS status Query WAAS status of GPS receiver" },
00130     { 0x39 ,NULL,"Input Configure position pinning Enable or disable position pinning of GPS receiver" },
00131     { 0x3A ,NULL,"Input Query position pinning Query position pinning status of the GPS receiver" },
00132     { 0x3B ,NULL,"Input Configure position pinning parameters Set position pinning parameters of GPS receiver" },
00133     { 0x3C ,NULL,"Input Configuration navigation mode Configure the navigation mode of GPS system" },
00134     { 0x3D ,NULL,"Input Query navigation mode Query the navigation mode of GPS receiver" },
00135     { 0x3E ,NULL,"Input Configure 1PPS mode Set 1PPS mode to the GPS receiver" },
00136     { 0x3F ,NULL,"Input Query 1PPS mode Query 1PPS mode of the GPS receiver" },
00137     { 0x80 ,Skytraq_0x80,"Output Software version Software revision of the receiver" },
00138     { 0x81 ,NULL,"Output Software CRC Software CRC of the receiver" },
00139     { 0x82 ,NULL,"Output Reserved Reserved" },
00140     { 0x83 ,Skytraq_0x83,"Output ACK ACK to a successful input message" },
00141     { 0x84 ,NULL,"Output NACK Response to an unsuccessful input message" },
00142     { 0x86 ,NULL,"Output Position update rate Position update rate of GPS system" },
00143     { 0x87 ,NULL,"Output GPS Almanac Data Outputting the GPS Almanac Data of GPS receiver"},
00144     { 0xAE ,NULL,"Output GPS Datum Datum used by the GPS receiver"},
00145     { 0xB1 ,NULL,"Output GPS Ephemeris Data Outputting the GPS Ephemeris Data of GPS receiver"},
00146     { 0xB3 ,NULL,"Output GPS WAAS status WAAS status of the GPS receiver"},
00147     { 0xB4 ,NULL,"Output GPS Position pinning status Position pinning status of the GPS receiver"},
00148     { 0xB5 ,NULL,"Output GPS navigation mode Navigation mode of the GPS receiver"},
00149     { 0xB6 ,NULL,"Output GPS 1PPS mode 1PPS mode of GPS receiver"},
00150     { 0xDC ,NULL,"Output MEAS_TIME Measurement time information"},
00151     { 0xDD ,NULL,"Output RAW_MEAS Raw channel measurements"},
00152     { 0xDE ,NULL,"Output SV_CH_STATUS SV and Channel status information"},
00153     { 0xDF ,NULL,"Output RCV_STATE GPS receiver navigation state"},
00154     { 0xE0 ,NULL,"Output SUBFRAME Subframe buffer data"},
00155     { 0xFF ,NULL,"****UNKNOWN****"}
00156 };
00157 
00158 unsigned char Skytraq_System_Restart[] = {0x01,0x01};
00159 unsigned char Skytraq_QUERY_SOFTWARE_VERSION[] = {0x02,0x01};
00160 unsigned char Skytraq_set_rate[] = {
00161     0x12,   /* ID */
00162     0x00,   /* 00: 1Hz / 01: 2Hz / 02: 4Hz / 03: 5Hz / 04: 10Hz / 05: 20Hz / Others: 20Hz */
00163     0x01,   /* Meas_time Enabling */
00164     0x01,   /* Raw_meas Enabling */
00165     0x00,   /* SV_CH_Staus Enabling */
00166     0x00,   /* RCV_State Enabling */
00167     0x00,   /* Subframe Enabling */
00168     0x00    /* Attributes */
00169 };
00170 
00171 void Skytraq_bin_send(unsigned short LEN,unsigned char *body)
00172 {
00173     unsigned short i;
00174     unsigned char sum;
00175     gps.putc(0xA0);
00176     gps.putc(0xA1);
00177     gps.putc((LEN >> 8) & 0x00ff);
00178     gps.putc(LEN & 0x00ff);
00179     sum = 0;
00180     for(i = 0;i < LEN;i++){
00181         gps.putc((char)(*(body+i)));
00182         sum = sum ^ *(body+i);
00183     }
00184     gps.putc(sum);
00185     gps.putc(0x0D);
00186     gps.putc(0x0A);
00187 }
00188 
00189 void Skytraq_bin_parse(unsigned char c,Skytraq_bin_info_t *info)
00190 {
00191     if(info->cjobst == 0){
00192         info->cjobst = (c == 0xA0) ? 1 : 0;
00193     } else if(info->cjobst == 1){
00194         info->cjobst = (c == 0xA1) ? 2 : 0;
00195     } else if(info->cjobst == 2){
00196         info->len = c;
00197         info->cjobst = 3;
00198     } else if(info->cjobst == 3){
00199         info->len = (info->len << 8) | c;
00200         info->cjobst = 4;
00201         info->sum = 0;
00202         info->recv = 0;
00203         if(info->len >= sizeof(info->body)){
00204             info->cjobst = 0;
00205         }
00206     } else if(info->cjobst == 4){
00207         if(info->recv == 0){
00208             info->ID = c;
00209         } else {
00210             info->body[info->recv - 1] = c;
00211         }
00212         info->sum = info->sum ^ c;
00213         info->recv++;
00214         if(info->recv >= info->len){
00215             info->cjobst = 5;
00216         }
00217     } else if(info->cjobst == 5){
00218         //debug.printf("sum[%02x][%02x]\n",info->sum,c);
00219         if(info->sum == c){
00220             info->cjobst = 6;
00221         } else {
00222             info->cjobst = 0;
00223             debug.printf("\nSUMERROR[%02x][%02x]\n",info->sum,c);
00224         }
00225     } else if(info->cjobst == 6){
00226         info->cjobst = (c == 0x0D) ? 7 : 0;
00227     } else if(info->cjobst == 7){
00228         info->cjobst = (c == 0x0A) ? 8 : 0;
00229     }
00230 }
00231 
00232 char *logDir = "/" FSNAME "/GPSLOG";
00233 unsigned short logNextCount = 0;
00234 int UseLogMemory = 0;
00235 
00236 char *logFile_getName(int addDir) {
00237     static char fname[32];
00238     if (addDir) {
00239         snprintf(fname,sizeof(fname)-1,"%s/RAW%05d.STQ",logDir,logNextCount);
00240     } else {
00241         snprintf(fname,sizeof(fname)-1,"RAW%05d.STQ",logNextCount);
00242     }
00243     return fname;
00244 }
00245 
00246 void logFile_Init() {
00247     DIR *d;
00248     struct dirent *p;
00249     unsigned short countCandidate;
00250     char *endptr;
00251     int sts;
00252 
00253     UseLogMemory = 0;
00254 
00255     sts = mkdir(logDir,0777);
00256     debug.printf("mkdir(\"%s\") - %d\n",logDir,sts);
00257     d = opendir(logDir);
00258     if ( d != NULL ) {
00259         while ( (p = readdir(d)) != NULL ) {
00260             debug.printf("FILE - %s\x0d\x0a", p->d_name);
00261             if (strlen(p->d_name) == (sizeof("RAWxxxxx.stq")-1)) {
00262                 if (    ((p->d_name[0] == 'R') || (p->d_name[0] == 'r'))
00263                         && ((p->d_name[1] == 'A') || (p->d_name[1] == 'a'))
00264                         && ((p->d_name[2] == 'W') || (p->d_name[2] == 'w'))) {
00265                 }
00266                 countCandidate = (unsigned short)strtoul(&(p->d_name[3]),&endptr,10);
00267                 if (strcasecmp(endptr,".STQ") == 0) {
00268                     if (countCandidate > logNextCount) {
00269                         logNextCount = countCandidate;
00270                     }
00271                 }
00272             }
00273         }
00274         closedir(d);
00275         logNextCount++;
00276         UseLogMemory = 1;
00277     }
00278 }
00279 
00280 unsigned char wbuf[512];
00281 int wcnt = 0;
00282 
00283 int main() {
00284     Skytraq_bin_info_t bin_info;
00285     int i;
00286     unsigned long recv_bytes = 0;
00287     FILE *fp = NULL;
00288     unsigned char c;
00289 
00290     bin_info.cjobst = 0;
00291 
00292     debug.format(8,Serial::None,1);
00293     debug.baud(115200);
00294     debug.printf("GPS Logger \"__S1315F__1\" Start (BUILD:[" __DATE__ "/" __TIME__ "])\x0d\x0a");
00295 
00296     PPS.rise(pps_rise);
00297 
00298     lcd.cls();
00299     lcd.locate(0,0);
00300     lcd.printf("S1315F Logger");
00301 
00302     lcd.locate(0,1);
00303     lcd.printf("Please wait");
00304     wait(1.0);
00305 
00306     logFile_Init();
00307 
00308     gps.format(8,Serial::None,1);
00309     gps.baud(115200);
00310     gps.recvStart();
00311 
00312     Skytraq_set_rate[1] = 0x05;
00313 #ifdef OUTPUT_RATE_1HZ
00314     Skytraq_set_rate[1] = 0x00;
00315 #endif
00316     lcd.locate(0,1);
00317     lcd.printf("S1315F Restart");
00318     Skytraq_bin_send(sizeof(Skytraq_System_Restart),Skytraq_System_Restart);
00319     
00320     wait(1.0);
00321 
00322     lcd.locate(0,1);
00323     lcd.printf("S1315F Configure");
00324 
00325     Skytraq_bin_send(sizeof(Skytraq_set_rate),Skytraq_set_rate);
00326     //Skytraq_bin_send(sizeof(Skytraq_QUERY_SOFTWARE_VERSION),Skytraq_QUERY_SOFTWARE_VERSION);
00327 
00328     timer.attach_us(timer_handler,1000*50);
00329 
00330     lcd.cls();
00331 
00332     while(1) {
00333         if(do_disp){
00334             do_disp = 0;
00335             lcd.locate(0,0);
00336             if(UseLogMemory){
00337                 lcd.printf("LOG:%s",logFile_getName(0));
00338             } else {
00339                 lcd.printf("NO SD Card");
00340             }
00341             lcd.locate(0,1);
00342             lcd.printf("R:%-10lu",recv_bytes);
00343         }
00344         if((logSW_state[0] == 0) && (logSW_state[1] == 0)){
00345             if((fp != NULL) && (bin_info.cjobst == 0)){
00346                 if(wcnt > 0){
00347                     fwrite(wbuf,sizeof(wbuf[0]),wcnt,fp);
00348                     wcnt = 0;
00349                 }
00350                 fclose(fp);
00351                 fp = NULL;
00352                 logNextCount++;
00353                 recv_bytes=0;
00354                 LOGLED=0;
00355             }
00356         } else if((logSW_state[0] != 0) && (logSW_state[1] != 0)){
00357             if((fp == NULL) && (bin_info.cjobst == 0)){
00358                 if((fp = fopen(logFile_getName(1),"w+b")) != NULL){
00359                     recv_bytes=0;
00360                     LOGLED=0.80;
00361                 }
00362             }
00363         }
00364 
00365         if (gps.readable()) {
00366             c = (unsigned char)(gps.getc());
00367             Skytraq_bin_parse(c,&bin_info);
00368             recv_bytes++;
00369             if(fp != NULL){
00370                 wbuf[wcnt] = c;
00371                 wcnt++;
00372                 if(wcnt >= sizeof(wbuf)){
00373                     fwrite(wbuf,sizeof(wbuf[0]),wcnt,fp);
00374                     wcnt = 0;
00375                 }
00376             }
00377             if(bin_info.cjobst == 8){
00378                 for(i = 0;Skytraq_ID_List[i].ID != 0xFF;i++){
00379                     if(bin_info.ID == Skytraq_ID_List[i].ID){
00380                         break;
00381                     }
00382                 }
00383                 debug.printf("ID=%02x len=%5d [%s]\n",bin_info.ID,bin_info.len,Skytraq_ID_List[i].name);
00384                 if(Skytraq_ID_List[i].func != NULL){
00385                     (*Skytraq_ID_List[i].func)(&bin_info);
00386                 }
00387                 bin_info.cjobst = 0;
00388             }
00389             //debug.printf("[%02x]",gps.getc());
00390         }
00391     }
00392 }