CaryCoders / Mbed 2 deprecated demo_SX1276_standalone

standalone sx1276 demo program

Dependencies:   SX1276Lib mbed

Fork of SX1276_GPS by CaryCoders

utilities.cpp@32:a2472bbe7c92, 2015-07-29 (annotated)

Committer:
ftagius
Date:
Wed Jul 29 12:14:42 2015 +0000
Revision:
32:a2472bbe7c92
Parent:
31:2c813f321db7
Child:
33:319cbac3b6eb
cleanup

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ftagius 29:0ea07cc7124b 1 #include "mbed.h"
ftagius 29:0ea07cc7124b 2 #include "sx1276-hal.h"
ftagius 29:0ea07cc7124b 3 #include "main.h"
ftagius 29:0ea07cc7124b 4 #include "debug.h"
ftagius 29:0ea07cc7124b 5 #include "serial_api.h"
ftagius 29:0ea07cc7124b 6
ftagius 32:a2472bbe7c92 7 void gps_setup(void)
ftagius 32:a2472bbe7c92 8 {
ftagius 32:a2472bbe7c92 9 printf("setting up gps\r\n");
ftagius 32:a2472bbe7c92 10 gpsd.setBaud(9600);
ftagius 32:a2472bbe7c92 11 gpsd.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA);
ftagius 32:a2472bbe7c92 12 gpsd.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
ftagius 32:a2472bbe7c92 13 gpsd.sendCommand(PGCMD_NOANTENNA);
ftagius 32:a2472bbe7c92 14 gpsd.day=01;
ftagius 32:a2472bbe7c92 15 gpsd.month=01;
ftagius 32:a2472bbe7c92 16 gpsd.year=15;
ftagius 32:a2472bbe7c92 17 gpsd.hour=1;
ftagius 32:a2472bbe7c92 18 gpsd.minute=1;
ftagius 32:a2472bbe7c92 19 gpsd.seconds=1;
ftagius 32:a2472bbe7c92 20 }
ftagius 32:a2472bbe7c92 21
ftagius 29:0ea07cc7124b 22 int get_kbd_str(char* buf, int size)
ftagius 29:0ea07cc7124b 23 {
ftagius 29:0ea07cc7124b 24 char c;
ftagius 29:0ea07cc7124b 25 int i;
ftagius 29:0ea07cc7124b 26 static int prev_len;
ftagius 29:0ea07cc7124b 27
ftagius 29:0ea07cc7124b 28 for (i = 0;;) {
ftagius 29:0ea07cc7124b 29 if (pc.readable()) {
ftagius 29:0ea07cc7124b 30 c = pc.getc();
ftagius 29:0ea07cc7124b 31 if (c == 8 && i > 0) {
ftagius 29:0ea07cc7124b 32 pc.putc(8);
ftagius 29:0ea07cc7124b 33 pc.putc(' ');
ftagius 29:0ea07cc7124b 34 pc.putc(8);
ftagius 29:0ea07cc7124b 35 i--;
ftagius 29:0ea07cc7124b 36 } else if (c == '\r') {
ftagius 29:0ea07cc7124b 37 if (i == 0) {
ftagius 29:0ea07cc7124b 38 if (app == APP_CHAT)
ftagius 29:0ea07cc7124b 39 return 0;
ftagius 29:0ea07cc7124b 40 else
ftagius 29:0ea07cc7124b 41 return prev_len; // repeat previous
ftagius 29:0ea07cc7124b 42 } else {
ftagius 29:0ea07cc7124b 43 buf[i] = 0; // null terminate
ftagius 29:0ea07cc7124b 44 prev_len = i;
ftagius 29:0ea07cc7124b 45 return i;
ftagius 29:0ea07cc7124b 46 }
ftagius 29:0ea07cc7124b 47 } else if (c == 3) {
ftagius 29:0ea07cc7124b 48 // ctrl-C abort
ftagius 29:0ea07cc7124b 49 return -1;
ftagius 29:0ea07cc7124b 50 } else if (i < size) {
ftagius 29:0ea07cc7124b 51 buf[i++] = c;
ftagius 29:0ea07cc7124b 52 pc.putc(c);
ftagius 29:0ea07cc7124b 53 }
ftagius 32:a2472bbe7c92 54 }
ftagius 32:a2472bbe7c92 55
ftagius 32:a2472bbe7c92 56 else {
ftagius 32:a2472bbe7c92 57 #if 0
ftagius 32:a2472bbe7c92 58 return 0;
ftagius 32:a2472bbe7c92 59 #else
ftagius 29:0ea07cc7124b 60 if (app == APP_PING)
ftagius 29:0ea07cc7124b 61 ping_pong();
ftagius 29:0ea07cc7124b 62 if (app == APP_HELLO)
ftagius 32:a2472bbe7c92 63 {
ftagius 32:a2472bbe7c92 64 // if (check_gps() == 0)
ftagius 32:a2472bbe7c92 65 // {
ftagius 32:a2472bbe7c92 66 return 0;
ftagius 32:a2472bbe7c92 67 //hello();
ftagius 32:a2472bbe7c92 68 // }
ftagius 32:a2472bbe7c92 69 }
ftagius 32:a2472bbe7c92 70
ftagius 29:0ea07cc7124b 71 if (app == APP_GPS)
ftagius 29:0ea07cc7124b 72 check_gps();
ftagius 29:0ea07cc7124b 73 #if 1
ftagius 29:0ea07cc7124b 74 if (app == APP_CHAT)
ftagius 29:0ea07cc7124b 75 {
ftagius 29:0ea07cc7124b 76 // printf("utility call to console chat\r\n");
ftagius 29:0ea07cc7124b 77 check_rx_chat();
ftagius 29:0ea07cc7124b 78 }
ftagius 29:0ea07cc7124b 79 #endif
ftagius 32:a2472bbe7c92 80 #endif
ftagius 29:0ea07cc7124b 81 }
ftagius 32:a2472bbe7c92 82
ftagius 29:0ea07cc7124b 83 } // ...for()
ftagius 29:0ea07cc7124b 84 }
ftagius 32:a2472bbe7c92 85 void OnTxTimeout( void )
ftagius 32:a2472bbe7c92 86 {
ftagius 32:a2472bbe7c92 87 Radio.Sleep( );
ftagius 32:a2472bbe7c92 88 State = TX_TIMEOUT;
ftagius 32:a2472bbe7c92 89 //debug_if( DEBUG_MESSAGE, "> OnTxTimeout\r\n" );
ftagius 32:a2472bbe7c92 90 }
ftagius 32:a2472bbe7c92 91
ftagius 32:a2472bbe7c92 92 void OnRxTimeout( void )
ftagius 32:a2472bbe7c92 93 {
ftagius 32:a2472bbe7c92 94 Radio.Sleep( );
ftagius 32:a2472bbe7c92 95 int i;
ftagius 32:a2472bbe7c92 96 if (pkt_count >= max_pkts)
ftagius 32:a2472bbe7c92 97 {
ftagius 32:a2472bbe7c92 98 pkt_count=0;
ftagius 32:a2472bbe7c92 99 }
ftagius 32:a2472bbe7c92 100
ftagius 32:a2472bbe7c92 101 // mark the rolling avg that a pkt has not been received
ftagius 32:a2472bbe7c92 102 pkt_data[pkt_count]=2;
ftagius 32:a2472bbe7c92 103
ftagius 32:a2472bbe7c92 104 for (i=0; i< BUFFER_SIZE; i++)
ftagius 32:a2472bbe7c92 105 BufferRx[ i ] = '\0'; // clear the buffer
ftagius 32:a2472bbe7c92 106 BufferSize = 0;
ftagius 32:a2472bbe7c92 107 State = RX_TIMEOUT;
ftagius 32:a2472bbe7c92 108 rxTimeout = false;
ftagius 32:a2472bbe7c92 109 // debug_if( DEBUG_MESSAGE, "> OnRxTimeout\r\n" );
ftagius 32:a2472bbe7c92 110 }
ftagius 32:a2472bbe7c92 111
ftagius 32:a2472bbe7c92 112 void OnRxError( void )
ftagius 32:a2472bbe7c92 113 {
ftagius 32:a2472bbe7c92 114 Radio.Sleep( );
ftagius 32:a2472bbe7c92 115 if (pkt_count >= max_pkts)
ftagius 32:a2472bbe7c92 116 {
ftagius 32:a2472bbe7c92 117 pkt_count=0;
ftagius 32:a2472bbe7c92 118 }
ftagius 32:a2472bbe7c92 119
ftagius 32:a2472bbe7c92 120 // mark the rolling avg that a pkt has not been received
ftagius 32:a2472bbe7c92 121 pkt_data[pkt_count]=2;
ftagius 32:a2472bbe7c92 122 State = RX_ERROR;
ftagius 32:a2472bbe7c92 123 debug_if( DEBUG_MESSAGE, "> OnRxError\r\n" );
ftagius 32:a2472bbe7c92 124 }
ftagius 32:a2472bbe7c92 125
ftagius 32:a2472bbe7c92 126
ftagius 32:a2472bbe7c92 127 char* itoa(int val, int base){
ftagius 32:a2472bbe7c92 128
ftagius 32:a2472bbe7c92 129 static char buf[32] = {0};
ftagius 32:a2472bbe7c92 130
ftagius 32:a2472bbe7c92 131 int i = 30;
ftagius 32:a2472bbe7c92 132
ftagius 32:a2472bbe7c92 133 for(; val && i ; --i, val /= base)
ftagius 32:a2472bbe7c92 134
ftagius 32:a2472bbe7c92 135 buf[i] = "0123456789abcdef"[val % base];
ftagius 32:a2472bbe7c92 136
ftagius 32:a2472bbe7c92 137 return &buf[i+1];
ftagius 32:a2472bbe7c92 138
ftagius 32:a2472bbe7c92 139 }
ftagius 32:a2472bbe7c92 140
ftagius 32:a2472bbe7c92 141 unsigned int randomSeed(){
ftagius 32:a2472bbe7c92 142 AnalogIn randIn(A0); // analog input for random number seed
ftagius 32:a2472bbe7c92 143 unsigned int rnum = 0; // int = 4 bytes - 32 bits
ftagius 32:a2472bbe7c92 144 // build 32 bit random number from the 2 lower bits from 16 analog reads
ftagius 32:a2472bbe7c92 145 for (int i = 0; i<4;i++){
ftagius 32:a2472bbe7c92 146 rnum = rnum | ((randIn.read_u16() & 15) << i*4);
ftagius 32:a2472bbe7c92 147 wait_ms(5);
ftagius 32:a2472bbe7c92 148 }
ftagius 32:a2472bbe7c92 149 return rnum;
ftagius 32:a2472bbe7c92 150 }
ftagius 32:a2472bbe7c92 151
ftagius 32:a2472bbe7c92 152 void configRxTx() {
ftagius 32:a2472bbe7c92 153
ftagius 32:a2472bbe7c92 154
ftagius 32:a2472bbe7c92 155 Radio.SetTxConfig( MODEM_LORA, TxPower, 0, Bandwidth,
ftagius 32:a2472bbe7c92 156 SpreadingFactor, CodingRate,
ftagius 32:a2472bbe7c92 157 LORA_PREAMBLE_LENGTH, LORA_FIX_LENGTH_PAYLOAD_ON,
ftagius 32:a2472bbe7c92 158 LORA_CRC_ENABLED, LORA_FHSS_ENABLED, LORA_NB_SYMB_HOP,
ftagius 32:a2472bbe7c92 159 LORA_IQ_INVERSION_ON, 2000000 );
ftagius 32:a2472bbe7c92 160
ftagius 32:a2472bbe7c92 161 Radio.SetRxConfig( MODEM_LORA, Bandwidth, SpreadingFactor,
ftagius 32:a2472bbe7c92 162 CodingRate, 0, LORA_PREAMBLE_LENGTH,
ftagius 32:a2472bbe7c92 163 LORA_SYMBOL_TIMEOUT, LORA_FIX_LENGTH_PAYLOAD_ON, 0,
ftagius 32:a2472bbe7c92 164 LORA_CRC_ENABLED, LORA_FHSS_ENABLED, LORA_NB_SYMB_HOP,
ftagius 32:a2472bbe7c92 165 LORA_IQ_INVERSION_ON, true );
ftagius 32:a2472bbe7c92 166 }
ftagius 32:a2472bbe7c92 167
ftagius 32:a2472bbe7c92 168 void print_bandwidth()
ftagius 32:a2472bbe7c92 169 {
ftagius 32:a2472bbe7c92 170 printf("bandwidth: ");
ftagius 32:a2472bbe7c92 171 switch (((Radio.Read(REG_LR_MODEMCONFIG1) & 0xf0) >> 4)) {
ftagius 32:a2472bbe7c92 172 case 0: printf("7.8KHz "); break;
ftagius 32:a2472bbe7c92 173 case 1: printf("10.4KHz "); break;
ftagius 32:a2472bbe7c92 174 case 2: printf("15.6KHz "); break;
ftagius 32:a2472bbe7c92 175 case 3: printf("20.8KHz "); break;
ftagius 32:a2472bbe7c92 176 case 4: printf("31.25KHz "); break;
ftagius 32:a2472bbe7c92 177 case 5: printf("41.7KHz "); break;
ftagius 32:a2472bbe7c92 178 case 6: printf("62.5KHz "); break;
ftagius 32:a2472bbe7c92 179 case 7: printf("125KHz "); break;
ftagius 32:a2472bbe7c92 180 case 8: printf("250KHz "); break;
ftagius 32:a2472bbe7c92 181 case 9: printf("500KHz "); break;
ftagius 32:a2472bbe7c92 182 default: printf("%x ", Radio.Read(REG_LR_MODEMCONFIG1)); break;
ftagius 32:a2472bbe7c92 183 }
ftagius 32:a2472bbe7c92 184 //printf("\r\n");
ftagius 32:a2472bbe7c92 185 }
ftagius 32:a2472bbe7c92 186
ftagius 32:a2472bbe7c92 187 void print_cr()
ftagius 32:a2472bbe7c92 188 {
ftagius 32:a2472bbe7c92 189 int cr = (Radio.Read(REG_LR_MODEMCONFIG1) & 0x0f)>>1;
ftagius 32:a2472bbe7c92 190 printf("coding rate: ");
ftagius 32:a2472bbe7c92 191 switch (cr)
ftagius 32:a2472bbe7c92 192 {
ftagius 32:a2472bbe7c92 193 case 1:
ftagius 32:a2472bbe7c92 194 printf("4/5");
ftagius 32:a2472bbe7c92 195 break;
ftagius 32:a2472bbe7c92 196 case 2:
ftagius 32:a2472bbe7c92 197 printf("4/6");
ftagius 32:a2472bbe7c92 198 break;
ftagius 32:a2472bbe7c92 199 case 3:
ftagius 32:a2472bbe7c92 200 printf("4/7");
ftagius 32:a2472bbe7c92 201 break;
ftagius 32:a2472bbe7c92 202 case 4:
ftagius 32:a2472bbe7c92 203 printf("4/8");
ftagius 32:a2472bbe7c92 204 break;
ftagius 32:a2472bbe7c92 205 default:
ftagius 32:a2472bbe7c92 206 printf("unknown");
ftagius 32:a2472bbe7c92 207 break;
ftagius 32:a2472bbe7c92 208
ftagius 32:a2472bbe7c92 209 }
ftagius 32:a2472bbe7c92 210 printf(" ");
ftagius 32:a2472bbe7c92 211 }
ftagius 32:a2472bbe7c92 212
ftagius 32:a2472bbe7c92 213 void print_power()
ftagius 32:a2472bbe7c92 214 {
ftagius 32:a2472bbe7c92 215 uint8_t paConfig = 0;
ftagius 32:a2472bbe7c92 216 uint8_t paDac = 0;
ftagius 32:a2472bbe7c92 217
ftagius 32:a2472bbe7c92 218 paConfig = Radio.Read( REG_PACONFIG );
ftagius 32:a2472bbe7c92 219 paDac = Radio.Read( REG_PADAC );
ftagius 32:a2472bbe7c92 220
ftagius 32:a2472bbe7c92 221 paConfig = ( paConfig & RF_PACONFIG_PASELECT_MASK ) | Radio.GetPaSelect( Radio.GetChannel()*1000000);
ftagius 32:a2472bbe7c92 222 paConfig = ( paConfig & RF_PACONFIG_MAX_POWER_MASK ) | 0x70;
ftagius 32:a2472bbe7c92 223 printf("channel = %f getpa = %x, paConfig =%x \r\n",Radio.GetChannel(), Radio.GetPaSelect((uint32_t)Radio.GetChannel()*1000000), paConfig );
ftagius 32:a2472bbe7c92 224 if( ( paConfig & RF_PACONFIG_PASELECT_PABOOST ) == RF_PACONFIG_PASELECT_PABOOST )
ftagius 32:a2472bbe7c92 225 {
ftagius 32:a2472bbe7c92 226 if( ( paDac & RF_PADAC_20DBM_ON ) == RF_PADAC_20DBM_ON )
ftagius 32:a2472bbe7c92 227 {
ftagius 32:a2472bbe7c92 228 printf("Power: %d dBm ", ( uint8_t )( ( uint16_t )( paConfig) & 0x0f ) + 5 );
ftagius 32:a2472bbe7c92 229 }
ftagius 32:a2472bbe7c92 230 else
ftagius 32:a2472bbe7c92 231 printf("Power: %d dBm ", ( uint8_t )( ( uint16_t )( paConfig) &0x0f) + 2 );
ftagius 32:a2472bbe7c92 232 }
ftagius 32:a2472bbe7c92 233 else
ftagius 32:a2472bbe7c92 234 {
ftagius 32:a2472bbe7c92 235 printf("Power: %d dBm ", ( uint8_t )( ( uint16_t )( paConfig) & 0x0f ) - 1 );
ftagius 32:a2472bbe7c92 236 }
ftagius 32:a2472bbe7c92 237
ftagius 32:a2472bbe7c92 238 }
ftagius 32:a2472bbe7c92 239
ftagius 32:a2472bbe7c92 240 void print_errata()
ftagius 32:a2472bbe7c92 241 {
ftagius 32:a2472bbe7c92 242
ftagius 32:a2472bbe7c92 243 int seqconfig1 = (Radio.Read(REG_SEQCONFIG1));
ftagius 32:a2472bbe7c92 244 int timer2 = (Radio.Read(REG_TIMER2COEF));
ftagius 32:a2472bbe7c92 245 printf("seqconfig1 = 0x%02x\r\n", seqconfig1);
ftagius 32:a2472bbe7c92 246 printf("timer2coef = 0x%02x\r\n", timer2);
ftagius 32:a2472bbe7c92 247
ftagius 32:a2472bbe7c92 248 }
ftagius 32:a2472bbe7c92 249 void print_status()
ftagius 32:a2472bbe7c92 250 {
ftagius 32:a2472bbe7c92 251
ftagius 32:a2472bbe7c92 252 printf("Radio version: 0x%02x Channel: %.1f MHz ", \
ftagius 32:a2472bbe7c92 253 Radio.Read(REG_VERSION),\
ftagius 32:a2472bbe7c92 254 Radio.GetChannel()\
ftagius 32:a2472bbe7c92 255 );
ftagius 32:a2472bbe7c92 256 print_power();
ftagius 32:a2472bbe7c92 257 print_bandwidth(); printf("\r\n");
ftagius 32:a2472bbe7c92 258 printf("Spreading Factor: %d ",SpreadingFactor);
ftagius 32:a2472bbe7c92 259 print_cr();
ftagius 32:a2472bbe7c92 260 if (isMaster)
ftagius 32:a2472bbe7c92 261 printf("Mode: master ");
ftagius 32:a2472bbe7c92 262 else
ftagius 32:a2472bbe7c92 263 printf("Mode: slave ");
ftagius 32:a2472bbe7c92 264
ftagius 32:a2472bbe7c92 265
ftagius 32:a2472bbe7c92 266 printf("\r\n");
ftagius 32:a2472bbe7c92 267 print_errata();
ftagius 32:a2472bbe7c92 268 printf("\r\n");
ftagius 32:a2472bbe7c92 269
ftagius 32:a2472bbe7c92 270 }
ftagius 32:a2472bbe7c92 271
ftagius 32:a2472bbe7c92 272
ftagius 32:a2472bbe7c92 273 void OnTxDone( void )
ftagius 32:a2472bbe7c92 274 {
ftagius 32:a2472bbe7c92 275 Radio.Sleep( );
ftagius 32:a2472bbe7c92 276 State = TX;
ftagius 32:a2472bbe7c92 277 pkt_count++;
ftagius 32:a2472bbe7c92 278
ftagius 32:a2472bbe7c92 279
ftagius 32:a2472bbe7c92 280 //debug_if( DEBUG_MESSAGE, "> OnTxDone\r\n" );
ftagius 32:a2472bbe7c92 281 }
ftagius 32:a2472bbe7c92 282
ftagius 32:a2472bbe7c92 283 void OnRxDone( uint8_t *payload, uint16_t size, int16_t rssi, int8_t snr)
ftagius 32:a2472bbe7c92 284 {
ftagius 32:a2472bbe7c92 285 Radio.Sleep( );
ftagius 32:a2472bbe7c92 286 if (pkt_count >= max_pkts)
ftagius 32:a2472bbe7c92 287 {
ftagius 32:a2472bbe7c92 288 pkt_count=0;
ftagius 32:a2472bbe7c92 289 }
ftagius 32:a2472bbe7c92 290
ftagius 32:a2472bbe7c92 291 // mark the rolling avg that a pkt has been received
ftagius 32:a2472bbe7c92 292 pkt_data[pkt_count]=1;
ftagius 32:a2472bbe7c92 293 //printf("rx done, pkt_count = %d\r\n", pkt_count);
ftagius 32:a2472bbe7c92 294
ftagius 32:a2472bbe7c92 295 BufferSize = size;
ftagius 32:a2472bbe7c92 296 int i;
ftagius 32:a2472bbe7c92 297 for (i=0; i< BUFFER_SIZE; i++)
ftagius 32:a2472bbe7c92 298 BufferRx[ i ] = '\0'; // clear the buffer
ftagius 32:a2472bbe7c92 299
ftagius 32:a2472bbe7c92 300 memcpy( BufferRx, payload, BufferSize );
ftagius 32:a2472bbe7c92 301 if (rssi > 100)
ftagius 32:a2472bbe7c92 302 {
ftagius 32:a2472bbe7c92 303 rssi *= -1;
ftagius 32:a2472bbe7c92 304 }
ftagius 32:a2472bbe7c92 305 RssiValue = rssi;
ftagius 32:a2472bbe7c92 306 SnrValue = snr;
ftagius 32:a2472bbe7c92 307 State = RX;
ftagius 32:a2472bbe7c92 308 rxTimeout = false;
ftagius 32:a2472bbe7c92 309 if (app == APP_PING)
ftagius 32:a2472bbe7c92 310 debug_if( DEBUG_MESSAGE, "> OnRxDone size=%d rssi=%d snr=%d \r\n", size, rssi, snr );
ftagius 32:a2472bbe7c92 311
ftagius 32:a2472bbe7c92 312 }
ftagius 32:a2472bbe7c92 313
ftagius 32:a2472bbe7c92 314 void find_distance()
ftagius 32:a2472bbe7c92 315 {
ftagius 32:a2472bbe7c92 316 //float dist = 0;
ftagius 32:a2472bbe7c92 317 float lat2 = 0;
ftagius 32:a2472bbe7c92 318 float lon2 = 0;
ftagius 32:a2472bbe7c92 319 float lat1 = 0;
ftagius 32:a2472bbe7c92 320 float lon1 = 0;
ftagius 32:a2472bbe7c92 321 float dLat = 0;
ftagius 32:a2472bbe7c92 322 float dLon = 0;
ftagius 32:a2472bbe7c92 323 float c = 0;
ftagius 32:a2472bbe7c92 324 float a = 0;
ftagius 32:a2472bbe7c92 325 float d = 0;
ftagius 32:a2472bbe7c92 326
ftagius 32:a2472bbe7c92 327 distance = 0;
ftagius 32:a2472bbe7c92 328 if (gpsd.lat_deg == 0.0)
ftagius 32:a2472bbe7c92 329 {
ftagius 32:a2472bbe7c92 330 distance = -1;
ftagius 32:a2472bbe7c92 331 return;
ftagius 32:a2472bbe7c92 332 }
ftagius 32:a2472bbe7c92 333 if (gpsd.lon_deg == 0.0)
ftagius 32:a2472bbe7c92 334 {
ftagius 32:a2472bbe7c92 335 distance = -1;
ftagius 32:a2472bbe7c92 336 return;
ftagius 32:a2472bbe7c92 337 }
ftagius 32:a2472bbe7c92 338
ftagius 32:a2472bbe7c92 339 lat1 = r_latitude;
ftagius 32:a2472bbe7c92 340 lon1 = r_longitude;
ftagius 32:a2472bbe7c92 341
ftagius 32:a2472bbe7c92 342 lat2 = gpsd.lat_deg;
ftagius 32:a2472bbe7c92 343 lon2 = gpsd.lon_deg;
ftagius 32:a2472bbe7c92 344 //Calculate Distance
ftagius 32:a2472bbe7c92 345 dLat = (lat2-lat1)*(PI/180);
ftagius 32:a2472bbe7c92 346 dLon = (lon2-lon1)*(PI/180);
ftagius 32:a2472bbe7c92 347 a = sin(dLat/2) * sin(dLat/2) +
ftagius 32:a2472bbe7c92 348 sin(dLon/2) * sin(dLon/2) * cos(lat1*(PI/180)) * cos(lat2*(PI/180));
ftagius 32:a2472bbe7c92 349 c = 2 * atan2(sqrt(a), sqrt(1-a));
ftagius 32:a2472bbe7c92 350 d = 6371000 * c;
ftagius 32:a2472bbe7c92 351
ftagius 32:a2472bbe7c92 352 //GPS is only precise to 5 meters, so throw out bad data
ftagius 32:a2472bbe7c92 353 if ( d <= 5) {
ftagius 32:a2472bbe7c92 354 d = 0;
ftagius 32:a2472bbe7c92 355 }
ftagius 32:a2472bbe7c92 356 distance = distance + d;
ftagius 32:a2472bbe7c92 357 // printf("distance: %f\r\n", gpsd.distance);
ftagius 32:a2472bbe7c92 358
ftagius 32:a2472bbe7c92 359 }
ftagius 32:a2472bbe7c92 360
ftagius 32:a2472bbe7c92 361 int check_gps(void)
ftagius 32:a2472bbe7c92 362 {
ftagius 32:a2472bbe7c92 363
ftagius 32:a2472bbe7c92 364 char c;
ftagius 32:a2472bbe7c92 365 int debug=0;
ftagius 32:a2472bbe7c92 366 c = gpsd.read(); //queries the GPS
ftagius 32:a2472bbe7c92 367 if (debug)
ftagius 32:a2472bbe7c92 368 {
ftagius 32:a2472bbe7c92 369 if (c) {
ftagius 32:a2472bbe7c92 370 printf("%c", c); //this line will echo the GPS data if not paused
ftagius 32:a2472bbe7c92 371 return 1;
ftagius 32:a2472bbe7c92 372
ftagius 32:a2472bbe7c92 373 }
ftagius 32:a2472bbe7c92 374 }
ftagius 32:a2472bbe7c92 375
ftagius 32:a2472bbe7c92 376 //check if we recieved a new message from GPS, if so, attempt to parse it,
ftagius 32:a2472bbe7c92 377 if ( gpsd.newNMEAreceived() ) {
ftagius 32:a2472bbe7c92 378 if (r_latitude != 0)
ftagius 32:a2472bbe7c92 379 {
ftagius 32:a2472bbe7c92 380 find_distance();
ftagius 32:a2472bbe7c92 381 //printf("distance is %f\r\n",distance);
ftagius 32:a2472bbe7c92 382 if (distance < 50000 )
ftagius 32:a2472bbe7c92 383 {
ftagius 32:a2472bbe7c92 384 if (distance >= 0)
ftagius 32:a2472bbe7c92 385 {
ftagius 32:a2472bbe7c92 386 cLCD.setCursor(0,1);
ftagius 32:a2472bbe7c92 387 cLCD.printf("d:%0.1fm per:%d ", distance,per);
ftagius 32:a2472bbe7c92 388 }
ftagius 32:a2472bbe7c92 389 }
ftagius 32:a2472bbe7c92 390 }
ftagius 32:a2472bbe7c92 391
ftagius 32:a2472bbe7c92 392 if ( !gpsd.parse(gpsd.lastNMEA()) )
ftagius 32:a2472bbe7c92 393 {
ftagius 32:a2472bbe7c92 394 // radfta led = !led;
ftagius 32:a2472bbe7c92 395 #if 0
ftagius 32:a2472bbe7c92 396 if (gpsd.lat_deg != 0)
ftagius 32:a2472bbe7c92 397 {
ftagius 32:a2472bbe7c92 398 cLCD.setCursor(0,1);
ftagius 32:a2472bbe7c92 399 cLCD.printf("d:Nx per:%d ", per);
ftagius 32:a2472bbe7c92 400 cLCD.printf("%0.4f %0.4f", gpsd.lat_deg, gpsd.lon_deg);
ftagius 32:a2472bbe7c92 401 }
ftagius 32:a2472bbe7c92 402 #endif
ftagius 32:a2472bbe7c92 403
ftagius 32:a2472bbe7c92 404
ftagius 32:a2472bbe7c92 405
ftagius 32:a2472bbe7c92 406 return 0;
ftagius 32:a2472bbe7c92 407 }
ftagius 32:a2472bbe7c92 408 else
ftagius 32:a2472bbe7c92 409 {
ftagius 32:a2472bbe7c92 410 // printf("GPS %02d/%02d/20%02d_%02d:%02d:%02d (UTC)\r\n",gpsd.month,gpsd.day,gpsd.year,gpsd.hour,gpsd.minute,gpsd.seconds);
ftagius 32:a2472bbe7c92 411 return 1;
ftagius 32:a2472bbe7c92 412 }
ftagius 32:a2472bbe7c92 413
ftagius 32:a2472bbe7c92 414 }
ftagius 32:a2472bbe7c92 415 else
ftagius 32:a2472bbe7c92 416 {
ftagius 32:a2472bbe7c92 417 //printf("GPS %02d/%02d/20%02d_%02d:%02d:%02d (UTC)\r\n",gpsd.month,gpsd.day,gpsd.year,gpsd.hour,gpsd.minute,gpsd.seconds);
ftagius 32:a2472bbe7c92 418 return 0;
ftagius 32:a2472bbe7c92 419 }
ftagius 32:a2472bbe7c92 420
ftagius 32:a2472bbe7c92 421 }
ftagius 32:a2472bbe7c92 422