0,0 → 1,360 |
/***************************************************************************** |
* Copyright (C) 2010 Sebastian Boehm, seb@exse.net * |
* * |
* This program 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 2 of the License. * |
* * |
* This program 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 this program; if not, write to the * |
* Free Software Foundation, Inc., * |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
* * |
*****************************************************************************/ |
|
|
#include <avr/io.h> |
#include <inttypes.h> |
#include <stdlib.h> |
#include <avr/pgmspace.h> |
|
#include "main.h" |
#include "lcd.h" |
#include "timer.h" |
#include "usart.h" |
|
#define TIMEOUT 200 // 2 sec |
|
uint8_t ck_a = 0; |
uint8_t ck_b = 0; |
uint8_t UBX_class = 0; |
uint8_t UBX_id = 0; |
uint8_t UBX_buffer[250]; |
uint8_t UBX_payload_counter = 0; |
|
void checksum(uint8_t); |
void UBX_process(void); |
uint32_t join_4_bytes(uint8_t*); |
|
uint8_t display_mode = 0; |
|
void gps(void) |
{ |
lcd_cls(); |
display_mode = 2; |
|
if (hardware == FC) |
{ |
lcd_printp_at(0, 3, PSTR("Only with NC !"), 0); |
timer = 100; |
while (timer > 0); |
return; |
} |
|
|
if(current_hardware != NC) SwitchToNC(); |
|
SwitchToGPS(); |
|
uint8_t mode = 0; |
|
// SendOutData ('d', ADDRESS_ANY, 1, &tmp_dat, 1); |
|
timer = TIMEOUT; |
uint8_t data = 0; |
uint8_t length = 0; |
uint8_t UBX_ck_a = 0; |
do |
{ |
// if (rxFlag == 1) |
if (uart_getc_nb(&data)) |
{ |
//rxFlag = 0; |
//data = rx_byte; |
timer = TIMEOUT; |
|
switch(mode) |
{ |
case 0: // init 1 |
if(data == 0xB5) |
{ |
UBX_payload_counter = 0; |
UBX_id = 0; |
UBX_class = 0; |
ck_a = 0; |
ck_b = 0; |
mode++; |
} |
break; |
case 1: // init 2 |
if(data == 0x62) |
{ |
mode++; |
} |
else |
{ |
mode = 0; |
} |
break; |
case 2: //class |
if(data != 1) |
{ |
mode = 0; |
} |
else |
{ |
checksum(data); |
UBX_class = data; |
mode++; |
} |
break; |
case 3: // id |
if((data != 48)&&(data != 6)&&(data != 18)&&(data != 2)) |
{ |
mode = 0; |
} |
else |
{ |
UBX_id = data; |
checksum(data); |
mode++; |
} |
break; |
case 4: // length lo |
if(data > 250) |
{ |
mode = 0; |
} |
else |
{ |
checksum(data); |
length = data; |
mode++; |
} |
break; |
case 5: // length hi |
if(data != 0) |
{ |
mode = 0; |
} |
else |
{ |
checksum(data); |
mode++; |
} |
break; |
case 6: // length hi |
length--; |
UBX_buffer[UBX_payload_counter] = data; |
checksum(data); |
UBX_payload_counter++; |
if(length==0) |
{ |
mode++; |
}; |
break; |
case 7: // check lo |
mode++; |
UBX_ck_a = data; |
break; |
case 8: // check hi |
mode=0; |
if((UBX_ck_a == ck_a)&&(data == ck_b)) |
{ |
UBX_process(); |
} |
} |
|
|
// write_ndigit_number_u (14, 0, data, 3, 0); |
} |
|
} |
while (!get_key_press (1 << KEY_ESC) && timer); |
get_key_press(KEY_ALL); |
|
SwitchToNC(); |
|
|
} |
|
void UBX_process() |
{ |
|
if ((get_key_press (1 << KEY_MINUS))||(display_mode ==2)) |
{ |
if (display_mode != 1) |
{ |
lcd_cls(); |
lcd_printp_at (0,0, PSTR("Fix Type : "), 0); |
lcd_printp_at (0,1, PSTR("Sat : "), 0); |
lcd_printp_at (0,2, PSTR("Accuracy : "), 0); |
lcd_printp_at (0,3, PSTR("PDOP : "), 0); |
lcd_printp_at (0,4, PSTR("Speed : "), 0); |
lcd_printp_at (0,5, PSTR("Long : "), 0); |
lcd_printp_at (0,6, PSTR("Lat : "), 0); |
lcd_printp_at (0,7, PSTR("Alt : "), 0); |
} |
display_mode = 1; |
} |
if (get_key_press (1 << KEY_PLUS)) |
{ |
if (display_mode == 1) |
{ |
lcd_cls(); |
} |
|
display_mode = 0; |
} |
|
if((UBX_class == 1)&&(UBX_id == 48)&&(display_mode == 0)) |
{ |
uint8_t channels = UBX_buffer[4]; |
|
uint8_t i = 0; |
for(i = 0; i < channels; i++) |
{ |
if (i > 15) break; |
uint8_t line; |
uint8_t col; |
|
if(i > 7) |
{ |
line = i-7;col = 11; |
} |
else |
{ |
col = 0; line = i; |
} |
write_ndigit_number_u (col, line, UBX_buffer[9 + 12*i], 3, 0); |
write_ndigit_number_u (col+4, line, UBX_buffer[12 + 12*i], 2, 0); |
if((UBX_buffer[10 + 12*i] & 3) == 3) |
{ |
lcd_printp_at (col+7,line, PSTR("O"), 0); |
} |
else if((UBX_buffer[10 + 12*i] & 1) == 1) |
{ |
lcd_printp_at (col+7,line, PSTR("X"), 0); |
} |
else if(UBX_buffer[11 + 12*i] > 4) |
{ |
lcd_printp_at (col+7,line, PSTR("x"), 0); |
} |
else if(UBX_buffer[11 + 12*i] > 1) |
{ |
lcd_printp_at (col+7,line, PSTR("-"), 0); |
} |
else |
{ |
lcd_printp_at (col+7,line, PSTR(" "), 0); |
} |
} |
|
} |
|
if(display_mode == 1) |
{ |
if((UBX_class == 1)&&(UBX_id == 6))//SVINFO |
{ |
switch (UBX_buffer[10]) |
{ |
case 4: |
case 3: |
lcd_printp_at (11,0, PSTR("3D"), 0); |
break; |
case 2: |
lcd_printp_at (11,0, PSTR("2D"), 0); |
break; |
default: |
lcd_printp_at (11,0, PSTR("no"), 0); |
} |
if((UBX_buffer[11] & 3) == 3) |
{ |
lcd_printp_at (17,0, PSTR("D"), 0); |
} |
else |
{ |
lcd_printp_at (17,0, PSTR(" "), 0); |
} |
|
if((UBX_buffer[11] & 1) == 1) |
{ |
lcd_printp_at (14,0, PSTR("ok"), 0); |
} |
else |
{ |
lcd_printp_at (14,0, PSTR(" "), 0); |
} |
lcd_write_number_u_at (11, 1, UBX_buffer[47]); |
uint16_t pdop = UBX_buffer[44]+UBX_buffer[45]*255; |
write_ndigit_number_u (11, 3, pdop/100, 2, 0); |
lcd_printp_at (13,3, PSTR("."), 0); |
write_ndigit_number_u (14, 3, (pdop % 100),2, 1); |
|
uint16_t acc = (uint16_t)join_4_bytes(&UBX_buffer[24]); |
write_ndigit_number_u (11, 2, acc, 5, 0); |
lcd_printp_at (17,2, PSTR("cm"), 0); |
} |
if((UBX_class == 1)&&(UBX_id == 18))//VELNED |
{ |
uint16_t speed = (uint16_t)((join_4_bytes(&UBX_buffer[20])*60*60)/100000); |
write_ndigit_number_u (11, 4, speed, 3, 0); |
lcd_printp_at (15,4, PSTR("km/h"), 0); |
|
} |
if((UBX_class == 1)&&(UBX_id == 2))//POSLLH |
{ |
uint16_t height = (uint16_t)(join_4_bytes(&UBX_buffer[16])/1000); |
write_ndigit_number_u (11, 7, height, 4, 0); |
lcd_printp_at (16,7, PSTR("m"), 0); |
|
uint32_t lon = join_4_bytes(&UBX_buffer[4]); |
write_ndigit_number_u (11, 5, (uint16_t)(lon/10000000), 3, 0); |
lcd_printp_at (14,5, PSTR("."), 0); |
write_ndigit_number_u (15, 5, (uint16_t)((lon/1000) % 10000), 4, 1); |
write_ndigit_number_u (19, 5, (uint16_t)((lon/10) % 100), 2, 1); |
|
uint32_t lat = join_4_bytes(&UBX_buffer[8]); |
write_ndigit_number_u (11, 6, (uint16_t)(lat/10000000), 3, 0); |
lcd_printp_at (14,6, PSTR("."), 0); |
write_ndigit_number_u (15, 6, (uint16_t)((lat/1000) % 10000), 4, 1); |
write_ndigit_number_u (19, 6, (uint16_t)((lat/10) % 100), 2, 1); |
|
} |
|
} |
|
|
} |
|
union long_union { |
uint32_t dword; |
uint8_t byte[4]; |
} longUnion; |
|
union int_union { |
uint16_t dword; |
uint8_t byte[2]; |
} intUnion; |
|
uint32_t join_4_bytes(uint8_t Buffer[]) |
{ |
longUnion.byte[0] = *Buffer; |
longUnion.byte[1] = *(Buffer+1); |
longUnion.byte[2] = *(Buffer+2); |
longUnion.byte[3] = *(Buffer+3); |
return(longUnion.dword); |
} |
|
|
void checksum(uint8_t data) |
{ |
ck_a += data; |
ck_b += ck_a; |
} |
|