Subversion Repositories Projects

Compare Revisions

Ignore whitespace Rev 1469 → Rev 1470

/Transportables_Koptertool/branch/GPL_PKT_V3_5_8a_FC086/gps.c
0,0 → 1,364
/*****************************************************************************
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de *
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net *
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net *
* Copyright (C) 2011 Harald Bongartz *
* *
* 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. *
* *
* *
* Credits to: *
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN *
* http://www.mikrokopter.de *
* Gregor "killagreg" Stobrawa for his version of the MK code *
* Thomas Kaiser "thkais" for the original project. See *
* http://www.ft-fanpage.de/mikrokopter/ *
* http://forum.mikrokopter.de/topic-4061-1.html *
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code *
* http://www.mylifesucks.de/oss/c-osd/ *
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility*
*****************************************************************************/
 
 
#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("Nur mit 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((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 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);
 
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);
}
 
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
{
uint32_t lon = join_4_bytes(&UBX_buffer[4]);
write_ndigit_number_u (10, 5, (uint16_t)(lon/10000000), 2, 0);
lcd_printp_at (12,5, PSTR("."), 0);
write_ndigit_number_u (13, 5, (uint16_t)((lon/1000) % 10000), 4, 1);
write_ndigit_number_u (17, 5, (uint16_t)((lon/10) % 100), 2, 1);
 
uint32_t lat = join_4_bytes(&UBX_buffer[8]);
write_ndigit_number_u (10, 6, (uint16_t)(lat/10000000), 2, 0);
lcd_printp_at (12,6, PSTR("."), 0);
write_ndigit_number_u (13, 6, (uint16_t)((lat/1000) % 10000), 4, 1);
write_ndigit_number_u (17, 6, (uint16_t)((lat/10) % 100), 2, 1);
 
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);
 
}
}
}
 
 
//--------------------------------------------------------------
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;
}