ublox.c

   1 #include <stdio.h>
   2 #include <string.h>
   3
   4 #include "ublox.h"
   5 #include "myuart.h"
   6 #include "byte2var.h"
   7
   8 // UBX packet内のデータ位置
   9 #define UBX_CLASS   2
  10 #define UBX_PAYLOAD 6
  11 // payload長さからパケット全体の長さ(checksum含む)をget
  12 #define UBX_PKTLEN(len_payload) (len_payload + 8)
  13
  14
  15
  16 // ublox_rcv_pkt()用
  17 #define UBX_STS_HEAD1   0
  18 #define UBX_STS_HEAD2   1
  19 #define UBX_STS_CLASS   2
  20 #define UBX_STS_ID      3
  21 #define UBX_STS_LEN1    4
  22 #define UBX_STS_LEN2    5
  23 #define UBX_STS_PAYLOAD 6
  24 #define UBX_STS_SUMA    7
  25 #define UBX_STS_SUMB    8
  26
  27 /*
  28  len: payload length
  29  */
  30 static void ublox_make_sum(unsigned char *pkt, unsigned int lenp, unsigned char *suma, unsigned char *sumb)
  31 {
  32     unsigned int    i;
  33     *suma = 0;
  34     *sumb = 0;
  35     for(i = 0; i < lenp+4; i++) {
  36         *suma += pkt[UBX_CLASS + i];
  37         *sumb += *suma;
  38     }
  39 }
  40 /*
  41  * GPSに送信するコマンドパケットの作成
  42  * unsgined char len: payloadのバイト数
  43  * return: 作成したパケットpktのバイト数
  44  */
  45 int ublox_make_cmd(unsigned char id1, unsigned char id2, unsigned int lenp, unsigned char *payload, unsigned char pkt[])
  46 {
  47     unsigned char   suma, sumb;
  48
  49     pkt[0] = 0xb5;
  50     pkt[1] = 0x62;
  51     pkt[2] = id1;
  52     pkt[3] = id2;
  53     pkt[4] = (0xFF & lenp);
  54     pkt[5] = (0xFF & (lenp >> 8));
  55     memcpy(&pkt[UBX_PAYLOAD], payload, lenp);
  56     // Check Sum
  57     ublox_make_sum(pkt, lenp, &suma, &sumb);
  58     pkt[UBX_PAYLOAD + lenp] = suma;
  59     pkt[UBX_PAYLOAD + lenp + 1] = sumb;
  60     return UBX_PKTLEN(lenp);
  61 }
  62 /*
  63  * Debug用
  64  * GPSから受信したパケットのモニタ出力
  65  */
  66 void ublox_debug_pkt_out(unsigned char *pkt, int len)
  67 {
  68     unsigned int i;
  69     char    buf[16];
  70
  71     for(i = 0; i < len; i++) {
  72         sprintf(buf, "%02X ", (unsigned char)(pkt[i] & 0xFF));
  73         PDEBUG(buf);
  74     }
  75     PDEBUG("\r\n");
  76 }
  77 /*
  78  * ubloxにデータ送信
  79  * polling
  80  */
  81 void ublox_send(unsigned char *pkt, unsigned int len_pkt)
  82 {
  83     unsigned int i;
  84
  85     for(i = 0; i < len_pkt; i++) {
  86 //        while(U1STAbits.UTXBF==1); /* Wait until TX buf read for new data */
  87         while(uart1_tx_is_full());
  88         uart1_putc(pkt[i]);
  89
  90     }
  91 }
  92 /**** コマンド送信 *************************/
  93 /*
  94  * CFG-PRT
  95  *  プロトコル選択
  96 */
  97 void ublox_send_cfg_prt(unsigned long bps)
  98 {
  99     const unsigned char   lenp = 20;
 100     unsigned char   payload[lenp];
 101     unsigned char   pkt[32];
 102     unsigned int i;
 103     unsigned int len_pkt;
 104
 105     payload[0] = 0x01;  // Port
 106     payload[1] = 0x00;
 107     payload[2] = 0x00;  // TxReady
 108     payload[3] = 0x00;
 109     // mode
 110     payload[4] = 0b11010000;    // 8bit
 111     payload[5] = 0b00001000;    // 1stop non-parity
 112     payload[6] = 0x00;
 113     payload[7] = 0x00;
 114     // baudrate
 115     payload[8] = (bps & 0xFF);
 116     payload[9] = ((bps >> 8)& 0xFF);
 117     payload[10] = ((bps >> 16)& 0xFF);
 118     payload[11] = ((bps >> 24)& 0xFF);
 119     payload[12] = 0x01;     // inProtoMask
 120     payload[13] = 0x00;
 121     payload[14] = 0x01;     // outProtoMask
 122     payload[15] = 0x00;
 123     payload[16] = 0x00;     // reserved
 124     payload[17] = 0x00;
 125     payload[18] = 0x00;
 126     payload[19] = 0x00;
 127
 128     len_pkt = ublox_make_cmd(UBX_CLS_CFG, UBX_ID_CFG_PRT, lenp, payload, pkt);
 129     ublox_send(pkt, len_pkt);
 130
 131 }
 132 /*
 133  CFG-NMEA
 134  */
 135 void ublox_send_cfg_nmea(void)
 136 {
 137     const unsigned char   lenp = 4;
 138     unsigned char   payload[lenp];
 139     unsigned char   pkt[32];
 140     unsigned int len_pkt;
 141
 142     payload[0] = 0x00;
 143     payload[1] = 0x23;
 144     payload[2] = 0xFF;
 145     payload[3] = 0x02;
 146
 147     len_pkt = ublox_make_cmd(UBX_CLS_CFG, 0x17, lenp, payload, pkt);
 148     ublox_send(pkt, len_pkt);
 149 }
 150 /*
 151  * CFG-MSG
 152  * パケットの出力レートを指定
 153  */
 154 void ublox_send_cfg_msg(void)
 155 {
 156     const unsigned char   lenp = 3;
 157     unsigned char   payload[lenp];
 158     unsigned char   pkt[32];
 159     unsigned int len_pkt;
 160
 161     payload[0] = UBX_CLS_NAV;
 162     payload[1] = UBX_ID_NAV_TIMEUTC;
 163     payload[2] = 1; // Rate[Hz]
 164
 165     len_pkt = ublox_make_cmd(UBX_CLS_CFG, UBX_ID_CFG_MSG, lenp, payload, pkt);
 166     ublox_send(pkt, len_pkt);
 167 }
 168 /*
 169  * CFG_TP5
 170  * TIMEPULSE2
 171  * AD用CLOCK出力
 172  */
 173 void ublox_send_cfg_tp5_timepulse2(void)
 174 {
 175     const unsigned char   lenp = 32;
 176     unsigned char   payload[lenp];
 177     unsigned char   pkt[64];
 178     unsigned int len_pkt;
 179
 180     memset(payload, 0, lenp);
 181
 182     payload[0] = 0x01;  // Timepulse2
 183
 184     payload[4] = 50;    // cable delay ns
 185
 186 //    payload[8] = 0x80;  // Freq 10MHz
 187 //    payload[9] = 0x96;
 188 //    payload[10] = 0x98;
 189 //    payload[11] = 0x00;
 190
 191     payload[8] = 0x00;  // Freq 7.372800MHz
 192     payload[9] = 0x80;
 193     payload[10] = 0x70;
 194     payload[11] = 0x00;
 195
 196 //    payload[12] = 0x80;  // Freq locked 10MHz
 197 //    payload[13] = 0x96;
 198 //    payload[14] = 0x98;
 199 //    payload[15] = 0x00;
 200
 201     payload[12] = 0x00;  // Freq locked 7.372800MHz
 202     payload[13] = 0x80;
 203     payload[14] = 0x70;
 204     payload[15] = 0x00;
 205
 206     payload[16] = 00;   // Pulse Duty
 207     payload[17] = 00;
 208     payload[18] = 00;
 209     payload[19] = 0x80;
 210
 211     payload[20] = 00;   // Pulse Duty locked
 212     payload[21] = 00;
 213     payload[22] = 00;
 214     payload[23] = 0x80;
 215
 216     // gridUtcGps,ploarity,alignToTow,isLength,isFreq,lockedOtherSet,LockGpsFreq,Active
 217     payload[28] = 0b11101111;   // Grid=GPS,Pol=Rise
 218
 219     len_pkt = ublox_make_cmd(UBX_CLS_CFG, UBX_ID_CFG_TP5, lenp, payload, pkt);
 220     ublox_send(pkt, len_pkt);
 221 }
 222
 223 /*
 224  * CFG TP5
 225  * TIMEPULSE1
 226  * 1Hz OUT
 227  */
 228 void ublox_send_cfg_tp5_timepulse1(void)
 229 {
 230     const unsigned char   lenp = 32;
 231     unsigned char   payload[lenp];
 232     unsigned char   pkt[64];
 233     unsigned int len_pkt;
 234
 235     memset(payload, 0, lenp);
 236
 237     payload[0] = 0x00; //Timepulse1
 238
 239     payload[4] = 50;    // cable delay ns
 240
 241     payload[8] = 0x40;  // Period us
 242     payload[9] = 0x42;
 243     payload[10] = 0x0F;
 244     payload[11] = 0x00;
 245
 246     payload[12] = 0x40;  // Period Locked us
 247     payload[13] = 0x42;
 248     payload[14] = 0x0F;
 249     payload[15] = 0x00;
 250
 251     payload[16] = 0x00;   // Pulse width us
 252     payload[17] = 0x00;
 253     payload[18] = 0x00;
 254     payload[19] = 0x00;
 255
 256 //    payload[20] = 0xA0;   // Pulse width locked 100,000us
 257 //    payload[21] = 0x86;
 258 //    payload[22] = 0x01;
 259 //    payload[23] = 0x00;
 260
 261     payload[20] = 0x20;   // Pulse width locked 500,000us
 262     payload[21] = 0xA1;
 263     payload[22] = 0x07;
 264     payload[23] = 0x00;
 265
 266     payload[20] = 0x00;   // Pulse width locked 800,000us
 267     payload[21] = 0x35;
 268     payload[22] = 0x0C;
 269     payload[23] = 0x00;
 270
 271
 272     payload[28] = 0b11110111;   // Grid=GPS,Pol=Rise
 273
 274     len_pkt = ublox_make_cmd(UBX_CLS_CFG, UBX_ID_CFG_TP5, lenp, payload, pkt);
 275     ublox_send(pkt, len_pkt);
 276 }
 277 /**** DEBUG ************************************************/
 278 #if 0
 279 void ublox_rcv_poll(void)
 280 {
 281     unsigned int i, data;
 282     char    buf[16];
 283     unsigned char    buf_rcv[48];
 284
 285     // Header wait
 286     for(i = 0; i < 1000; i++) {
 287 //        while(DataRdyUART1() == 0);
 288 //        data = ReadUART1();
 289         while(U1STAbits.URXDA == 0);
 290         data = U1RXREG;
 291         if ((data & 0xFF) == 0xB5) break;
 292     }
 293     // rcv
 294     for(i = 0; i < 48; i++) {
 295         if (U1STAbits.OERR == 1) {
 296             U1STAbits.OERR = 0;
 297         }
 298         buf_rcv[i]= U1RXREG;
 299         while(U1STAbits.URXDA == 0);
 300     }
 301     // debug out
 302     for(i = 0; i < 48; i++) {
 303 //        while(DataRdyUART1() == 0);
 304 //        data = ReadUART1();
 305         sprintf(buf, "%02X ", buf_rcv[i]);
 306         PDEBUG(buf);
 307     }
 308
 309     PDEBUG("\r\n");
 310 }
 311 #endif
 312 /*
 313  * polling 受信
 314  */
 315 void ublox_rcv_poll(void)
 316 {
 317     unsigned int i, data;
 318     char    buf[16];
 319
 320     // Header wait
 321     for(i = 0; i < 1000; i++) {
 322 //        while(DataRdyUART1() == 0);
 323 //        data = ReadUART1();
 324         while(uart1_rcvbuf_is_data() == 0);
 325         data = uart1_rcvbuf_getc();
 326         sprintf(buf, "%02X ", data);
 327         PDEBUG(buf);
 328     }
 329 }
 330 /**** パケット受信 *************************************/
 331 /*
 332  * NAV-TIMEUTCデコード
 333  * return
 334  *  0=OK
 335  *  -1=エラー
 336  */
 337
 338 int ublox_decode_nav_timeutc(unsigned char *payload, UbloxNavTimeUtc *g)
 339 {
 340     g->tow = byte4_to_ulong(payload);   // ms
 341     g->tacc = byte4_to_ulong(payload+4);    // ns
 342     g->nano = byte4_to_long(payload+8);     // ns
 343     g->year = byte2_to_uint(payload+12);
 344     g->month = byte1_to_uchar(payload+14);
 345     g->day = byte1_to_uchar(payload+15);
 346     g->hour = byte1_to_uchar(payload+16);
 347     g->min = byte1_to_uchar(payload+17);
 348     g->sec = byte1_to_uchar(payload+18);
 349     g->valid = byte1_to_uchar(payload+19);
 350     return 0;
 351 }
 352 /*
 353  * 受信バッファから1packet取り出す
 354  * UART割り込み使用
 355  * return
 356  *  0=受信OK
 357  *  -1=受信エラー or タイムアウト
 358  */
 359 //TODO: timeout処理, timeout指定
 360 //TODO: check sum check
 361 int ublox_rcv_pkt(unsigned char *class, unsigned char *id, unsigned int *len, unsigned char *payload)
 362 {
 363     unsigned int i, data;
 364     char    sts = UBX_STS_HEAD1;
 365     unsigned int ptr;
 366     unsigned char   suma, sumb;
 367
 368     while(1) {
 369         while(uart1_rcvbuf_is_data() == 0);
 370         data = uart1_rcvbuf_getc();
 371         switch(sts) {
 372             case UBX_STS_HEAD1:
 373                 if (data == 0xB5) sts = UBX_STS_HEAD2;
 374                 break;
 375             case UBX_STS_HEAD2:
 376                 if (data == 0x62) sts = UBX_STS_CLASS;
 377                 break;
 378             case UBX_STS_CLASS:
 379                 *class = data;
 380                 sts = UBX_STS_ID;
 381                 break;
 382             case UBX_STS_ID:
 383                 *id = data;
 384                 sts = UBX_STS_LEN1;
 385                 break;
 386             case UBX_STS_LEN1:
 387                 *len = data;
 388                 sts = UBX_STS_LEN2;
 389                 break;
 390             case UBX_STS_LEN2:
 391                 *len += ((data << 8) & 0xFF00);
 392                 ptr = 0;
 393                 sts = UBX_STS_PAYLOAD;
 394                 break;
 395             case UBX_STS_PAYLOAD:
 396                 payload[ptr++] = data;
 397                 if (ptr >= *len) sts = UBX_STS_SUMA;
 398                 break;
 399             case UBX_STS_SUMA:
 400                 suma = data;
 401                 sts = UBX_STS_SUMB;
 402                 break;
 403             case UBX_STS_SUMB:
 404                 sumb = data;
 405                 return 0;
 406         }
 407     }
 408 }
 409 /*
 410  * 受信バッファからACKまたはNAKを取り出す
 411  * return
 412  * 　1=ACK受信
 413  * 　0=NACK受信
 414  * 　-1=timeout
 415  */
 416 // TODO: timeout処理,timeoutを引数で指定
 417 int ublox_rcv_ack(void)
 418 {
 419     unsigned char class, id;
 420     unsigned int    len;
 421     unsigned char   payload[128];
 422
 423     while(1) {
 424         ublox_rcv_pkt(&class, &id, &len, payload);
 425         if (class == 0x05 && id == 0x01) {
 426             // ACK
 427             PDEBUG("ACK\r\n");
 428             return 1;
 429         } else if (class == 0x05 && id == 0x00) {
 430             // NACK
 431             PDEBUG("NACK\r\n");
 432             return 0;
 433         }
 434     }
 435 }