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 #if 0
 111 static void
 112 polar2rect (t_rectangular* r, t_polar* p) {
 113     r->east = sin(p->alpha) * p->delta;
 114     r->north= cos(p->alpha) * p->delta;
 115 }
 116 #endif
 117 
 118 static int
 119 radian2deg (double alpha) {
 120     return (int) (alpha * RHO + 360.5) % 360;
 121 }
 122 
 123 typedef struct {
 124     t_regression lat_w;
 125     t_regression lon_w;
 126 } t_georeg;
 127 
 128 static void
 129 georegInit (t_georeg* reg, int size, t_regression_xy * buffer1, t_regression_xy * buffer2) {
 130     regressionInit (&reg->lat_w, size, buffer1);
 131     regressionInit (&reg->lon_w, size, buffer2);
 132 }
 133 
 134 static void
 135 georegAdd (t_georeg* reg, double time, t_geo* geo) {
 136     regressionAdd (&reg->lat_w, time, geo->lat_w);
 137     regressionAdd (&reg->lon_w, time, geo->lon_w);
 138 }
 139 
 140 static void
 141 georegActual (t_georeg* reg, t_geo* geo) {
 142     geo->lat_w = regressionActual (&reg->lat_w);
 143     geo->lon_w = regressionActual (&reg->lon_w);
 144 }
 145 
 146 static void
 147 georegChange (t_georeg* reg, t_geo* geo) {
 148     geo->lat_w = regressionChange (&reg->lat_w);
 149     geo->lon_w = regressionChange (&reg->lon_w);
 150 }
 151 
 152 static double now;                                     // seconds of day
 153 static t_geo gps;                                      // measured noisy position
 154 static t_georeg georeg;                                // regression for actual position
 155 static t_geo ist;                                      // actual position of regression
 156 static t_geo speed;                                    // actual position of regression in Grad/s
 157 static t_rectangular mspeed;                           // actual position of regressionin m/s (N/E)
 158 static t_polar pspeed;                                 // actual position of regression in m/s + angle
 159 static int direction;                                  // direction
 160 static t_geo soll;                                     // nominal position
 161 static t_geo delta;                                    // distance to destination(in Grad)
 162 static t_rectangular rdelta;                           // distance to destination (metric N/E)
 163 static t_polar pdelta;                                 // distance to destination (metric+english)
 164 static int peil;                                       // bearing to destination
 165 static int angle;                                      // angle on display from march to the target
 166 static t_regression deltareg;                          // heading and speed
 167 
 168 #ifdef SIMULATED_GPS
 169     #include "gps_simulate.c"  
 170 #endif
 171 
 172 //=======================================================================
 173 //
 174 //  Bakery algoritmn mutex lock / unlock
 175 //
 176 #define NUM_TASKS 6
 177 static volatile unsigned entering[NUM_TASKS] = { 0 };
 178 static volatile unsigned number[NUM_TASKS] = { 0 };
 179 
 180 void lock(int n_task) {
 181   int i ;
 182   unsigned max = 0;    
 183   entering[n_task] = 1;
 184   for (i = 0; i < NUM_TASKS; i++) {
 185     if (number[i] > max) max = number[i];
 186   }
 187   number[n_task] = max + 1 ;
 188   entering[n_task] = 0;
 189   for (i = 0; i < NUM_TASKS; i++) {
 190     if( i != n_task) {
 191         while ( entering[i] !=0 ) { msleep(50) ; }
 192         while ((number[i] != 0) && ( number[i] < number[n_task] || (number[i] == number[n_task] && i < n_task) )) { msleep(50);}
 193     }
 194   }
 195 }
 196  
 197 void unlock(int n_task)
 198 {
 199   number[n_task] = 0;
 200 }
 201  
 202 //=======================================================================
 203 //
 204 //  Helper functions - called from within running tasks
 205 //
 206 
 207 void draw_txt_centered(int line, char *buf, int color)
 208 {
 209     draw_txt_string( (camera_screen.width/FONT_WIDTH-strlen(buf))>>1 , line, buf, color) ;
 210 }
 211 
 212 static void compass_display_task();
 213 static void run_compass_task(int mode){  
 214     
 215     navigation_mode = mode ;
 216     
 217     if (gps_compass_lock == 0 )
 218     {
 219         gps_compass_lock = 1 ;   
 220         CreateTask("GPS_COMPASS", 0x19, 0x1000, compass_display_task);   
 221     }
 222 }
 223 
 224 static void test_timezone(){                           // creates timezone.txt file with current timezone info
 225   
 226     char text[2][30];
 227     char home_name[30];
 228     int timezone_1, timezone_2;
 229     char * endptr;
 230 
 231     sprintf(home_name, "A/GPS/Timezone/Timezone.txt");
 232 
 233     FILE* fp = fopen(home_name, "r");
 234     if( fp )
 235     {
 236         fgets(text[1], 15, fp);
 237         fgets(text[2], 15, fp);
 238         fclose(fp);
 239 
 240         g_d_lat_nav = (strtol (text[1], &endptr, 10 )) / 10000000.0;
 241         g_d_lon_nav = (strtol (text[2], &endptr, 10 )) / 10000000.0;
 242 
 243         timezone_1 = (int)((g_d_lon_nav+7.5)/15);
 244         timezone_2 = (int)((g_d_lon+7.5)/15);
 245 
 246         if (timezone_1 != timezone_2)
 247         {
 248             gui_mbox_init(LANG_INFORMATION, LANG_MENU_GPS_t_2, MBOX_BTN_OK|MBOX_TEXT_CENTER, NULL); //Timezone has changed!
 249         }
 250     }
 251 }
 252 
 253 static char *load_bitmap(char *datei){
 254     
 255     FILE *fd;
 256     char *bitmap;
 257     int bitmap_size;
 258     size_t result;
 259 
 260     fd = fopen ( datei, "r" );
 261 
 262     fseek (fd, 0, SEEK_END);
 263     bitmap_size = ftell (fd);
 264     fseek (fd, 0, SEEK_SET);
 265 
 266     bitmap = (char*) malloc (sizeof(char)*bitmap_size);
 267     result = fread (bitmap,1,bitmap_size,fd);
 268     fclose (fd);
 269 
 270     return bitmap;
 271 }
 272 
 273 static double draw_gps_course_info(int count){             // calculate & display course information
 274 
 275     char vBuf[512];
 276     int angle1=0;
 277 
 278     soll.lat_w = g_d_lat_nav;
 279     soll.lon_w = g_d_lon_nav;
 280 
 281     now = (double) (count);
 282     gps.lat_w = g_d_lat;                                                    // read GPS (with noise)
 283     gps.lon_w = g_d_lon;
 284     georegAdd               (&georeg, now, &gps);                           // feed the regression
 285     georegActual            (&georeg, &ist);                                // read averaged position
 286     georegChange            (&georeg, &speed);                              // read averaged velocity
 287     geo2rect                (&speed, &mspeed, ist.lat_w);                   // Convert to m/s
 288     rect2polar              (&mspeed, &pspeed);                             // convert to direction and speed.
 289     direction = radian2deg  (pspeed.alpha);                                 // get direction
 290     geodiff                 (&ist, &soll, &delta);                          // Distance in degrees (N/E)
 291     geo2rect                (&delta, &rdelta, ist.lat_w);                   // Distance in m (je N/E)
 292     rect2polar              (&rdelta, &pdelta);                             // Distance in degrees m and grad
 293     peil    = radian2deg    (pdelta.alpha);                                 // Bearing to destination
 294     angle  = radian2deg     (pdelta.alpha-pspeed.alpha);                    // Angle relative to the direction of march
 295     regressionAdd           (&deltareg, now, pdelta.delta);                 // do the regression
 296     double eta = regressionReverse (&deltareg, 0);                          // estimated time of arrival
 297     double rest= eta - now;                                                 // Time = arrival time - now
 298 
 299     if (abs(regressionChange (&deltareg))<0.5 || rest < 5.0) rest = 0.0;
 300 
 301     if (camera_info.state.gui_mode_none)
 302     {
 303         angle1=(int)angle;
 304         char anz1[40];
 305         char anz2[40];
 306         char image[9];        
 307 
 308         if (navigation_mode > 0)
 309         {
 310             sprintf(vBuf, lang_str(LANG_MENU_GPS_t_9), (int)pdelta.delta);
 311             draw_txt_string(16, 9, vBuf, MAKE_COLOR(COLOR_GREY_DK_TRANS, COLOR_WHITE));
 312 
 313             int s = (int)rest;
 314             int hour = s / 3600;
 315             s = s % 3600;
 316             int minute =  s / 60;
 317             s = s % 60;
 318             int second = s;
 319 
 320             sprintf(vBuf, lang_str(LANG_MENU_GPS_t_10), hour, minute, second);
 321             draw_txt_string(16, 10, vBuf, MAKE_COLOR(COLOR_GREY_DK_TRANS, COLOR_WHITE));
 322 
 323             sprintf(vBuf, lang_str(LANG_MENU_GPS_t_11), formatDouble (anz1, (pspeed.delta * 3.6), 0, 1));
 324             draw_txt_string(16, 11, vBuf, MAKE_COLOR(COLOR_GREY_DK_TRANS, COLOR_WHITE));
 325             sprintf(vBuf, lang_str(LANG_MENU_GPS_t_12), direction);
 326             draw_txt_string(16, 12, vBuf, MAKE_COLOR(COLOR_GREY_DK_TRANS, COLOR_WHITE));
 327             sprintf(vBuf, lang_str(LANG_MENU_GPS_t_13), (int)angle);
 328             draw_txt_string(16, 13, vBuf, MAKE_COLOR(COLOR_GREY_DK_TRANS, COLOR_WHITE));
 329 
 330             if (navigation_mode==1)
 331             {
 332                 sprintf(image, "%s", camera_jpeg_current_filename());
 333                 image[8] = '\0';
 334                 sprintf(vBuf, lang_str(LANG_MENU_GPS_t_14), image);  // "Navigation to photo: %s started"
 335                 draw_txt_centered(1, vBuf, MAKE_COLOR(COLOR_GREY_DK_TRANS, COLOR_RED));
 336             }
 337             
 338             if (navigation_mode==2)
 339             {
 340                 sprintf(vBuf, lang_str(LANG_MENU_GPS_t_17));         // "Navigation to Home started"
 341                 draw_txt_centered(1, vBuf, MAKE_COLOR(COLOR_GREY_DK_TRANS, COLOR_RED));
 342             }
 343 
 344             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 "
 345             draw_txt_centered(2, vBuf, MAKE_COLOR(COLOR_GREY_DK_TRANS, COLOR_RED));
 346         }
 347         else
 348         {
 349             angle1= direction;
 350             sprintf(vBuf, lang_str(LANG_MENU_GPS_t_16), (int)angle1);   // "heading = %i°"
 351             draw_txt_string(16, 10, vBuf, MAKE_COLOR(COLOR_GREY_DK_TRANS, COLOR_WHITE));            
 352             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 "
 353             draw_txt_string(16, 11, vBuf, MAKE_COLOR(COLOR_GREY_DK_TRANS, COLOR_WHITE));  
 354         }
 355     }
 356 
 357     return angle1;
 358 }
 359 
 360 // draw animated compass needle on GUI
 361 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){
 362  
 363 /*
 364     char vBuf[32];
 365     sprintf(vBuf, "%d", angle );      
 366     draw_txt_string(30, 0, vBuf, MAKE_COLOR(COLOR_GREY_DK_TRANS, COLOR_RED));   
 367 */
 368     if(bitmap)
 369     {
 370         int mx=0;
 371         int my=0;
 372         int px=0;
 373         int py=0;
 374 
 375         int pos=0;
 376         int x_n;
 377         int y_n;
 378 
 379         if (angle==0 || angle==360)
 380         {
 381             s_w = 0;
 382             c_w = -1;
 383         }
 384 
 385         if (angle==270)
 386         {
 387             s_w = -1;
 388             c_w = 0;
 389         }
 390 
 391         for(pos=0; pos<extent; pos++)
 392         {
 393             int data = bitmap[pos];
 394             if (data >= 49)
 395             {
 396                 px=offset_x+mx;
 397                 py=offset_y+my;
 398 
 399                 x_n = m_x + Round(((c_w * (m_x - px)) + (s_w * (m_y - py))),2);
 400                 y_n = m_y + Round(((c_w * (m_y - py)) - (s_w * (m_x - px))),2);
 401 
 402                 if (data == 49)
 403                 {
 404                     draw_pixel(x_n, y_n, f_v_1);
 405                 }
 406                 if (data == 50)
 407                 {
 408                     draw_pixel(x_n, y_n, f_v_2);
 409                 }
 410             }
 411             if (data == 10 || data == 13)
 412             {
 413                 mx=0;
 414                 my++;
 415                 pos++;
 416             }
 417             else
 418             {
 419                 mx++;
 420             }
 421         }
 422 
 423     }
 424 }
 425 
 426 // draw animated compass on GUI
 427 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){
 428 
 429     if(bitmap)
 430     {
 431         int extent = strlen(bitmap);
 432         int pos=0;
 433         int mx=0;
 434         int my=0;
 435 
 436         for(pos=0; pos<extent; pos++)
 437         {
 438             int data = bitmap[pos];
 439             if (data == 48)
 440             {
 441                 draw_pixel(o_x+mx, o_y+my, f_v_0);
 442             }
 443             if (data == 49)
 444             {
 445                 draw_pixel(o_x+mx, o_y+my, f_v_1);
 446             }
 447             if (data == 50)
 448             {
 449                 draw_pixel(o_x+mx, o_y+my, f_v_2);
 450             }
 451             if (data == 51)
 452             {
 453                 draw_pixel(o_x+mx, o_y+my, f_v_4);
 454             }
 455             if (data == 10 || data == 13)
 456             {
 457                 mx=0;
 458                 my++;
 459                 pos++;
 460             }
 461             else
 462             {
 463                 mx++;
 464             }
 465         }
 466 
 467     }
 468 }
 469 
 470 //=======================================================================
 471 //
 472 // TASK : displays satellite dish image when CHDK GPS functionality enabled
 473 //
 474 static void show_sat_symbol_task(){
 475     
 476     char *bitmap = load_bitmap("A/CHDK/DATA/GPS_Sat.txt");
 477 
 478     if(bitmap)
 479     {
 480         int extent1 = strlen(bitmap);
 481         int pos1=0;
 482         int mx1=0;
 483         int my1=0;
 484         int o_x1=270;
 485         int o_y1=0;
 486         int f_v_0;
 487         int f_h_0;
 488         int f_v_1;
 489         int f_h_1;
 490         int f_v_2;
 491         int f_h_2;
 492         int f_v_3;
 493         int f_h_3;
 494         int f_v_4;
 495         int f_h_4;
 496 
 497         while (exit_show_task==0) 
 498         {           
 499             int stat = g_d_stat;
 500             
 501             mx1=0;
 502             my1=0;
 503 
 504             f_v_0=COLOR_TRANSPARENT;
 505             f_h_0=COLOR_TRANSPARENT;
 506 
 507             f_v_1=COLOR_GREEN;
 508             f_h_1=COLOR_GREEN;
 509 
 510             switch (stat)
 511             {
 512                 case 0 :
 513                     f_v_1= COLOR_RED ;
 514                     f_h_1= COLOR_RED ;
 515                     break ;
 516                 case 1 :
 517                     f_v_1= COLOR_YELLOW ;
 518                     f_h_1= COLOR_YELLOW ;
 519                     break ;
 520                 case 2 :
 521                     f_v_1= COLOR_GREEN ;
 522                     f_h_1= COLOR_GREEN ;
 523                     break ;
 524             }
 525             
 526             if (camera_info.state.mode_play)
 527             {
 528                 gps_img_data *igps = ( gps_img_data *) camera_jpeg_current_gps();
 529 
 530                 f_v_1= COLOR_RED ;
 531                 f_h_1= COLOR_RED ;
 532                 if (igps->latitudeRef[0] && igps->longitudeRef[0])
 533                 {
 534                     f_v_1= COLOR_YELLOW ;
 535                     f_h_1= COLOR_YELLOW ;
 536                     if (igps->height[1])
 537                     {
 538                         f_v_1= COLOR_GREEN ;
 539                         f_h_1= COLOR_GREEN ;
 540                     }
 541                 }
 542             }
 543 
 544             f_v_2= COLOR_BLACK ;
 545             f_h_2= COLOR_BLACK ;
 546 
 547             f_v_3= COLOR_YELLOW ;
 548             f_h_3= COLOR_YELLOW ;
 549 
 550             f_v_4= COLOR_BLUE ;
 551             f_h_4= COLOR_BLUE ;
 552 
 553             for(pos1=0; pos1<extent1; pos1++)
 554             {
 555                 int data = bitmap[pos1];
 556                 if (data == 48)
 557                 {
 558                     draw_pixel(o_x1+mx1, o_y1+my1, f_v_0);
 559                 }
 560                 if (data == 49)
 561                 {
 562                     draw_pixel(o_x1+mx1, o_y1+my1, f_v_1);
 563                 }
 564                 if (data == 50)
 565                 {
 566                     draw_pixel(o_x1+mx1, o_y1+my1, f_v_2);
 567                 }
 568                 if (data == 51)
 569                 {
 570                     draw_pixel(o_x1+mx1, o_y1+my1, f_v_3);
 571                 }
 572                 if (data == 52)
 573                 {
 574                     draw_pixel(o_x1+mx1, o_y1+my1, f_v_4);
 575                 }
 576                 if (data == 10 || data == 13)
 577                 {
 578                     mx1=0;
 579                     my1++;
 580                     pos1++;
 581                 }
 582                 else
 583                 {
 584                     mx1++;
 585                 }
 586             }
 587             msleep(1000);           
 588         }
 589         free(bitmap);
 590     }
 591     gps_show_lock = 0 ;     
 592     ExitTask();
 593 }
 594 
 595 //=======================================================================
 596 //
 597 // TASK : updates GPS data structures continuously if CHDK GPS functionality is enabled
 598 //
 599 static void gps_data_task(){
 600     
 601     test_timezone_once=0;
 602     
 603     while (exit_data_task==0)
 604     {
 605         tGPS gps;
 606         int tim00=0;
 607         int tim01=0;
 608         int tim02=0;
 609         unsigned long t;
 610         static struct tm *ttm;        
 611                           
 612   #ifndef SIMULATED_GPS
 613         GPS_UpdateData();  
 614         get_property_case(camera_info.props.gps, &gps, sizeof(gps));
 615   #else     
 616         GPS_FakeData() ;
 617         memcpy( &gps , &gps_dummy_data, sizeof(gps) );
 618   #endif
 619   
 620         t=time(NULL);
 621         ttm = localtime(&t);
 622         
 623         lock(0);
 624         
 625             g_d_lat=0.0;
 626             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));
 627             if (gps.latitudeRef[0]=='S') g_d_lat=-g_d_lat;
 628 
 629             g_d_lon=0.0;
 630             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));
 631             if (gps.longitudeRef[0]=='W') g_d_lon=-g_d_lon;
 632 
 633             g_d_hei=0.0;
 634             g_d_hei=gps.height[0]/(gps.height[1]*1.0);
 635 
 636     /**************
 637     char vBuf[256], lat[40],lon[40] ;
 638     sprintf(vBuf, "%d.%d.%d  %d.%d.%d",
 639         gps.latitude[0] / gps.latitude[1] , 
 640         gps.latitude[2] / gps.latitude[3] ,
 641         gps.latitude[4] / gps.latitude[5] ,   
 642         gps.longitude[0] / gps.longitude[1] , 
 643         gps.longitude[2] / gps.longitude[3] ,
 644         gps.longitude[4] / gps.longitude[5] );      
 645     draw_txt_string(1, 0, vBuf, MAKE_COLOR(COLOR_GREY_DK_TRANS, COLOR_RED));    
 646     formatDouble (lat, g_d_lat, 0, 7),
 647     formatDouble (lon, g_d_lon, 0, 7),    
 648     sprintf(vBuf,"lat=%s lon=%s",lat,lon );
 649     draw_txt_string(1, 1, vBuf, MAKE_COLOR(COLOR_GREY_DK_TRANS, COLOR_RED));              
 650     ************/
 651     
 652             sprintf(g_d_tim, "%02d:%02d:%02d", ttm->tm_hour, ttm->tm_min, ttm->tm_sec);
 653             sprintf(g_d_dat, "%04d-%02d-%02d", ttm->tm_year+1900, ttm->tm_mon+1, ttm->tm_mday);
 654 
 655             g_d_stat=0;
 656             if ((int)g_d_lat == 0) {g_d_stat = 0; }
 657             if ((int)g_d_lat != 0 && (int)g_d_hei == 0) {g_d_stat=1; }
 658             if ((int)g_d_lat != 0 && (int)g_d_hei != 0) {g_d_stat=2; }
 659 
 660             if (g_d_stat > 0)
 661             {
 662                 tim00=(int)(gps.timeStamp[0]/gps.timeStamp[1]);
 663                 tim01=(int)(gps.timeStamp[2]/gps.timeStamp[3]);
 664                 tim02=(int)(gps.timeStamp[4]/gps.timeStamp[5]);
 665                 sprintf(g_d_tim_gps, "%02d:%02d:%02d", tim00, tim01, tim02);
 666 
 667                 sprintf(g_d_dat_gps, "%c%c%c%c-%c%c-%c%c",
 668                         gps.dateStamp[0],
 669                         gps.dateStamp[1],
 670                         gps.dateStamp[2],
 671                         gps.dateStamp[3],
 672                         gps.dateStamp[5],
 673                         gps.dateStamp[6],
 674                         gps.dateStamp[8],
 675                         gps.dateStamp[9]);
 676 
 677                 if (((int)conf.gps_test_timezone == 1) && (test_timezone_once==0))
 678                 {
 679                     test_timezone();
 680                     test_timezone_once=1;
 681                 }
 682             }
 683 
 684         unlock(0);
 685 
 686         if ((int)conf.gps_show_symbol == 1)
 687         {
 688             if ( gps_show_lock == 0 )
 689             {                
 690                 exit_show_task= 0 ;      
 691                 gps_show_lock = 1 ;
 692                 CreateTask("GPS_SHOW", 0x19, 0x800, show_sat_symbol_task);
 693             }
 694         } 
 695         else exit_show_task=1; 
 696         
 697         msleep(900);
 698     } 
 699     exit_show_task=1; 
 700     gps_data_lock = 0 ;    
 701     ExitTask();
 702 }
 703 
 704 //=======================================================================
 705 //
 706 // TASK : updates onsceen tracking icon (globe)
 707 //
 708 static void tracking_icon_task(){
 709     
 710     char * bitmap1 = load_bitmap("A/CHDK/DATA/GPS_Track_1.txt");
 711     char * bitmap2 = load_bitmap("A/CHDK/DATA/GPS_Track_2.txt");
 712     char * bitmap3 = load_bitmap("A/CHDK/DATA/GPS_Track_3.txt");
 713 
 714     if(bitmap1 && bitmap2 && bitmap2 )
 715     {
 716         int extent1 = strlen(bitmap1);
 717         int pos1=0;
 718         int mx1=0;
 719         int my1=0;
 720         int o_x1=315;
 721         int o_y1=0;
 722         int f_v_0;
 723         int f_h_0;
 724         int f_v_1;
 725         int f_h_1;
 726         int f_v_2;
 727         int f_h_2;
 728         int f_v_3;
 729         int f_h_3;
 730         int f_v_4;
 731         int f_h_4;
 732         int f_v_5;
 733         int f_h_5;
 734         int blink=0;
 735         int data;
 736 
 737         while (exit_track_task == 0)
 738         {
 739             mx1=0;
 740             my1=0;
 741 
 742             f_v_0=COLOR_TRANSPARENT;
 743             f_h_0=COLOR_TRANSPARENT;
 744 
 745             f_v_1= COLOR_BLACK ;
 746             f_h_1= COLOR_BLACK ;
 747 
 748             f_v_2= COLOR_BLUE ;
 749             f_h_2= COLOR_BLUE ;
 750 
 751             f_v_3= COLOR_YELLOW ;
 752             f_h_3= COLOR_YELLOW ;
 753 
 754             f_v_4= COLOR_RED ;
 755             f_h_4= COLOR_RED ;
 756 
 757             f_v_5= COLOR_GREEN ;
 758             f_h_5= COLOR_GREEN ;
 759 
 760             int stat = g_d_stat ;
 761             
 762             for(pos1=0; pos1<extent1; pos1++)
 763             {
 764                 if ((blink==1) && (stat > 0))
 765                 {
 766                     data = bitmap2[pos1];
 767                 }
 768                 else
 769                 {
 770                     data = bitmap1[pos1];
 771                 }
 772                 if ((blink==1) && (navigation_mode > 0) && (stat > 0))
 773                 {
 774                     data = bitmap3[pos1];
 775                 }
 776                 if (data == 48)
 777                 {
 778                     draw_pixel(o_x1+mx1, o_y1+my1, f_v_0);
 779                 }
 780                 if (data == 49)
 781                 {
 782                     draw_pixel(o_x1+mx1, o_y1+my1, f_v_1);
 783                 }
 784                 if (data == 50)
 785                 {
 786                     draw_pixel(o_x1+mx1, o_y1+my1, f_v_2);
 787                 }
 788                 if (data == 51)
 789                 {
 790                     draw_pixel(o_x1+mx1, o_y1+my1, f_v_3);
 791                 }
 792                 if (data == 52)
 793                 {
 794                     draw_pixel(o_x1+mx1, o_y1+my1, f_v_4);
 795                 }
 796                 if (data == 53)
 797                 {
 798                     draw_pixel(o_x1+mx1, o_y1+my1, f_v_5);
 799                 }
 800                 if (data == 10 || data == 13)
 801                 {
 802                     mx1=0;
 803                     my1++;
 804                     pos1++;
 805                 }
 806                 else
 807                 {
 808                     mx1++;
 809                 }
 810             }
 811 
 812             msleep(1000);
 813             
 814             if (blink==0)
 815             {
 816                 blink=1;
 817             }
 818             else
 819             {
 820                 blink=0;
 821             }
 822         }        
 823     }
 824     if (bitmap1) free(bitmap1) ;
 825     if (bitmap2) free(bitmap2) ;
 826     if (bitmap3) free(bitmap3) ;
 827     gps_track_lock = 0 ; 
 828     ExitTask();
 829 }
 830 
 831 //=======================================================================
 832 //
 833 // TASK : started by waypoint recording code when no GPS signal and picture was taken
 834 //
 835 static void no_signal_task(){
 836    
 837     char vBuf[100];
 838     FILE *fd0;
 839     char w1[20];
 840     char w2[30];
 841     char w3[10];
 842     int o=1, p=1, q=0;
 843     int blite_off=0;
 844     unsigned long bat;
 845     int abort=0;
 846     
 847     gps_key_trap = KEY_DISPLAY ;
 848     
 849     while ( time_to_end !=0 )
 850     {
 851 
 852         // Test battery
 853         if ( (((time_to_end) % 60)) == 0 )
 854         {
 855             bat=get_batt_perc();
 856             if (bat <= (int)conf.gps_batt)
 857             {
 858                 int zba;
 859                 for(zba=30; zba>0; zba--)
 860                 {
 861                     sprintf(vBuf, lang_str(LANG_MENU_GPS_t_3));      // "Battery below setting!"
 862                     draw_txt_centered(8, vBuf, MAKE_COLOR(COLOR_YELLOW, COLOR_RED));
 863                     sprintf(vBuf, lang_str(LANG_MENU_GPS_t_4),zba); // "Camera will shutdown in %02d seconds!"
 864                     draw_txt_centered(9, vBuf, MAKE_COLOR(COLOR_RED, COLOR_BLUE));
 865                     if ( (((zba) % 2)) == 0 )
 866                     {
 867                         debug_led(0);
 868                         if ((int)conf.gps_batt_warn == 1)
 869                         {
 870                             play_sound(6);
 871                         }
 872                     }
 873                     else
 874                     {
 875                         debug_led(1);
 876                     }
 877                     msleep(1000);
 878                 }
 879                 camera_shutdown_in_a_second();
 880                 gps_nosignal_lock = 0 ;                 
 881                 ExitTask();
 882             }
 883         }
 884 
 885         // cancel automatic shut-off if DISP key pressed
 886         if (gps_key_trap == -1 )
 887         {
 888             gps_key_trap = 0;             
 889             abort = 1;
 890             time_to_end = 3600;
 891             TurnOnBackLight();
 892             gui_mbox_init(LANG_INFORMATION, LANG_MENU_GPS_t_5, MBOX_BTN_OK|MBOX_TEXT_CENTER, NULL); //Automatic shutdown cancelled!
 893         }
 894 
 895         // manage backlight and display
 896         if ( abort == 0)
 897         {
 898             // update LED blink
 899             if (q==0)
 900             {
 901                 debug_led(0);
 902                 q=1;
 903             }
 904             else
 905             {
 906                 debug_led(1);
 907                 q=0;
 908             }
 909 
 910             // Display countdown of time to finish in mm: ss
 911             int s = (int)time_to_end;
 912             int minute =  s / 60;
 913             s = s % 60;
 914             int second = s;
 915 
 916             if ( (int)conf.gps_countdown==0 )
 917             {
 918                 sprintf(vBuf, lang_str(LANG_MENU_GPS_t_6),minute, second);  // "Camera will wait for GPS for %01d:%02d"
 919             }
 920             else
 921             {
 922                 sprintf(vBuf, " %01d:%02d",minute, second);
 923             }
 924             draw_txt_centered(0, vBuf, MAKE_COLOR(COLOR_GREY_DK_TRANS, COLOR_RED));
 925 
 926             // Switch on the display when countdown <30 seconds
 927             if ((blite_off==1) && (time_to_end <=30))
 928             {
 929                 TurnOnBackLight();
 930             }
 931 
 932             // start countdown if key pressed and backlight is off
 933             if ((blite_off==0) && (kbd_get_pressed_key() !=0))     // any key pressed ?
 934             {
 935                 time_to_end=(int)conf.gps_wait_for_signal;
 936                 o=1;
 937                 p=1;
 938             }
 939 
 940             // start countdown and turn on backlight if key pressed and backlight is off
 941             if ((blite_off==1) && (kbd_get_pressed_key() !=0))     // any key pressed ?
 942             {
 943                 TurnOnBackLight();
 944                 blite_off=0;
 945                 time_to_end=(int)conf.gps_wait_for_signal; 
 946                 o=1;
 947                 p=1;
 948             }
 949 
 950             // switch to play mode
 951             if ((o == (int)conf.gps_rec_play_time) && ((int)conf.gps_rec_play_set == 1))
 952             {
 953                 if (camera_info.state.mode_rec)
 954                 {
 955                     levent_set_play();
 956                 }
 957             }
 958 
 959             // turn off backlight if timed out
 960             if ((p == (int)conf.gps_play_dark_time) && ((int)conf.gps_play_dark_set == 1))
 961             {
 962                 if (camera_info.state.mode_play)
 963                 {
 964                     TurnOffBackLight();
 965                     blite_off=1;
 966                 }
 967             }
 968         }
 969 
 970         // tag waypoint reached
 971 
 972         lock(1) ;
 973             int l_stat      = g_d_stat ;
 974             double l_lat       = g_d_lat ;
 975             double l_lon       = g_d_lon ;
 976             double l_hei       = g_d_hei ;
 977             char l_tim[10];
 978             char l_dat[12];
 979             char l_tim_gps[10];
 980             char l_dat_gps[12];   
 981             strncpy( l_tim ,      g_d_tim,    10 );         
 982             strncpy( l_dat ,      g_d_dat,    12 );
 983             strncpy( l_dat_gps ,  g_d_dat_gps,10 );
 984             strncpy( l_tim_gps ,  g_d_tim_gps,12 );
 985         unlock(1);       
 986     
 987         if ( ((((int)conf.gps_2D_3D_fix) == 1) && (l_stat == 1)) || \
 988              ((((int)conf.gps_2D_3D_fix) == 1) && (l_stat == 2)) || \
 989              ((((int)conf.gps_2D_3D_fix) == 2) && (l_stat == 2)) || \
 990              ((((int)conf.gps_2D_3D_fix) == 3) && (l_stat == 2)) || \
 991              ((((int)conf.gps_2D_3D_fix) == 3) && (l_stat == 1) && (time_to_end < 3)) || \
 992              ((((int)conf.gps_2D_3D_fix) == 0) && (time_to_end < 3)) )
 993         {
 994             TurnOnBackLight();
 995             msleep (1000);
 996 
 997             fd0 = fopen ( "A/GPS/Tagging/wp.tmp", "r" );
 998 
 999             do
1000             {
1001                 fgets (w1, 15, fd0);
1002                 fseek (fd0, 1, SEEK_CUR);
1003                 fgets (w2, 23, fd0);
1004                 fseek (fd0, 1, SEEK_CUR);
1005                 fgets (w3, 1, fd0);
1006 
1007                 char lat[40];
1008                 char lon[40];
1009                 char hei[40];
1010 
1011                 char vBuf1[512];
1012                 char image_name[20];
1013                 char wp_name[30];
1014                 char gpx_name[30];
1015                 char gpx_image_name[20];
1016                 unsigned long t;
1017                 static struct tm *ttm;
1018                 t=time(NULL);
1019                 ttm = localtime(&t);
1020 
1021                 sprintf(wp_name, "A/GPS/Tagging/%04d_%02d_%02d.wp", ttm->tm_year+1900, ttm->tm_mon+1, ttm->tm_mday);
1022                 sprintf(gpx_name, "A/GPS/Tagging/%04d_%02d_%02d.gpx", ttm->tm_year+1900, ttm->tm_mon+1, ttm->tm_mday);
1023 
1024                 sprintf(image_name, "%s", w1);
1025                 sprintf(gpx_image_name, "%s", w1);
1026 
1027                 sprintf(vBuf1, "%s;%s;%s;%s;%s;%s",
1028                         image_name,
1029                         formatDouble (lat, l_lat, 0, 7),
1030                         formatDouble (lon, l_lon, 0, 7),
1031                         formatDouble (hei, l_hei, 0, 2),
1032                         w2,
1033                         w3);
1034 
1035                 FILE* fp = fopen(wp_name, "a");
1036 
1037                 if( fp )
1038                 {
1039                     fwrite(vBuf1, strlen(vBuf1), 1, fp);
1040                 }
1041 
1042                 fclose(fp);
1043 
1044                 fp = fopen(gpx_name, "r");
1045                 if( fp == NULL)
1046                 {
1047                     fp = fopen(gpx_name, "a");
1048                     if( fp )
1049                     {
1050                         sprintf(vBuf1, "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n");
1051                         fwrite(vBuf1, strlen(vBuf1), 1, fp);
1052                         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");
1053                         fwrite(vBuf1, strlen(vBuf1), 1, fp);
1054                         sprintf(vBuf1, "</gpx>");
1055                         fwrite(vBuf1, strlen(vBuf1), 1, fp);
1056                         fclose(fp);
1057                     }
1058                 }
1059 
1060 
1061                 fp = fopen(gpx_name, "a");
1062                 if( fp )
1063                 {
1064                     fseek (fp, -6, SEEK_END);
1065 
1066                     sprintf(vBuf1, "   <wpt lat=\"%s\" lon=\"%s\">\n",formatDouble (lat, l_lat, 0, 7), formatDouble (lon, l_lon, 0, 7));
1067                     fwrite(vBuf1, strlen(vBuf1), 1, fp);
1068                     sprintf(vBuf1, "    <ele>%s</ele>\n",formatDouble (hei, l_hei, 0, 2));
1069                     fwrite(vBuf1, strlen(vBuf1), 1, fp);
1070                     sprintf(vBuf1, "    <time>%s</time>\n", w2);
1071                     fwrite(vBuf1, strlen(vBuf1), 1, fp);
1072                     sprintf(vBuf1, "    <gpst>%sT%sZ</gpst>\n", l_dat_gps, l_tim_gps);
1073                     fwrite(vBuf1, strlen(vBuf1), 1, fp);
1074                     sprintf(vBuf1, "    <temp>%i</temp>\n", (int)get_optical_temp());
1075                     fwrite(vBuf1, strlen(vBuf1), 1, fp);
1076                     sprintf(vBuf1, "    <name>%s</name>\n", gpx_image_name);
1077                     fwrite(vBuf1, strlen(vBuf1), 1, fp);
1078                     sprintf(vBuf1, "   </wpt>\n");
1079                     fwrite(vBuf1, strlen(vBuf1), 1, fp);
1080                     sprintf(vBuf1, "</gpx>");
1081                     fwrite(vBuf1, strlen(vBuf1), 1, fp);
1082                 }
1083                 fclose(fp);
1084             }
1085             while (!feof(fd0));
1086 
1087             fclose (fd0);
1088             if ( abort == 0)
1089             {
1090                 time_to_end=1;
1091             }
1092 
1093         }
1094 
1095         msleep(1000);
1096         o++;
1097         p++;
1098 
1099 
1100         // gps functionality disabled ?
1101         if ((int)conf.gps_on_off ==0)
1102         {
1103             debug_led(0);
1104             gps_nosignal_lock = 0;             
1105             ExitTask();
1106         }
1107 
1108         time_to_end--;
1109     }
1110 
1111     int zba, zba1;
1112 
1113     if (abort == 0)
1114     {
1115         for(zba=15; zba>0; zba--)           // outer loop = 30 seconds
1116         {
1117             sprintf(vBuf, lang_str(LANG_MENU_GPS_t_4), zba);     // "Camera will shutdown in %02d seconds!"
1118             draw_txt_centered(2, vBuf, MAKE_COLOR(COLOR_WHITE, COLOR_BLUE));
1119             sprintf(vBuf, lang_str(LANG_MENU_GPS_t_18));         //"To cancel [Press half]"
1120             draw_txt_centered(3, vBuf, MAKE_COLOR(COLOR_WHITE, COLOR_RED));
1121             play_sound(6);
1122 
1123             for(zba1=100; zba1>0; zba1--)   // inner loop = 2 seconds
1124             {
1125                 if (kbd_get_pressed_key() == KEY_SHOOT_HALF)
1126                     {
1127                         TurnOnBackLight();
1128                         debug_led(0);
1129                         gps_nosignal_lock = 0 ; 
1130                         ExitTask();
1131                     }
1132                 msleep(2);
1133             }
1134         }        
1135         camera_shutdown_in_a_second();
1136     }
1137 
1138     TurnOnBackLight();
1139 
1140     // beep 5 seconds
1141     if ((int)conf.gps_beep_warn == 1)
1142     {
1143         for(zba=5; zba>0; zba--) play_sound(6);
1144     }
1145     
1146     debug_led(0);
1147     gps_nosignal_lock = 0 ; 
1148     ExitTask();
1149 
1150 }
1151 
1152 
1153 //=======================================================================
1154 //
1155 // TASK : started by init_gps_logging_task() to record logging data
1156 //
1157 static void gps_logging_task(){
1158       
1159     char lat[40];
1160     char lon[40];
1161     char hei[40];
1162     char vBuf[512];
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] */