Toshihisa T
/
S1315FLogger
Skytraq S1315F-RAW-EVK Logger
Diff: main.cpp
- Revision:
- 0:e0ec137da369
- Child:
- 1:d1bb695fe3bc
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Sat Dec 18 11:17:21 2010 +0000 @@ -0,0 +1,324 @@ + +/* + * 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 + +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, "sd"); + +int logSW_state[2] = { 0 , 0 }; +int logSW_idx = 0; +unsigned long jif = 0; +void timer_handler() +{ + logSW_state[logSW_idx] = (LogSW != 0) ? 1 : 0; + logSW_idx = (logSW_idx + 1) & 1; + jif++; +} + +int pps_count = 0; +void pps_rise() +{ + pps_count++; + if(pps_count & 1) + PPSLED=0.20; + 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; +} + +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 ,NULL,"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****"} +}; + +char Skytraq_QUERY_SOFTWARE_VERSION[] = {0xA0,0xA1,0x00,0x02,0x02,0x00,0x02,0x0D,0x0A}; + +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){ + if(c == 0xa0){ + info->cjobst = 1; + } else { + info->cjobst = 0; + } + } else if(info->cjobst == 1){ + if(c == 0xa1){ + info->cjobst = 2; + } else { + info->cjobst = 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); + } + } +} + +unsigned char wbuf[512]; +int wcnt = 0; + +int main() { + Skytraq_bin_info_t bin_info; + int i; + unsigned long recv_count = 0; + unsigned long recv_bytes = 0; + FILE *fp = NULL; + int save_done = 0; + unsigned char c; + int do_log = 0; + + 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); + + 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 + Skytraq_bin_send(sizeof(Skytraq_set_rate),Skytraq_set_rate); + + timer.attach_us(timer_handler,1000*100); + + while(1) { +// for(i = 0;i < sizeof(Skytraq_QUERY_SOFTWARE_VERSION);i++){ +// gps.putc(Skytraq_QUERY_SOFTWARE_VERSION[i]); +// } + if((logSW_state[0] == 0) && (logSW_state[0] == 0)){ + LOGLED=0; + do_log = 0; +#if 0 + if(recv_bytes >= (2024*1024)){ + fclose(fp); + fp = NULL; + debug.printf("FILE Close\n"); + save_done = 1; + } +#endif + } + if((logSW_state[0] != 0) && (logSW_state[0] != 0)){ + LOGLED=0.20; + do_log = 1; +#if 0 + if(fp == NULL){ + fp = fopen("/sd/rawdata.dat","w+b"); + debug.printf("FILE Open=%p\n",fp); + wcnt = 0; + } +#endif + } + + 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]),sizeof(wbuf)/sizeof(wbuf[0]),fp); + wcnt = 0; + } + } + if(bin_info.cjobst == 6){ + for(i = 0;Skytraq_ID_List[i].ID != 0xFF;i++){ + if(bin_info.ID == Skytraq_ID_List[i].ID){ + break; + } + } + recv_count++; + lcd.locate(0,1); + lcd.printf("R:%-10lu %c",recv_bytes,(do_log) ? '*' : ' '); + 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()); + } + } +}