File content as of revision 1:360c4a23cb1d:

/* ////////////////////////////////////////////////////////////////////////////
** 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.

    LeonardoMbos 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: 
            ShowPathLcd();
            Previous=4;    
            break; 
            
        default:
            showInfoLcd();
            break;
    }
    //restart timer for other options
    os.SetTimer(SHOW_LCD_TMR, SHOW_LCD_TIMER, 0); 
 }
}

void ShowPathLcd(void)
{
    static int ChooseDir=0;
    DegMinSec DecCoord;

    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);
    
    Deg2DegMinSec(nmea_ndeg2degree(Dest.lat), &DecCoord);
    lcd.locate(6,0);
    lcd.printf("%d%d\'%.0f", DecCoord.Deg, DecCoord.Min, DecCoord.Sec);
    lcd.printf("%c", Dest.lat >= 0 ? 'N': 'S');  
    
    Deg2DegMinSec(nmea_ndeg2degree(Dest.lon), &DecCoord);
    lcd.locate(6,1);
    lcd.printf("%d%d\'%.0f", DecCoord.Deg, DecCoord.Min, DecCoord.Sec);
    lcd.printf("%c", Dest.lon >= 0 ? 'E': 'W');  
    
    lcd.locate(6,2);
    lcd.printf(" dist m");
    lcd.locate(6,3);
    lcd.printf("%4.3f",Path.Dist);
    

}

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;
}