imuboard/gps_venus.c

   1 #include <stdio.h>
   2 #include <stdlib.h>
   3 #include <stdint.h>
   4 #include <string.h>
   5
   6 #include <aversive.h>
   7 #include <aversive/endian.h>
   8 #include <aversive/wait.h>
   9 #include <callout.h>
  10 #include <uart.h>
  11
  12 #include "main.h"
  13 #include "gps.h"
  14 #include "gps_venus.h"
  15
  16 /* note: enabling debug will make the code fail due to rx queue full */
  17 #define debug_printf(fmt, a...) do { } while (0)
  18 /* #define debug_printf(fmt, ...) printf_P(PSTR(fmt), ##__VA_ARGS__) */
  19
  20 #define GPS_UART 0
  21
  22 #define GPS_PERIOD_MS 2
  23
  24 /*
  25   https://code.google.com/p/hardware-ntp/source/browse/trunk/pcb-1/venus634.c?r=12
  26  */
  27
  28 /* some useful defines */
  29 #define START1 0xA0
  30 #define START2 0xA1
  31 #define END1 0x0D
  32 #define END2 0x0A
  33
  34 /* proccessing states */
  35
  36 #define S_RESET 0
  37 #define S_POSTRESET 1
  38 #define S_START1 2
  39 #define S_START2 3
  40 #define S_LENGTH1 4
  41 #define S_LENGTH2 5
  42 #define S_DISCARD 6
  43 #define S_COPY 7
  44 #define S_ENDCOPY 8
  45 #define S_END 9
  46 #define S_END1 10
  47
  48 #define MAX_LEN 0x200
  49 struct gps_frame {
  50         uint16_t len;
  51         uint16_t cur_len;
  52         uint8_t data[MAX_LEN];
  53 };
  54
  55 static struct gps_frame rxframe;
  56
  57 static struct callout gps_timer;
  58
  59 /* INPUT FORMATS */
  60
  61 /* packet types */
  62 /* 0x01 - System Restart */
  63 #define M_RESTART 0x01
  64 typedef struct {
  65         uint8_t start_mode; /* 01 = Hot; 02 = Warm; 03 = Cold */
  66         uint16_t utc_year; /* >= 1980 */
  67         uint8_t utc_month;
  68         uint8_t utc_day;
  69         uint8_t utc_hour;
  70         uint8_t utc_minute;
  71         uint8_t utc_second;
  72         int16_t latitude; /* -9000 to 9000, 1/100th degree, positive north */
  73         int16_t longitude; /* -18000 to 18000, 1/100th degree, positive east */
  74         int16_t altitude; /* -1000 to 18300, meters */
  75 } m_restart_t;
  76
  77
  78 /* 0x02 - Query Software Version */
  79 #define M_QUERYVER 0x02
  80 typedef struct {
  81         uint8_t software_type; /* 01 = System Code */
  82 } m_queryver_t;
  83
  84
  85 /* 0x05 - Serial Port Configuration */
  86 #define M_SERCONF 0x05
  87 typedef struct {
  88         uint8_t port; /* 00 = COM1 (the only port) */
  89         uint8_t baud; /* 00 = 4800; 01 = 9600; 02 = 19200; 03 = 38400;
  90                          04 = 57600; 05 = 115200; */
  91         uint8_t update; /* 00 = SRAM only; 01 = SRAM and FLASH */
  92 } m_serconf_t;
  93
  94
  95
  96 /* 0x08 - nmea interval */
  97 #define M_NMEAINTERVAL 0x08
  98 typedef struct {
  99         uint8_t gga;
 100         uint8_t gsa;
 101         uint8_t gsv;
 102         uint8_t gll;
 103         uint8_t rmc;
 104         uint8_t vtg;
 105         uint8_t zda;
 106         uint8_t attributes; /*
 107                               0 - sram
 108                               1 - flash
 109                              */
 110
 111 } m_nmeainterval_t;
 112
 113
 114 /* 0x09 - Query Software Version */
 115 #define M_OUTPUT 0x09
 116 typedef struct {
 117         uint8_t msg_type; /*
 118                             00 = no output
 119                             01 = nmea output
 120                             02 = binary output
 121                            */
 122         uint8_t attributes; /*
 123                               0 - update ram
 124                               1 - update ram & flash
 125                             */
 126 } m_output_t;
 127
 128
 129 /* 0x0E - Update Rage */
 130 #define M_RATE 0x0E
 131 typedef struct {
 132         uint8_t rate; /* Hz */
 133         uint8_t attributes; /*
 134                               0 - update ram
 135                               1 - update ram & flash
 136                             */
 137 } m_rate_t;
 138
 139
 140
 141 /* 0x37 - configure waas */
 142 #define M_WAAS 0x37
 143 typedef struct {
 144         uint8_t enable; /* 01 = enable */
 145         uint8_t attributes;
 146 } m_waas_t;
 147
 148 /* 0x39 - configure position pinning */
 149 #define M_PINNING 0x39
 150 typedef struct {
 151         uint8_t enable; /* 0 = default, 1 = enable, 2 = disable */
 152         uint8_t attributes; /* 0 = ram, 1 = ram+flash */
 153 } m_pinning_t;
 154
 155 /* 0x3B - configure position pinning */
 156 #define M_PINNING_PARAMS 0x3B
 157 typedef struct {
 158         uint16_t pin_speed;
 159         uint16_t pin_cnt;
 160         uint16_t unpin_speed;
 161         uint16_t unpin_cnt;
 162         uint16_t unpin_dist;
 163         uint8_t attributes; /* 0 = ram, 1 = ram+flash */
 164 } m_pinning_params_t;
 165
 166 /* 0x3C - nav mode */
 167 #define M_NAVMODE 0x3c
 168 typedef struct {
 169         uint8_t navmode; /* 00 = car
 170                              01 = pedestrian
 171                            */
 172         uint8_t attributes;
 173 } m_navmode_t;
 174
 175
 176 static void serial1_tx_cout(uint8_t c)
 177 {
 178         debug_printf("%.2X ", c);
 179         uart_send(GPS_UART, c);
 180 }
 181
 182 static void venus634_send(uint8_t type, void *payload, uint16_t len)
 183 {
 184         uint8_t crc = 0, n;
 185
 186         debug_printf("SEND ");
 187
 188         /* now send the message */
 189         /* header */
 190         serial1_tx_cout(START1);
 191         serial1_tx_cout(START2);
 192         serial1_tx_cout(((len+1) >> 8) & 0xff);
 193         serial1_tx_cout((len+1) & 0xff);
 194         /* type and payload */
 195         serial1_tx_cout(type);
 196         crc ^= type;
 197         for (n = 0; n < len; n++) {
 198                 serial1_tx_cout(*(uint8_t *)payload);
 199                 crc ^= *(uint8_t *)payload;
 200                 payload++;
 201         }
 202         /* checksum and tail */
 203         serial1_tx_cout(crc);
 204         serial1_tx_cout(END1);
 205         serial1_tx_cout(END2);
 206
 207         debug_printf("\n");
 208 }
 209
 210 static void venus634_restart(void)
 211 {
 212         m_restart_t restart;
 213
 214         memset(&restart, 0, sizeof(m_restart_t));
 215         restart.start_mode = 0x03; /* COLD */
 216
 217         venus634_send(M_RESTART, &restart, sizeof(m_restart_t));
 218
 219         return;
 220 }
 221
 222 __attribute__((unused))
 223 static void venus634_config_serial(void)
 224 {
 225         m_serconf_t serconf;
 226
 227         memset(&serconf, 0, sizeof(m_serconf_t));
 228         serconf.port = 0;
 229         serconf.baud = 4;
 230         serconf.update = 1;
 231
 232         venus634_send(M_SERCONF, &serconf, sizeof(m_serconf_t));
 233
 234         return;
 235 }
 236
 237 static void venus634_msg_type(uint8_t mode)
 238 {
 239         m_output_t output;
 240
 241         memset(&output, 0, sizeof(m_output_t));
 242         output.msg_type = mode;
 243         output.attributes = 0; /* ram, init may freeze when update in flash */
 244
 245         venus634_send(M_OUTPUT, &output, sizeof(m_output_t));
 246
 247         return;
 248 }
 249
 250
 251 static void venus634_rate(void)
 252 {
 253         m_rate_t rate;
 254
 255         memset(&rate, 0, sizeof(m_rate_t));
 256         rate.rate = 5;
 257         rate.attributes = 0; /* ram, init may freeze when update in flash */
 258
 259         venus634_send(M_RATE, &rate, sizeof(m_rate_t));
 260
 261         return;
 262 }
 263
 264 /* Wide Area Augmentation System: use ground stations to increase the precision
 265  * of gps position */
 266 static void venus634_waas(void)
 267 {
 268         m_waas_t waas;
 269
 270         memset(&waas, 0, sizeof(m_waas_t));
 271         waas.enable = 1;
 272         waas.attributes = 0; /* ram, init may freeze when update in flash */
 273
 274         venus634_send(M_WAAS, &waas, sizeof(m_waas_t));
 275
 276         return;
 277 }
 278
 279 /* Configure position pinning – Enable or disable position pinning of GNSS
 280  * receiver */
 281 static void venus634_pinning(void)
 282 {
 283         m_pinning_t pinning;
 284
 285         memset(&pinning, 0, sizeof(m_pinning_t));
 286         pinning.enable = 2;
 287         pinning.attributes = 0; /* ram */
 288
 289         venus634_send(M_PINNING, &pinning, sizeof(m_pinning_t));
 290 }
 291
 292 /* Configure position pinning parameters */
 293 static void venus634_pinning_params(void)
 294 {
 295         m_pinning_params_t pinning_params;
 296
 297         /* just set everything to 0 */
 298         memset(&pinning_params, 0, sizeof(m_pinning_params_t));
 299         pinning_params.attributes = 0; /* ram */
 300
 301         venus634_send(M_PINNING_PARAMS, &pinning_params,
 302                 sizeof(m_pinning_params_t));
 303 }
 304
 305
 306 /* Tell we are a car instead of a pedestrian */
 307 __attribute__((unused))
 308 static void venus634_navmode(void)
 309 {
 310         m_navmode_t navmode;
 311
 312         memset(&navmode, 0, sizeof(m_navmode_t));
 313         navmode.navmode = 0; /* car */
 314         navmode.attributes = 0; /* ram, init may freeze when update in flash */
 315
 316         venus634_send(M_NAVMODE, &navmode, sizeof(m_navmode_t));
 317
 318         return;
 319 }
 320
 321 /* frequency of NMEA messages */
 322 __attribute__((unused))
 323 static void venus634_nmea_interval(void)
 324 {
 325         m_nmeainterval_t nmeainterval;
 326
 327         memset(&nmeainterval, 0, sizeof(m_nmeainterval_t));
 328
 329         /* set frequency for nmea: 1 = once every one position fix, 2 = once
 330          * every two position fix, ... */
 331         nmeainterval.gga = 1; /* GPGGA interval - GPS Fix Data*/
 332         nmeainterval.gsa = 1; /* GNSS DOPS and Active Satellites */
 333         nmeainterval.gsv = 1; /* GNSS Satellites in View */
 334         nmeainterval.gll = 1; /* Geographic Position - Latitude longitude */
 335         nmeainterval.rmc = 1; /* Recomended Minimum Specific GNSS Sentence */
 336         nmeainterval.vtg = 1; /* Course Over Ground and Ground Speed */
 337         nmeainterval.zda = 1; /* Time & Date*/
 338
 339         nmeainterval.attributes = 1; /* ram & flash */
 340
 341         venus634_send(M_NMEAINTERVAL, &nmeainterval, sizeof(m_nmeainterval_t));
 342
 343         return;
 344 }
 345
 346 static int8_t recv_cb(uint8_t byte)
 347 {
 348         uint16_t i;
 349
 350         /* bytes 0 and 1 are start bytes */
 351         if (rxframe.cur_len == 0) {
 352                 if (byte != START1) {
 353                         debug_printf("bad start1 %.2X\n", byte);
 354                         goto reset_buf;
 355                 }
 356         }
 357         else if (rxframe.cur_len == 1) {
 358                 if (byte != START2) {
 359                         debug_printf("bad start2 %.2X\n", byte);
 360                         goto reset_buf;
 361                 }
 362         }
 363         /* bytes 2 and 3 are the length of frame in network order */
 364         else if (rxframe.cur_len == 2) {
 365                 rxframe.len = (uint16_t)byte << 8;
 366         }
 367         else if (rxframe.cur_len == 3) {
 368                 rxframe.len |= (uint16_t)byte;
 369                 if (rxframe.len > MAX_LEN) {
 370                         debug_printf("bad len %d\n", rxframe.len);
 371                         goto reset_buf;
 372                 }
 373         }
 374         /* next bytes are data (the 4 below is the size of header) */
 375         else if ((rxframe.cur_len - 4) < rxframe.len) {
 376                 rxframe.data[rxframe.cur_len - 4] = byte;
 377         }
 378         /* then it's the crc */
 379         else if ((rxframe.cur_len - 4) == rxframe.len) {
 380                 uint8_t crc = 0;
 381
 382                 for (i = 0; i < rxframe.len; i++)
 383                         crc ^= rxframe.data[i];
 384                 if (byte != crc) {
 385                         debug_printf("invalid crc\n");
 386                         goto reset_buf;
 387                 }
 388
 389         }
 390         /* and the last 2 bytes are stop bytes */
 391         else if ((rxframe.cur_len - 4) == (rxframe.len + 1)) {
 392                 if (byte != END1) {
 393                         debug_printf("bad end1 %.2X\n", byte);
 394                         goto reset_buf;
 395                 }
 396         }
 397         else if ((rxframe.cur_len - 4) == (rxframe.len + 2)) {
 398                 if (byte != END2) {
 399                         debug_printf("bad end2 %.2X\n", byte);
 400                         goto reset_buf;
 401                 }
 402                 debug_printf("valid frame received\n");
 403                 rxframe.cur_len = 0;
 404                 return 0;
 405         }
 406         else /* should not happen */
 407                 goto reset_buf;
 408
 409         rxframe.cur_len ++;
 410         return 1;
 411
 412  reset_buf:
 413         rxframe.cur_len = 0;
 414         return 1;
 415 }
 416
 417 static int recv_msg(void)
 418 {
 419         int ret;
 420         uint8_t irq_flags;
 421         int16_t c;
 422         uint16_t t1, t2, diff;
 423
 424         IRQ_LOCK(irq_flags);
 425         t1 = global_ms;
 426         IRQ_UNLOCK(irq_flags);
 427
 428         printf_P(PSTR("recv_msg()\n"));
 429         while (1) {
 430                 IRQ_LOCK(irq_flags);
 431                 t2 = global_ms;
 432                 IRQ_UNLOCK(irq_flags);
 433                 diff = t2 - t1;
 434                 if (diff > 1000) {
 435                         printf_P(PSTR("recv_msg timeout\n"));
 436                         return -1;
 437                 }
 438
 439                 c = uart_recv_nowait(GPS_UART);
 440                 if (c < 0)
 441                         continue;
 442
 443                 debug_printf("%2.2x\n", c);
 444
 445                 ret = recv_cb(c);
 446                 if (ret == 0)
 447                         return 0;
 448         }
 449 }
 450
 451 static int wait_ack(int msg_type)
 452 {
 453         uint8_t irq_flags;
 454         int ret;
 455         uint16_t t1, t2, diff;
 456
 457         IRQ_LOCK(irq_flags);
 458         t1 = global_ms;
 459         IRQ_UNLOCK(irq_flags);
 460
 461
 462         while (1) {
 463                 ret = recv_msg();
 464                 if (ret < 0)
 465                         return -1;
 466
 467                 IRQ_LOCK(irq_flags);
 468                 t2 = global_ms;
 469                 IRQ_UNLOCK(irq_flags);
 470                 diff = t2 - t1;
 471                 if (diff > 1000) {
 472                         printf_P(PSTR("wait_ack timeout\n"));
 473                         return -1;
 474                 }
 475
 476                 /* retry if it's not the expected message */
 477                 if (rxframe.data[0] != 0x83 && rxframe.data[0] != 0x84)
 478                         continue;
 479                 if (rxframe.data[1] != msg_type)
 480                         continue;
 481
 482                 if (rxframe.data[0] == 0x83)
 483                         printf_P(PSTR("ACK\n"));
 484                 else if (rxframe.data[0] == 0x84)
 485                         printf_P(PSTR("NACK\n"));
 486                 else
 487                         printf_P(PSTR("ZARB\n"));
 488                 break;
 489         }
 490
 491         return 0;
 492 }
 493
 494 static int decode_gps_pos(uint8_t *buf, uint16_t len)
 495 {
 496         struct gps_pos *pos = (struct gps_pos *)buf;
 497         uint8_t irq_flags;
 498
 499         if (len != sizeof(*pos))
 500                 return -1;
 501
 502         if (pos->msg_id != 0xA8)
 503                 return -1;
 504
 505         /* fix endianness inside the frame */
 506         pos->gps_week = ntohs(pos->gps_week);
 507         pos->tow = ntohl(pos->tow);
 508
 509         pos->latitude = ntohl(pos->latitude);
 510         pos->longitude = ntohl(pos->longitude);
 511         pos->altitude = ntohl(pos->altitude);
 512
 513         pos->sea_altitude = ntohl(pos->sea_altitude);
 514
 515         pos->gdop = ntohs(pos->gdop);
 516         pos->pdop = ntohs(pos->pdop);
 517         pos->hdop = ntohs(pos->hdop);
 518         pos->vdop = ntohs(pos->vdop);
 519         pos->tdop = ntohs(pos->tdop);
 520
 521         pos->ecef_vx = ntohl(pos->ecef_vx);
 522         pos->ecef_vy = ntohl(pos->ecef_vy);
 523         pos->ecef_vz = ntohl(pos->ecef_vz);
 524
 525         /* update global structure */
 526         IRQ_LOCK(irq_flags);
 527         memcpy(&gps_pos, pos, sizeof(gps_pos));
 528         IRQ_UNLOCK(irq_flags);
 529
 530         return 0;
 531 }
 532
 533 /* called on timer event */
 534 static void gps_venus_cb(struct callout_mgr *cm, struct callout *tim, void *arg)
 535 {
 536         static int16_t last_valid = 0;
 537         static uint8_t is_valid = 0;
 538         int16_t ms, diff;
 539         int16_t c;
 540         int8_t ret;
 541         uint8_t irq_flags;
 542
 543         (void)cm;
 544         (void)tim;
 545         (void)arg;
 546
 547         IRQ_LOCK(irq_flags);
 548         ms = global_ms;
 549         IRQ_UNLOCK(irq_flags);
 550
 551         while (1) {
 552                 c = uart_recv_nowait(GPS_UART);
 553                 if (c < 0) /* no more char */
 554                         break;
 555
 556                 ret = recv_cb(c);
 557                 if (ret == 0) {
 558                         decode_gps_pos(rxframe.data, rxframe.len);
 559                         if (0)
 560                                 display_gps_pos();
 561
 562                         last_valid = ms;
 563                         is_valid = 1;
 564                 }
 565         }
 566
 567         diff = ms - last_valid;
 568         if (is_valid == 1 && diff > 500) {
 569
 570                 is_valid = 0;
 571                 /* update global structure */
 572                 IRQ_LOCK(irq_flags);
 573                 memset(&gps_pos, 0, sizeof(gps_pos));
 574                 gps_pos.latitude = 0xffffffff;
 575                 IRQ_UNLOCK(irq_flags);
 576         }
 577
 578         callout_schedule(cm, tim, GPS_PERIOD_MS);
 579 }
 580
 581 static void venus634_configure(void)
 582 {
 583         /* ask the GPS to reset */
 584         printf_P(PSTR("init..."));
 585         venus634_restart();
 586         wait_ack(M_RESTART);
 587
 588         printf_P(PSTR("binmode..."));
 589         venus634_msg_type(2); /* binary */
 590         wait_ack(M_OUTPUT);
 591
 592         printf_P(PSTR("waas..."));
 593         venus634_waas();
 594         wait_ack(M_WAAS);
 595
 596         printf_P(PSTR("pinning..."));
 597         venus634_pinning();
 598         wait_ack(M_PINNING);
 599
 600         printf_P(PSTR("pinning parameters..."));
 601         venus634_pinning_params();
 602         wait_ack(M_PINNING_PARAMS);
 603
 604         printf_P(PSTR("rate..."));
 605         venus634_rate();
 606         wait_ack(M_RATE);
 607
 608         printf_P(PSTR("GPS configuration done !\n"));
 609 }
 610
 611 /*
 612   https://www.sparkfun.com/datasheets/GPS/Modules/AN0003_v1.4.14_FlashOnly.pdf
 613 */
 614
 615 int gps_venus_init(void)
 616 {
 617         venus634_configure();
 618
 619         callout_init(&gps_timer, gps_venus_cb, NULL, GPS_PRIO);
 620         callout_schedule(&imuboard.intr_cm, &gps_timer, GPS_PERIOD_MS); /* every 2ms */
 621
 622         return 0;
 623 }