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  * lenp: 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 lenp: 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] = 0x00;  // Freq 7.372800MHz
 187     payload[9] = 0x80;
 188     payload[10] = 0x70;
 189     payload[11] = 0x00;
 190
 191     payload[12] = 0x00;  // Freq locked 7.372800MHz
 192     payload[13] = 0x80;
 193     payload[14] = 0x70;
 194     payload[15] = 0x00;
 195
 196     payload[16] = 00;   // Pulse Duty
 197     payload[17] = 00;
 198     payload[18] = 00;
 199     payload[19] = 0x80;
 200
 201     payload[20] = 00;   // Pulse Duty locked
 202     payload[21] = 00;
 203     payload[22] = 00;
 204     payload[23] = 0x80;
 205
 206     // gridUtcGps,ploarity,alignToTow,isLength,isFreq,lockedOtherSet,LockGpsFreq,Active
 207     payload[28] = 0b11101111;   // Grid=GPS,Pol=Rise
 208
 209     len_pkt = ublox_make_cmd(UBX_CLS_CFG, UBX_ID_CFG_TP5, lenp, payload, pkt);
 210     ublox_send(pkt, len_pkt);
 211 }
 212
 213 /*
 214  * CFG TP5
 215  * TIMEPULSE1
 216  * 1Hz OUT
 217  */
 218 void ublox_send_cfg_tp5_timepulse1(void)
 219 {
 220     const unsigned char   lenp = 32;
 221     unsigned char   payload[lenp];
 222     unsigned char   pkt[64];
 223     unsigned int len_pkt;
 224
 225     memset(payload, 0, lenp);
 226
 227     payload[0] = 0x00; //Timepulse1
 228
 229     payload[4] = 50;    // cable delay ns
 230
 231     payload[8] = 0x40;  // Period 1,000,000us
 232     payload[9] = 0x42;
 233     payload[10] = 0x0F;
 234     payload[11] = 0x00;
 235
 236     payload[12] = 0x40;  // Period Locked us
 237     payload[13] = 0x42;
 238     payload[14] = 0x0F;
 239     payload[15] = 0x00;
 240
 241     payload[16] = 0x20;   // Pulse width 500,000us
 242     payload[17] = 0xA1;
 243     payload[18] = 0x07;
 244     payload[19] = 0x00;
 245
 246     payload[20] = 0xA0;   // Pulse width locked 100,000us
 247     payload[21] = 0x86;
 248     payload[22] = 0x01;
 249     payload[23] = 0x00;
 250
 251     payload[28] = 0b11110111;   // Grid=GPS,Pol=Rise
 252
 253     len_pkt = ublox_make_cmd(UBX_CLS_CFG, UBX_ID_CFG_TP5, lenp, payload, pkt);
 254     ublox_send(pkt, len_pkt);
 255 }
 256 /**** DEBUG ************************************************/
 257 #if 0
 258 void ublox_rcv_poll(void)
 259 {
 260     unsigned int i, data;
 261     char    buf[16];
 262     unsigned char    buf_rcv[48];
 263
 264     // Header wait
 265     for(i = 0; i < 1000; i++) {
 266 //        while(DataRdyUART1() == 0);
 267 //        data = ReadUART1();
 268         while(U1STAbits.URXDA == 0);
 269         data = U1RXREG;
 270         if ((data & 0xFF) == 0xB5) break;
 271     }
 272     // rcv
 273     for(i = 0; i < 48; i++) {
 274         if (U1STAbits.OERR == 1) {
 275             U1STAbits.OERR = 0;
 276         }
 277         buf_rcv[i]= U1RXREG;
 278         while(U1STAbits.URXDA == 0);
 279     }
 280     // debug out
 281     for(i = 0; i < 48; i++) {
 282 //        while(DataRdyUART1() == 0);
 283 //        data = ReadUART1();
 284         sprintf(buf, "%02X ", buf_rcv[i]);
 285         PDEBUG(buf);
 286     }
 287
 288     PDEBUG("\r\n");
 289 }
 290 #endif
 291 /*
 292  * polling 受信
 293  */
 294 void ublox_rcv_poll(void)
 295 {
 296     unsigned int i, data;
 297     char    buf[16];
 298
 299     // Header wait
 300     for(i = 0; i < 1000; i++) {
 301 //        while(DataRdyUART1() == 0);
 302 //        data = ReadUART1();
 303         while(uart1_rcvbuf_is_data() == 0);
 304         data = uart1_rcvbuf_getc();
 305         sprintf(buf, "%02X ", data);
 306         PDEBUG(buf);
 307     }
 308 }
 309 /**** パケット受信 *************************************/
 310 /*
 311  * NAV-TIMEUTCデコード
 312  * return
 313  *  0=OK
 314  *  -1=エラー
 315  */
 316
 317 int ublox_decode_nav_timeutc(unsigned char *payload, UbloxNavTimeUtc *g)
 318 {
 319     g->tow = byte4_to_ulong(payload);   // ms
 320     g->tacc = byte4_to_ulong(payload+4);    // ns
 321     g->nano = byte4_to_long(payload+8);     // ns
 322     g->year = byte2_to_uint(payload+12);
 323     g->month = byte1_to_uchar(payload+14);
 324     g->day = byte1_to_uchar(payload+15);
 325     g->hour = byte1_to_uchar(payload+16);
 326     g->min = byte1_to_uchar(payload+17);
 327     g->sec = byte1_to_uchar(payload+18);
 328     g->valid = byte1_to_uchar(payload+19);
 329     return 0;
 330 }
 331 /*
 332  * 受信バッファから1packet取り出す
 333  * UART割り込み使用
 334  * return
 335  *  0=受信OK
 336  *  -1=受信エラー or タイムアウト
 337  */
 338 //TODO: timeout処理, timeout指定
 339 //TODO: check sum check
 340 int ublox_rcv_pkt(unsigned char *class, unsigned char *id, unsigned int *len, unsigned char *payload)
 341 {
 342     unsigned int i, data;
 343     char    sts = UBX_STS_HEAD1;
 344     unsigned int ptr;
 345     unsigned char   suma, sumb;
 346
 347     while(1) {
 348         while(uart1_rcvbuf_is_data() == 0);
 349         data = uart1_rcvbuf_getc();
 350         switch(sts) {
 351             case UBX_STS_HEAD1:
 352                 if (data == 0xB5) sts = UBX_STS_HEAD2;
 353                 break;
 354             case UBX_STS_HEAD2:
 355                 if (data == 0x62) sts = UBX_STS_CLASS;
 356                 break;
 357             case UBX_STS_CLASS:
 358                 *class = data;
 359                 sts = UBX_STS_ID;
 360                 break;
 361             case UBX_STS_ID:
 362                 *id = data;
 363                 sts = UBX_STS_LEN1;
 364                 break;
 365             case UBX_STS_LEN1:
 366                 *len = data;
 367                 sts = UBX_STS_LEN2;
 368                 break;
 369             case UBX_STS_LEN2:
 370                 *len += ((data << 8) & 0xFF00);
 371                 ptr = 0;
 372                 sts = UBX_STS_PAYLOAD;
 373                 break;
 374             case UBX_STS_PAYLOAD:
 375                 payload[ptr++] = data;
 376                 if (ptr >= *len) sts = UBX_STS_SUMA;
 377                 break;
 378             case UBX_STS_SUMA:
 379                 suma = data;
 380                 sts = UBX_STS_SUMB;
 381                 break;
 382             case UBX_STS_SUMB:
 383                 sumb = data;
 384                 return 0;
 385         }
 386     }
 387 }
 388 /*
 389  * 受信バッファからACKまたはNAKを取り出す
 390  * return
 391  * 　1=ACK受信
 392  * 　0=NACK受信
 393  * 　-1=timeout
 394  */
 395 // TODO: timeout処理,timeoutを引数で指定
 396 int ublox_rcv_ack(void)
 397 {
 398     unsigned char class, id;
 399     unsigned int    len;
 400     unsigned char   payload[128];
 401
 402     while(1) {
 403         ublox_rcv_pkt(&class, &id, &len, payload);
 404         if (class == 0x05 && id == 0x01) {
 405             // ACK
 406 //            PDEBUG("ACK\r\n");
 407             return 1;
 408         } else if (class == 0x05 && id == 0x00) {
 409             // NACK
 410 //            PDEBUG("NACK\r\n");
 411             return 0;
 412         }
 413     }
 414 }