Diff: LeonardoMbos.cpp

Revision:

0:d177c0087d1f

Child:

1:360c4a23cb1d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LeonardoMbos.cpp	Sun Jan 29 16:06:12 2012 +0000
@@ -0,0 +1,925 @@
+/* ////////////////////////////////////////////////////////////////////////////
+** File:      LeonardoMbos.cpp
+*/    
+/*               12345678901234567890                                        */                               
+ char  Ver1[] = "Leonardo's GPS 0.1.0";
+ char  Ver2[] = "  by Guiott  01-12";
+/**
+* \mainpage Leonardo.cpp
+* \author   Guido Ottaviani-->guido@guiott.com<--
+* \version  0.1.0
+* \date     01/2012
+* \details This is a test program to study the capability of a GPS module to
+   control navigation of a robot in an outdoor environment.
+        
+   This version is developed within the mbos RTOS:
+   http://mbed.org/users/AndrewL/libraries/mbos/lqn3ca
+        
+   It uses NMEA library by Tim (xtimor@gmail.com) 
+   available on public mbed library or http://nmea.sourceforge.net
+        
+ The original gmath.c has been modified to fix a bug in nmea_distance_ellipsoid() function
+ according to bug report ID: 2945855
+ 
+ http://sourceforge.net/tracker/?func=detail&aid=2945855&group_id=192054&atid=939854
+ 
+    // while ((delta_lambda > 1e-12) && (remaining_steps > 0))  original by xtimor
+    while (( remaining_steps == 20 ) || ((fabs(delta_lambda) > 1e-12) && (remaining_steps > 0)))
+    
+ the original code always returns a zero distance if the arrival point longitude
+ is equal or smaller than the starting point one.
+    
+ The mbed module is interfaced with a Garmin GPS sensor (used in standard mode)
+ with an external antenna, a 20x4 LCD text display and a 5 keys keypad.
+ A curiosity:
+ All the hardware components of the test set are mounted on the top of a wooden 
+ box, looking like a kind of a steampunk navigator. When my daughter looked at 
+ the box she said: "it looks alike the navigator of the da Vinci car".
+ This is the reason why the name of this project is "Leonardo".
+**
+-------------------------------------------------------------------------------
+* \copyright 2012 Guido Ottaviani
+guido@guiott.com
+
+    LeonardoMbos is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    dsPID33 is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with LeonardoMbos.cpp.  If not, see <http://www.gnu.org/licenses/>.
+    
+-------------------------------------------------------------------------------      
+*/
+#include "Common.h"
+#include "mbed.h"
+#include <string>
+#include "TextLCD.h"
+#include "nmea/nmea.h"
+#include "Init.h" 
+#include "Prototype.h"
+#include "mbos.h"
+#include "Tasks.h"
+#include <Watchdog.h>
+
+TextLCD lcd(p12, p11, p24, p23, p22, p21, TextLCD::LCD20x4); // rs, e, d4-d7
+
+Watchdog wd;
+
+// Set up RTOS;
+mbos os(NUM_TASKS, NUM_TIMERS, NUM_RESOURCES);
+
+int main() 
+{
+    Initialize();
+    if (wd.WatchdogCausedReset())
+    {
+        pc.printf("Watchdog caused reset**********************************\r\n");
+    }
+    wd.Configure(WDT_TIMER);       // sets the timeout interval
+
+    os.Start(); // the RTOS starts now
+}
+
+//functions =====================================================================
+
+int CmpRead(void)
+{/**
+ *\brief Magnetic Compass reading 
+ */
+    int Cmp;
+
+    // To Be Done
+    Cmp = 0; // ************debug
+    return(Cmp);
+}
+
+void mbosIdleTask(void)
+{/**
+ *\brief TASK 0 watchdog kick
+ */
+ while(1)
+ {
+    wd.Service();       // kick the dog before the timeout
+ }
+}
+
+void TempTask(void)
+{/**
+ *\brief TASK 8 used to temporary test some functions 
+ */
+ static int TempAng;
+ while (1)
+ {
+  os.WaitEvent(TEMP_EVT); 
+  
+  if((TempAng+=5) >=360)
+  {
+    TempAng=0;
+  }
+  Ang[Mag]=TempAng;
+  Ang[Dir]=360-TempAng;
+  Ang[Gps]=180-TempAng;
+  printf("%i %i  %i \n", Ang[0], Ang[1], Ang[2]);
+ }
+}
+
+void GpsStringParse(void)
+{/**
+ *\brief parse the GPS string to extract info
+ */
+    gps.attach(NULL);   // Suspend serial interrupt while buffer change
+    size = writePointer;
+    writePointer = 0;
+    bufferSelect++;     // Change buffer
+    gps.attach( &GpsSerialIsr );  // Resume serial interrupt
+    nmea_parse(&parser, msgBuff[(bufferSelect-1)&1], size, &info);
+    Coordinates(); // transform nmea coordinates in decimal degrees
+}
+
+void GpsDist(void)
+{/**
+ *\brief compute the distance and direction (forward and reverse) 
+    from point A to point B on the ellipsoid
+ */
+  
+    nmea_info2pos(&info, &Pos[0]);  // current position  
+    nmea_info2pos(&Dest, &Pos[1]);  // destination 
+
+    Path.Dist = nmea_distance_ellipsoid(&Pos[0], &Pos[1], &Path.Azimuth[1], &Path.Azimuth[0]);
+
+    if(Path.Azimuth[0] > NMEA_PI) 
+    {// reverse direction
+        Path.Azimuth[0] = Path.Azimuth[0] - NMEA_PI; 
+    }
+    else
+    {
+        Path.Azimuth[1] = Path.Azimuth[1] + NMEA_PI; 
+    }
+
+    for(int i=0; i < 2; i++)
+    {
+        Path.Azimuth[i]=(nmea_radian2degree(Path.Azimuth[i]));
+        if(Path.Azimuth[i] > 360)
+        {
+            Path.Azimuth[i] = Path.Azimuth[i] - 360;
+        }
+        if(Path.Azimuth[i] < 0)
+        {
+            Path.Azimuth[i] = Path.Azimuth[i] + 360;
+        }
+    }
+}
+
+void LcdLightDimTask(void)
+{/** 
+ *\brief TASK 7, control the LCD backlight intensity to smoothly
+         switch it on or off
+ */
+ static float LightVal=1;
+        
+ while(1)
+ {       
+    os.WaitEvent(LCD_LIGHT_DIM_ON_EVT | LCD_LIGHT_DIM_OFF_EVT); 
+    if(os.GetEvent()==LCD_LIGHT_DIM_ON_EVT)
+    {
+      if(LightVal<1)
+      {
+        for (LightVal=0; LightVal<=1; LightVal+=0.01f)
+        {
+            LcdBklight=LightVal;
+            wait_ms(10);
+        }
+      }
+    // Set the timer for the power saving backlight switch off
+    os.ClearTimer(LCD_LIGHT_DIM_OFF_TMR);
+    os.SetTimer(LCD_LIGHT_DIM_OFF_TMR, LCD_LIGHT_DIM_TIMER, 0);
+    }
+    else if(os.GetEvent()==LCD_LIGHT_DIM_OFF_EVT)
+    {// The dimming off is slower than lighting up
+        for (LightVal=1; LightVal>=0; LightVal-=0.01f)
+        {
+            LcdBklight=LightVal;           
+            wait_ms(30);
+        }        
+    }
+ }
+}
+
+void Coordinates(void)
+{/**
+ *\brief transform nmea coordinates in decimal degrees
+ */
+    degrees = trunc(info.lat / 100.0);
+    minutes = info.lat - (degrees * 100.0);
+    latitude = degrees + minutes / 60.0;
+    degrees = trunc(info.lon / 100.0);
+    minutes = info.lon - (degrees * 100.0);
+    longitude = degrees + minutes / 60.0;
+ }
+  
+void Deg2DegMinSec(double DecDeg, DegMinSec *DecSec)
+{/**
+ *\brief convert decimalDeg to Deg Min decimalSec
+ */
+    DecSec->Deg = trunc(DecDeg);
+    double MinDec = (DecDeg - DecSec->Deg);
+    DecSec->Min = trunc(MinDec * 60);
+    DecSec->Sec = (MinDec * 3600) - (DecSec->Min * 60);
+ }
+   
+void ShowPcTask(void) 
+{/**
+ *\brief TASK 6, if a PC is connected, debug information can be sent to the console
+ */
+ static int it = 0;
+ int i;
+ DegMinSec DecCoord;
+    
+ os.SetTimer(SHOW_PC_TMR, 1000, 1000);
+ while(1)
+ {
+    os.WaitEvent(SHOW_PC_EVT); 
+
+    if(pc.readable())
+    {// wait for an input
+        PcMonitor = (pc.getc()-48); // digit a number to en/dis-able debug
+     }
+     
+    if( PcMonitor==1 || PcMonitor>5)
+    {// Display Info parameters 
+        pc.printf(
+        "%03d, Lat: %f, Lon: %f, Sig:%d, Fix:%d, Inuse:%d\r\n",
+        it++, latitude, longitude, info.sig, info.fix, info.satinfo.inuse );
+        for (i = 0; i < NMEA_MAXSAT; i++) 
+            {
+            if (info.satinfo.sat[i].sig > 0)
+            pc.printf("  sat_id:%02d, sig:%02d, Inuse:%d\r\n",
+                      info.satinfo.sat[i].id , info.satinfo.sat[i].sig, 
+                      info.satinfo.sat[i].in_use);
+            }
+        pc.printf("\r\n");
+    }
+    
+    if( PcMonitor==2 || PcMonitor>5)
+    {// Display Distance parameters 
+        Deg2DegMinSec(nmea_radian2degree(Pos[0].lat), &DecCoord);
+        pc.printf("Lat1:%d %d\'%.3f\"  ", DecCoord.Deg, DecCoord.Min, DecCoord.Sec);
+        Deg2DegMinSec(nmea_radian2degree(Pos[0].lon), &DecCoord);
+        pc.printf("Lon1:%d %d\'%.3f\"  ", DecCoord.Deg, DecCoord.Min, DecCoord.Sec);
+        Deg2DegMinSec(nmea_radian2degree(Pos[1].lat), &DecCoord);
+        pc.printf("Lat2:%d %d\'%.3f\"  ", DecCoord.Deg, DecCoord.Min, DecCoord.Sec);
+        Deg2DegMinSec(nmea_radian2degree(Pos[1].lon), &DecCoord);
+        pc.printf("Lon2:%d %d\'%.3f\"  \n", DecCoord.Deg, DecCoord.Min, DecCoord.Sec);
+        pc.printf("Dist:%f  Azimuth Start:%f  Azimuth Final:%f \n\n",
+                  Path.Dist, Path.Azimuth[0], Path.Azimuth[1]); 
+    }
+  }   
+}
+
+void ShowLcdTask(void)
+{/**
+ *\brief TASK 3, display desired data on LCD
+ */
+ 
+ static int Previous=0;
+
+ os.SetTimer(SHOW_LCD_TMR, SHOW_LCD_TIMER, 0);
+ while(1)
+ {
+ os.WaitEvent(SHOW_LCD_EVT);
+
+    if(Previous != Menu)
+    {// clear the display when function changes
+        lcd.cls();
+        Previous=Menu;
+    }
+    
+    switch(Menu)
+    {
+        case 0:
+            showSatLcd();
+            Previous=0;
+            break;
+            
+        case 1:
+            if(info.sig != 0)
+            {
+                showInfoLcd();
+            }
+            else
+            {
+                showSatLcd();
+            }
+            Previous=1;    
+            break;
+        
+        case 2:
+            showMenuLcd();
+            Previous=2;    
+            break;
+            
+        case 3:
+            showMenuLcd1();
+            Previous=3;    
+            break;  
+            
+        case 4: 
+            static int ChooseDir=0;
+            Ang[Gps]=info.direction;
+            Ang[Mag]=CmpRead()-info.declination; //Compass reading corrected by declination
+            Ang[Dir]=Path.Azimuth[0];
+
+            ChooseDir++;
+            if(ChooseDir<7)
+            {
+                showDirLcd(Mag);
+            }
+            else if(ChooseDir>=7 && ChooseDir<12)
+            {
+                showDirLcd(Gps);
+            }
+            else if(ChooseDir>=12)
+            {
+                ChooseDir=0;
+            }
+            showDirLcd(Dir);
+
+            Previous=4;    
+            break; 
+            
+        default:
+            showInfoLcd();
+            break;
+    }
+    //restart timer for other options
+    os.SetTimer(SHOW_LCD_TMR, SHOW_LCD_TIMER, 0); 
+ }
+}
+
+void showDirLcd(int Indx)
+{/**
+ *\brief  display a sort of compass on LCD
+ */
+    int Angle;
+
+    Angle=Ang[Indx]/18;
+
+    lcd.locate(CmpPos[Indx],0);
+    lcd.printf("  %c  ",0xA5);
+    lcd.locate(CmpPos[Indx],1);
+    lcd.printf("%c   %c",0xA5, 0xA5);
+    lcd.locate(CmpPos[Indx],2);
+    lcd.printf("%c   %c",0xA5, 0xA5);
+    lcd.locate(CmpPos[Indx],3);
+    lcd.printf("  %c  ",0xA5);
+    
+    lcd.locate(CmpPos[Indx]+1,1);
+    lcd.printf(Lab[Indx]);
+    lcd.locate(CmpPos[Indx]+1,2);
+    lcd.printf("%03i",Ang[Indx]);
+        
+    switch (Angle)
+    {
+        case 0:
+            lcd.locate(CmpPos[Indx]+2,0);
+            lcd.printf("%c",0xFF);
+            break;
+        case 1:
+            lcd.locate(CmpPos[Indx]+2,0);
+            lcd.printf("%c",0xFF);
+            lcd.locate(CmpPos[Indx]+3,0);
+            lcd.printf("%c",0xFF);
+            break;
+        case 2:
+            lcd.locate(CmpPos[Indx]+3,0);
+            lcd.printf("%c",0xFF);
+            break;        
+        case 3:
+            lcd.locate(CmpPos[Indx]+3,0);
+            lcd.printf("%c",0xFF);
+            lcd.locate(CmpPos[Indx]+4,1);
+            lcd.printf("%c",0xFF);
+            break;        
+        case 4:
+            lcd.locate(CmpPos[Indx]+4,1);
+            lcd.printf("%c",0xFF);
+            break;
+        case 5:       
+            lcd.locate(CmpPos[Indx]+4,1);
+            lcd.printf("%c",0xFF);
+            lcd.locate(CmpPos[Indx]+4,2);
+            lcd.printf("%c",0xFF);
+            break;
+        case 6:   
+            lcd.locate(CmpPos[Indx]+4,2);
+            lcd.printf("%c",0xFF);
+            break;
+        case 7:       
+            lcd.locate(CmpPos[Indx]+4,2);
+            lcd.printf("%c",0xFF);
+            lcd.locate(CmpPos[Indx]+3,3);
+            lcd.printf("%c",0xFF);
+            break;
+        case 8:   
+            lcd.locate(CmpPos[Indx]+3,3);
+            lcd.printf("%c",0xFF);
+            break;
+        case 9:       
+            lcd.locate(CmpPos[Indx]+3,3);
+            lcd.printf("%c",0xFF);
+            lcd.locate(CmpPos[Indx]+2,3);
+            lcd.printf("%c",0xFF);
+            break;
+        case 10:   
+            lcd.locate(CmpPos[Indx]+2,3);
+            lcd.printf("%c",0xFF);
+            break;     
+        case 11:       
+            lcd.locate(CmpPos[Indx]+2,3);
+            lcd.printf("%c",0xFF);
+            lcd.locate(CmpPos[Indx]+1,3);
+            lcd.printf("%c",0xFF);
+            break;
+        case 12:   
+            lcd.locate(CmpPos[Indx]+1,3);
+            lcd.printf("%c",0xFF);
+            break;     
+        case 13:       
+            lcd.locate(CmpPos[Indx]+1,3);
+            lcd.printf("%c",0xFF);
+            lcd.locate(CmpPos[Indx]+0,2);
+            lcd.printf("%c",0xFF);
+            break;
+        case 14:   
+            lcd.locate(CmpPos[Indx]+0,2);
+            lcd.printf("%c",0xFF);
+            break;  
+        case 15:       
+            lcd.locate(CmpPos[Indx]+0,2);
+            lcd.printf("%c",0xFF);
+            lcd.locate(CmpPos[Indx]+0,1);
+            lcd.printf("%c",0xFF);
+            break;
+        case 16:   
+            lcd.locate(CmpPos[Indx]+0,1);
+            lcd.printf("%c",0xFF);
+            break;  
+        case 17:       
+            lcd.locate(CmpPos[Indx]+0,1);
+            lcd.printf("%c",0xFF);
+            lcd.locate(CmpPos[Indx]+1,0);
+            lcd.printf("%c",0xFF);
+            break;   
+        case 18:   
+            lcd.locate(CmpPos[Indx]+1,0);
+            lcd.printf("%c",0xFF);
+            break;  
+        case 19:       
+            lcd.locate(CmpPos[Indx]+1,0);
+            lcd.printf("%c",0xFF);
+            lcd.locate(CmpPos[Indx]+2,0);
+            lcd.printf("%c",0xFF);
+            break;   
+    }
+}
+
+void showMenuLcd(void)
+{/**
+ *\brief  display a selection menu on LCD
+ */
+    lcd.locate(0,0);
+    lcd.printf("1 - item 1");
+    lcd.locate(0,1);
+    lcd.printf("2 - item 2");
+    lcd.locate(0,2);
+    lcd.printf("3 - item 3");
+    lcd.locate(0,3);
+    lcd.printf("4 - item 4");    
+}
+
+void showMenuLcd1(void)
+{/**
+ *\brief  display a selection menu on LCD
+ */
+    lcd.locate(0,0);
+    lcd.printf("5 - item 5");
+    lcd.locate(0,1);
+    lcd.printf("6 - item 6");
+    lcd.locate(0,2);
+    lcd.printf("7 - item 7");
+    lcd.locate(0,3);
+    lcd.printf("8 - item 8");    
+}
+
+void showSatLcd(void) 
+{/**
+ *\brief displays satellite informations
+ */
+    for (int i = 0; i < NMEA_MAXSAT; i++) 
+    {
+        if(info.satinfo.sat[i].id < 20)
+        {
+            lcd.locate((info.satinfo.sat[i].id % 20) - 1,0);
+        }
+        else
+        {
+            lcd.locate((info.satinfo.sat[i].id % 20) - 1,1);    
+        }
+        
+        if (info.satinfo.sat[i].sig > 0)
+        {
+            lcd.printf("%d",info.satinfo.sat[i].sig/10);
+        }
+        else
+        {
+            lcd.printf(" ");    
+        }                   
+    }
+    lcd.locate(0,2);
+    lcd.printf("12345678901234567890");
+    lcd.locate(0, 3);
+    lcd.printf("P%2.1f H%2.1f V%2.1f %iD",
+    info.PDOP, info.HDOP, info.VDOP,info.fix);
+}    
+    
+void showInfoLcd(void) 
+{/**
+ *\brief Show nmea info on LCD 
+ */
+
+//    static int lastSat = 0;
+//    int satInview = 0;
+
+ if(info.sig != 0)
+ { 
+    lcd.locate(0, 0);
+    lcd.printf("%2.5f%c %3.5f%c",
+            latitude, info.lat >= 0 ? 'N': 'S',  
+            longitude, info.lon >= 0 ? 'E': 'W');
+  
+    lcd.locate(0, 1);
+    lcd.printf("H%.0f D%.0f_%.1f %iD", info.elv, info.direction, info.declination, info.fix);
+
+    lcd.locate(0, 2);
+    lcd.printf("P%2.1f H%2.1f V%2.1f S%i/%i",
+        info.PDOP, info.HDOP, info.VDOP, info.satinfo.inuse, info.satinfo.inview);
+        
+   /* lcd.locate(0, 3);
+    lcd.printf("%02d-%02d-%04d %02d:%02d:%02d ", 
+        info.utc.day, info.utc.mon + 1, info.utc.year + 1900,
+        info.utc.hour, info.utc.min, info.utc.sec); // Display JST (UTC + 9)
+   */
+
+/*
+    lcd.locate(0, 5);
+    for (int i = 0; i < NMEA_MAXSAT; i++) {
+        if (info.satinfo.sat[i].sig > 0) {
+            satInview++;
+            lcd.printf("  sat_id:%02d, sig:%02d, Inuse:%d \n",
+                      info.satinfo.sat[i].id , info.satinfo.sat[i].sig,
+                      info.satinfo.sat[i].in_use);
+        }
+    }
+    for (int j = satInview; j <= lastSat; j++)
+        lcd.printf("                             \n");     // delete line
+    lastSat = satInview;
+ */
+ 
+ } 
+ else
+ {
+    lcd.cls();
+    lcd.locate(0,0);
+    lcd.printf("NO FIX"); 
+ }
+
+ // Grab a snapshot of the current RTC time.
+ time_t seconds = time(NULL);
+ char buffer[32];
+ if((info.sig != 0)&&(SetTimeOk!=0))  
+ {
+    strftime(buffer, 32, "%x %X", localtime(&seconds));
+ }
+ else
+ {// if GPS time not valid doesn't display seconds
+     strftime(buffer, 32, "%x %H:%M", localtime(&seconds));
+     SetTimeOk = 0; // RTC was not set to a valid time 
+ }
+ lcd.locate(0,3);
+ lcd.printf("%s", buffer);
+}
+
+void SetTimeTask(void)
+{/**
+ *\brief TASK 5, Set RTC system time if the GPS time is valid
+ */
+ struct tm t;
+ 
+ os.SetTimer(SET_TIME_TMR, 60000, 60000);
+ while(1)
+ {  
+ os.WaitEvent(SET_TIME_EVT);
+
+    if(info.sig != 0)    
+    {
+       t.tm_mday=info.utc.day;
+       t.tm_mon=info.utc.mon;
+       t.tm_year=info.utc.year; 
+       t.tm_hour=info.utc.hour;
+       t.tm_min=info.utc.min;
+       t.tm_sec=info.utc.sec;        
+          
+        time_t seconds = mktime(&t);       
+        set_time(seconds);
+        
+        SetTimeOk = 1; // RTC was set 
+    }
+ }    
+}
+
+void LedBlinkTask(void)
+{/**
+ *\brief TASK 4
+         LED 1: Quick blink=NOT fix, Slow blink=fix OK  
+         LED 2: blinks proportionally to HDOP
+         LED 3: blinks proportionally to VDOP
+ */
+
+ static int LedCnt1=0;
+ //    static int LedCnt2=0;
+ int OnH=0;
+ int OnV=0;
+
+ #define MAX 20
+  
+ os.SetTimer(LED_BLINK_TMR, 100, 100);
+
+ while(1)
+ {
+    os.WaitEvent(LED_BLINK_EVT);
+            
+    if((info.HDOP>0)&&(info.HDOP<=2))
+    {
+        OnH=2;
+    }
+    else if((info.HDOP>2)&&(info.HDOP<=4))
+    {
+        OnH=4;    
+    }
+    else if((info.HDOP>4)&&(info.HDOP<=6))
+    {
+        OnH=6;    
+    }
+    else if((info.HDOP>6))
+    {
+        OnH=MAX/2;    
+    }        
+
+    if((info.VDOP>0)&&(info.VDOP<=2))
+    {
+        OnV=2;
+    }
+    else if((info.VDOP>2)&&(info.VDOP<=4))
+    {
+        OnV=4;    
+    }
+    else if((info.VDOP>4)&&(info.VDOP<=6))
+    {
+        OnV=6;    
+    }
+    else if((info.VDOP>6))
+    {
+        OnV=MAX/2;    
+    } 
+           
+    if(info.sig == 0)
+    {
+        led1=!led1;
+        led2=0;
+        led3=0;
+    }
+    else
+    {
+       if(LedCnt1<=MAX/2)
+       {
+            LedCnt1++;
+            led1=0;
+            if(LedCnt1<OnH)
+            {
+               led2=!led2; 
+            }
+            else 
+            {
+                led2=0;
+            }    
+            
+            if(LedCnt1<OnV)
+            {
+               led3=!led3; 
+            }
+            else 
+            {
+                led3=0;
+            }          
+       } 
+       else if((LedCnt1>MAX/2)&&(LedCnt1<=MAX))
+       {
+            LedCnt1++;
+            led1=1;
+            led2=0;
+            led3=0;
+       }
+       else if(LedCnt1>MAX)
+       {
+            LedCnt1=0;       
+       }
+    }
+ }
+}
+
+
+void KeypadTask(void)
+{/**
+ *\brief TASK 2, Keypad management. 
+         It uses just one ADC port. Five keys switch a 5 resistor ladder
+         obtaining a unique voltage for each key, with a priority that 
+         depends from the position of the resistor in the ladder.
+         This function makes an average of 50 analog values (50ms) before 
+         confirming the output, to filter out some noise and debounce keys
+ */
+
+    static float KeypadAcc;         // accumulator to compute average
+    static float KeypadPrev;        // previous value to compute variation 
+    static int KeypadCount;         // all samples number
+    static int KeypadRealCount;     // valid samples only
+    float KeypadVal;  
+    static char KeyPrev='-';
+    
+    os.SetTimer(KEYPAD_TMR, 1, 1);
+    while(1)
+    {
+        os.WaitEvent(KEYPAD_EVT);
+        float InValue = KeypadIn.read();
+        if ((InValue > 0.3) && (abs(InValue - KeypadPrev) < 0.1))
+        {// makes the average only of the values above a threshold 
+         // and within an almost stable range
+            KeypadAcc+=InValue;
+            KeypadRealCount++;
+        }
+            
+        KeypadCount++;
+        KeypadPrev=InValue;
+    
+        if (KeypadCount >=50)
+        {
+            if(KeypadRealCount > 25)
+            {
+                KeypadVal=KeypadAcc/KeypadRealCount;
+            }
+            else
+            {// not enough values to average
+    
+                KeypadVal=0;
+            }
+           
+            KeypadAcc=0;
+            KeypadCount=0;
+            KeypadRealCount=0;
+         
+            if(KeypadVal <0.15)
+            {
+                Key='-';
+            }
+            else if(KeypadVal>=0.3 && KeypadVal<0.35)
+            {
+                Key='E';
+            }
+            else if(KeypadVal>=0.42 && KeypadVal<0.50)
+            {
+                Key='v';
+            }
+            else if(KeypadVal>=0.60 && KeypadVal<0.65)
+            {
+                Key='^';
+            }
+            else if(KeypadVal>=0.74 && KeypadVal<0.78)
+            {
+                Key='>';
+            }        
+            else if(KeypadVal>=0.85)
+            {
+                Key='<';
+            }
+            
+            if (Key!='-' && Key!=KeyPrev)
+            {// switch on the LCD backlight if key pressed
+                os.SetEvent(LCD_LIGHT_DIM_ON_EVT, LCD_LIGHT_DIM_TASK);
+            }
+            KeyPrev=Key;
+            switch (Key)
+            {
+                case '^':
+                    Menu=0;
+                break;
+     
+                case '>':
+                    Menu=1;
+                break;           
+     
+                case 'v':
+                    Menu=2;
+                break;
+                
+                case '<':
+                    Menu=3;
+                break;
+                
+                case 'E':
+                    Menu=4;
+                break;
+            }     
+       }
+   } 
+}
+
+void trace_h(const char *str, int str_size) 
+{/**
+ *\brief output on console what's received on GPS serial
+          Callback function for NMEA parser buffer trace
+ */
+    if( PcMonitor==5 || PcMonitor>5)
+    {
+        for (int i = 0; i < str_size; i++)
+        {
+            pc.putc(*str++);
+        }        
+    }
+}
+
+void error_h(const char *str, int str_size) 
+{/**
+ *\brief Callback function for NMEA parser error
+ */
+    for (int i = 0; i < str_size; i++)
+    {
+        pc.putc(*str++);
+    }    
+}
+
+void GpsSerialTask(void) 
+{/**
+ *\brief TASK 1, wait for the event then get the input char
+ */
+ while(1)
+ {
+    os.WaitEvent(GPS_SERIAL_IN_EVT);
+    GpsStringParse();
+            
+    Dest.lat= 4151.32496; // ***************debug
+    Dest.lon= 1229.34; // *************debug
+
+    GpsDist();
+ }
+}
+
+void GpsSerialIsr(void) 
+{/**
+ *\brief Interrupt handler for serial Rx
+         set the event for the serial task
+ */
+    char c = gps.getc();
+    msgBuff[bufferSelect & 1][writePointer] = c;
+    if (writePointer++ == BUFF_SIZE)
+    {
+        writePointer = 0;
+    }
+    if (writePointer > 200) 
+    {// GPS input buffer full, start computing coordinates
+        os.SetEvent(GPS_SERIAL_IN_EVT, GPS_SERIAL_TASK);
+    }
+}
+
+double trunc(double v) 
+{/**
+ *\brief Return nearest integer vaule less than input
+ *
+ *\parameters double variable to get nearest ingeger
+ *
+ *\return double 
+ */
+    if(v < 0.0) 
+    {
+        v*= -1.0;
+        v = floor(v);
+        v*=-1.0;
+    } else {
+        v = floor(v);
+    }
+    return v;
+}