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