boards/ek-lm3s1968/examples/gps/main.c

   1 /**
   2  * \file
   3  * <!--
   4  * This file is part of BeRTOS.
   5  *
   6  * Bertos is free software; you can redistribute it and/or modify
   7  * it under the terms of the GNU General Public License as published by
   8  * the Free Software Foundation; either version 2 of the License, or
   9  * (at your option) any later version.
  10  *
  11  * This program is distributed in the hope that it will be useful,
  12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14  * GNU General Public License for more details.
  15  *
  16  * You should have received a copy of the GNU General Public License
  17  * along with this program; if not, write to the Free Software
  18  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  19  *
  20  * As a special exception, you may use this file as part of a free software
  21  * library without restriction.  Specifically, if other files instantiate
  22  * templates or use macros or inline functions from this file, or you compile
  23  * this file and link it with other files to produce an executable, this
  24  * file does not by itself cause the resulting executable to be covered by
  25  * the GNU General Public License.  This exception does not however
  26  * invalidate any other reasons why the executable file might be covered by
  27  * the GNU General Public License.
  28  *
  29  * Copyright 2010 Develer S.r.l. (http://www.develer.com/)
  30  *
  31  * -->
  32  *
  33  * \author Andrea Righi <arighi@develer.com>
  34  *
  35  * \brief DevelGPS demo application with gps.
  36  */
  37
  38 #include "hw/hw_led.h"
  39
  40 #include <cpu/irq.h>
  41
  42 #include <drv/lcd_rit128x96.h>
  43 #include <drv/timer.h>
  44 #include <drv/ser.h>
  45 #include <drv/flash.h>
  46 #include <drv/kbd.h>
  47
  48 #include <io/kblock.h>
  49 #include <io/kfile_block.h>
  50
  51 #include <kern/proc.h>
  52 #include <kern/sem.h>
  53
  54 #include <net/nmea.h>
  55
  56 #include <gfx/font.h>
  57 #include <gfx/text.h>
  58 #include <gfx/gfx_p.h> /* BM_PLOT() */
  59 #include <gui/menu.h>
  60 #include <math.h>
  61
  62 #include "compass.h"
  63
  64 /* OLED/GUI stuff */
  65 #include "logo.c"
  66 #define KEY_MASK (K_UP | K_DOWN | K_LEFT | K_RIGHT | K_OK)
  67 extern Font font_gohu;
  68 static uint8_t raster[RAST_SIZE(LCD_WIDTH, LCD_HEIGHT)];
  69 static Bitmap lcd_bitmap;
  70
  71 #define SCRSVR_TIME 60000
  72 static ticks_t scrsvr_timestamp;
  73 static bool is_lcd_off;
  74
  75 /* Serial and NMEA stuff */
  76 static Serial ser_port;
  77 static nmeap_context_t nmea;
  78 static NmeaGga gga;
  79 static NmeaRmc rmc;
  80 static NmeaVtg vtg;
  81 static bool nmea_update;
  82
  83 static long prev_b;
  84 static long lat, lon;
  85 static long target_lat, target_lon;
  86
  87 /* Storage stuff */
  88 #define GPS_POS_MAGIC 0xdeadbeef
  89 static Flash flash_blk;
  90 static KFileBlock flash;
  91
  92 struct SettingsData
  93 {
  94         uint32_t magic;
  95         long target_lat, target_lon;
  96 };
  97
  98 static void flash_load_target(void)
  99 {
 100         struct SettingsData data;
 101
 102         kfile_seek(&flash.fd, -sizeof(data), KSM_SEEK_END);
 103         kfile_read(&flash.fd, &data, sizeof(data));
 104
 105         if (data.magic == GPS_POS_MAGIC)
 106         {
 107                 target_lat = data.target_lat;
 108                 target_lon = data.target_lon;
 109         }
 110 }
 111
 112 static void flash_save_target(void)
 113 {
 114         struct SettingsData data;
 115
 116         data.magic = GPS_POS_MAGIC;
 117         data.target_lat = target_lat;
 118         data.target_lon = target_lon;
 119
 120         kfile_seek(&flash.fd, -sizeof(data), KSM_SEEK_END);
 121         kfile_write(&flash.fd, &data, sizeof(data));
 122         kfile_flush(&flash.fd);
 123 }
 124
 125
 126 /* Display management */
 127 INLINE void video_off(void)
 128 {
 129         unsigned long delta =
 130                 (long)ticks_to_ms(timer_clock_unlocked()) -
 131                 (long)scrsvr_timestamp;
 132
 133         if (!is_lcd_off && delta > SCRSVR_TIME)
 134         {
 135                 rit128x96_off();
 136                 is_lcd_off = true;
 137         }
 138 }
 139
 140 INLINE void video_on(void)
 141 {
 142         if (is_lcd_off)
 143         {
 144                 rit128x96_on();
 145                 is_lcd_off = false;
 146         }
 147         scrsvr_timestamp = ticks_to_ms(timer_clock_unlocked());
 148 }
 149
 150 INLINE void repaint(Bitmap *bm)
 151 {
 152         rit128x96_blitBitmap(bm);
 153 }
 154
 155 INLINE keymask_t keypad_peek(void)
 156 {
 157         keymask_t key = kbd_peek();
 158
 159         if (key & KEY_MASK)
 160         {
 161                 if (is_lcd_off)
 162                         key = 0;
 163                 video_on();
 164         }
 165         return key;
 166 }
 167
 168 /* Status LED thread */
 169 static void NORETURN led_process(void)
 170 {
 171         while (1)
 172         {
 173                 video_off();
 174                 if (!nmea_update)
 175                 {
 176                         timer_delay(1000);
 177                         continue;
 178                 }
 179                 LED_ON();
 180                 timer_delay(100);
 181                 LED_OFF();
 182                 nmea_update = false;
 183         }
 184 }
 185
 186 /* NMEA parser */
 187 static void nmea_callback(nmeap_context_t *context, void *data, void *user_data)
 188 {
 189         (void)context;
 190         (void)data;
 191         (void)user_data;
 192
 193         lat = (long)gga.latitude;
 194         lon = (long)gga.longitude;
 195
 196         nmea_update = true;
 197 }
 198
 199 static void NORETURN ser_process(void)
 200 {
 201         while (1)
 202         {
 203                 nmea_poll(&nmea, &ser_port.fd);
 204                 kfile_clearerr(&ser_port.fd);
 205         }
 206 }
 207
 208 /* Target position screen */
 209 static void target(Bitmap *bm)
 210 {
 211         const long STEP = 10000000,
 212                 MAX_LAT = 90000000, MIN_LAT = -90000000,
 213                 MAX_LON = 180000000, MIN_LON = -180000000;
 214         long step = STEP, target;
 215         int row = 0, pos = 1;
 216         keymask_t key;
 217
 218         gfx_bitmapClear(bm);
 219         video_on();
 220         text_xprintf(bm, 0, 0,
 221                 STYLEF_UNDERLINE | TEXT_CENTER | TEXT_FILL,
 222                 "Target position");
 223         while (1)
 224         {
 225                 if (!is_lcd_off)
 226                 {
 227                         text_xprintf(bm, 3, 0, TEXT_FILL,
 228                                         "Lat:  %02ld.%06ld %c %s",
 229                                         ABS(target_lat) / 1000000L,
 230                                         ABS(target_lat) % 1000000,
 231                                         target_lat >= 0 ? 'N' : 'S',
 232                                         row == 0 ? "<-" : "");
 233                         text_xprintf(bm, row ? 4 : 6, 0,
 234                                         TEXT_FILL | TEXT_CENTER, " ");
 235                         text_xprintf(bm, row ? 6 : 4, 0,
 236                                         TEXT_FILL, "%*c",
 237                                         (step < 1000000L) ?
 238                                                 pos + 7 : pos + 6, '^');
 239                         text_xprintf(bm, 5, 0, TEXT_FILL,
 240                                         "Lon: %03ld.%06ld %c %s",
 241                                         ABS(target_lon) / 1000000L,
 242                                         ABS(target_lon) % 1000000L,
 243                                         target_lon >= 0 ? 'E' : 'W',
 244                                         row == 1 ? "<-" : "");
 245                         repaint(bm);
 246                 }
 247                 key = keypad_peek();
 248                 if (key & K_UP)
 249                 {
 250                         if (row == 0)
 251                         {
 252                                 target = target_lat + step;
 253                                 if (target <= MAX_LAT)
 254                                 {
 255                                         if (target_lat < 0 && target > 0)
 256                                                 target_lat = ABS(target_lat) +
 257                                                                 step;
 258                                         else
 259                                                 target_lat = target;
 260                                 }
 261                         }
 262                         else
 263                         {
 264                                 target = target_lon + step;
 265                                 if (target <= MAX_LON)
 266                                 {
 267                                         if (target_lon < 0 && target > 0)
 268                                                 target_lon = ABS(target_lon) +
 269                                                                 step;
 270                                         else
 271                                                 target_lon = target;
 272                                 }
 273                         }
 274                 }
 275                 else if (key & K_DOWN)
 276                 {
 277                         if (row == 0)
 278                         {
 279                                 target = target_lat - step;
 280                                 if (target >= MIN_LAT)
 281                                 {
 282                                         if (target_lat > 0 && target < 0)
 283                                                 target_lat = -ABS(target_lat) -
 284                                                                 step;
 285                                         else
 286                                                 target_lat = target;
 287                                 }
 288                         }
 289                         else
 290                         {
 291                                 target = target_lon - step;
 292                                 if (target >= MIN_LON)
 293                                 {
 294                                         if (target_lon > 0 && target < 0)
 295                                                 target_lon = -ABS(target_lon) -
 296                                                                 step;
 297                                         else
 298                                                 target_lon = target;
 299                                 }
 300                         }
 301                 }
 302                 else if (key & K_LEFT)
 303                 {
 304                         if (step < STEP)
 305                         {
 306                                 step *= 10;
 307                                 pos--;
 308                         }
 309                 }
 310                 else if (key & K_RIGHT)
 311                 {
 312                         if (step >= 10)
 313                         {
 314                                 step /= 10;
 315                                 pos++;
 316                         }
 317                 }
 318                 else if (key & K_OK)
 319                 {
 320                         if (row++ == 1)
 321                                 break;
 322                          /* Move to longigude */
 323                 }
 324                 cpu_relax();
 325         }
 326         flash_save_target();
 327 }
 328
 329 /* Compass management */
 330 static void draw_compass(Bitmap *bm)
 331 {
 332         const int R = LCD_HEIGHT / 3, R_SMALL = 10;
 333         long x, y, x1, y1, x2, y2, d;
 334         int i;
 335
 336         d = distance(lat / 1E6, lon / 1E6, target_lat / 1E6, target_lon / 1E6);
 337
 338         x = (long)((float)R * (1 + sin(deg2rad((float)prev_b))));
 339         y = (long)((float)R * (1 - cos(deg2rad((float)prev_b))));
 340
 341         x1 = R - R_SMALL + (long)((float)R_SMALL *
 342                 (1 + sin(deg2rad((float)prev_b - 120.0))));
 343         y1 = R - R_SMALL + (long)((float)R_SMALL *
 344                 (1 - cos(deg2rad((float)prev_b - 120.0))));
 345         x2 = R - R_SMALL + (long)((float)R_SMALL *
 346                 (1 + sin(deg2rad((float)prev_b + 120.0))));
 347         y2 = R - R_SMALL + (long)((float)R_SMALL *
 348                 (1 - cos(deg2rad((float)prev_b + 120.0))));
 349
 350         gfx_bitmapClear(bm);
 351         /* Print direction heading and degrees */
 352         text_xprintf(bm, 0, 5, 0, "%s", "N");
 353         text_xprintf(bm, 0, 0, TEXT_RIGHT, "%s", compass_heading(prev_b));
 354         text_xprintf(bm, 1, 0, TEXT_RIGHT, "%ld deg.", prev_b);
 355         /* Print distance */
 356         text_xprintf(bm, 2, 0, TEXT_RIGHT, "%ld %s",
 357                         d >= 1000 ? d / 1000 : d,
 358                         d >= 1000 ? "Km" : "m");
 359         /* Print current and target position */
 360         text_xprintf(bm, 6, 0, TEXT_FILL, "%02ld.%06ld%c%03ld.%06ld%c",
 361                         ABS(lat) / 1000000L,
 362                         ABS(lat) % 1000000,
 363                         lat >= 0 ? 'N' : 'S',
 364                         ABS(lon) / 1000000L,
 365                         ABS(lon) % 1000000L,
 366                         lon >= 0 ? 'E' : 'W');
 367         text_xprintf(bm, 7, 0, TEXT_FILL, "%02ld.%06ld%c%03ld.%06ld%c",
 368                         ABS(target_lat) / 1000000L,
 369                         ABS(target_lat) % 1000000,
 370                         target_lat >= 0 ? 'N' : 'S',
 371                         ABS(target_lon) / 1000000L,
 372                         ABS(target_lon) % 1000000L,
 373                         target_lon >= 0 ? 'E' : 'W');
 374         /* Draw the circle */
 375         for (i = 0; i < 360; i++)
 376                 BM_PLOT(bm,
 377                         (long)((float)R * (1 + sin(deg2rad((float)i)))),
 378                         (long)((float)R * (1 - cos(deg2rad((float)i)))));
 379         /* Draw the needle */
 380         gfx_rectFill(bm, R - 2, R - 2, R + 2, R + 2);
 381         gfx_line(bm, R, R, x1, y1);
 382         gfx_line(bm, R, R, x2, y2);
 383         gfx_line(bm, x1, y1, x, y);
 384         gfx_line(bm, x2, y2, x, y);
 385
 386         repaint(bm);
 387 }
 388
 389 static void compass(Bitmap *bm)
 390 {
 391         long b, inc;
 392
 393         video_on();
 394         while (1)
 395         {
 396                 if (!is_lcd_off)
 397                 {
 398                         b = bearing(lat / 1E6, lon / 1E6,
 399                                         target_lat / 1E6, target_lon / 1E6);
 400                         inc = ABS(b - prev_b) < 360 - ABS(b - prev_b) ?  1 : -1;
 401                         /* Compass animation */
 402                         if (b < prev_b)
 403                                 inc = -inc;
 404                         while (prev_b != b)
 405                         {
 406                                 prev_b = (prev_b + inc) % 360;
 407                                 if (prev_b < 0)
 408                                         prev_b = 359;
 409                                 if (!(prev_b % 5))
 410                                         draw_compass(bm);
 411                                 if (keypad_peek() & KEY_MASK)
 412                                         return;
 413                         }
 414                         draw_compass(bm);
 415                 }
 416                 cpu_relax();
 417                 if (keypad_peek() & KEY_MASK)
 418                         return;
 419         }
 420 }
 421
 422 /* GPS receiver status */
 423 static const char *gps_fix[] =
 424 {
 425         "invalid",
 426         "GPS",
 427         "DGPS",
 428         "PPS",
 429         "RTK",
 430         "float-RTK",
 431         "estimated",
 432         "manual",
 433         "simulation",
 434 };
 435
 436 static void gps_data(Bitmap *bm)
 437 {
 438         struct tm tm;
 439         char buf[32];
 440
 441         video_on();
 442         gfx_bitmapClear(bm);
 443         while (1)
 444         {
 445                 if (!is_lcd_off)
 446                 {
 447                         if (!rmc.time)
 448                         {
 449                                 text_xprintf(bm, 3, 0,
 450                                         TEXT_CENTER | TEXT_FILL, "No GPS data");
 451                         }
 452                         else
 453                         {
 454                                 gmtime_r(&rmc.time, &tm);
 455
 456                                 text_xprintf(bm, 0, 0, TEXT_FILL,
 457                                                 "Lat. %ld.%06ld%c",
 458                                                 ABS(lat) / 1000000L,
 459                                                 ABS(lat) % 1000000,
 460                                                 lat >= 0 ? 'N' : 'S');
 461                                 text_xprintf(bm, 1, 0, TEXT_FILL,
 462                                                 "Lon. %ld.%06ld%c",
 463                                                 ABS(lon) / 1000000L,
 464                                                 ABS(lon) % 1000000L,
 465                                                 lon >= 0 ? 'E' : 'W');
 466                                 text_xprintf(bm, 2, 0, TEXT_FILL,
 467                                                 "Alt. %ld", gga.altitude);
 468                                 text_xprintf(bm, 3, 0, TEXT_FILL,
 469                                                 "Speed: %d", vtg.km_speed);
 470                                 if (gga.quality < countof(gps_fix))
 471                                         text_xprintf(bm, 4, 0, TEXT_FILL,
 472                                                         "Fix: %s",
 473                                                         gps_fix[gga.quality]);
 474                                 else
 475                                         text_xprintf(bm, 4, 0, TEXT_FILL,
 476                                                         "Fix: %d", gga.quality);
 477                                 text_xprintf(bm, 5, 0, TEXT_FILL,
 478                                                 "Satellites: %d",
 479                                                 gga.satellites);
 480                                 strftime(buf, sizeof(buf),
 481                                                 "Date: %Y-%m-%d %a", &tm);
 482                                 text_xprintf(bm, 6, 0, TEXT_FILL, "%s", buf);
 483                                 strftime(buf, sizeof(buf),
 484                                                 "Time: %H:%M:%S (UTC)", &tm);
 485                                 text_xprintf(bm, 7, 0, TEXT_FILL, "%s", buf);
 486                         }
 487                         repaint(bm);
 488                 }
 489                 if (keypad_peek() & KEY_MASK)
 490                         break;
 491                 cpu_relax();
 492         }
 493 }
 494
 495 /* BeRTOS screen */
 496 static void about(Bitmap *bm)
 497 {
 498         gfx_bitmapClear(bm);
 499         video_on();
 500         text_xprintf(bm, 7, 0,
 501                 STYLEF_UNDERLINE | TEXT_CENTER | TEXT_FILL,
 502                 "http://www.bertos.org");
 503         repaint(bm);
 504         if (!is_lcd_off)
 505         {
 506                 const uint8_t *p = &logo[BITMAP_HEADER_SIZE];
 507                 uint8_t h = logo[BITMAP_HEIGHT_OFFSET];
 508                 uint8_t w = logo[BITMAP_WIDTH_OFFSET];
 509                 uint8_t r;
 510
 511                 for (r = 0; r < h; r++)
 512                 {
 513                         rit128x96_blitRaw(p,
 514                                 (128 - w) / 2, 70 - r, w, 1);
 515                         p += w / 2;
 516                 }
 517         }
 518         while (!(keypad_peek() & KEY_MASK))
 519                 cpu_relax();
 520 }
 521
 522 static struct MenuItem main_items[] =
 523 {
 524         {(const_iptr_t)"Target", 0, (MenuHook)target, (iptr_t)&lcd_bitmap},
 525         {(const_iptr_t)"Compass", 0, (MenuHook)compass, (iptr_t)&lcd_bitmap},
 526         {(const_iptr_t)"GPS data", 0, (MenuHook)gps_data, (iptr_t)&lcd_bitmap},
 527         {(const_iptr_t)"About...", 0, (MenuHook)about, (iptr_t)&lcd_bitmap},
 528         {(const_iptr_t)0, 0, NULL, (iptr_t)NULL}
 529 };
 530
 531 static struct Menu main_menu =
 532 {
 533         main_items, "DevelGPS v0.1", MF_STICKY | MF_SAVESEL, &lcd_bitmap, 0, rit128x96_blitBitmap
 534 };
 535
 536 static void init(void)
 537 {
 538         IRQ_ENABLE;
 539
 540         kdbg_init();
 541         timer_init();
 542         proc_init();
 543
 544         scrsvr_timestamp = ticks_to_ms(timer_clock_unlocked());
 545         LED_INIT();
 546
 547         flash_init(&flash_blk, 0);
 548         kfileblock_init(&flash, &flash_blk.blk);
 549         flash_load_target();
 550
 551         ser_init(&ser_port, SER_UART1);
 552         ser_setbaudrate(&ser_port, 38400);
 553
 554         nmeap_init(&nmea, NULL);
 555         nmeap_addParser(&nmea, "GPGGA", nmea_gpgga, nmea_callback, &gga);
 556         nmeap_addParser(&nmea, "GPRMC", nmea_gprmc, nmea_callback, &rmc);
 557         nmeap_addParser(&nmea, "GPVTG", nmea_gpvtg, nmea_callback, &vtg);
 558
 559         rit128x96_init();
 560         gfx_bitmapInit(&lcd_bitmap, raster, LCD_WIDTH, LCD_HEIGHT);
 561         gfx_setFont(&lcd_bitmap, &font_gohu);
 562         repaint(&lcd_bitmap);
 563
 564         kbd_init();
 565 }
 566
 567 int main(void)
 568 {
 569         init();
 570
 571         proc_new(led_process, NULL, KERN_MINSTACKSIZE * 2, NULL);
 572         proc_new(ser_process, NULL, KERN_MINSTACKSIZE * 2, NULL);
 573
 574         while (1)
 575         {
 576                 iptr_t res = menu_handle(&main_menu);
 577                 if (res != MENU_TIMEOUT)
 578                         video_on();
 579                 cpu_relax();
 580         }
 581 }