Subversion Repositories Projects

Rev

Blame | Last modification | View Log | RSS feed

/*****************************************************************************
 *   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;
}