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 #define debug_printf(fmt, a...) do { } while (0)
  22 /* #define debug_printf(fmt, ...) printf_P(PSTR(fmt), ##__VA_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         uint8_t attributes; /*
 127                               0 - update ram
 128                               1 - update ram & flash
 129                             */
 130 } m_output_t;
 131
 132
 133 /* 0x0E - Update Rage */
 134 #define M_RATE 0x0E
 135 typedef struct {
 136         uint8_t rate; /* Hz */
 137         uint8_t attributes; /*
 138                               0 - update ram
 139                               1 - update ram & flash
 140                             */
 141 } m_rate_t;
 142
 143
 144
 145 /* 0x37 - configure waas */
 146 #define M_WAAS 0x37
 147 typedef struct {
 148         uint8_t enable; /* 01 = enable */
 149         uint8_t attributes;
 150 } m_waas_t;
 151
 152 /* 0x3C - nav mode */
 153 #define M_NAVMODE 0x3c
 154 typedef struct {
 155         uint8_t navmode; /* 00 = car
 156                              01 = pedestrian
 157                            */
 158         uint8_t attributes;
 159 } m_navmode_t;
 160
 161
 162 #define DBG_SEND
 163 #define DBG_RECV
 164
 165 void serial1_tx_cout(uint8_t c)
 166 {
 167         debug_printf("%.2X ", c);
 168         uart_send(GPS_UART, c);
 169 }
 170
 171 void venus634_send(uint8_t type, void *payload, uint16_t len)
 172 {
 173         uint8_t crc = 0, n;
 174
 175         debug_printf("SEND ");
 176
 177         /* now send the message */
 178         /* header */
 179         serial1_tx_cout(START1);
 180         serial1_tx_cout(START2);
 181         serial1_tx_cout(((len+1) >> 8) & 0xff);
 182         serial1_tx_cout((len+1) & 0xff);
 183         /* type and payload */
 184         serial1_tx_cout(type);
 185         crc ^= type;
 186         for (n = 0; n < len; n++) {
 187                 serial1_tx_cout(*(uint8_t *)payload);
 188                 crc ^= *(uint8_t *)payload;
 189                 payload++;
 190         }
 191         /* checksum and tail */
 192         serial1_tx_cout(crc);
 193         serial1_tx_cout(END1);
 194         serial1_tx_cout(END2);
 195
 196         debug_printf("\n");
 197 }
 198
 199 void venus634_restart(void)
 200 {
 201         m_restart_t restart;
 202
 203         memset(&restart,0,sizeof(m_restart_t));
 204         restart.start_mode = 0x03; /* COLD */
 205
 206         venus634_send(M_RESTART,&restart,sizeof(m_restart_t));
 207
 208         return;
 209 }
 210
 211 void venus634_config_serial(void)
 212 {
 213         m_serconf_t serconf;
 214
 215         memset(&serconf,0,sizeof(m_serconf_t));
 216         serconf.port = 0;
 217         serconf.baud = 4;
 218         serconf.update = 1;
 219
 220         venus634_send(M_SERCONF,&serconf,sizeof(m_serconf_t));
 221
 222         return;
 223 }
 224
 225
 226 void venus634_msg_type(void)
 227 {
 228         m_output_t output;
 229
 230         memset(&output,0,sizeof(m_output_t));
 231         output.msg_type = 0x02; /* binary msg */
 232         output.attributes = 0; /* ram, init may freeze when update in flash */
 233
 234         venus634_send(M_OUTPUT,&output,sizeof(m_output_t));
 235
 236         return;
 237 }
 238
 239
 240 void venus634_rate(void)
 241 {
 242         m_rate_t rate;
 243
 244         memset(&rate,0,sizeof(m_rate_t));
 245         rate.rate = 20;
 246         rate.attributes = 0; /* ram */
 247
 248         venus634_send(M_RATE,&rate,sizeof(m_rate_t));
 249
 250         return;
 251 }
 252
 253 /* Wide Area Augmentation System: use ground stations to increase the precision
 254  * of gps position */
 255 void venus634_waas(void)
 256 {
 257         m_waas_t waas;
 258
 259         memset(&waas,0,sizeof(m_waas_t));
 260         waas.enable = 1;
 261         waas.attributes = 0; /* ram */
 262
 263         venus634_send(M_WAAS,&waas,sizeof(m_waas_t));
 264
 265         return;
 266 }
 267
 268 /* Tell we are a car instead of a pedestrian */
 269 void venus634_navmode(void)
 270 {
 271         m_navmode_t navmode;
 272
 273         memset(&navmode,0,sizeof(m_navmode_t));
 274         navmode.navmode = 0; /* car */
 275         navmode.attributes = 0; /* ram */
 276
 277         venus634_send(M_NAVMODE,&navmode,sizeof(m_navmode_t));
 278
 279         return;
 280 }
 281
 282 /* frequency of NMEA messages */
 283 void venus634_nmea_interval(void)
 284 {
 285         m_nmeainterval_t nmeainterval;
 286
 287         memset(&nmeainterval,0,sizeof(m_nmeainterval_t));
 288
 289         /* set frequency for nmea: 1 = once every one position fix, 2 = once
 290          * every two position fix, ... */
 291         nmeainterval.gga = 1; /* GPGGA interval - GPS Fix Data*/
 292         nmeainterval.gsa = 1; /* GNSS DOPS and Active Satellites */
 293         nmeainterval.gsv = 1; /* GNSS Satellites in View */
 294         nmeainterval.gll = 1; /* Geographic Position - Latitude longitude */
 295         nmeainterval.rmc = 1; /* Recomended Minimum Specific GNSS Sentence */
 296         nmeainterval.vtg = 1; /* Course Over Ground and Ground Speed */
 297         nmeainterval.zda = 1; /* Time & Date*/
 298
 299         nmeainterval.attributes = 1; /* ram flash */
 300
 301         venus634_send(M_NMEAINTERVAL,&nmeainterval,sizeof(m_nmeainterval_t));
 302
 303         return;
 304 }
 305
 306 int8_t recv_cb(uint8_t byte)
 307 {
 308         uint16_t i;
 309
 310         /* bytes 0 and 1 are start bytes */
 311         if (rxframe.cur_len == 0) {
 312                 if (byte != START1) {
 313                         debug_printf("bad start1 %.2X\n", byte);
 314                         goto reset_buf;
 315                 }
 316         }
 317         else if (rxframe.cur_len == 1) {
 318                 if (byte != START2) {
 319                         debug_printf("bad start2 %.2X\n", byte);
 320                         goto reset_buf;
 321                 }
 322         }
 323         /* bytes 2 and 3 are the length of frame in network order */
 324         else if (rxframe.cur_len == 2) {
 325                 rxframe.len = (uint16_t)byte << 8;
 326         }
 327         else if (rxframe.cur_len == 3) {
 328                 rxframe.len |= (uint16_t)byte;
 329                 if (rxframe.len > MAX_LEN) {
 330                         debug_printf("bad len %d\n", rxframe.len);
 331                         goto reset_buf;
 332                 }
 333         }
 334         /* next bytes are data (the 4 below is the size of header) */
 335         else if ((rxframe.cur_len - 4) < rxframe.len) {
 336                 rxframe.data[rxframe.cur_len - 4] = byte;
 337         }
 338         /* then it's the crc */
 339         else if ((rxframe.cur_len - 4) == rxframe.len) {
 340                 uint8_t crc = 0;
 341
 342                 for (i = 0; i < rxframe.len; i++)
 343                         crc ^= rxframe.data[i];
 344                 if (byte != crc) {
 345                         debug_printf("invalid crc\n");
 346                         goto reset_buf;
 347                 }
 348
 349         }
 350         /* and the last 2 bytes are stop bytes */
 351         else if ((rxframe.cur_len - 4) == (rxframe.len + 1)) {
 352                 if (byte != END1) {
 353                         debug_printf("bad end1 %.2X\n", byte);
 354                         goto reset_buf;
 355                 }
 356         }
 357         else if ((rxframe.cur_len - 4) == (rxframe.len + 2)) {
 358                 if (byte != END2) {
 359                         debug_printf("bad end2 %.2X\n", byte);
 360                         goto reset_buf;
 361                 }
 362                 debug_printf("valid frame received\n");
 363                 rxframe.cur_len = 0;
 364                 return 0;
 365         }
 366         else /* should not happen */
 367                 goto reset_buf;
 368
 369         rxframe.cur_len ++;
 370         return 1;
 371
 372  reset_buf:
 373         rxframe.cur_len = 0;
 374         return 1;
 375 }
 376
 377 int recv_msg(void)
 378 {
 379         int ret;
 380         int16_t c;
 381
 382         while (1) {
 383                 /* XXX use select for timeout */
 384                 c = uart_recv(GPS_UART);
 385                 ret = recv_cb(c);
 386                 if (ret == 0)
 387                         return 0;
 388         }
 389 }
 390
 391 int wait_ack(int msg_type)
 392 {
 393         int ret;
 394
 395         while (1) {
 396                 ret = recv_msg();
 397                 if (ret < 0)
 398                         return -1;
 399
 400                 /* retry if it's not the expected message */
 401                 if (rxframe.data[0] != 0x83 && rxframe.data[0] != 0x84)
 402                         continue;
 403                 if (rxframe.data[1] != msg_type)
 404                         continue;
 405
 406                 if (rxframe.data[0] == 0x83)
 407                         printf_P(PSTR("ACK\n"));
 408                 else if (rxframe.data[0] == 0x84)
 409                         printf_P(PSTR("NACK\n"));
 410                 else
 411                         printf_P(PSTR("ZARB\n"));
 412                 break;
 413         }
 414
 415         return 0;
 416 }
 417
 418 static int decode_gps_pos(uint8_t *buf, uint16_t len)
 419 {
 420         struct gps_pos *pos = (struct gps_pos *)buf;
 421         uint8_t irq_flags;
 422
 423         if (len != sizeof(*pos))
 424                 return -1;
 425
 426         if (pos->msg_id != 0xA8) /* XXX not tested */
 427                 return -1;
 428
 429         gps_pos.gps_week = ntohs(pos->gps_week);
 430         gps_pos.tow = ntohl(pos->tow);
 431
 432         gps_pos.latitude = ntohl(pos->latitude);
 433         gps_pos.longitude = ntohl(pos->longitude);
 434         gps_pos.altitude = ntohl(pos->altitude);
 435
 436         gps_pos.sea_altitude = ntohl(pos->sea_altitude);
 437
 438         gps_pos.gdop = ntohs(pos->gdop);
 439         gps_pos.pdop = ntohs(pos->pdop);
 440         gps_pos.hdop = ntohs(pos->hdop);
 441         gps_pos.vdop = ntohs(pos->vdop);
 442         gps_pos.tdop = ntohs(pos->tdop);
 443
 444         gps_pos.ecef_vx = ntohl(pos->ecef_vx);
 445         gps_pos.ecef_vy = ntohl(pos->ecef_vy);
 446         gps_pos.ecef_vz = ntohl(pos->ecef_vz);
 447
 448         /* update global structure */
 449         IRQ_LOCK(irq_flags);
 450         memcpy(&gps_pos, pos, sizeof(gps_pos));
 451         IRQ_UNLOCK(irq_flags);
 452
 453         return 0;
 454 }
 455
 456 /* display current GPS position stored in the global variable */
 457 static void display_gps(void)
 458 {
 459         printf_P(PSTR("id %.2X mode %.2X svnum %.2X gpsw %.4X "
 460                         "tow %.10"PRIu32"\t"),
 461                 gps_pos.msg_id,
 462                 gps_pos.mode,
 463                 gps_pos.sv_num,
 464                 gps_pos.gps_week,
 465                 gps_pos.tow);
 466
 467         printf_P(PSTR("lat %.8"PRIx32" long %.8"PRIx32" alt %.8"PRIx32"\n"),
 468                 gps_pos.latitude,
 469                 gps_pos.longitude,
 470                 gps_pos.altitude);
 471
 472         printf_P(PSTR("gdop %3.3f pdop %3.3f hdop %3.3f vdop %3.3f "
 473                         "tdop %3.3f\n"),
 474                 (double)gps_pos.gdop/100.,
 475                 (double)gps_pos.pdop/100.,
 476                 (double)gps_pos.hdop/100.,
 477                 (double)gps_pos.vdop/100.,
 478                 (double)gps_pos.tdop/100.);
 479
 480         printf_P(PSTR("lat %3.5f long %3.5f alt %3.5f sea_alt %3.5f\n"),
 481                 (double)gps_pos.latitude/10000000.,
 482                 (double)gps_pos.longitude/10000000.,
 483                 (double)gps_pos.altitude/100.,
 484                 (double)gps_pos.sea_altitude/100.);
 485
 486         printf_P(PSTR("vx %3.3f vy %3.3f vz %3.3f\n"),
 487                 (double)gps_pos.ecef_vx/100.,
 488                 (double)gps_pos.ecef_vy/100.,
 489                 (double)gps_pos.ecef_vz/100.);
 490 }
 491
 492 static void gps_venus_cb(struct callout_mgr *cm, struct callout *tim, void *arg)
 493 {
 494         int16_t c;
 495         int ret;
 496
 497         (void)cm;
 498         (void)tim;
 499         (void)arg;
 500
 501         while (1) {
 502                 c = uart_recv_nowait(GPS_UART);
 503                 if (c < 0) /* no more char */
 504                         goto resched;
 505
 506                 ret = recv_cb(c);
 507                 if (ret == 0) {
 508                         decode_gps_pos(rxframe.data, rxframe.len);
 509                         if (0)
 510                                 display_gps();
 511                 }
 512         }
 513
 514  resched:
 515         callout_schedule(cm, tim, 2);
 516 }
 517
 518 static void venus634_configure(void)
 519 {
 520         /* ask the GPS to reset */
 521         printf_P(PSTR("init..."));
 522         venus634_restart();
 523         wait_ack(M_RESTART);
 524
 525         /* it seems we must wait that the GPS is restarted, else it doesn't work
 526          * properly */
 527         wait_ms(500);
 528
 529         printf_P(PSTR("binmode..."));
 530         venus634_msg_type();
 531         wait_ack(M_OUTPUT);
 532
 533         printf_P(PSTR("waas..."));
 534         venus634_waas();
 535         wait_ack(M_WAAS);
 536
 537         printf_P(PSTR("rate..."));
 538         venus634_rate();
 539         wait_ack(M_RATE);
 540
 541         printf_P(PSTR("GPS configuration done !\n"));
 542 }
 543
 544 /*
 545   https://www.sparkfun.com/datasheets/GPS/Modules/AN0003_v1.4.14_FlashOnly.pdf
 546 */
 547
 548 int gps_venus_init(void)
 549 {
 550         venus634_configure();
 551
 552         callout_init(&gps_timer, gps_venus_cb, NULL, GPS_PRIO);
 553         callout_schedule(&imuboard.intr_cm, &gps_timer, 2); /* every 2ms */
 554
 555         return 0;
 556 }
 557
 558 void gps_get_pos(struct gps_pos *pos)
 559 {
 560         memcpy(pos, &gps_pos, sizeof(*pos));
 561 }
 562
 563 int gps_loop(void)
 564 {
 565         uint32_t ms;
 566         uint8_t flags, prio;
 567         int16_t len;
 568         char buf[128];
 569         struct gps_pos pos;
 570
 571         while (1) {
 572
 573                 IRQ_LOCK(flags);
 574                 ms = global_ms;
 575                 IRQ_UNLOCK(flags);
 576
 577                 /* get position (prevent modification of gps pos during copy) */
 578                 prio = callout_mgr_set_prio(&imuboard.intr_cm, GPS_PRIO);
 579                 gps_get_pos(&pos);
 580                 callout_mgr_restore_prio(&imuboard.intr_cm, prio);
 581
 582                 /* XXX copy */
 583                 len = snprintf(buf, sizeof(buf),
 584                         "%"PRIu32" "
 585                         "svnum %.2X lat %3.5f long %3.5f "
 586                         "alt %3.5f sea_alt %3.5f\n",
 587                         ms, gps_pos.sv_num,
 588                         (double)gps_pos.latitude/10000000.,
 589                         (double)gps_pos.longitude/10000000.,
 590                         (double)gps_pos.altitude/100.,
 591                         (double)gps_pos.sea_altitude/100.);
 592
 593
 594                 if (sd_log_enabled()) {
 595
 596                         if (sd_log_write(buf, len) != len) {
 597                                 printf_P(PSTR("error writing to file\n"));
 598                                 return -1;
 599                         }
 600                 }
 601                 else
 602                         printf_P(PSTR("%s"), buf);
 603
 604         }
 605
 606         return 0;
 607 }