Blame |
Last modification |
View Log
| RSS feed
/*****************************************************************************
* 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;
}