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