GPSrcvr.cpp

00001 /*
00002  * mbed Application program / GPS receiver control and 1PPS output
00003  *
00004  * Copyright (c) 2004,'09,'10,'16 Kenji Arai / JH1PJL
00005  *  http://www.page.sannet.ne.jp/kenjia/index.html
00006  *  http://mbed.org/users/kenjiArai/
00007  *      Created: March     28th, 2004   Kenji Arai
00008  *      updated: July      25th, 2009   for PIC24USB
00009  *      updated: January   16th, 2010   change to GPS-GM318
00010  *      updated: April     24th, 2010   for mbed / NXP LPC1768
00011  *      Revised: Nomeber   13th, 2016
00012  */
00013 
00014 //  Include --------------------------------------------------------------------
00015 #include    <string.h>
00016 #include    "mbed.h"
00017 #include    "GPSrcvr.h"
00018 #if defined(TARGET_NUCLEO_F411RE) || defined(TARGET_NUCLEO_F446RE)
00019 #include    "iSerial.h"
00020 #elif defined(TARGET_STM32F746NG)
00021 #include    "RingBuffer.h"
00022 #endif
00023 
00024 //  Definition -----------------------------------------------------------------
00025 //#define     USE_DEBUG
00026 
00027 #ifdef      USE_DEBUG
00028 #define     U_DEBUGBAUD(x)  pc.baud(x)
00029 #define     U_DEBUG(...)    pc.printf(__VA_ARGS__)
00030 #define     DBG(c)          pc.putc(c)
00031 #else
00032 #define     U_DEBUGBAUD(x)  {;}
00033 #define     U_DEBUG(...)    {;}
00034 #define     DBG(c)          {;}
00035 #endif
00036 
00037 #define     GSP_BUF_B       (128 * 3)
00038 #define     GPS_BUF_S       (128 * 2)
00039 
00040 //  Object ---------------------------------------------------------------------
00041 #if defined(TARGET_NUCLEO_F411RE) || defined(TARGET_NUCLEO_F446RE)
00042 DigitalIn   gps_rx(PC_7);       // for checking GPS RX line
00043 iSerial     gps(NC, PC_7, 0, 1024); // GPS Data receive
00044 #error "This is only on DISCO-F746NG!!"
00045 #elif defined(TARGET_STM32F746NG)
00046 RingBuffer  rxbuf(1024);        // can receive all information every one sec
00047 DigitalIn   gps_rx(PF_6);       // for checking GPS RX line
00048 Serial      gps(NC, PF_6);      // GPS Data receive
00049 #else
00050 #error "Target is only Nucleo-F411RE(F446RE) or DISCO-F746NG!!!"
00051 #endif
00052 
00053 extern Serial pc;
00054 
00055 //  RAM ------------------------------------------------------------------------
00056 bool        gps_is_okay_flag;
00057 uint8_t     gps_ready;
00058 uint8_t     gps_status;
00059 uint8_t     gps_rmc_ready;
00060 char        GPS_Buffer[GSP_BUF_B];       //GPS data buffer
00061 char        MsgBuf_RMC[GPS_BUF_S];
00062 char        MsgBuf_GSA[GPS_BUF_S];
00063 char        MsgBuf_GGA[GPS_BUF_S];
00064 
00065 //  Function prototypes --------------------------------------------------------
00066 uint8_t     check_gps_3d(void);
00067 uint8_t     check_gps_ready(void);
00068 void        get_time_and_date(struct tm *pt);
00069 void        getline_gps(void);
00070 void        gps_data_rcv(void);
00071 #if defined(TARGET_STM32F746NG)
00072 void        iGPSrcv_initialize(void);
00073 int         iGPS_readable(void);
00074 int         iGPS_getc(void);
00075 #endif
00076 
00077 ////////////////////////////////////////////////////////////////////////////////
00078 // Receive GPS data using Interrupt handler
00079 //
00080 // !!! Takes urgent and dirty solution by due to IRQ restriction
00081 //      on mbed library (reason is unknown as of today)
00082 // reference source file:stm32f746xx.h
00083 //     under /mbed-dev/targets/cmsis/TARGET_STM/TARGET_NUCLEO_F746ZG
00084 ////////////////////////////////////////////////////////////////////////////////
00085 #if defined(TARGET_STM32F746NG)
00086 
00087 // ------ Interrupt Function ------
00088 void rx_handler(void)
00089 {
00090     uint32_t reg = UART7->ISR;
00091     if (reg && USART_ISR_RXNE){ // data is ready to read
00092         UART7->ISR &= ~(USART_ISR_RXNE + USART_ISR_PE + USART_ISR_FE
00093                         + USART_ISR_NE + USART_ISR_ORE);
00094         rxbuf.save((unsigned char)UART7->RDR);   
00095     }
00096 }
00097 
00098 void iGPSrcv_initialize(void)
00099 {
00100     __disable_irq();
00101     UART7->CR1 &= ~(USART_CR1_TE + USART_CR1_TCIE + USART_CR1_TXEIE
00102                      + USART_CR1_PEIE + USART_CR1_IDLEIE);
00103     UART7->CR1 |= (USART_CR1_RXNEIE + USART_CR1_RE + USART_CR1_UE);
00104     NVIC_SetVector(UART7_IRQn, (uint32_t)rx_handler);
00105     NVIC_ClearPendingIRQ(UART7_IRQn);
00106     NVIC_EnableIRQ(UART7_IRQn);
00107     __enable_irq();
00108 #if 0
00109     printf("UART7->CR1  0x%08x:0x%08x\r\n", &UART7->CR1, UART7->CR1);
00110     printf("UART7->CR2  0x%08x:0x%08x\r\n", &UART7->CR2, UART7->CR2);
00111     printf("UART7->CR3  0x%08x:0x%08x\r\n", &UART7->CR3, UART7->CR3);
00112     printf("UART7->BRR  0x%08x:0x%08x\r\n", &UART7->BRR, UART7->BRR);
00113     printf("UART7->GTPR 0x%08x:0x%08x\r\n", &UART7->GTPR, UART7->GTPR);
00114     printf("UART7->RTOR 0x%08x:0x%08x\r\n", &UART7->RTOR, UART7->RTOR);
00115     printf("UART7->RQR  0x%08x:0x%08x\r\n", &UART7->RQR, UART7->RQR);
00116     printf("UART7->ISR  0x%08x:0x%08x\r\n", &UART7->ISR, UART7->ISR);
00117     printf("UART7->ICR  0x%08x:0x%08x\r\n", &UART7->ICR, UART7->ICR);
00118 #endif
00119 }
00120 
00121 int iGPS_readable(void)
00122 {
00123     return rxbuf.check();
00124 }
00125 
00126 int iGPS_getc(void)
00127 {
00128     while(!rxbuf.check()){;}    // wait receiving a character
00129     return rxbuf.read();
00130 }
00131 
00132 #endif
00133 
00134 ////////////////////////////////////////////////////////////////////////////////
00135 //  Parse GPS data
00136 //      Module Type: u-blux7 NEO-7M
00137 ////////////////////////////////////////////////////////////////////////////////
00138 // Get RMC & GAA data
00139 void gps_data_rcv(void)
00140 {
00141     int8_t i;
00142     time_t old_ave_sec[2];
00143     uint32_t diff = 0;
00144     time_t seconds;
00145     struct tm   t_gps;
00146 
00147     seconds = time(NULL);
00148     U_DEBUG("\r\nCurrent Time: %s\r\n", ctime(&seconds));
00149     old_ave_sec[0] = 0;
00150     old_ave_sec[1] = 0;
00151     // Wait long interval (every 1sec)
00152     for (uint32_t i = 0; i < 100000; i++){
00153         if (gps_rx == 0){
00154             i = 0;
00155         }
00156     }
00157     U_DEBUG("GPS line is Hi\r\n");
00158 #if defined(TARGET_STM32F746NG)
00159     //  Clear GPS RX line errors(over run)
00160     UART7->ICR = 0;
00161     UART7->CR1 = 0;
00162     UART7->CR1 = 5;
00163     iGPSrcv_initialize();
00164 #endif
00165     U_DEBUG("Start GPS!!");
00166     while(true) {
00167         getline_gps();          // Get GPS data from UART
00168         if (strncmp(GPS_Buffer, "$GPRMC",6) == 0) {
00169             for (i=0; GPS_Buffer[i] != 0; i++) {// Copy msg to RMC buffer
00170                 MsgBuf_RMC[i] = GPS_Buffer[i];
00171             }
00172             MsgBuf_RMC[i+1] = 0;
00173             DBG('2');
00174             if (gps_ready == 3) {
00175                 DBG('3');
00176                 get_time_and_date(&t_gps);
00177                 seconds = mktime(&t_gps);
00178                 if (old_ave_sec[0] == 0){
00179                     old_ave_sec[0] = seconds;
00180                 } else if (old_ave_sec[1] == 0){
00181                     old_ave_sec[1] = seconds;
00182                 } else {
00183                     if (old_ave_sec[0] >= old_ave_sec[1]){
00184                         diff = old_ave_sec[0] - old_ave_sec[1];
00185                     } else {
00186                         diff = old_ave_sec[1] - old_ave_sec[0];
00187                     }
00188                     if (diff > 100 ){
00189                         old_ave_sec[0] = seconds;
00190                         old_ave_sec[1] = 0;
00191                     } else {
00192                         if (old_ave_sec[0] > old_ave_sec[1]){
00193                             diff = seconds - old_ave_sec[0];
00194                             if (diff < 100){
00195                                 old_ave_sec[1] = seconds;
00196                             }
00197                         } else {
00198                             diff = seconds - old_ave_sec[1];
00199                             if (diff < 100){
00200                                 old_ave_sec[0] = seconds;
00201                             }
00202                         }
00203                         set_time(seconds);
00204                         DBG('4');
00205                         return;
00206                     }
00207                 }
00208             }
00209         } else if (strncmp(GPS_Buffer, "$GPGSA",6) == 0) {
00210             for (i=0; GPS_Buffer[i] != 0; i++) {// Copy msg to GSA buffer
00211                 MsgBuf_GSA[i] = GPS_Buffer[i];
00212             }
00213             MsgBuf_GSA[i+1] = 0;
00214             gps_ready = check_gps_3d();
00215             DBG('x');
00216         } else if (strncmp(GPS_Buffer, "$GPGGA",6) == 0) {
00217             for (i=0; GPS_Buffer[i] != 0; i++) {// Copy msg to GGA buffer
00218                 MsgBuf_GGA[i] = GPS_Buffer[i];
00219             }
00220             MsgBuf_GGA[i+1] = 0;
00221             gps_status = check_gps_ready();
00222             DBG('1');
00223         }
00224     }
00225 }
00226 
00227 bool check_gps_is_okay()
00228 {
00229     return gps_is_okay_flag;
00230 }
00231 
00232 // Get line of data from GPS
00233 void getline_gps(void)
00234 {
00235     char *p = GPS_Buffer;
00236 
00237 #if defined(TARGET_NUCLEO_F411RE) || defined(TARGET_NUCLEO_F446RE)
00238     while (gps.getc()  != '$'){;} 
00239 #elif defined(TARGET_STM32F746NG)
00240     while (iGPS_getc() != '$'){;}
00241 #endif
00242     *p++ = '$';
00243 #if 0
00244     U_DEBUG("\r\n");
00245 #else
00246     pc.printf("\r\n");
00247 #endif
00248     for (char i= 0;i < 150;i++){
00249 #if defined(TARGET_NUCLEO_F411RE) || defined(TARGET_NUCLEO_F446RE)
00250         *p = gps.getc(); 
00251 #elif defined(TARGET_STM32F746NG)
00252         *p = iGPS_getc();
00253 #endif
00254 #if 0
00255         DBG(*p);
00256 #else
00257         pc.putc(*p);
00258 #endif
00259         if (*p == '\r'){
00260             *++p = '\n';
00261             *++p = 0;
00262             *++p = 0;
00263             U_DEBUG("Get one GPS data\r\n");
00264             return;
00265         } else {
00266             p++;
00267         }
00268     }
00269     *++p = 0;
00270     *++p = 0;
00271 }
00272 
00273 // ASCII to hex
00274 uint8_t hex(char c){
00275     if(c<= '/') return( ERR );
00276     if(((c -= '0')<= 9 || 10 <= ( c -= 'A' - '0' - 10)) && c <= 15) {
00277         return((uint8_t)c);
00278     }
00279     return(ERR);
00280 }
00281 
00282 // Search next ',' (comma)
00283 char *next_comma( char *p ){
00284     while (1) {
00285         if ( *p== ',' ) {
00286             return ++p;
00287         } else if ( *p == 0 ) {
00288             return (char*)-1;
00289         } else {
00290             p++;
00291         }
00292     }
00293 }
00294 
00295 // 2 digits ASCII char to one byte hex
00296 unsigned char   change_acii_hex( char*p ){
00297     unsigned char c;
00298 
00299     c = hex(*p++);              /* No concern ERR condition! */
00300     c = (c * 10) + hex(*p++);
00301     return c;
00302 }
00303 
00304 // Skip number of comma
00305 char *num_of_comma( char *p, int8_t n ){
00306     for (; n> 0; n--) {
00307         if ( (p = next_comma(p)) == (char*) -1 ) {
00308             return (char*)-1;
00309         }
00310     }
00311     return p;
00312 }
00313 
00314 // Get GPS status (1=Not fix, 2=2D, 3=3D)
00315 uint8_t check_gps_3d(void){
00316     char *p;
00317     uint8_t x;
00318     uint8_t d;
00319 
00320     // pick-up time
00321     p = MsgBuf_GSA;
00322     p = num_of_comma(p,1);  // skip ','
00323     // reach to "Position Fix Indicator"
00324     if (*p == 'A') {
00325         ++p; //','
00326         ++p; // 1 or 2 or 3
00327         d = hex(*p++);
00328         if (d != ERR) {
00329             x = d;
00330         } else {
00331             x = 1;
00332         }
00333     } else {
00334         x = 1;
00335     }
00336     return x;
00337 }
00338 
00339 // Get GPS status and number of satelites
00340 uint8_t check_gps_ready(void){
00341     char *p;
00342     uint8_t x;
00343     uint8_t d;
00344 
00345     // pick-up time
00346     p = MsgBuf_GGA;
00347     p = num_of_comma(p,6);  // skip ','
00348     // reach to "Position Fix Indicator"
00349     if (*p == '1' || *p == '2') {
00350         ++p; //','
00351         ++p; // Number
00352         d = hex(*p++);
00353         if (d != ERR) {
00354             x = d * 10;
00355             x += (uint32_t)hex(*p++);
00356             if (*p != ',') {
00357                 x = 0;
00358             }
00359         } else {
00360             x = 0;
00361         }
00362     } else {
00363         x = 0;
00364     }
00365     return x;
00366 }
00367 
00368 //  Get time(UTC) from GPS data
00369 void get_time_and_date(struct tm *pt){
00370     char *p;
00371 
00372     p = MsgBuf_RMC;
00373     p = num_of_comma(p,1);  /* skip one ',' */
00374     pt->tm_hour   = change_acii_hex(p);
00375     p += 2;
00376     pt->tm_min = change_acii_hex(p);
00377     p += 2;
00378     pt->tm_sec = change_acii_hex(p);
00379     p = MsgBuf_RMC;
00380     p = num_of_comma(p,9);  /* skip one ',' */
00381     pt->tm_mday  = change_acii_hex(p);
00382     p += 2;
00383     pt->tm_mon = change_acii_hex(p) - 1;
00384     p += 2;
00385     pt->tm_year  = change_acii_hex(p) + 100;
00386 }