root/core/gps.c

/* [<][>][^][v][top][bottom][index][help] */

DEFINITIONS

This source file includes following definitions.
  1. geodiff
  2. geo2rect
  3. rect2polar
  4. polar2rect
  5. radian2deg
  6. georegInit
  7. georegAdd
  8. georegActual
  9. georegChange
  10. lock
  11. unlock
  12. draw_txt_centered
  13. run_compass_task
  14. test_timezone
  15. load_bitmap
  16. draw_gps_course_info
  17. draw_compass_needle
  18. draw_compass
  19. show_sat_symbol_task
  20. gps_data_task
  21. tracking_icon_task
  22. no_signal_task
  23. gps_logging_task
  24. compass_display_task
  25. gps_waypoint
  26. gps_write_timezone
  27. gps_write_home
  28. init_gps_startup
  29. init_gps_logging_task
  30. init_gps_compass_task
  31. init_gps_navigate_to_home
  32. init_gps_navigate_to_photo

   1 
   2 #include "camera_info.h"
   3 #include "stdlib.h"
   4 #include "properties.h"
   5 #include "task.h"
   6 #include "modes.h"
   7 #include "debug_led.h"
   8 #include "shutdown.h"
   9 #include "sound.h"
  10 #include "backlight.h"
  11 #include "temperature.h"
  12 #include "file_counter.h"
  13 #include "gps.h"
  14 #include "gps_math.h"
  15 
  16 #include "stddef.h"
  17 #include "conf.h"
  18 #include "string.h"
  19 #include "keyboard.h"
  20 #include "lang.h"
  21 #include "levent.h"
  22 #include "math.h"
  23 #include "gui.h"
  24 #include "gui_draw.h"
  25 #include "gui_batt.h"
  26 #include "gui_lang.h"
  27 #include "gui_mbox.h"
  28 
  29 // for testing on cameras without a GPS chip - add #define CAM_HAS_GPS 1 to their platform_camera.h file for the test
  30 //#define SIMULATED_GPS 1    
  31 #undef  SIMULATED_GPS 
  32 
  33 // gps task control
  34 
  35 static int gps_data_lock ;      // task running flags
  36 static int gps_show_lock;
  37 static int gps_record_lock;
  38 static int gps_track_lock;
  39 static int gps_compass_lock ;
  40 static int gps_nosignal_lock;    
  41 
  42 static int exit_data_task =0;  // request task to exit flags
  43 static int exit_show_task=0;
  44 static int exit_record_task=0;
  45 static int exit_track_task=0;
  46 static int exit_compass_task=0;
  47 static int exit_nosignal_task=0;
  48 
  49 //static data for gps task
  50 
  51 static int test_timezone_once=0;
  52 
  53 static char g_d_tim[10];
  54 static char g_d_dat[12];
  55 static char g_d_tim_gps[10];
  56 static char g_d_dat_gps[12];
  57 static int  g_d_stat;
  58 
  59 static double g_d_lat_nav=0.0;
  60 static double g_d_lon_nav=0.0;
  61 static double g_d_lat;
  62 static double g_d_lon;
  63 static double g_d_hei;
  64 
  65 static int time_to_end=0;
  66 
  67 static int navigation_mode=0;       // 0=none, 1=to image,  2=home
  68 
  69 static t_regression_xy buffer1[50];
  70 static t_regression_xy buffer2[50];
  71 static t_regression_xy buffer3[50];
  72 
  73 #define PI      (3.1415926535)
  74 #define RHO     (180.0/3.1415926535)
  75 #define EARTH   (40.e6/360.0)
  76 
  77 typedef struct {
  78     double lat_w;
  79     double lon_w;
  80 } t_geo;
  81 
  82 typedef struct {
  83     double north;
  84     double east;
  85 } t_rectangular;
  86 
  87 typedef struct {
  88     double delta;
  89     double alpha;   // radian
  90 } t_polar;
  91 
  92 static void
  93 geodiff (t_geo * von, t_geo* nach, t_geo* delta) {
  94     delta->lat_w = nach->lat_w - von->lat_w;
  95     delta->lon_w = nach->lon_w - von->lon_w;
  96 }
  97 
  98 static void
  99 geo2rect (t_geo * g, t_rectangular* r, double lat_w) {
 100     r->north= g->lat_w * EARTH;
 101     r->east = g->lon_w * EARTH * cos(lat_w/RHO);
 102 }
 103 
 104 static void
 105 rect2polar (t_rectangular* r, t_polar* p) {
 106     p->delta = sqrt (r->north*r->north + r->east*r->east);
 107     p->alpha = arctan2 (r->east, r->north);
 108 }
 109 
 110 static void
 111 polar2rect (t_rectangular* r, t_polar* p) {
 112     r->east = sin(p->alpha) * p->delta;
 113     r->north= cos(p->alpha) * p->delta;
 114 }
 115 
 116 static int
 117 radian2deg (double alpha) {
 118     return (int) (alpha * RHO + 360.5) % 360;
 119 }
 120 
 121 typedef struct {
 122     t_regression lat_w;
 123     t_regression lon_w;
 124 } t_georeg;
 125 
 126 static void
 127 georegInit (t_georeg* reg, int size, t_regression_xy * buffer1, t_regression_xy * buffer2) {
 128     regressionInit (&reg->lat_w, size, buffer1);
 129     regressionInit (&reg->lon_w, size, buffer2);
 130 }
 131 
 132 static void
 133 georegAdd (t_georeg* reg, double time, t_geo* geo) {
 134     regressionAdd (&reg->lat_w, time, geo->lat_w);
 135     regressionAdd (&reg->lon_w, time, geo->lon_w);
 136 }
 137 
 138 static void
 139 georegActual (t_georeg* reg, t_geo* geo) {
 140     geo->lat_w = regressionActual (&reg->lat_w);
 141     geo->lon_w = regressionActual (&reg->lon_w);
 142 }
 143 
 144 static void
 145 georegChange (t_georeg* reg, t_geo* geo) {
 146     geo->lat_w = regressionChange (&reg->lat_w);
 147     geo->lon_w = regressionChange (&reg->lon_w);
 148 }
 149 
 150 static double now;                                     // seconds of day
 151 static t_geo gps;                                      // measured noisy position
 152 static t_georeg georeg;                                // regression for actual position
 153 static t_geo ist;                                      // actual position of regression
 154 static t_geo speed;                                    // actual position of regression in Grad/s
 155 static t_rectangular mspeed;                           // actual position of regressionin m/s (N/E)
 156 static t_polar pspeed;                                 // actual position of regression in m/s + angle
 157 static int direction;                                  // direction
 158 static t_geo soll;                                     // nominal position
 159 static t_geo delta;                                    // distance to destination(in Grad)
 160 static t_rectangular rdelta;                           // distance to destination (metric N/E)
 161 static t_polar pdelta;                                 // distance to destination (metric+english)
 162 static int peil;                                       // bearing to destination
 163 static int angle;                                      // angle on display from march to the target
 164 static t_regression deltareg;                          // heading and speed
 165 
 166 #ifdef SIMULATED_GPS
 167     #include "gps_simulate.c"  
 168 #endif
 169 
 170 //=======================================================================
 171 //
 172 //  Bakery algoritmn mutex lock / unlock
 173 //
 174 #define NUM_TASKS 6
 175 static volatile unsigned entering[NUM_TASKS] = { 0 };
 176 static volatile unsigned number[NUM_TASKS] = { 0 };
 177 
 178 void lock(int n_task) {
 179   int i ;
 180   unsigned max = 0;    
 181   entering[n_task] = 1;
 182   for (i = 0; i < NUM_TASKS; i++) {
 183     if (number[i] > max) max = number[i];
 184   }
 185   number[n_task] = max + 1 ;
 186   entering[n_task] = 0;
 187   for (i = 0; i < NUM_TASKS; i++) {
 188     if( i != n_task) {
 189         while ( entering[i] !=0 ) { msleep(50) ; }
 190         while ((number[i] != 0) && ( number[i] < number[n_task] || (number[i] == number[n_task] && i < n_task) )) { msleep(50);}
 191     }
 192   }
 193 }
 194  
 195 void unlock(int n_task)
 196 {
 197   number[n_task] = 0;
 198 }
 199  
 200 //=======================================================================
 201 //
 202 //  Helper functions - called from within running tasks
 203 //
 204 
 205 void draw_txt_centered(int line, char *buf, int color)
 206 {
 207     draw_txt_string( (camera_screen.width/FONT_WIDTH-strlen(buf))>>1 , line, buf, color) ;
 208 }
 209 
 210 static void compass_display_task();
 211 static void run_compass_task(int mode){  
 212     
 213     navigation_mode = mode ;
 214     
 215     if (gps_compass_lock == 0 )
 216     {
 217         gps_compass_lock = 1 ;   
 218         CreateTask("GPS_COMPASS", 0x19, 0x1000, compass_display_task);   
 219     }
 220 }
 221 
 222 static void test_timezone(){                           // creates timezone.txt file with current timezone info
 223   
 224     char text[2][30];
 225     char home_name[30];
 226     int timezone_1, timezone_2;
 227     char * endptr;
 228 
 229     sprintf(home_name, "A/GPS/Timezone/Timezone.txt");
 230 
 231     FILE* fp = fopen(home_name, "r");
 232     if( fp )
 233     {
 234         fgets(text[1], 15, fp);
 235         fgets(text[2], 15, fp);
 236         fclose(fp);
 237 
 238         g_d_lat_nav = (strtol (text[1], &endptr, 10 )) / 10000000.0;
 239         g_d_lon_nav = (strtol (text[2], &endptr, 10 )) / 10000000.0;
 240 
 241         timezone_1 = (int)((g_d_lon_nav+7.5)/15);
 242         timezone_2 = (int)((g_d_lon+7.5)/15);
 243 
 244         if (timezone_1 != timezone_2)
 245         {
 246             gui_mbox_init(LANG_INFORMATION, LANG_MENU_GPS_t_2, MBOX_BTN_OK|MBOX_TEXT_CENTER, NULL); //Timezone has changed!
 247         }
 248     }
 249 }
 250 
 251 static char *load_bitmap(char *datei){
 252     
 253     FILE *fd;
 254     char *bitmap;
 255     int bitmap_size;
 256     size_t result;
 257 
 258     fd = fopen ( datei, "r" );
 259 
 260     fseek (fd, 0, SEEK_END);
 261     bitmap_size = ftell (fd);
 262     fseek (fd, 0, SEEK_SET);
 263 
 264     bitmap = (char*) malloc (sizeof(char)*bitmap_size);
 265     result = fread (bitmap,1,bitmap_size,fd);
 266     fclose (fd);
 267 
 268     return bitmap;
 269 }
 270 
 271 static double draw_gps_course_info(int count){             // calculate & display course information
 272 
 273     char vBuf[512];
 274     int angle1=0;
 275 
 276     soll.lat_w = g_d_lat_nav;
 277     soll.lon_w = g_d_lon_nav;
 278 
 279     now = (double) (count);
 280     gps.lat_w = g_d_lat;                                                    // read GPS (with noise)
 281     gps.lon_w = g_d_lon;
 282     georegAdd               (&georeg, now, &gps);                           // feed the regression
 283     georegActual            (&georeg, &ist);                                // read averaged position
 284     georegChange            (&georeg, &speed);                              // read averaged velocity
 285     geo2rect                (&speed, &mspeed, ist.lat_w);                   // Convert to m/s
 286     rect2polar              (&mspeed, &pspeed);                             // convert to direction and speed.
 287     direction = radian2deg  (pspeed.alpha);                                 // get direction
 288     geodiff                 (&ist, &soll, &delta);                          // Distance in degrees (N/E)
 289     geo2rect                (&delta, &rdelta, ist.lat_w);                   // Distance in m (je N/E)
 290     rect2polar              (&rdelta, &pdelta);                             // Distance in degrees m and grad
 291     peil    = radian2deg    (pdelta.alpha);                                 // Bearing to destination
 292     angle  = radian2deg     (pdelta.alpha-pspeed.alpha);                    // Angle relative to the direction of march
 293     regressionAdd           (&deltareg, now, pdelta.delta);                 // do the regression
 294     double eta = regressionReverse (&deltareg, 0);                          // estimated time of arrival
 295     double rest= eta - now;                                                 // Time = arrival time - now
 296 
 297     if (abs(regressionChange (&deltareg))<0.5 || rest < 5.0) rest = 0.0;
 298 
 299     if (camera_info.state.gui_mode_none)
 300     {
 301         angle1=(int)angle;
 302         char anz1[40];
 303         char anz2[40];
 304         char image[9];        
 305 
 306         if (navigation_mode > 0)
 307         {
 308             sprintf(vBuf, lang_str(LANG_MENU_GPS_t_9), (int)pdelta.delta);
 309             draw_txt_string(16, 9, vBuf, MAKE_COLOR(COLOR_GREY_DK_TRANS, COLOR_WHITE));
 310 
 311             int s = (int)rest;
 312             int hour = s / 3600;
 313             s = s % 3600;
 314             int minute =  s / 60;
 315             s = s % 60;
 316             int second = s;
 317 
 318             sprintf(vBuf, lang_str(LANG_MENU_GPS_t_10), hour, minute, second);
 319             draw_txt_string(16, 10, vBuf, MAKE_COLOR(COLOR_GREY_DK_TRANS, COLOR_WHITE));
 320 
 321             sprintf(vBuf, lang_str(LANG_MENU_GPS_t_11), formatDouble (anz1, (pspeed.delta * 3.6), 0, 1));
 322             draw_txt_string(16, 11, vBuf, MAKE_COLOR(COLOR_GREY_DK_TRANS, COLOR_WHITE));
 323             sprintf(vBuf, lang_str(LANG_MENU_GPS_t_12), direction);
 324             draw_txt_string(16, 12, vBuf, MAKE_COLOR(COLOR_GREY_DK_TRANS, COLOR_WHITE));
 325             sprintf(vBuf, lang_str(LANG_MENU_GPS_t_13), (int)angle);
 326             draw_txt_string(16, 13, vBuf, MAKE_COLOR(COLOR_GREY_DK_TRANS, COLOR_WHITE));
 327 
 328             if (navigation_mode==1)
 329             {
 330                 sprintf(image, "%s", camera_jpeg_current_filename());
 331                 image[8] = '\0';
 332                 sprintf(vBuf, lang_str(LANG_MENU_GPS_t_14), image);  // "Navigation to photo: %s started"
 333                 draw_txt_centered(1, vBuf, MAKE_COLOR(COLOR_GREY_DK_TRANS, COLOR_RED));
 334             }
 335             
 336             if (navigation_mode==2)
 337             {
 338                 sprintf(vBuf, lang_str(LANG_MENU_GPS_t_17));         // "Navigation to Home started"
 339                 draw_txt_centered(1, vBuf, MAKE_COLOR(COLOR_GREY_DK_TRANS, COLOR_RED));
 340             }
 341 
 342             sprintf(vBuf, lang_str(LANG_MENU_GPS_t_15), formatDouble (anz1, g_d_lat_nav, 0, 7), formatDouble (anz2, g_d_lon_nav, 0, 7)); //"latitude=%s  -  longitude=%s "
 343             draw_txt_centered(2, vBuf, MAKE_COLOR(COLOR_GREY_DK_TRANS, COLOR_RED));
 344         }
 345         else
 346         {
 347             angle1= direction;
 348             sprintf(vBuf, lang_str(LANG_MENU_GPS_t_16), (int)angle1);   // "heading = %i°"
 349             draw_txt_string(16, 10, vBuf, MAKE_COLOR(COLOR_GREY_DK_TRANS, COLOR_WHITE));            
 350             sprintf(vBuf, lang_str(LANG_MENU_GPS_t_15), formatDouble (anz1, g_d_lat_nav, 0, 7), formatDouble (anz2, g_d_lon_nav, 0, 7)); //"latitude=%s  -  longitude=%s "
 351             draw_txt_string(16, 11, vBuf, MAKE_COLOR(COLOR_GREY_DK_TRANS, COLOR_WHITE));  
 352         }
 353     }
 354 
 355     return angle1;
 356 }
 357 
 358 // draw animated compass needle on GUI
 359 static void draw_compass_needle (int angle, double s_w, double c_w, char *bitmap, int extent, int m_x, int m_y, int offset_x, int offset_y, int f_v_1, int f_h_1, int f_v_2, int f_h_2){
 360  
 361 /*
 362     char vBuf[32];
 363     sprintf(vBuf, "%d", angle );      
 364     draw_txt_string(30, 0, vBuf, MAKE_COLOR(COLOR_GREY_DK_TRANS, COLOR_RED));   
 365 */
 366     if(bitmap)
 367     {
 368         int mx=0;
 369         int my=0;
 370         int px=0;
 371         int py=0;
 372 
 373         int pos=0;
 374         int x_n;
 375         int y_n;
 376 
 377         if (angle==0 || angle==360)
 378         {
 379             s_w = 0;
 380             c_w = -1;
 381         }
 382 
 383         if (angle==270)
 384         {
 385             s_w = -1;
 386             c_w = 0;
 387         }
 388 
 389         for(pos=0; pos<extent; pos++)
 390         {
 391             int data = bitmap[pos];
 392             if (data >= 49)
 393             {
 394                 px=offset_x+mx;
 395                 py=offset_y+my;
 396 
 397                 x_n = m_x + Round(((c_w * (m_x - px)) + (s_w * (m_y - py))),2);
 398                 y_n = m_y + Round(((c_w * (m_y - py)) - (s_w * (m_x - px))),2);
 399 
 400                 if (data == 49)
 401                 {
 402                     draw_pixel(x_n, y_n, f_v_1);
 403                 }
 404                 if (data == 50)
 405                 {
 406                     draw_pixel(x_n, y_n, f_v_2);
 407                 }
 408             }
 409             if (data == 10 || data == 13)
 410             {
 411                 mx=0;
 412                 my++;
 413                 pos++;
 414             }
 415             else
 416             {
 417                 mx++;
 418             }
 419         }
 420 
 421     }
 422 }
 423 
 424 // draw animated compass on GUI
 425 static void draw_compass (char *bitmap, int o_x, int o_y, int f_v_0, int f_h_0, int f_v_1, int f_h_1, int f_v_2, int f_h_2, int f_v_4, int f_h_4){
 426 
 427     if(bitmap)
 428     {
 429         int extent = strlen(bitmap);
 430         int pos=0;
 431         int mx=0;
 432         int my=0;
 433 
 434         for(pos=0; pos<extent; pos++)
 435         {
 436             int data = bitmap[pos];
 437             if (data == 48)
 438             {
 439                 draw_pixel(o_x+mx, o_y+my, f_v_0);
 440             }
 441             if (data == 49)
 442             {
 443                 draw_pixel(o_x+mx, o_y+my, f_v_1);
 444             }
 445             if (data == 50)
 446             {
 447                 draw_pixel(o_x+mx, o_y+my, f_v_2);
 448             }
 449             if (data == 51)
 450             {
 451                 draw_pixel(o_x+mx, o_y+my, f_v_4);
 452             }
 453             if (data == 10 || data == 13)
 454             {
 455                 mx=0;
 456                 my++;
 457                 pos++;
 458             }
 459             else
 460             {
 461                 mx++;
 462             }
 463         }
 464 
 465     }
 466 }
 467 
 468 //=======================================================================
 469 //
 470 // TASK : displays satellite dish image when CHDK GPS functionality enabled
 471 //
 472 static void show_sat_symbol_task(){
 473     
 474     char *bitmap = load_bitmap("A/CHDK/DATA/GPS_Sat.txt");
 475 
 476     if(bitmap)
 477     {
 478         int extent1 = strlen(bitmap);
 479         int pos1=0;
 480         int mx1=0;
 481         int my1=0;
 482         int o_x1=270;
 483         int o_y1=0;
 484         int nm=1;
 485         int f_v_0;
 486         int f_h_0;
 487         int f_v_1;
 488         int f_h_1;
 489         int f_v_2;
 490         int f_h_2;
 491         int f_v_3;
 492         int f_h_3;
 493         int f_v_4;
 494         int f_h_4;
 495 
 496         while (exit_show_task==0) 
 497         {           
 498             int stat = g_d_stat;
 499             
 500             mx1=0;
 501             my1=0;
 502 
 503             f_v_0=COLOR_TRANSPARENT;
 504             f_h_0=COLOR_TRANSPARENT;
 505 
 506             f_v_1=COLOR_GREEN;
 507             f_h_1=COLOR_GREEN;
 508 
 509             switch (stat)
 510             {
 511                 case 0 :
 512                     f_v_1= COLOR_RED ;
 513                     f_h_1= COLOR_RED ;
 514                     break ;
 515                 case 1 :
 516                     f_v_1= COLOR_YELLOW ;
 517                     f_h_1= COLOR_YELLOW ;
 518                     break ;
 519                 case 2 :
 520                     f_v_1= COLOR_GREEN ;
 521                     f_h_1= COLOR_GREEN ;
 522                     break ;
 523             }
 524             
 525             if (camera_info.state.mode_play)
 526             {
 527                 gps_img_data *igps = ( gps_img_data *) camera_jpeg_current_gps();
 528 
 529                 f_v_1= COLOR_RED ;
 530                 f_h_1= COLOR_RED ;
 531                 if (igps->latitudeRef[0] && igps->longitudeRef[0])
 532                 {
 533                     f_v_1= COLOR_YELLOW ;
 534                     f_h_1= COLOR_YELLOW ;
 535                     if (igps->height[1])
 536                     {
 537                         f_v_1= COLOR_GREEN ;
 538                         f_h_1= COLOR_GREEN ;
 539                     }
 540                 }
 541             }
 542 
 543             f_v_2= COLOR_BLACK ;
 544             f_h_2= COLOR_BLACK ;
 545 
 546             f_v_3= COLOR_YELLOW ;
 547             f_h_3= COLOR_YELLOW ;
 548 
 549             f_v_4= COLOR_BLUE ;
 550             f_h_4= COLOR_BLUE ;
 551 
 552             for(pos1=0; pos1<extent1; pos1++)
 553             {
 554                 int data = bitmap[pos1];
 555                 if (data == 48)
 556                 {
 557                     draw_pixel(o_x1+mx1, o_y1+my1, f_v_0);
 558                 }
 559                 if (data == 49)
 560                 {
 561                     draw_pixel(o_x1+mx1, o_y1+my1, f_v_1);
 562                 }
 563                 if (data == 50)
 564                 {
 565                     draw_pixel(o_x1+mx1, o_y1+my1, f_v_2);
 566                 }
 567                 if (data == 51)
 568                 {
 569                     draw_pixel(o_x1+mx1, o_y1+my1, f_v_3);
 570                 }
 571                 if (data == 52)
 572                 {
 573                     draw_pixel(o_x1+mx1, o_y1+my1, f_v_4);
 574                 }
 575                 if (data == 10 || data == 13)
 576                 {
 577                     mx1=0;
 578                     my1++;
 579                     pos1++;
 580                 }
 581                 else
 582                 {
 583                     mx1++;
 584                 }
 585             }
 586             msleep(1000);           
 587         }
 588         free(bitmap);
 589     }
 590     gps_show_lock = 0 ;     
 591     ExitTask();
 592 }
 593 
 594 //=======================================================================
 595 //
 596 // TASK : updates GPS data structures continuously if CHDK GPS functionality is enabled
 597 //
 598 static void gps_data_task(){
 599     
 600     test_timezone_once=0;
 601     
 602     while (exit_data_task==0)
 603     {
 604         tGPS gps;
 605         int tim00=0;
 606         int tim01=0;
 607         int tim02=0;
 608         unsigned long t;
 609         static struct tm *ttm;        
 610                           
 611   #ifndef SIMULATED_GPS
 612         GPS_UpdateData();  
 613         get_property_case(camera_info.props.gps, &gps, sizeof(gps));
 614   #else     
 615         GPS_FakeData() ;
 616         memcpy( &gps , &gps_dummy_data, sizeof(gps) );
 617   #endif
 618   
 619         t=time(NULL);
 620         ttm = localtime(&t);
 621         
 622         lock(0);
 623         
 624             g_d_lat=0.0;
 625             g_d_lat=(gps.latitude[0]/(gps.latitude[1]*1.0)) + (gps.latitude[2]/(gps.latitude[3]*60.0)) + (gps.latitude[4]/(gps.latitude[5]*3600.0));
 626             if (gps.latitudeRef[0]=='S') g_d_lat=-g_d_lat;
 627 
 628             g_d_lon=0.0;
 629             g_d_lon=(gps.longitude[0]/(gps.longitude[1]*1.0)) + (gps.longitude[2]/(gps.longitude[3]*60.0)) + (gps.longitude[4]/(gps.longitude[5]*3600.0));
 630             if (gps.longitudeRef[0]=='W') g_d_lon=-g_d_lon;
 631 
 632             g_d_hei=0.0;
 633             g_d_hei=gps.height[0]/(gps.height[1]*1.0);
 634 
 635     /**************
 636     char vBuf[256], lat[40],lon[40] ;
 637     sprintf(vBuf, "%d.%d.%d  %d.%d.%d",
 638         gps.latitude[0] / gps.latitude[1] , 
 639         gps.latitude[2] / gps.latitude[3] ,
 640         gps.latitude[4] / gps.latitude[5] ,   
 641         gps.longitude[0] / gps.longitude[1] , 
 642         gps.longitude[2] / gps.longitude[3] ,
 643         gps.longitude[4] / gps.longitude[5] );      
 644     draw_txt_string(1, 0, vBuf, MAKE_COLOR(COLOR_GREY_DK_TRANS, COLOR_RED));    
 645     formatDouble (lat, g_d_lat, 0, 7),
 646     formatDouble (lon, g_d_lon, 0, 7),    
 647     sprintf(vBuf,"lat=%s lon=%s",lat,lon );
 648     draw_txt_string(1, 1, vBuf, MAKE_COLOR(COLOR_GREY_DK_TRANS, COLOR_RED));              
 649     ************/
 650     
 651             sprintf(g_d_tim, "%02d:%02d:%02d", ttm->tm_hour, ttm->tm_min, ttm->tm_sec);
 652             sprintf(g_d_dat, "%04d-%02d-%02d", ttm->tm_year+1900, ttm->tm_mon+1, ttm->tm_mday);
 653 
 654             g_d_stat=0;
 655             if ((int)g_d_lat == 0) {g_d_stat = 0; }
 656             if ((int)g_d_lat != 0 && (int)g_d_hei == 0) {g_d_stat=1; }
 657             if ((int)g_d_lat != 0 && (int)g_d_hei != 0) {g_d_stat=2; }
 658 
 659             if (g_d_stat > 0)
 660             {
 661                 tim00=(int)(gps.timeStamp[0]/gps.timeStamp[1]);
 662                 tim01=(int)(gps.timeStamp[2]/gps.timeStamp[3]);
 663                 tim02=(int)(gps.timeStamp[4]/gps.timeStamp[5]);
 664                 sprintf(g_d_tim_gps, "%02d:%02d:%02d", tim00, tim01, tim02);
 665 
 666                 sprintf(g_d_dat_gps, "%c%c%c%c-%c%c-%c%c",
 667                         gps.dateStamp[0],
 668                         gps.dateStamp[1],
 669                         gps.dateStamp[2],
 670                         gps.dateStamp[3],
 671                         gps.dateStamp[5],
 672                         gps.dateStamp[6],
 673                         gps.dateStamp[8],
 674                         gps.dateStamp[9]);
 675 
 676                 if (((int)conf.gps_test_timezone == 1) && (test_timezone_once==0))
 677                 {
 678                     test_timezone();
 679                     test_timezone_once=1;
 680                 }
 681             }
 682 
 683         unlock(0);
 684 
 685         if ((int)conf.gps_show_symbol == 1)
 686         {
 687             if ( gps_show_lock == 0 )
 688             {                
 689                 exit_show_task= 0 ;      
 690                 gps_show_lock = 1 ;
 691                 CreateTask("GPS_SHOW", 0x19, 0x800, show_sat_symbol_task);
 692             }
 693         } 
 694         else exit_show_task=1; 
 695         
 696         msleep(900);
 697     } 
 698     exit_show_task=1; 
 699     gps_data_lock = 0 ;    
 700     ExitTask();
 701 }
 702 
 703 //=======================================================================
 704 //
 705 // TASK : updates onsceen tracking icon (globe)
 706 //
 707 static void tracking_icon_task(){
 708     
 709     char * bitmap1 = load_bitmap("A/CHDK/DATA/GPS_Track_1.txt");
 710     char * bitmap2 = load_bitmap("A/CHDK/DATA/GPS_Track_2.txt");
 711     char * bitmap3 = load_bitmap("A/CHDK/DATA/GPS_Track_3.txt");
 712 
 713     if(bitmap1 && bitmap2 && bitmap2 )
 714     {
 715         int extent1 = strlen(bitmap1);
 716         int pos1=0;
 717         int mx1=0;
 718         int my1=0;
 719         int o_x1=315;
 720         int o_y1=0;
 721         int f_v_0;
 722         int f_h_0;
 723         int f_v_1;
 724         int f_h_1;
 725         int f_v_2;
 726         int f_h_2;
 727         int f_v_3;
 728         int f_h_3;
 729         int f_v_4;
 730         int f_h_4;
 731         int f_v_5;
 732         int f_h_5;
 733         int blink=0;
 734         int data;
 735 
 736         while (exit_track_task == 0)
 737         {
 738             mx1=0;
 739             my1=0;
 740 
 741             f_v_0=COLOR_TRANSPARENT;
 742             f_h_0=COLOR_TRANSPARENT;
 743 
 744             f_v_1= COLOR_BLACK ;
 745             f_h_1= COLOR_BLACK ;
 746 
 747             f_v_2= COLOR_BLUE ;
 748             f_h_2= COLOR_BLUE ;
 749 
 750             f_v_3= COLOR_YELLOW ;
 751             f_h_3= COLOR_YELLOW ;
 752 
 753             f_v_4= COLOR_RED ;
 754             f_h_4= COLOR_RED ;
 755 
 756             f_v_5= COLOR_GREEN ;
 757             f_h_5= COLOR_GREEN ;
 758 
 759             int stat = g_d_stat ;
 760             
 761             for(pos1=0; pos1<extent1; pos1++)
 762             {
 763                 if ((blink==1) && (stat > 0))
 764                 {
 765                     data = bitmap2[pos1];
 766                 }
 767                 else
 768                 {
 769                     data = bitmap1[pos1];
 770                 }
 771                 if ((blink==1) && (navigation_mode > 0) && (stat > 0))
 772                 {
 773                     data = bitmap3[pos1];
 774                 }
 775                 if (data == 48)
 776                 {
 777                     draw_pixel(o_x1+mx1, o_y1+my1, f_v_0);
 778                 }
 779                 if (data == 49)
 780                 {
 781                     draw_pixel(o_x1+mx1, o_y1+my1, f_v_1);
 782                 }
 783                 if (data == 50)
 784                 {
 785                     draw_pixel(o_x1+mx1, o_y1+my1, f_v_2);
 786                 }
 787                 if (data == 51)
 788                 {
 789                     draw_pixel(o_x1+mx1, o_y1+my1, f_v_3);
 790                 }
 791                 if (data == 52)
 792                 {
 793                     draw_pixel(o_x1+mx1, o_y1+my1, f_v_4);
 794                 }
 795                 if (data == 53)
 796                 {
 797                     draw_pixel(o_x1+mx1, o_y1+my1, f_v_5);
 798                 }
 799                 if (data == 10 || data == 13)
 800                 {
 801                     mx1=0;
 802                     my1++;
 803                     pos1++;
 804                 }
 805                 else
 806                 {
 807                     mx1++;
 808                 }
 809             }
 810 
 811             msleep(1000);
 812             
 813             if (blink==0)
 814             {
 815                 blink=1;
 816             }
 817             else
 818             {
 819                 blink=0;
 820             }
 821         }        
 822     }
 823     if (bitmap1) free(bitmap1) ;
 824     if (bitmap2) free(bitmap2) ;
 825     if (bitmap3) free(bitmap3) ;
 826     gps_track_lock = 0 ; 
 827     ExitTask();
 828 }
 829 
 830 //=======================================================================
 831 //
 832 // TASK : started by waypoint recording code when no GPS signal and picture was taken
 833 //
 834 static void no_signal_task(){
 835    
 836     char vBuf[100];
 837     FILE *fd0;
 838     char w1[20];
 839     char w2[30];
 840     char w3[10];
 841     int o=1, p=1, q=0;
 842     int blite_off=0;
 843     unsigned long bat;
 844     int abort=0;
 845     
 846     gps_key_trap = KEY_DISPLAY ;
 847     
 848     while ( time_to_end !=0 )
 849     {
 850 
 851         // Test battery
 852         if ( (((time_to_end) % 60)) == 0 )
 853         {
 854             bat=get_batt_perc();
 855             if (bat <= (int)conf.gps_batt)
 856             {
 857                 int zba;
 858                 for(zba=30; zba>0; zba--)
 859                 {
 860                     sprintf(vBuf, lang_str(LANG_MENU_GPS_t_3));      // "Battery below setting!"
 861                     draw_txt_centered(8, vBuf, MAKE_COLOR(COLOR_YELLOW, COLOR_RED));
 862                     sprintf(vBuf, lang_str(LANG_MENU_GPS_t_4),zba); // "Camera will shutdown in %02d seconds!"
 863                     draw_txt_centered(9, vBuf, MAKE_COLOR(COLOR_RED, COLOR_BLUE));
 864                     if ( (((zba) % 2)) == 0 )
 865                     {
 866                         debug_led(0);
 867                         if ((int)conf.gps_batt_warn == 1)
 868                         {
 869                             play_sound(6);
 870                         }
 871                     }
 872                     else
 873                     {
 874                         debug_led(1);
 875                     }
 876                     msleep(1000);
 877                 }
 878                 camera_shutdown_in_a_second();
 879                 gps_nosignal_lock = 0 ;                 
 880                 ExitTask();
 881             }
 882         }
 883 
 884         // cancel automatic shut-off if DISP key pressed
 885         if (gps_key_trap == -1 )
 886         {
 887             gps_key_trap = 0;             
 888             abort = 1;
 889             time_to_end = 3600;
 890             TurnOnBackLight();
 891             gui_mbox_init(LANG_INFORMATION, LANG_MENU_GPS_t_5, MBOX_BTN_OK|MBOX_TEXT_CENTER, NULL); //Automatic shutdown cancelled!
 892         }
 893 
 894         // manage backlight and display
 895         if ( abort == 0)
 896         {
 897             // update LED blink
 898             if (q==0)
 899             {
 900                 debug_led(0);
 901                 q=1;
 902             }
 903             else
 904             {
 905                 debug_led(1);
 906                 q=0;
 907             }
 908 
 909             // Display countdown of time to finish in mm: ss
 910             int s = (int)time_to_end;
 911             int minute =  s / 60;
 912             s = s % 60;
 913             int second = s;
 914 
 915             if ( (int)conf.gps_countdown==0 )
 916             {
 917                 sprintf(vBuf, lang_str(LANG_MENU_GPS_t_6),minute, second);  // "Camera will wait for GPS for %01d:%02d"
 918             }
 919             else
 920             {
 921                 sprintf(vBuf, " %01d:%02d",minute, second);
 922             }
 923             draw_txt_centered(0, vBuf, MAKE_COLOR(COLOR_GREY_DK_TRANS, COLOR_RED));
 924 
 925             // Switch on the display when countdown <30 seconds
 926             if ((blite_off==1) && (time_to_end <=30))
 927             {
 928                 TurnOnBackLight();
 929             }
 930 
 931             // start countdown if key pressed and backlight is off
 932             if ((blite_off==0) && (kbd_get_pressed_key() !=0))     // any key pressed ?
 933             {
 934                 time_to_end=(int)conf.gps_wait_for_signal;
 935                 o=1;
 936                 p=1;
 937             }
 938 
 939             // start countdown and turn on backlight if key pressed and backlight is off
 940             if ((blite_off==1) && (kbd_get_pressed_key() !=0))     // any key pressed ?
 941             {
 942                 TurnOnBackLight();
 943                 blite_off=0;
 944                 time_to_end=(int)conf.gps_wait_for_signal; 
 945                 o=1;
 946                 p=1;
 947             }
 948 
 949             // switch to play mode
 950             if ((o == (int)conf.gps_rec_play_time) && ((int)conf.gps_rec_play_set == 1))
 951             {
 952                 if (camera_info.state.mode_rec)
 953                 {
 954                     levent_set_play();
 955                 }
 956             }
 957 
 958             // turn off backlight if timed out
 959             if ((p == (int)conf.gps_play_dark_time) && ((int)conf.gps_play_dark_set == 1))
 960             {
 961                 if (camera_info.state.mode_play)
 962                 {
 963                     TurnOffBackLight();
 964                     blite_off=1;
 965                 }
 966             }
 967         }
 968 
 969         // tag waypoint reached
 970 
 971         lock(1) ;
 972             int l_stat      = g_d_stat ;
 973             double l_lat       = g_d_lat ;
 974             double l_lon       = g_d_lon ;
 975             double l_hei       = g_d_hei ;
 976             char l_tim[10];
 977             char l_dat[12];
 978             char l_tim_gps[10];
 979             char l_dat_gps[12];   
 980             strncpy( l_tim ,      g_d_tim,    10 );         
 981             strncpy( l_dat ,      g_d_dat,    12 );
 982             strncpy( l_dat_gps ,  g_d_dat_gps,10 );
 983             strncpy( l_tim_gps ,  g_d_tim_gps,12 );
 984         unlock(1);       
 985     
 986         if ( ((((int)conf.gps_2D_3D_fix) == 1) && (l_stat == 1)) || \
 987              ((((int)conf.gps_2D_3D_fix) == 1) && (l_stat == 2)) || \
 988              ((((int)conf.gps_2D_3D_fix) == 2) && (l_stat == 2)) || \
 989              ((((int)conf.gps_2D_3D_fix) == 3) && (l_stat == 2)) || \
 990              ((((int)conf.gps_2D_3D_fix) == 3) && (l_stat == 1) && (time_to_end < 3)) || \
 991              ((((int)conf.gps_2D_3D_fix) == 0) && (time_to_end < 3)) )
 992         {
 993             TurnOnBackLight();
 994             msleep (1000);
 995 
 996             fd0 = fopen ( "A/GPS/Tagging/wp.tmp", "r" );
 997 
 998             do
 999             {
1000                 fgets (w1, 15, fd0);
1001                 fseek (fd0, 1, SEEK_CUR);
1002                 fgets (w2, 23, fd0);
1003                 fseek (fd0, 1, SEEK_CUR);
1004                 fgets (w3, 1, fd0);
1005 
1006                 char lat[40];
1007                 char lon[40];
1008                 char hei[40];
1009 
1010                 char vBuf1[512];
1011                 char image_name[20];
1012                 char wp_name[30];
1013                 char gpx_name[30];
1014                 char gpx_image_name[20];
1015                 unsigned long t;
1016                 static struct tm *ttm;
1017                 t=time(NULL);
1018                 ttm = localtime(&t);
1019 
1020                 sprintf(wp_name, "A/GPS/Tagging/%04d_%02d_%02d.wp", ttm->tm_year+1900, ttm->tm_mon+1, ttm->tm_mday);
1021                 sprintf(gpx_name, "A/GPS/Tagging/%04d_%02d_%02d.gpx", ttm->tm_year+1900, ttm->tm_mon+1, ttm->tm_mday);
1022 
1023                 sprintf(image_name, "%s", w1);
1024                 sprintf(gpx_image_name, "%s", w1);
1025 
1026                 sprintf(vBuf1, "%s;%s;%s;%s;%s;%s",
1027                         image_name,
1028                         formatDouble (lat, l_lat, 0, 7),
1029                         formatDouble (lon, l_lon, 0, 7),
1030                         formatDouble (hei, l_hei, 0, 2),
1031                         w2,
1032                         w3);
1033 
1034                 FILE* fp = fopen(wp_name, "a");
1035 
1036                 if( fp )
1037                 {
1038                     fwrite(vBuf1, strlen(vBuf1), 1, fp);
1039                 }
1040 
1041                 fclose(fp);
1042 
1043                 fp = fopen(gpx_name, "r");
1044                 if( fp == NULL)
1045                 {
1046                     fp = fopen(gpx_name, "a");
1047                     if( fp )
1048                     {
1049                         sprintf(vBuf1, "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n");
1050                         fwrite(vBuf1, strlen(vBuf1), 1, fp);
1051                         sprintf(vBuf1, "<gpx xmlns:xsi=\"http://www.w3.org/2001/XMLSchema-instance\" xmlns:xsd=\"http://www.w3.org/2001/XMLSchema\" version=\"1.1\" creator=\"CHDK\" xmlns=\"http://www.topografix.com/GPX/1/1\">\n");
1052                         fwrite(vBuf1, strlen(vBuf1), 1, fp);
1053                         sprintf(vBuf1, "</gpx>");
1054                         fwrite(vBuf1, strlen(vBuf1), 1, fp);
1055                         fclose(fp);
1056                     }
1057                 }
1058 
1059 
1060                 fp = fopen(gpx_name, "a");
1061                 if( fp )
1062                 {
1063                     fseek (fp, -6, SEEK_END);
1064 
1065                     sprintf(vBuf1, "   <wpt lat=\"%s\" lon=\"%s\">\n",formatDouble (lat, l_lat, 0, 7), formatDouble (lon, l_lon, 0, 7));
1066                     fwrite(vBuf1, strlen(vBuf1), 1, fp);
1067                     sprintf(vBuf1, "    <ele>%s</ele>\n",formatDouble (hei, l_hei, 0, 2));
1068                     fwrite(vBuf1, strlen(vBuf1), 1, fp);
1069                     sprintf(vBuf1, "    <time>%s</time>\n", w2);
1070                     fwrite(vBuf1, strlen(vBuf1), 1, fp);
1071                     sprintf(vBuf1, "    <gpst>%sT%sZ</gpst>\n", l_dat_gps, l_tim_gps);
1072                     fwrite(vBuf1, strlen(vBuf1), 1, fp);
1073                     sprintf(vBuf1, "    <temp>%i</temp>\n", (int)get_optical_temp());
1074                     fwrite(vBuf1, strlen(vBuf1), 1, fp);
1075                     sprintf(vBuf1, "    <name>%s</name>\n", gpx_image_name);
1076                     fwrite(vBuf1, strlen(vBuf1), 1, fp);
1077                     sprintf(vBuf1, "   </wpt>\n");
1078                     fwrite(vBuf1, strlen(vBuf1), 1, fp);
1079                     sprintf(vBuf1, "</gpx>");
1080                     fwrite(vBuf1, strlen(vBuf1), 1, fp);
1081                 }
1082                 fclose(fp);
1083             }
1084             while (!feof(fd0));
1085 
1086             fclose (fd0);
1087             if ( abort == 0)
1088             {
1089                 time_to_end=1;
1090             }
1091 
1092         }
1093 
1094         msleep(1000);
1095         o++;
1096         p++;
1097 
1098 
1099         // gps functionality disabled ?
1100         if ((int)conf.gps_on_off ==0)
1101         {
1102             debug_led(0);
1103             gps_nosignal_lock = 0;             
1104             ExitTask();
1105         }
1106 
1107         time_to_end--;
1108     }
1109 
1110     int zba, zba1;
1111 
1112     if (abort == 0)
1113     {
1114         for(zba=15; zba>0; zba--)           // outer loop = 30 seconds
1115         {
1116             sprintf(vBuf, lang_str(LANG_MENU_GPS_t_4), zba);     // "Camera will shutdown in %02d seconds!"
1117             draw_txt_centered(2, vBuf, MAKE_COLOR(COLOR_WHITE, COLOR_BLUE));
1118             sprintf(vBuf, lang_str(LANG_MENU_GPS_t_18));         //"To cancel [Press half]"
1119             draw_txt_centered(3, vBuf, MAKE_COLOR(COLOR_WHITE, COLOR_RED));
1120             play_sound(6);
1121 
1122             for(zba1=100; zba1>0; zba1--)   // inner loop = 2 seconds
1123             {
1124                 if (kbd_get_pressed_key() == KEY_SHOOT_HALF)
1125                     {
1126                         TurnOnBackLight();
1127                         debug_led(0);
1128                         gps_nosignal_lock = 0 ; 
1129                         ExitTask();
1130                     }
1131                 msleep(2);
1132             }
1133         }        
1134         camera_shutdown_in_a_second();
1135     }
1136 
1137     TurnOnBackLight();
1138 
1139     // beep 5 seconds
1140     if ((int)conf.gps_beep_warn == 1)
1141     {
1142         for(zba=5; zba>0; zba--) play_sound(6);
1143     }
1144     
1145     debug_led(0);
1146     gps_nosignal_lock = 0 ; 
1147     ExitTask();
1148 
1149 }
1150 
1151 
1152 //=======================================================================
1153 //
1154 // TASK : started by init_gps_logging_task() to record logging data
1155 //
1156 static void gps_logging_task(){
1157       
1158     char lat[40];
1159     char lon[40];
1160     char hei[40];
1161     char vBuf[512];
1162     int r=0;
1163     unsigned long t;
1164     char gpx_name[30];
1165     static struct tm *ttm;
1166     t=time(NULL);
1167     ttm = localtime(&t);
1168 
1169     mkdir_if_not_exist("A/GPS");
1170     mkdir_if_not_exist("A/GPS/Logging");
1171     sprintf(gpx_name, "A/GPS/Logging/%02d_%02d-%02d_%02d.gpx", ttm->tm_mon+1, ttm->tm_mday, ttm->tm_hour, ttm->tm_min);
1172 
1173     FILE* fp = fopen(gpx_name, "w");
1174     if( fp )
1175     {
1176         sprintf(vBuf, "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n");
1177         fwrite(vBuf, strlen(vBuf), 1, fp);
1178         sprintf(vBuf, "<gpx xmlns:xsi=\"http://www.w3.org/2001/XMLSchema-instance\" xmlns:xsd=\"http://www.w3.org/2001/XMLSchema\" version=\"1.1\" creator=\"CHDK\" xmlns=\"http://www.topografix.com/GPX/1/1\">\n");
1179         fwrite(vBuf, strlen(vBuf), 1, fp);
1180         sprintf(vBuf, " <metadata />\n");
1181         fwrite(vBuf, strlen(vBuf), 1, fp);
1182         sprintf(vBuf, " <trk>\n");
1183         fwrite(vBuf, strlen(vBuf), 1, fp);
1184         sprintf(vBuf, "  <name />\n");
1185         fwrite(vBuf, strlen(vBuf), 1, fp);
1186         sprintf(vBuf, "  <cmt />\n");
1187         fwrite(vBuf, strlen(vBuf), 1, fp);
1188         sprintf(vBuf, "  <trkseg>\n");
1189         fwrite(vBuf, strlen(vBuf), 1, fp);
1190         sprintf(vBuf, "  </trkseg>\n");
1191         fwrite(vBuf, strlen(vBuf), 1, fp);
1192         sprintf(vBuf, " </trk>\n");
1193         fwrite(vBuf, strlen(vBuf), 1, fp);
1194         sprintf(vBuf, "</gpx>\n");
1195         fwrite(vBuf, strlen(vBuf), 1, fp);
1196     }
1197     fclose(fp);
1198     int zae=1, zae_1=0, bat;
1199     
1200     while ( exit_record_task==0 )
1201     {
1202         if (kbd_get_pressed_key() !=0)   // any key pressed ?
1203         {
1204             TurnOnBackLight();
1205             zae_1=0;
1206         }
1207 
1208         zae_1++;
1209         if ((zae_1 == (int)conf.gps_rec_play_time_1) && ((int)conf.gps_rec_play_set_1 == 1))
1210         {
1211             if (camera_info.state.mode_rec)
1212             {
1213                 levent_set_play();
1214             }
1215         }
1216         if ((zae_1 == (int)conf.gps_play_dark_time_1) && ((int)conf.gps_play_dark_set_1 == 1))
1217         {
1218             if (camera_info.state.mode_play)
1219             {
1220                 TurnOffBackLight();
1221             }
1222         }
1223         if ( (((zae_1) % 2)) == 0 )
1224         {
1225             debug_led(0);
1226         }
1227         else
1228         {
1229             debug_led(1);
1230         }
1231         if ( (((zae * (int)conf.gps_track_time) % 60)) == 0 )
1232         {
1233             bat=get_batt_perc();
1234             if (bat <= (int)conf.gps_batt)
1235             {
1236                 int zba;
1237                 for(zba=30; zba>0; zba--)
1238                 {
1239                     sprintf(vBuf, lang_str(LANG_MENU_GPS_t_3));  // "Battery below setting!"
1240                     draw_txt_centered(8, vBuf, MAKE_COLOR(COLOR_GREY_DK_TRANS, COLOR_RED));
1241                     sprintf(vBuf, lang_str(LANG_MENU_GPS_t_4),zba);
1242                     draw_txt_centered(9, vBuf, MAKE_COLOR(COLOR_GREY_DK_TRANS, COLOR_RED));
1243                     if ( (((zba) % 2)) == 0 )
1244                     {
1245                         debug_led(0);
1246                         play_sound(6);
1247                     }
1248                     else
1249                     {
1250                         debug_led(1);
1251                     }
1252                     msleep(1000);
1253                 }
1254                 camera_shutdown_in_a_second();
1255                 gps_record_lock = 0 ; 
1256                 ExitTask();
1257             }
1258         }
1259         
1260         lock(2) ;
1261             int l_stat   = g_d_stat ;
1262             double l_lat = g_d_lat ;
1263             double l_lon = g_d_lon ;
1264             double l_hei = g_d_hei ;
1265             char l_tim[10];
1266             char l_dat[12];  
1267             strncpy( l_tim, g_d_tim, 10 );         
1268             strncpy( l_dat, g_d_dat, 12 );          
1269         unlock(2) ;
1270         
1271         if (l_stat == 1 || l_stat == 2)
1272         {
1273             fp = fopen(gpx_name, "a");
1274             if( fp )
1275             {
1276                 fseek (fp, -27, SEEK_END);
1277                 sprintf(vBuf, "   <trkpt lat=\"%s\" lon=\"%s\">\n",formatDouble (lat, l_lat, 0, 7), formatDouble (lon, l_lon, 0, 7));
1278                 fwrite(vBuf, strlen(vBuf), 1, fp);
1279                 sprintf(vBuf, "    <ele>%s</ele>\n",formatDouble (hei, l_hei, 0, 2));
1280                 fwrite(vBuf, strlen(vBuf), 1, fp);
1281                 sprintf(vBuf, "    <time>%sT%sZ</time>\n", l_dat, l_tim);
1282                 fwrite(vBuf, strlen(vBuf), 1, fp);
1283                 sprintf(vBuf, "   </trkpt>\n");
1284                 fwrite(vBuf, strlen(vBuf), 1, fp);
1285                 sprintf(vBuf, "  </trkseg>\n");
1286                 fwrite(vBuf, strlen(vBuf), 1, fp);
1287                 sprintf(vBuf, " </trk>\n");
1288                 fwrite(vBuf, strlen(vBuf), 1, fp);
1289                 sprintf(vBuf, "</gpx>\n");
1290                 fwrite(vBuf, strlen(vBuf), 1, fp);
1291             }
1292             fclose(fp);
1293         }
1294         msleep((int)conf.gps_track_time*1000);
1295         zae++;
1296     }
1297     debug_led(0);
1298     gps_record_lock = 0 ; 
1299     ExitTask();
1300 
1301 }
1302 
1303 //=======================================================================
1304 //
1305 // TASK : compass GUI display
1306 //
1307 static void compass_display_task(){
1308 
1309     #define BITMAP_WIDTH  61
1310     #define BITMAP_HEIGHT 61
1311 
1312     georegInit (&georeg, (int)conf.gps_compass_smooth, buffer1, buffer2);
1313     regressionInit (&deltareg, 20, buffer3);
1314 
1315     int stat ;
1316     double angle=0;
1317     double w=0.0;
1318     double c_w;
1319     double s_w;
1320     int n=0;
1321     int count=0;
1322     int m=0;
1323     int old_angle=0;
1324 
1325     int offset_x = 32;
1326     int m_x = offset_x + 31;
1327     int offset_y = 150;     //128
1328     int m_y = offset_y + 31;
1329 
1330     int f_v_0=COLOR_TRANSPARENT;
1331     int f_h_0=COLOR_TRANSPARENT;
1332 
1333     int f_v_1=COLOR_BLUE;
1334     int f_h_1=COLOR_BLUE;
1335 
1336     int f_v_2= COLOR_WHITE ;
1337     int f_h_2= COLOR_WHITE ;
1338 
1339     int f_v_3= COLOR_BLACK ;
1340     int f_h_3= COLOR_BLACK ;
1341 
1342     int f_v_4= COLOR_BLACK ;
1343     int f_h_4= COLOR_BLACK ;
1344 
1345     double old_c_w=cos(0);
1346     double old_s_w=sin(0);
1347 
1348     char *bitmap1 = load_bitmap("A/CHDK/DATA/GPS_needle.txt");
1349     char *bitmap2 = load_bitmap("A/CHDK/DATA/GPS_compass.txt");
1350     int extent = strlen(bitmap1);
1351 
1352     while (exit_compass_task == 0) 
1353     {
1354         lock(3);
1355             angle= draw_gps_course_info(count);
1356             stat = g_d_stat ;
1357         unlock(3);
1358         
1359         count++;
1360         m=n;
1361         
1362         if ((int)angle != 0) { m=angle; }
1363 
1364         switch ( stat )
1365         {
1366             case 0 : 
1367                 f_v_1= COLOR_RED ;
1368                 f_h_1= COLOR_RED ;
1369                 break ;
1370             case 1 :
1371                 f_v_1= COLOR_YELLOW ;
1372                 f_h_1= COLOR_YELLOW ;
1373                 break ;
1374             case 2 :
1375                 f_v_1= COLOR_GREEN ;
1376                 f_h_1= COLOR_GREEN ;
1377                 break ;
1378         }
1379         
1380         if (camera_info.state.gui_mode_none)
1381         {
1382             draw_compass (bitmap2, offset_x - 27, offset_y -14, f_v_0, f_h_0, f_v_1, f_h_1, f_v_2, f_h_2, f_v_4, f_h_4);
1383         }
1384         if (stat == 2 )
1385         {
1386             f_v_1= COLOR_BLUE ;
1387             f_h_1= COLOR_BLUE ;
1388         }
1389         if (m>=0 && m<180)
1390         {
1391             w=(180-m)*3.141592653589793/180;
1392         }
1393         if (m >=180 && m<=360)
1394         {
1395             w=(540-m)*3.141592653589793/180;
1396         }
1397 
1398         c_w=cos(w);
1399         s_w=sin(w);
1400 
1401         if (camera_info.state.gui_mode_none)
1402         {
1403             draw_compass_needle (old_angle, old_s_w, old_c_w, bitmap1, extent, m_x, m_y, offset_x, offset_y, f_v_0, f_h_0, f_v_0, f_h_0);
1404             draw_compass_needle (m, s_w, c_w, bitmap1, extent, m_x, m_y, offset_x, offset_y, f_v_1, f_h_1, f_v_3, f_h_3);
1405         }
1406         old_angle=m;
1407         old_c_w=cos(w);
1408         old_s_w=sin(w);
1409         msleep((int)conf.gps_compass_time*1000);
1410     }
1411     if (bitmap1) free(bitmap1) ;
1412     if (bitmap2) free(bitmap2) ;
1413     gps_compass_lock = 0 ;     
1414     ExitTask();
1415 }
1416 
1417 
1418 //=======================================================================
1419 //
1420 //  Functions called from outside this module :
1421 //      - GUI menu support functions
1422 //      - background tasks
1423 //
1424 
1425 // gps_waypoint()
1426 //  - called from spytask after each shot (if gps_waypoint_save is set)
1427 //  - logs current gpx data if available, otherwise starts gpx_no_signal task to wait for data
1428 void gps_waypoint(){     
1429     
1430     char lat[40];
1431     char lon[40];
1432     char hei[40];
1433 
1434     char vBuf[512];
1435     char image_name[20];
1436     char wp_name[30];
1437     char gpx_name[30];
1438     char gpx_image_name[20];
1439 
1440     unsigned long t;
1441     static struct tm *ttm;
1442     t=time(NULL);
1443     ttm = localtime(&t);
1444 
1445     mkdir_if_not_exist("A/GPS");
1446     mkdir_if_not_exist("A/GPS/Tagging");
1447     sprintf(wp_name, "A/GPS/Tagging/%04d_%02d_%02d.wp", ttm->tm_year+1900, ttm->tm_mon+1, ttm->tm_mday);
1448     sprintf(gpx_name, "A/GPS/Tagging/%04d_%02d_%02d.gpx", ttm->tm_year+1900, ttm->tm_mon+1, ttm->tm_mday);
1449     sprintf(image_name, "IMG_%04d.JPG", get_target_file_num());
1450 
1451     lock(4);
1452         int l_stat         = g_d_stat ;
1453         double l_lat       = g_d_lat ;
1454         double l_lon       = g_d_lon ;
1455         double l_hei       = g_d_hei ;
1456         char l_tim[10];
1457         char l_dat[12];
1458         char l_tim_gps[10];
1459         char l_dat_gps[12];   
1460         strncpy( l_tim ,      g_d_tim,     10 );         
1461         strncpy( l_dat ,      g_d_dat,     12 );
1462         strncpy( l_dat_gps ,  g_d_dat_gps, 10 );
1463         strncpy( l_tim_gps ,  g_d_tim_gps, 12 );        
1464     unlock(4);
1465         
1466     if ((l_stat >= ((int)conf.gps_2D_3D_fix)) || ((((int)conf.gps_2D_3D_fix) == 3) && (l_stat == 2)))
1467     {
1468         sprintf(vBuf, "%s;%s;%s;%s;%sT%sZ;%i\n",
1469                 image_name,
1470                 formatDouble (lat, l_lat, 0, 7),
1471                 formatDouble (lon, l_lon, 0, 7),
1472                 formatDouble (hei, l_hei, 0, 2),
1473                 l_dat, l_tim,
1474                 get_optical_temp());
1475 
1476         FILE* fp = fopen(wp_name, "a");
1477 
1478         if( fp )
1479         {
1480             fwrite(vBuf, strlen(vBuf), 1, fp);
1481         }
1482         fclose(fp);
1483 
1484         fp = fopen(gpx_name, "r");
1485         if( fp == NULL)
1486         {
1487             fp = fopen(gpx_name, "a");
1488             if( fp )
1489             {
1490                 sprintf(vBuf, "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n");
1491                 fwrite(vBuf, strlen(vBuf), 1, fp);
1492                 sprintf(vBuf, "<gpx xmlns:xsi=\"http://www.w3.org/2001/XMLSchema-instance\" xmlns:xsd=\"http://www.w3.org/2001/XMLSchema\" version=\"1.1\" creator=\"CHDK\" xmlns=\"http://www.topografix.com/GPX/1/1\">\n");
1493                 fwrite(vBuf, strlen(vBuf), 1, fp);
1494                 sprintf(vBuf, "</gpx>");
1495                 fwrite(vBuf, strlen(vBuf), 1, fp);
1496                 fclose(fp);
1497             }
1498         }
1499 
1500 
1501         sprintf(gpx_image_name, "IMG_%04d.JPG", get_target_file_num());
1502 
1503         fp = fopen(gpx_name, "a");
1504         if( fp )
1505         {
1506             fseek (fp, -6, SEEK_END);
1507 
1508             sprintf(vBuf, "   <wpt lat=\"%s\" lon=\"%s\">\n",formatDouble (lat, l_lat, 0, 7), formatDouble (lon, l_lon, 0, 7));
1509             fwrite(vBuf, strlen(vBuf), 1, fp);
1510             sprintf(vBuf, "    <ele>%s</ele>\n",formatDouble (hei, l_hei, 0, 2));
1511             fwrite(vBuf, strlen(vBuf), 1, fp);
1512             sprintf(vBuf, "    <time>%sT%sZ</time>\n", l_dat, l_tim);
1513             fwrite(vBuf, strlen(vBuf), 1, fp);
1514             sprintf(vBuf, "    <gpst>%sT%sZ</gpst>\n", l_dat_gps, l_tim_gps);
1515             fwrite(vBuf, strlen(vBuf), 1, fp);
1516             sprintf(vBuf, "    <temp>%i</temp>\n", (int)get_optical_temp());
1517             fwrite(vBuf, strlen(vBuf), 1, fp);
1518             sprintf(vBuf, "    <name>%s</name>\n", gpx_image_name);
1519             fwrite(vBuf, strlen(vBuf), 1, fp);
1520             sprintf(vBuf, "   </wpt>\n");
1521             fwrite(vBuf, strlen(vBuf), 1, fp);
1522             sprintf(vBuf, "</gpx>");
1523             fwrite(vBuf, strlen(vBuf), 1, fp);
1524         }
1525         fclose(fp);
1526     }
1527     else
1528     {
1529          FILE* fp ;
1530          
1531         sprintf(vBuf, "%s %sT%sZ %i\n",
1532                 image_name,
1533                 l_dat, l_tim,
1534                 get_optical_temp());
1535           
1536         if ( gps_nosignal_lock == 0 )
1537         {              
1538             gps_nosignal_lock = 1 ;            
1539             CreateTask("GPS_NOSIGNAL", 0x19, 0x1200, no_signal_task);
1540             fp = fopen("A/GPS/Tagging/wp.tmp", "w");
1541         }
1542         else
1543         {
1544             fp = fopen("A/GPS/Tagging/wp.tmp", "a");
1545         }
1546         
1547 
1548 
1549         if( fp )
1550         {
1551             fwrite(vBuf, strlen(vBuf), 1, fp);
1552             fclose(fp);
1553         }
1554         
1555         time_to_end=(int)conf.gps_wait_for_signal;
1556     }
1557         
1558 }
1559 
1560 
1561 
1562 void gps_write_timezone(){          // called from gui.c when "Set position as current timezone" is selected in the UI
1563      
1564     char vBuf[30];
1565     char timezone_name[30];
1566 
1567     lock(5);                      // note : kbd task lock = 5
1568         int l_stat   = g_d_stat ;
1569         double l_lat = g_d_lat ;
1570         double l_lon = g_d_lon ;
1571     unlock(5);
1572     
1573     if (l_stat !=0)
1574     {
1575         mkdir_if_not_exist("A/GPS");
1576         mkdir_if_not_exist("A/GPS/Timezone");
1577         sprintf(timezone_name, "A/GPS/Timezone/Timezone.txt");
1578         
1579         FILE* fp = fopen(timezone_name, "w");
1580         if( fp )
1581         {
1582             sprintf(vBuf, "%i\n", (long)(l_lat*10000000));
1583             fwrite(vBuf, strlen(vBuf), 1, fp);
1584             sprintf(vBuf, "%i\n", (long)(l_lon*10000000));
1585             fwrite(vBuf, strlen(vBuf), 1, fp);
1586         }
1587         fclose(fp);
1588     }
1589     else
1590     {
1591         gui_mbox_init(LANG_ERROR, LANG_MENU_GPS_t_1, MBOX_BTN_OK|MBOX_TEXT_CENTER, NULL); //No GPS!
1592     }
1593 }
1594 
1595 void gps_write_home(){             // called from gui.c when "Set position as home location" is selected in the UI
1596     
1597     char vBuf[30];
1598     char home_name[30];
1599 
1600     lock(5);                       // note : kbd task lock = 5
1601         int l_stat = g_d_stat ;
1602         double l_lat = g_d_lat ;
1603         double l_lon = g_d_lon ;
1604     unlock(5);
1605         
1606 
1607     if (l_stat !=0)
1608     {
1609         mkdir_if_not_exist("A/GPS");
1610         mkdir_if_not_exist("A/GPS/Navigation");
1611         sprintf(home_name, "A/GPS/Navigation/Home.txt");
1612 
1613         FILE* fp = fopen(home_name, "w");
1614         if( fp )
1615         {
1616             sprintf(vBuf, "%i\n", (long)(l_lat*10000000));
1617             fwrite(vBuf, strlen(vBuf), 1, fp);
1618             sprintf(vBuf, "%i\n", (long)(l_lon*10000000));
1619             fwrite(vBuf, strlen(vBuf), 1, fp);
1620         }
1621         fclose(fp);
1622     }
1623     else
1624     {
1625         gui_mbox_init(LANG_ERROR, LANG_MENU_GPS_t_1, MBOX_BTN_OK|MBOX_TEXT_CENTER, NULL); //No GPS!
1626     }
1627 }
1628 
1629 void init_gps_startup(int stop_request)                       // called from spytask
1630 {
1631     if ( stop_request == 0 )
1632     {
1633          if ( gps_data_lock == 0) 
1634         {
1635             exit_data_task = 0;             
1636             gps_data_lock = 1 ;            
1637             CreateTask("GPS_DATA", 0x19, 0x800, gps_data_task);
1638         }
1639     }
1640     else 
1641     {
1642         if ( exit_data_task != 1)
1643         {
1644             exit_data_task =1;        
1645             exit_show_task=1;
1646             exit_record_task=1;
1647             exit_track_task=1;
1648             exit_compass_task=1;
1649             exit_nosignal_task=1;        
1650         }
1651     }
1652 }
1653 
1654 void init_gps_logging_task(int stop_request){               // called from gui.c to start the gpx logging
1655     exit_track_task = exit_record_task = stop_request ;
1656     
1657     if ( stop_request == 0 ) 
1658     {
1659         if (gps_record_lock == 0)
1660         {
1661             gps_record_lock  = 1 ;                
1662             CreateTask("GPS_RECORD", 0x19, 0x800, gps_logging_task);
1663         }
1664 
1665         if (((int)conf.gps_track_symbol==1) && (gps_track_lock == 0))
1666         {
1667             gps_track_lock = 1 ;                
1668             CreateTask("GPS_TRACK", 0x19, 0x800, tracking_icon_task);
1669 
1670         }
1671     } ; 
1672 }
1673 
1674 void init_gps_compass_task(int stop_request){               // called from gui.c to start the GUI compass display
1675         
1676     exit_compass_task = stop_request ; 
1677     if ( stop_request == 0 )
1678     {     
1679         run_compass_task(0);
1680     }   
1681 }
1682 
1683 
1684 int init_gps_navigate_to_home(int stop_request){            // called from gui.c when navigate home selected from GUI
1685 
1686     exit_compass_task = stop_request ;
1687     if ( stop_request == 0 )
1688     {      
1689         char text[2][30];
1690         char home_name[30];
1691         char * endptr;
1692 
1693         sprintf(home_name, "A/GPS/Navigation/Home.txt");
1694 
1695         FILE* fp = fopen(home_name, "r");
1696         if( fp )
1697         {
1698             fgets(text[1], 15, fp);
1699             fgets(text[2], 15, fp);
1700         }
1701         fclose(fp);
1702 
1703         g_d_lat_nav = (strtol (text[1], &endptr, 10 )) / 10000000.0;
1704         g_d_lon_nav = (strtol (text[2], &endptr, 10 )) / 10000000.0;
1705 
1706         if ((int)g_d_lat_nav != 0)
1707         {        
1708             run_compass_task(2);          
1709             return(1);
1710         }
1711         else
1712         {
1713             gui_mbox_init(LANG_ERROR, LANG_MENU_GPS_t_7, MBOX_BTN_OK|MBOX_TEXT_CENTER, NULL); //Navigation to Home Loc is not possible!
1714             navigation_mode=0;
1715         }
1716     }
1717     return(0);
1718 }
1719 
1720 int init_gps_navigate_to_photo(int stop_request){                  // called from gui.c when show navi selected by GUI
1721 
1722     exit_compass_task = stop_request ; 
1723 
1724     if (stop_request == 0)
1725     {      
1726         gps_img_data *igps = ( gps_img_data *) camera_jpeg_current_gps();
1727 
1728         g_d_lat_nav=0.0;
1729         g_d_lon_nav=0.0;
1730 
1731         char image[9];
1732         strncpy(image, camera_jpeg_current_filename(),8) ;
1733         image[8] = '\0';
1734 
1735         if (igps->latitudeRef[0] && igps->longitudeRef[0])
1736         {
1737             g_d_lat_nav=(double)igps->latitude[0]/(double)igps->latitude[1]*1.0 +
1738                                  igps->latitude[2]/(igps->latitude[3]*60.0) +
1739                                  igps->latitude[4]/(igps->latitude[5]*3600.0);
1740             if (igps->latitudeRef[0] == 'S') g_d_lat_nav = -g_d_lat_nav;
1741             g_d_lon_nav=(double)igps->longitude[0]/(double)igps->longitude[1]*1.0 +
1742                                  igps->longitude[2]/(igps->longitude[3]*60.0) +
1743                                  igps->longitude[4]/(igps->longitude[5]*3600.0);
1744             if (igps->longitudeRef[0] == 'W') g_d_lon_nav = -g_d_lon_nav;
1745              
1746             run_compass_task(1);
1747             return(1);
1748         }
1749         else
1750         {
1751             char vBuf[256];
1752             sprintf(vBuf, lang_str(LANG_MENU_GPS_t_8), image);  //"Cant navigate to photo: %s!"
1753             gui_mbox_init(LANG_ERROR, (int)vBuf, MBOX_BTN_OK|MBOX_TEXT_CENTER, NULL); //Cant navigate to photo: %s!
1754             navigation_mode=0;
1755         }
1756     }   
1757     return(0) ;
1758 }
1759 
1760 
1761 /****  eof  ****/

/* [<][>][^][v][top][bottom][index][help] */