File content as of revision 29:0ea07cc7124b:

#include "mbed.h"
#include "sx1276-hal.h"
#include "main.h"
#include "debug.h"
#include "vt100.h"
#include "serial_api.h"
#include "GPS.h"
#include <math.h>
#define PI 3.14159265

void start_hello(void)
{
    int len = get_kbd_str(pcbuf, PCBUF_SIZE);
    printf("in start gps/hello\r\n");
    if (len < 0) {
        fflush(stdout);
        Radio.Rx( RX_TIMEOUT_VALUE );
        Radio.Tx( TX_TIMEOUT_VALUE );
        Radio.Sleep( );
        if (gpsEnabled)
            debug("GPS exchange ended\r\n");
        else
            debug("Hello exchange ended\r\n");    
        app = APP_CONSOLE;
        print_help();
        printf("> ");
        fflush(stdout);
        return;
    }
    else
    {   
        hello();
    }
}

void find_distance()
{
    //float dist = 0;
    float lat2 = 0;
    float lon2 = 0;
    float lat1 = 0;
    float lon1 = 0;
    float dLat = 0;
    float dLon = 0;
    float c = 0;
    float a = 0;
    float d = 0;
    
    gpsd.distance = 0;
    if (gpsd.l_latitude == 0.0)
    {
        gpsd.distance = -1;
        return;
    }
    if (gpsd.l_longitude == 0.0)
    {
        gpsd.distance = -1;
        return;
    }
   
    //lat1 = 35.73244;
    //lon1 = -78.79333;
    
    lat1 = gpsd.r_latitude;
    lon1 = gpsd.r_longitude;
        
    lat2 = gpsd.l_latitude;
    lon2 = gpsd.l_longitude;
    //Calculate Distance
    dLat = (lat2-lat1)*(PI/180);
    dLon = (lon2-lon1)*(PI/180);
    a = sin(dLat/2) * sin(dLat/2) +
    sin(dLon/2) * sin(dLon/2) * cos(lat1*(PI/180)) * cos(lat2*(PI/180));
    c = 2 * atan2(sqrt(a), sqrt(1-a));
    d = 6371000 * c;
 
    //GPS is only precise to 5 meters, so throw out bad data
    if ( d <= 5) {
       d = 0;
    }
    gpsd.distance = gpsd.distance + d;
   // printf("distance: %f\r\n", gpsd.distance);
  
}
 
void check_gps(void)
{
    static int gps_count=0;
    
    if (gps_count >= 0)
    {
        //printf("checking gps\r\n");
        gps_count=0;  // reset for next cycle
        if(gpsd.sample()) 
        {
            led = !led;   
            static int swap=0;
            //printf("gps sample is true \r\n");
            if (gpsd.l_latitude != 0)
            {
                if (RADIO_INSTALLED)
                {
                    cLCD.setCursor(0,1);
                    cLCD.printf("d:NA per:%d      ", per);
                    //cLCD.printf("%0.4f %0.4f", gpsd.l_latitude, gpsd.l_longitude);
                }    
                else
                {
                    if (swap)
                    {
                    cLCD.setCursor(0,0);
                    cLCD.printf("Lat: %0.6f ", gpsd.l_latitude);
                    cLCD.setCursor(0,1);
                    cLCD.printf("Lon: %0.6f ", gpsd.l_longitude);
                    swap = 0;
                    }
                    else
                    {
                    cLCD.setCursor(0,1);
                    cLCD.printf("Lat: %0.6f ", gpsd.l_latitude);
                    cLCD.setCursor(0,0);
                    cLCD.printf("Lon: %0.6f ", gpsd.l_longitude);
                    swap = 1;
                    }
                }

                if (gpsd.r_latitude != 0)
                {
                    find_distance();
                    if (gpsd.distance < 50000 )
                    {
                        if (gpsd.distance >= 0)
                            cLCD.setCursor(0,1);
                            cLCD.printf("d:%0.1fm per:%d      ", gpsd.distance,per);
                    }
                }
            }    
            else
            {
                cLCD.setCursor(0,1);
                cLCD.printf("No GPS. per:%d      ",per);
            }
        }
        else      
        {  
            //printf("gps sample is false \r\n");
            cLCD.setCursor(0,1);
            cLCD.printf("No GPS. per:%d      ",per);
        }
    }
    else
    {
        #if 0
        if (gps_count % 5)
            gpsd.flushSerialBuffer();
        #endif
        //printf("not checking gps\r\n");
        gps_count++;
    }
       
}

void hello(void)
{
    srand ( randomSeed() );
    // generate a random wait time
    int mywait = ((rand()%2 +1));
    int i=0;
    int rcvd=0;
    int lost=0;
 
    for (i=0;i<max_pkts;i++)
    {
        
        if (pkt_data[i]==1)
        {
            //printf("rcvd, pkt count=%d\r\n",i);
            rcvd++;
        }
        else if (pkt_data[i] == 2)
        {
            //printf("lose, pkt count=%d\r\n",i);
            lost++;
        }
     
    }
    if ((rcvd + lost) > 0)
    {
        per = 100 - (rcvd*100 / (rcvd + lost));
        //printf("rcvd = %d, lost = %d per = %d\r\n",rcvd, lost, per);
    }
    else 
        per = 0;

 
    switch( State )
    {
  
        case RX_TIMEOUT:

            printf("rx timeout in gps/hello loop. ");
            printf("per=%d\r\n", per);
            gpsd.r_latitude = 0;
            gpsd.r_longitude = 0;
            // printf("rcv - rssi = %d snr = %d msg=%s\r\n", RssiValue, SnrValue, BufferRx);
            cLCD.setCursor(0,0);
            cLCD.printf("RI:NA SR:NA      ");
        case RX:
             
            if( BufferSize > 0 )
            {
                if( strncmp( ( const char* )BufferRx, ( const char* )HelloMsg, 5 ) == 0 )
                {
                    led = !led;     
                    printf("rcv - rssi = %d snr = %d msg=%s ", RssiValue, SnrValue, BufferRx );
                    printf("per=%d\r\n", per);
                    cLCD.setCursor(0,0);
                    cLCD.printf("RI:%d SR:%d   ", RssiValue, SnrValue);
                }
                else
                {
                  // float latitude, longitude;
                  if(sscanf((char *)BufferRx, "GP,%f,%f", &gpsd.r_latitude, &gpsd.r_longitude ) >= 1) 
                  {                    
                    led = !led;     
                    printf("rssi=%d snr=%d rlat=%0.5f rlon=%0.5f llat=%0.5f llon=%0.5f dst=%0.0fm ", RssiValue, SnrValue, gpsd.r_latitude, gpsd.r_longitude, gpsd.l_latitude, gpsd.l_longitude, gpsd.distance);
                    printf("per=%d\r\n", per);

                    cLCD.setCursor(0,0);
                    cLCD.printf("RI:%d SR:%d    ", RssiValue,SnrValue);
                    //printf("Msg received - Lat: %0.4f Lon: %0.4f\r\n", latitude, longitude);
                  }
                  else
                    printf("strange received message: %s\r\n", BufferRx);
                 } 
            }
       case RX_ERROR:
            if (gpsEnabled)
            {
                check_gps();
            }   
            if (gpsEnabled)
                sprintf((char *)BufferTx, "GP,%4.5f,%4.5f", gpsd.l_latitude, gpsd.l_longitude);
            else
                strcpy( ( char* )BufferTx, ( char* )HelloMsg );
               
            BufferSize=strlen((char *)BufferTx);
            //printf("Sending new buffer\r\n");
            Radio.Send( BufferTx, BufferSize );
            State = LOWPOWER;

            break;
        case TX:    
            //printf("tx state\r\n");
            //wait_ms (30);
 
            Radio.Rx( RX_TIMEOUT_VALUE / mywait  );
            State = LOWPOWER;
            break;
         case TX_TIMEOUT:
            debug("tx timeout\r\n");
            Radio.Tx( TX_TIMEOUT_VALUE );
            State = LOWPOWER;
            break;
        case LOWPOWER:
            wait_ms (5);
            break;
        default:
            debug("state is set to low power\r\n");
            State = LOWPOWER;
            break;
    }    
}