Subversion Repositories Projects

/**
 * source for the Bluetooth driver
 * @file bluetooth.c
 * @author Linus Lotz<lotz@in.tum.de>
 * @author Salomon Sickert
 */

//2013 Cebra, Erweiterung um BT Local-ID Abfrage

#include <string.h>
#include "../cpu.h"
#define __DELAY_BACKWARD_COMPATIBLE__
#include <util/delay.h>
#include <avr/interrupt.h>
#include <avr/pgmspace.h>
#include <stdlib.h>

#include "bluetooth.h"
#include "../main.h"
#ifdef HWVERSION3_9
#include "../uart/uart1.h"
#include "../uart/usart.h"
#include "../timer/timer.h"
#include "fifo.h"
#include "error.h"
#include "../lcd/lcd.h"
#include "../eeprom/eeprom.h"
#include "../setup/setup.h"
#include "bluetooth.h"
#include "../tracking/tracking.h"


//#define SaveMem

//
// Baudrate for the UART-connection to the BTM-222 on SQUIRREL
//

#define SQUIRREL

#ifdef SQUIRREL
#define UART_BAUD_RATE      19200
#endif

#ifdef NUT
#define UART_BAUD_RATE      19200
#endif


typedef enum {
        BT_RAW,
        BT_DATA,
        BT_CMD,
        BT_NOECHO,
        BT_NOANSWER
} communication_mode_t;

#define BT_CMD_TIMEOUT_MS 2000

typedef enum {
        BT_TEST,                                // AT
        BT_CONNECT,                             // ATA
        BT_DISCONNECT,                  // ATH
        BT_CLEAR_ADDRESS,               // ATD0
        BT_SET_ADDRESS,                 // ATD=_____
        BT_GET_LOCALID,                  // ATB? Inquire the Local BD address
        BT_FIND_DEVICES,                // ATF?
        BT_DISABLE_AUTOCONNECT,         // ATO1
        BT_ENABLE_AUTOCONNECT,          // ATO0
        BT_SET_MASTER,                  // ATR0
        BT_SET_SLAVE,                   // ATR1
        BT_SET_PIN,                     // ATP=1234
        BT_SET_2400,                    // ATL* Baudrate 2400
        BT_SET_4800,                    // ATL0 Baudrate 4800
        BT_SET_9600,                    // ATL1 Baudrate 9600
        BT_SET_19200,                   // ATL2 Baudrate 19200
        BT_SET_38400,                   // ATL3 Baudrate 38400
        BT_SET_57600,                   // ATL4 Baudrate 57600
        BT_SET_115200,                  // ATL5 Baudrate 115200
        BT_SET_NOANSWER,                // ATQ1 Rückmeldungen aus
        BT_SET_NOECHO,                  // ATE0 ECHO deaktivieren
        BT_SET_ANSWER,                  // ATQ0 Rückmeldungen
        BT_SET_ECHO,                    // ATE1 ECHO aktivieren
        BT_SET_DEFAULT,                 // Defaultwerte setzen
        BT_SET_NAME,                    // Devicename
        BT_SET_DISPWRDOWN               // disable auto Powerdown
} bt_cmd_t;

//TODO: FIFO Grösse
#define IN_FIFO_SIZE 512

char localID[15]="12345678901234";
static uint8_t bt_buffer[IN_FIFO_SIZE];
static fifo_t in_fifo;

char bt_rx_buffer[RXD_BUFFER_SIZE];
volatile uint8_t bt_rx_len;
volatile uint8_t bt_rx_ready = 0;
uint8_t rx_GPS;
static char start = '$';
static char end = '\n';

char data_decode[RXD_BUFFER_SIZE];
volatile uint16_t rx_timeout;

uint8_t EchoAnswerOn;                   //Merkzelle
static bt_mode_t bt_mode = BLUETOOTH_SLAVE;
static communication_mode_t comm_mode = BT_CMD;

uint8_t i = 0;
//uint8_t NoEcho = 0;
//uint8_t NoAnswer = 0;

uint8_t bt_devicecount = 0;

uint8_t bt_rxerror = 0;

device_info device_list[NUTS_LIST];

uint8_t BT_New_Baudrate = 0;              //Merkzelle für zu setzende Baudrate



// Set a timeout of Y ms and a Conditon X, which have to be true while timeout
#define while_timeout(X, Y) for(uint16_t __timeout = 0; __timeout++ <= Y && (X); Delay_MS(Y ? 1 : 0))

//--------------------------------------------------------------
void Delay_MS(int count)
{
        for (int i = 0; i < count; i++)
                _delay_ms(1);
}


void bt_start(void)
{
  if (Config.BTIsSlave == true) EchoAnswerOn = false; else EchoAnswerOn = true;
}

//--------------------------------------------------------------
void uart_receive(void)
{
        unsigned int uart_data;

        while (!fifo_is_full(&in_fifo))
        {
                uart_data = uart1_getc();

//              USART_puts(".");

                switch (uart_data & 0xFF00) {
                        // Framing Error detected, i.e no stop bit detected
                case UART_FRAME_ERROR:
#ifdef DEBUG
                        warn_pgm(PSTR("FRM ERR"));
#endif
                        bt_rxerror++;
                        return;

                        // Overrun, a character already presend in the UART UDR register was
                        // not read by the interrupt handler before the next character arrived,
                        // one or more received characters have been dropped
                        //
                case UART_OVERRUN_ERROR:
#ifdef DEBUG
                        warn_pgm(PSTR("OVR ERR"));
#endif
                        bt_rxerror++;
                        return;

                        // We are not reading the receive buffer fast enough,
                        // one or more received character have been dropped
                        //
                case UART_BUFFER_OVERFLOW:
#ifdef DEBUG
                        warn_pgm(PSTR("BUF ERR"));
#endif
                        bt_rxerror++;
                        return;

                        // UART Inputbuffer empty, nothing to do
                case UART_NO_DATA:
                        return;

                default:
                {
                        fifo_write(&in_fifo, uart_data);
#ifdef DEBUG
                        USART_putc(uart_data);
#endif
                }
                }
        }
#ifdef DEBUG
        warn_pgm(PSTR("FIFO OVR ERR"));
#endif
}


//--------------------------------------------------------------
static void uart_send(const char *data, const uint8_t length)
{
#ifdef SND_DEBUG
  debug_pgm(PSTR("bt_uart_send"));
  if (comm_mode == BT_CMD) debug_pgm(PSTR("bt_uart_send:BT_CMD")); else debug_pgm(PSTR("bt_uart_send:Wrong comm-mode"));
  if (EchoAnswerOn == true) debug_pgm(PSTR("bt_uart_send:EchoAnswer ON")); else debug_pgm(PSTR("bt_uart_send:EchoAnswer OFF"));

#endif

        char echo;

//      lcd_printp_at (i++, 1, PSTR("."), 0);
        for (uint8_t i = 0; i < length; i++)
        {

#ifdef SND_DEBUG
                USART_putc((data[i]));    //test
#endif
//              debug_pgm(PSTR("bt_init_S"));

                if (uart1_putc(data[i]) == 0)
                {
#ifdef SND_DEBUG
                        warn_pgm(PSTR("UART: Remote not ready"));
#endif
                        return;
                }

                if (comm_mode == BT_RAW)
                        _delay_ms(50);

                if (comm_mode == BT_DATA)
                        _delay_ms(1);


                if ((comm_mode == BT_CMD) && (EchoAnswerOn == true))
                {
#ifdef SND_DEBUG
                        warn_pgm(PSTR ("UARTsend: get Echo"));
#endif
                        uint8_t x = 0;
                        for (; x < 3; x++)
                        {

                                while_timeout(fifo_is_empty(&in_fifo), 200)
//                              for(uint16_t __timeout = 0; __timeout++ <= 200 && (fifo_is_empty(&in_fifo)); _delay_ms(200 ? 1 : 0))

                                uart_receive();


                                fifo_read(&in_fifo, &echo);

                                if (echo != data[i]) {
                                        if (uart1_putc(data[i]) == 0)
                                        {
#ifdef SND_DEBUG
                                                warn_pgm(PSTR ("UART: Remote not ready"));
#endif
                                                return;
                                        }
                                }
                                else
                                        break;
                        }

                        if (x == 3)
                        {
                                error_putc(data[i]);
#ifdef DEBUG
                                error_pgm(PSTR("BT: WRONG ECHO"));
                                //_delay_ms(2000);
#endif
                        }
                }
                else
                  {
                    for(uint16_t __timeout = 0; __timeout++ <= 200 && (fifo_is_empty(&in_fifo)); _delay_ms(200 ? 1 : 0))
                    {
                            uart_receive();
                    }
                    fifo_read(&in_fifo, &echo);
#ifdef SND_DEBUG
                    warn_pgm(PSTR ("UARTsend: skip Echo"));
#endif
                  }
        }
}


//--------------------------------------------------------------
static uint16_t send_cmd(const bt_cmd_t command, const char *data)
{
  uint16_t CommandDelay=0;        // nach BTM222 Kommandos verschiedene Verzögerungszeiten bevor es weitergehen kann

//      _delay_ms(500);         // org 500 300 zu wenig
        char full_command[20];  // Maximum command size

        switch (command)
        {
        case BT_SET_PIN:
                strcpy_P(full_command, PSTR("ATP="));
                for (uint8_t i = 0; i < bt_pin_length; i++)
                {
                        full_command[i+4] = Config.bt_pin[i];
                }
                full_command[(bt_pin_length+4)] =0;
                CommandDelay = 100;             //100ms
                break;

        case BT_SET_DEFAULT:
                strcpy_P(full_command, PSTR("ATZ0"));
                CommandDelay = 1000;
                break;


        case BT_SET_2400:
                strcpy_P(full_command, PSTR("ATL*"));
                CommandDelay = 100;
#ifdef DEBUG
                                debug_pgm(PSTR("ATL*"));
#endif
                break;
        case BT_SET_4800:
                strcpy_P(full_command, PSTR("ATL0"));
                CommandDelay = 100;
#ifdef DEBUG
                                debug_pgm(PSTR("ATL0"));
#endif
                break;
        case BT_SET_9600:
                strcpy_P(full_command, PSTR("ATL1"));
                CommandDelay = 100;
#ifdef DEBUG
                                debug_pgm(PSTR("ATL1"));
#endif
                break;

        case BT_SET_19200:
                strcpy_P(full_command, PSTR("ATL2"));
                CommandDelay = 100;
#ifdef DEBUG
                                debug_pgm(PSTR("ATL2"));
#endif
                break;

        case BT_SET_38400:
                strcpy_P(full_command, PSTR("ATL3"));
                CommandDelay = 100;
#ifdef DEBUG
                                debug_pgm(PSTR("ATL3"));
#endif
                break;

        case BT_SET_57600:
                strcpy_P(full_command, PSTR("ATL4"));
                CommandDelay = 100;
#ifdef DEBUG
                                debug_pgm(PSTR("ATL4"));
#endif
                break;

        case BT_SET_115200:
                strcpy_P(full_command, PSTR("ATL5"));
                CommandDelay = 100;
#ifdef DEBUG
                                debug_pgm(PSTR("ATL5"));
#endif
                break;

        case BT_SET_NOANSWER:
                strcpy_P(full_command, PSTR("ATQ1"));
                CommandDelay = 100;
#ifdef DEBUG
                                debug_pgm(PSTR("ATQ1"));
#endif
                break;

        case BT_SET_NOECHO:
                strcpy_P(full_command, PSTR("ATE0"));
                CommandDelay = 100;
#ifdef DEBUG
                                debug_pgm(PSTR("ATE0"));
#endif
                break;

        case BT_SET_ANSWER:
                strcpy_P(full_command, PSTR("ATQ0"));
                CommandDelay = 100;
#ifdef DEBUG
                                debug_pgm(PSTR("ATQ0"));
#endif
                break;

        case BT_SET_ECHO:
                strcpy_P(full_command, PSTR("ATE1"));
                CommandDelay = 100;
#ifdef DEBUG
                                debug_pgm(PSTR("ATE1"));
#endif
                break;

        case BT_TEST:
                strcpy_P(full_command, PSTR("AT"));
                CommandDelay = 100;
#ifdef DEBUG
                                debug_pgm(PSTR("AT"));
#endif
                break;

        case BT_CONNECT:
                strcpy_P(full_command, PSTR("ATA"));
                CommandDelay = 100;
#ifdef DEBUG
                                debug_pgm(PSTR("ATA"));
#endif
                break;

        case BT_DISCONNECT:
                strcpy_P(full_command, PSTR("ATH"));
                CommandDelay = 100;
#ifdef DEBUG
                                debug_pgm(PSTR("ATH"));
#endif
                break;

        case BT_CLEAR_ADDRESS:
                strcpy_P(full_command, PSTR("ATD0"));
                CommandDelay = 100;
                break;

        case BT_SET_ADDRESS:
                strcpy_P(full_command, PSTR("ATD="));
                memcpy((full_command + strlen(full_command)), data, 12);
                full_command[16] = 0;
                CommandDelay = 100;
#ifdef DEBUG
                                debug_pgm(PSTR("ATLD="));
#endif
                break;

        case BT_FIND_DEVICES:
                strcpy_P(full_command, PSTR("ATF?"));
                CommandDelay = 100;
#ifdef DEBUG
                                debug_pgm(PSTR("ATF?"));
#endif
                break;

        case BT_DISABLE_AUTOCONNECT:
                strcpy_P(full_command, PSTR("ATO1"));
                CommandDelay = 3500;
#ifdef DEBUG
                                debug_pgm(PSTR("ATO1"));
#endif
                break;
        case BT_ENABLE_AUTOCONNECT:
                strcpy_P(full_command, PSTR("ATO0"));
                CommandDelay = 3500;
#ifdef DEBUG
                                debug_pgm(PSTR("ATO0"));
#endif
                break;
        case BT_SET_MASTER:
                strcpy_P(full_command, PSTR("ATR0"));
                CommandDelay = 3000;
#ifdef DEBUG
                                debug_pgm(PSTR("ATR0"));
#endif
                break;

        case BT_SET_SLAVE:
                strcpy_P(full_command, PSTR("ATR1"));
                CommandDelay = 3000;
#ifdef DEBUG
                                debug_pgm(PSTR("ATR1"));
#endif
                break;

        case BT_SET_NAME:
                strcpy_P(full_command, PSTR("ATN="));
                for (uint8_t i = 0; i < bt_name_len; i++)
                {
                        full_command[i + 4] = Config.bt_name[i];
                }
                full_command[(bt_name_len + 4)] = 0;
                CommandDelay = 100;
#ifdef DEBUG
                                debug_pgm(PSTR("ATN="));
#endif
                break;

        case BT_SET_DISPWRDOWN:
                strcpy_P(full_command, PSTR("ATS1"));
                CommandDelay = 100;
#ifdef DEBUG
                                debug_pgm(PSTR("ATS1"));
#endif
                break;
        case BT_GET_LOCALID:
                strcpy_P(full_command, PSTR("ATB?"));
                CommandDelay = 100;
#ifdef DEBUG
                                debug_pgm(PSTR("ATB?"));
#endif
                break;
        default:
                warn_pgm(PSTR("CMD UNK"));
                return false;
        }

        strcat_P(full_command, PSTR("\r"));

        // throw away your television
        uart_receive();
        fifo_clear(&in_fifo);
//      debug_pgm(PSTR("bt_init3, send command"));
        // send command
        uart_send(full_command, strlen(full_command));

//TODO: hier ist ein Fehler bei den Bedingungen
//      if (command == (BT_SET_NOECHO||BT_SET_NOANSWER||BT_SET_ECHO||BT_SET_ANSWER))
//        {
//          uart_receive();
//          fifo_clear(&in_fifo);
//          _delay_ms(CommandDelay);
//#ifdef DEBUG
//              debug_pgm(PSTR("send_cmd: Echo Answer no Response"));
//#endif
////        return true;
//
//        }
//      else
          {
            // get response
#ifdef DEBUG
              debug_pgm(PSTR("send_cmd:get Response"));
#endif
            while_timeout(true, BT_CMD_TIMEOUT_MS)
            {
                    uart_receive();
                    if (command== BT_GET_LOCALID)
                      {
                        _delay_ms(CommandDelay);
                        return bt_getID();

                      }
                    if (fifo_strstr_pgm(&in_fifo, PSTR("OK\r\n")))
                    {
                            info_pgm(PSTR("CMD SEND: OK"));
                            _delay_ms(CommandDelay);
                            return true;
                    }

                    if (fifo_strstr_pgm(&in_fifo, PSTR("ERROR\r\n")))
                    {
#ifdef DEBUG
                            info_pgm(PSTR("CMD SEND: Error"));
                            _delay_ms(2000);
#endif
                            return false;
                    }
            }
          }

//#ifdef DEBUG
//      if (command != BT_TEST)
//              warn_pgm(PSTR("CMD SEND: TIMEOUT"));
//              _delay_ms(2000);
//#endif


        return false;
}

//bt_init

//--------------------------------------------------------------
//void test(void)
//{
//      comm_mode = BT_RAW;
//      for (uint8_t i = 0; i < 2; i++)
//              if (send_cmd(BT_TEST, NULL))
//                      break;
//      comm_mode = BT_CMD;
//}


#ifndef SaveMem

//--------------------------------------------------------------
static void clean_line(void)
{
        while_timeout(true, 50)
        uart_receive();
        fifo_strstr_pgm(&in_fifo, PSTR("\r\n"));
}

static communication_mode_t update_comm_mode(uint16_t timeout_ms)
{
        while_timeout(true, timeout_ms)
//            for(uint16_t __timeout = 0; __timeout++ <= true && (timeout_ms); _delay_ms(true ? 1 : 0))


        {
                uart_receive();

                if (fifo_strstr_pgm(&in_fifo, PSTR("DISCONNECT")))
                {
                        clean_line();
//                      test();

                        comm_mode = BT_CMD;
                        send_cmd(BT_TEST, NULL);
                        return comm_mode;
                }

                if (fifo_strstr_pgm(&in_fifo, PSTR("CONNECT")))
                {
                        _delay_ms(100); //don't delete this, else there will be no success!!!!!!!!!
                        comm_mode = BT_DATA;
                        return comm_mode;
                }

                if (fifo_strstr_pgm (&in_fifo, PSTR("Time out,Fail to connect!")))
                {
                        clean_line();
#ifdef DEBUG
                        debug_pgm(PSTR("CONNECT FAILED\n"));
#endif
//                      test();
                        send_cmd(BT_TEST, NULL);
                        comm_mode = BT_CMD;
                        return comm_mode;
                }
        }

        return comm_mode;
}
#endif
//--------------------------------------------------------------
uint16_t bt_setbaud(uint8_t baudrate)
{
        uint8_t init_error = false;
        uint8_t BT_found = 0;
        i = 0;

//        set_BTOn();

        lcd_cls();
        lcd_printp_at (0, 0, PSTR("BT set new Baudrate.."), 0);

        SetBaudUart1(Old_Baudrate);
        fifo_init(&in_fifo, bt_buffer, IN_FIFO_SIZE);
        _delay_ms(100);

        fifo_clear(&in_fifo);
//        send_cmd(BT_TEST, NULL);
//        comm_mode = BT_NOECHO;
//        send_cmd(BT_SET_ECHO, NULL);
//        send_cmd(BT_SET_ANSWER, NULL);
        bt_set_EchoAnswer(true);


#ifdef DEBUG
        debug_pgm(PSTR("Check with PKT Baudrate"));
#endif

        if (send_cmd(BT_TEST, NULL)) // Test mit PKT_Baudrate
        {
#ifdef DEBUG
                debug_pgm(PSTR("BT found with PKT Baudrate"));
#endif
        BT_found = 1;
        }

        if (BT_found == 0)
          {
           lcd_printp_at (0, 1, PSTR("Error1 set Baudrate.."), 0);
           _delay_ms(2100);
           set_BTOff();
           return false;
          }

        else

        {
                /* Set comm_mode to CMD */
                comm_mode = BT_CMD;

                switch (baudrate)
                  {
//                  case Baud_2400 : {   /* Set BTM Baudrate */
//                                    if (!(send_cmd(BT_SET_2400, NULL))) init_error = true;
//                                    SetBaudUart1(Baud_2400);
//                                    _delay_ms(100);
//                                    break;
//                                   }
                  case Baud_4800 : {   /* Set BTM Baudrate */
                                    if (!(send_cmd(BT_SET_4800, NULL))) init_error = true;
                                    SetBaudUart1(Baud_4800);
                                    _delay_ms(100);
                                    break;
                                   }
                  case Baud_9600 : {   /* Set BTM Baudrate */
                                    if (!(send_cmd(BT_SET_9600, NULL))) init_error = true;
                                    SetBaudUart1(Baud_9600);
                                    _delay_ms(100);
                                    break;
                                   }
                  case Baud_19200 : {   /* Set BTM Baudrate */
                                    if (!(send_cmd(BT_SET_19200, NULL))) init_error = true;
                                    SetBaudUart1(Baud_19200);
                                    _delay_ms(100);
                                    break;
                                   }
                  case Baud_38400 : {   /* Set BTM Baudrate */
                                    if (!(send_cmd(BT_SET_38400, NULL))) init_error = true;
                                    SetBaudUart1(Baud_38400);
                                    _delay_ms(100);
                                    break;
                                   }
                  case Baud_57600 : {   /* Set BTM Baudrate */
                                    if (!(send_cmd(BT_SET_57600, NULL))) init_error = true;
                                    SetBaudUart1(Baud_57600);
                                    _delay_ms(100);
                                    break;
                                   }
                  case Baud_115200 : {   /* Set BTM Baudrate */
                                    if (!(send_cmd(BT_SET_115200, NULL))) init_error = true;
                                    SetBaudUart1(Baud_115200);
                                    _delay_ms(100);
                                    break;
                                   }
                  break;
                  }

//                /* Set BTM Echo aus */
//                send_cmd(BT_SET_NOECHO, NULL);
////              test();
//                comm_mode = BT_NOECHO;
//                /* Set BTM Answer aus */
//                send_cmd(BT_SET_NOANSWER, NULL);
////              test();
                bt_set_EchoAnswer(false);
                bt_mode = BLUETOOTH_SLAVE;

//                set_BTOff();


                if (!init_error)
                {
                        lcd_printp_at (0, 2, PSTR("set Baudrate ok"), 0);
                        _delay_ms(2100);
                        return true;
                }
                else
                  {
                          lcd_printp_at (0, 2, PSTR("set Baudrate Error"), 0);
                          _delay_ms(2100);
                          return true;
                  }

        }
}

void bt_set_EchoAnswer (uint8_t onoff)

{
  if (onoff == true)
    {
//      if (EchoAnswerOn==false)
//        {

          /* Set comm_mode to CMD */
          comm_mode = BT_CMD;
          send_cmd(BT_SET_ECHO, NULL);
          send_cmd(BT_SET_ANSWER, NULL);
          EchoAnswerOn = true;
          send_cmd(BT_TEST, NULL);

//          fifo_clear(&in_fifo);
//        }
#ifdef DEBUG
       debug_pgm(PSTR("bt_set_EchoAnswer: on"));
#endif
    }
  else
    {
//      if (EchoAnswerOn==true)
//        {
            /* Set comm_mode to CMD */
          comm_mode = BT_CMD;
          EchoAnswerOn = false;
          send_cmd(BT_SET_NOECHO, NULL);
          send_cmd(BT_SET_NOANSWER, NULL);
//          fifo_clear(&in_fifo);

//       }
#ifdef DEBUG
       debug_pgm(PSTR("bt_set_EchoAnswer: off"));
#endif
    }

}


//--------------------------------------------------------------
uint16_t bt_init(void)
{
        uint8_t init_error = false;
        uint8_t BT_found = 0;
        i = 0;

        set_BTOn();

        lcd_cls();
        lcd_printp_at (0, 0, PSTR("BT initialisieren.."), 0);
//      _delay_ms(200);

        for (uint8_t z = (bt_name_length); z > 0; z--)
        {
                if (Config.bt_name[z - 1] != ' ')
                {
                        bt_name_len = z;
                        break;
                }
        }

//      uart1_init(UART_BAUD_SELECT(57600, F_CPU));
        SetBaudUart1(Config.PKT_Baudrate);
        fifo_init(&in_fifo, bt_buffer, IN_FIFO_SIZE);
        _delay_ms(100);
//      debug_pgm(PSTR("bt_init"));
        uart_receive();
//      debug_pgm(PSTR("bt_init1"));
        fifo_clear(&in_fifo);
        bt_disconnect();
        bt_set_EchoAnswer(true);


//      debug_pgm(PSTR("bt_init2"));
#ifdef DEBUG
        debug_pgm(PSTR("Check with PKT Baudrate"));
#endif
        send_cmd(BT_TEST, NULL); // Schrott löschen
        send_cmd(BT_TEST, NULL); // Schrott löschen
        if (send_cmd(BT_TEST, NULL)) // Test mit 57600
        {
#ifdef DEBUG
                debug_pgm(PSTR("BT found with PKT Baudrate"));
#endif
        BT_found = 1;
        }

        if (BT_found == 0)
        {
#ifdef DEBUG
                debug_pgm(PSTR("Check with 19200"));
#endif
//              uart1_init(UART_BAUD_SELECT(19200, F_CPU));// Test mit 19200
                SetBaudUart1(Baud_19200);
//              _delay_ms(100);
                send_cmd(BT_TEST, NULL); // Schrott löschen
                send_cmd(BT_TEST, NULL); // Schrott löschen
                bt_set_EchoAnswer(true);

                if (send_cmd(BT_TEST, NULL))
                {
#ifdef DEBUG
                        debug_pgm(PSTR("19200 OK"));
#endif

                        if (send_cmd(BT_TEST, NULL))
                        {
#ifdef DEBUG
                                debug_pgm(PSTR("BT found 19200 Baud"));

#endif
                                Old_Baudrate = Baud_19200;
                                BT_found = 2;
                        }
                }
        }
        if (BT_found == 0)
        {
#ifdef DEBUG
                debug_pgm(PSTR("Check with 38400"));
#endif
//              uart1_init(UART_BAUD_SELECT(19200, F_CPU));// Test mit 19200
                SetBaudUart1(Baud_38400);
//                _delay_ms(100);
                send_cmd(BT_TEST, NULL); // Schrott löschen
//                comm_mode = BT_NOECHO;
//                send_cmd(BT_SET_ECHO, NULL);
//                comm_mode = BT_NOANSWER;
//                send_cmd(BT_SET_ANSWER, NULL);
//                comm_mode = BT_CMD;
                bt_set_EchoAnswer(true);

                if (send_cmd(BT_TEST, NULL))
                {
#ifdef DEBUG
                        debug_pgm(PSTR("38400 OK"));
#endif
                        if (send_cmd(BT_TEST, NULL))
                        {
#ifdef DEBUG
                                debug_pgm(PSTR("BT found 38400 Baud"));
#endif
                                Old_Baudrate = Baud_38400;
                                BT_found = 2;
                        }
                }
        }

        if (BT_found == 0)
        {
#ifdef DEBUG
                debug_pgm(PSTR("Check with 9600"));
#endif
//              uart1_init(UART_BAUD_SELECT(9600, F_CPU));//test mit 9600
                SetBaudUart1(Baud_9600);
//              _delay_ms(100);
                send_cmd(BT_TEST, NULL);
//              comm_mode = BT_NOECHO;
//              send_cmd(BT_SET_ECHO, NULL);
//              comm_mode = BT_NOANSWER;
//              send_cmd(BT_SET_ANSWER, NULL);
//              comm_mode = BT_CMD;
                bt_set_EchoAnswer(true);
                if (send_cmd(BT_TEST, NULL));
                {
#ifdef DEBUG
                        debug_pgm(PSTR("9600 OK"));
#endif
                        if (send_cmd(BT_TEST, NULL))
                        {
#ifdef DEBUG
                                debug_pgm(PSTR("BT found 9600 Baud"));
#endif
                                Old_Baudrate = Baud_9600;
                                BT_found = 3;
                        }
                }
        }

        if (BT_found == 0)
        {
#ifdef DEBUG
                debug_pgm(PSTR("Check with 57600"));
#endif
//              uart1_init(UART_BAUD_SELECT(9600, F_CPU));//test mit 9600
                SetBaudUart1(Baud_57600);
//              _delay_ms(100);
                send_cmd(BT_TEST, NULL);
//                comm_mode = BT_NOECHO;
//                send_cmd(BT_SET_ECHO, NULL);
//                comm_mode = BT_NOANSWER;
//                send_cmd(BT_SET_ANSWER, NULL);
//                comm_mode = BT_CMD;
                bt_set_EchoAnswer(true);
                if (send_cmd(BT_TEST, NULL));
                {
#ifdef DEBUG
                        debug_pgm(PSTR("57600 OK"));
#endif
                        if (send_cmd(BT_TEST, NULL))
                        {
#ifdef DEBUG
                                debug_pgm(PSTR("BT found 57600 Baud"));
#endif
                                Old_Baudrate = Baud_57600;
                                BT_found = 4;
                        }
                }
        }


        if (BT_found == 0)
        {
#ifdef DEBUG
                debug_pgm(PSTR("Check with 4800"));
#endif
//                uart1_init(UART_BAUD_SELECT(4800, F_CPU));//test mit 4800
                SetBaudUart1(Baud_4800);
//                _delay_ms(100);
                send_cmd(BT_TEST, NULL);
//                comm_mode = BT_NOECHO;
//                send_cmd(BT_SET_ECHO, NULL);
//                comm_mode = BT_NOANSWER;
//                send_cmd(BT_SET_ANSWER, NULL);
//                comm_mode = BT_CMD;
                bt_set_EchoAnswer(true);
                if (send_cmd(BT_TEST, NULL));
                {
#ifdef DEBUG
                        debug_pgm(PSTR("4800 OK"));
#endif
                        if (send_cmd(BT_TEST, NULL))
                        {
#ifdef DEBUG
                                debug_pgm(PSTR("BT found 4800 Baud"));
#endif
                                Old_Baudrate = Baud_4800;
                                BT_found = 5;
                        }
                }
        }


        if (BT_found > 0)
        {

#ifdef DEBUG
                debug_pgm(PSTR("BT found !"));
#endif

            /* Set comm_mode to CMD */
                comm_mode = BT_CMD;
//              test();
//              if (BTIsSlave==false)
//                {

//                    fifo_init(&in_fifo, bt_buffer, IN_FIFO_SIZE);
                    _delay_ms(100);
    //          test();
                    /* Clear remote address */
                    if(!(send_cmd(BT_CLEAR_ADDRESS, NULL)))
                            init_error = true;
    //          test();
                    /* Set BTM to SLAVE */
                    if (!(send_cmd(BT_SET_SLAVE, NULL)))
                            init_error = true;
    //          test();
                    /* Set BTM PIN */
                    if(!(send_cmd(BT_SET_PIN, NULL)))
                            init_error = true;
    //          test();
                    /* Set BTM Name */
                    if(!(send_cmd(BT_SET_NAME, NULL)))
                            init_error = true;
                    _delay_ms(300);
    //          test();
                    if(!(send_cmd(BT_SET_DISPWRDOWN, NULL)))
                            init_error = true;
                    if(!(send_cmd(BT_GET_LOCALID, NULL)))
                            init_error = true;





//                }

                    /* Set BTM Baudrate */

//                    if (!(send_cmd(BT_SET_57600, NULL)))
//                            init_error = true;
                    if (!bt_setbaud(Config.PKT_Baudrate)) init_error = true;


//                    uart1_init(UART_BAUD_SELECT(57600, F_CPU));
                    SetBaudUart1(Config.PKT_Baudrate);



//              /* Set BTM Echo aus */
//              send_cmd(BT_SET_NOECHO, NULL);
////            test();
//              comm_mode = BT_NOECHO;
//              /* Set BTM Answer aus */
//              send_cmd(BT_SET_NOANSWER, NULL);
////            test();

                bt_set_EchoAnswer (false);
                bt_mode = BLUETOOTH_SLAVE;

                set_BTOff();


                if (!init_error)
                {
                        WriteBTInitFlag();  // Init merken
                        WriteBTSlaveFlag();

                        bt_start();
                        return true;
                }
                else
                        return false;
        }
        else
        {
                set_BTOff();
                return false;
        }

}


#ifndef SaveMem

//--------------------------------------------------------------
uint8_t bt_set_mode(const bt_mode_t mode)
{
#ifdef DEBUG
                    debug_pgm(PSTR("bt_setmode: set Mode"));
#endif

//      if (update_comm_mode(0) == BT_DATA) // 30.1.2012 CB
//              return false;

        if (mode == bt_mode)
                return true;

        if (mode == BLUETOOTH_MASTER)
          {
            comm_mode = BT_CMD;
            bt_set_EchoAnswer(true);

//          _delay_ms(1000);
                if (send_cmd(BT_SET_MASTER, NULL))
                {
                    bt_mode = BLUETOOTH_MASTER;
                    send_cmd(BT_DISABLE_AUTOCONNECT, NULL);
                    WriteBTMasterFlag();

#ifdef DEBUG
                    debug_pgm(PSTR("bt_setmode: Master is set"));
#endif
                  }
          }
        if (mode == BLUETOOTH_SLAVE)
          {
                comm_mode = BT_CMD;
                bt_set_EchoAnswer(true);

                if (send_cmd(BT_ENABLE_AUTOCONNECT, NULL))
                {
                  bt_mode = BLUETOOTH_SLAVE;
                  send_cmd(BT_SET_SLAVE, NULL);
                  WriteBTSlaveFlag();
                  bt_set_EchoAnswer(false);
                  comm_mode = BT_CMD;

#ifdef DEBUG
                  debug_pgm(PSTR("bt_setmode: Slave is set"));
#endif
                }
          }

//      if (bt_mode == BLUETOOTH_MASTER)  debug_pgm(PSTR("bt_mode:BLUETOOTH_MASTER "));
//      if (bt_mode == BLUETOOTH_SLAVE)  debug_pgm(PSTR("bt_mode:BLUETOOTH_SLAVE"));
//        if (mode == BLUETOOTH_MASTER)  debug_pgm(PSTR("mode:BLUETOOTH_MASTER "));
//        if (mode == BLUETOOTH_SLAVE)  debug_pgm(PSTR("mode:BLUETOOTH_SLAVE"));

        return (mode == bt_mode);
}


//--------------------------------------------------------------
uint16_t bt_receive(void *data, uint8_t length, uint16_t timeout_ms)
{
        uint8_t rec_length = 0;
        uint8_t i = 0;

//      while_timeout(true, timeout_ms);
        for(uint16_t __timeout = 0; __timeout++ <= true && (timeout_ms); _delay_ms(true ? 1 : 0))
        {
                if (i == length)
                        return true;

                uart_receive();

                if (fifo_is_empty(&in_fifo))
                        continue;

                if (update_comm_mode(0) != BT_DATA)
                {
#ifdef DEBUG
                        debug_pgm(PSTR("not connected"));
#endif
                        return false;
                }
                // We have a connection
                if (timeout_ms == 0)
                        timeout_ms += 2000;

                if (fifo_is_empty(&in_fifo))
                        continue;

                // Find starting point of packet
                if (!rec_length)
                {
                        fifo_read(&in_fifo, (char *)&rec_length);

                        if (rec_length != length)
                        {
                                rec_length = 0;
                        }
                        else
                        {
                                // You've got mail!
                                timeout_ms += 2000;
                        }
                }
                else
                {
                        fifo_read(&in_fifo, (char *)data + i);
                        i++;
                }
        }
        return false;
}

#endif

#ifndef SaveMem


//--------------------------------------------------------------
uint16_t bt_send(void *data, const uint8_t length)
{
        if (update_comm_mode(0) == BT_CMD)
                return false;

        uart_send((const char *)&length, 1);
        uart_send((char *)data, length);
        return (update_comm_mode(0) == BT_DATA);
}


#ifdef SQUIRREL

//--------------------------------------------------------------
uint16_t bt_connect(const char *address)
{
  fifo_init(&in_fifo, bt_buffer, IN_FIFO_SIZE);
  uart_receive();
  fifo_clear(&in_fifo);

        // Maybe we already disconnected???
        if (BT_DATA == update_comm_mode(0))
        {
#ifdef DEBUG
                debug_pgm(PSTR("We are still connected..."));
#endif
                return false;
        }
//      test();
        send_cmd(BT_TEST, NULL);


/*
        if (!send_cmd(BT_DISABLE_AUTOCONNECT, address))
                return false;
*/

//      Test();
        send_cmd(BT_TEST, NULL);
#ifdef DEBUG
        debug_pgm (PSTR ("SET_ADD"));
#endif

        if (!send_cmd(BT_SET_ADDRESS, address))
                return false;

//      test();
        send_cmd(BT_TEST, NULL);
#ifdef DEBUG
        debug_pgm (PSTR ("CONNECT"));
#endif

        if (!send_cmd(BT_CONNECT, NULL))
                return false;
#ifdef DEBUG
        debug_pgm (PSTR ("WAIT FOR COMM"));
#endif
        return (BT_DATA == update_comm_mode(20000));
}


//--------------------------------------------------------------
uint16_t bt_disconnect(void)
{


        if (BT_CMD == update_comm_mode(0))
        {
                fifo_clear(&in_fifo);
                return true;
        }

        // Switch to online cmd mode
        for (uint8_t i = 0; i < 4; i++)
        {
                const char plus = '+';
                uart_send(&plus, 1);
                _delay_ms(1000);
        }

        comm_mode = BT_CMD;

        if (!send_cmd(BT_DISCONNECT, NULL))
                return false;

//      test();
        if (!send_cmd(BT_CLEAR_ADDRESS, NULL))
                return false;
//      test();

        if (BT_CMD == update_comm_mode(10000))
        {
                fifo_clear(&in_fifo);
                return true;
        }
#ifdef DEBUG
        debug_pgm(PSTR("Still in DATA??"));
#endif
        return false;

}

/*

BTM-222 Softwareversion 4.35
Inquiry Results:
           111111111222222222233333333334
01234567890123456789012345678901234567890

1  LE091452          0024-2C-BEB0CA
2  E71 c             0024-7C-3EC9B9

BTM-222 Softwareversion 6.26
Inquiry Results:
1  E71 c  0024-7C-3EC9B9 N.A.
2  LE091452  0024-2C-BEB0CA N.A.

*/


//--------------------------------------------------------------
void copy_mac(const char *src, char *dst)
{
        uint8_t off = 0;

        for (uint8_t i = 0; i < 40; i++)
        {
            if (src[i] == '-') if (src[i+3] == '-')// MAC Adresse suchen
                                {
                                  off = i-4;
                                  break;
                                }
        }

        for (uint8_t i = 0; i < 14; i++)
        {
                if (src[i + off] == '-')
                        off++;

                dst[i] = src[i + off];
        }
}
//--------------------------------------------------------------
void copy_localID(const char *src, char *dst)
{
        uint8_t off = 0;

//        for (uint8_t i = 0; i < 40; i++)
//        {
//            if (src[i] == '-') if (src[i+3] == '-')// MAC Adresse suchen
//                                {
//                                  off = i-4;
//                                  break;
//                                }
//        }

        for (uint8_t i = 0; i < 14; i++)
        {
                if (src[i + off] == '-')
                        off++;

                dst[i] = src[i + off];
        }
}
//--------------------------------------------------------------
void copy_DevName(const char *src, char *dst)
{
        uint8_t off = 0;


        for (uint8_t i = 0; i < 14; i++)
        {
            if (src[i] == ' ') if (src[i+1] == ' ') break; // nach zwei Leerzeichen ist der Name zuende
             dst[i] = src[i + off];
        }
}

//--------------------------------------------------------------
uint16_t bt_discover(char result[8][12])


{


        if (!bt_set_mode(BLUETOOTH_MASTER))
                return false;

  send_cmd(BT_TEST, NULL);
        if (!send_cmd(BT_FIND_DEVICES, NULL))
          {
                return false;
          }

        char buffer[255];       //oversized, but who cares?
        char *bufferhead = buffer;
        uint16_t pos = 0;
        uint16_t Timeout = 40000;
        uint16_t pos1 = 0;
        uint16_t posC = 0;
#ifdef DEBUG
        debug_pgm(PSTR("discover2"));
#endif
        do
        {
                uart_receive();
                Timeout--;
                pos1++;
                posC++;
                _delay_ms(1);
                  write_ndigit_number_u(0,5,fifo_getcount(&in_fifo),5,0,0);
                if (posC ==1000)
                  {
                   lcd_printp_at (i++, 1, PSTR("."), 0);
                   posC = 0;

                  }


                if (fifo_is_full(&in_fifo)) break;
#ifdef DEBUG
                if (fifo_search(&in_fifo, PSTR("Found."))) debug_pgm(PSTR("Suchen ende1"));
#endif
        }

//      while (((Timeout > 0) ||(!fifo_strstr_pgm(&in_fifo, PSTR("Inquiry Results:\r\n")))) && (!fifo_strstr_pgm(&in_fifo, PSTR("Found"))));
        while ((Timeout > 0)||(!fifo_strstr_pgm(&in_fifo, PSTR("Inquiry Results:\r\n"))));
#ifdef DEBUG
        debug_pgm(PSTR("Suchen ende2"));


        if (Timeout == 0) debug_pgm(PSTR("Timeout"));

        if (fifo_is_full(&in_fifo)) debug_pgm(PSTR("Fifo Overrun, zuviele BT Devices"));
#endif
        while (!fifo_is_empty(&in_fifo))
        {
                // Get next line
                while (!fifo_cmp_pgm(&in_fifo, PSTR("\r\n")))
                {
                        fifo_read(&in_fifo, bufferhead);
                        bufferhead++;
                }
                // terminate string
                *bufferhead = 0;

                //reset bufferhead
                bufferhead = buffer;

                if (strlen(buffer) == 0)
                        continue;       //the empty line before end of inquiry

                if (strstr_P(buffer, PSTR("Inquiry End")))
//              if (searchend)
                {
                        fifo_clear(&in_fifo);
//                      test();
                        return true;
                }


                        copy_DevName(&buffer[3],device_list[pos].DevName);
                        device_list[pos].DevName[14] = 0;   // Stringende

                        copy_mac(&buffer[3], device_list[pos].mac);

//                      for (uint16_t i = 0; i < 15; i++)
//                        {
//
////                        USART_putc((device_list[pos].DevName[i]));
//                          lcd_print_hex((device_list[pos].DevName[i]),0);
//                        }
//                      USART_putc('\n');
//
//
//                        for (uint16_t i = 0; i < 12; i++)
//                          {
//
//                            USART_putc((device_list[pos].mac[i]));
//
//                          }
//
//                        USART_putc('\n');
//                        USART_putc('\r');
                        pos++;
        }

        return false;
}


//--------------------------------------------------------------
uint8_t bt_getID (void)


{


        char buffer[255];       //oversized, but who cares?
        char *bufferhead = buffer;
//        uint16_t pos = 0;
#ifdef DEBUG
        debug_pgm(PSTR("bt_getID1"));
#endif

        while_timeout(!fifo_strstr_pgm(&in_fifo, PSTR("r\n")),
                      2000)
            uart_receive();

#ifdef DEBUG
        debug_pgm(PSTR("bt_getID:Suchen ende2"));


//        if (Timeout == 0) debug_pgm(PSTR("Timeout"));

        if (fifo_is_full(&in_fifo)) debug_pgm(PSTR("bt_getID:Fifo Overrun"));
#endif

        while (!fifo_is_empty(&in_fifo))
        {
          // Get next line
          while (!fifo_cmp_pgm(&in_fifo, PSTR("\r\n")))
            {
                  fifo_read(&in_fifo, bufferhead);
                  bufferhead++;
//                        write_ndigit_number_u(10,4,fifo_getcount(&in_fifo),5,0,0);
            }
                // terminate string
          *bufferhead = 0;

          //reset bufferhead
          bufferhead = buffer;
          if (strlen(buffer) == 0)
          continue;       //the empty line before end of inquiry

          copy_localID(&buffer[0], &localID[0]);

          for(uint8_t i = 0; i < 13; i++)
           {
              lcd_putc (i, 6, localID[i],0);
              Config.bt_Mac[i] = localID[i];
           }
//          lcd_printp_at (0, 7, PSTR("lokale ID hier"), 0);
////          write_ndigit_number_u(0,4,fifo_getcount(&in_fifo),5,0,0);
//          while (!get_key_press (1 << KEY_ENTER));
#ifdef DEBUG
          debug_pgm(PSTR("bt_getID:Copy ID"));
#endif
          if ( fifo_strstr_pgm(&in_fifo, PSTR("OK")))
               {
                  fifo_clear(&in_fifo);
#ifdef DEBUG
                  debug_pgm(PSTR("bt_getID:OK found"));
#endif
                  return true;
               }
          else return false;


          return true;

//          pos++;
        }

        return false;
}



device_info device_list[NUTS_LIST];

void bt_downlink_init(void)
{


        fifo_init(&in_fifo, bt_buffer, IN_FIFO_SIZE);
        _delay_ms(100);
//      debug_pgm(PSTR("bt_downlink_init"));
        uart_receive();
        fifo_clear(&in_fifo);
//        send_cmd(BT_TEST, NULL);
#ifdef DEBUG
        debug_pgm(PSTR("downlink_init Start"));
#endif
        if (Config.BTIsSlave == true)          // nur Init wenn BT ist Slave
          {
#ifdef DEBUG
             debug_pgm(PSTR("downlink_init:try to set Master"));
#endif
//            comm_mode = BT_NOECHO;
//
//            if (!send_cmd (BT_SET_ECHO,NULL)) {
//#ifdef DEBUG
//                    debug_pgm(PSTR("downlink_init:Couldn't set Echo!"));
//#endif
//            }
//
//            comm_mode = BT_NOANSWER;
//            if (!send_cmd(BT_SET_ANSWER,NULL)) {
//#ifdef DEBUG
//                    debug_pgm(PSTR("downlink_init:Couldn't set Answer!"));
//#endif
//
//            }
            comm_mode = BT_CMD;
//           send_cmd(BT_TEST, NULL);

             bt_set_EchoAnswer(true);


            if (!bt_set_mode(BLUETOOTH_MASTER))

            {
#ifdef DEBUG
                    debug_pgm(PSTR("downlink_init:Couldn't set master!"));
#endif
                    return;
            }
    #ifdef DEBUG
            debug_pgm(PSTR("downlink_init:master is set "));
    #endif

//            WriteBTMasterFlag();  // Master merken
            comm_mode = BT_CMD;
          }
        else
          {
#ifdef DEBUG
             debug_pgm(PSTR("downlink_init:Master was set"));
#endif
             comm_mode = BT_CMD;
          }
}


void bt_searchDevice(void)              //Bluetoothgeräte suchen

{

  char result[8][12];



  for (uint8_t i = 0; i < 8; i++)                 // alte Liste löschen
           for (uint8_t j = 0; j < 12; j++)
                   result[i][j] = 0;
#ifdef DEBUG
   debug_pgm(PSTR("Search Device:BT_discover"));
#endif
   if (bt_discover(result))
   {
           bt_devicecount = 0;
#ifdef DEBUG
           debug_pgm(PSTR("Search Device:Search ok"));
#endif
           for (uint8_t i = 0; i < 8; i++)
           {
                   if (valid(i))
                     bt_devicecount++;
                   else break;
           }
   }
#ifdef DEBUG
   else

           debug_pgm(PSTR("Search Device:Search failed"));
#endif
//    }

}

//
//--------------------------------------------------------------
volatile uint8_t bt_receiveNMEA(void)
{

        char received;
        static uint8_t line_flag = 1;
        static char* ptr_write = bt_rx_buffer;
        uint32_t timeout=400000;

       while(!timeout == 0){

           if (bt_rx_ready == 1)
                  return true;

            uart_receive();
            if (fifo_is_empty(&in_fifo)) timeout--;
            if (fifo_is_empty(&in_fifo)) continue;

            timeout = 400000;
            fifo_read(&in_fifo, &received);
//#ifdef DEBUG
//                        USART_putc(received);
//#endif
                // Find starting point of packet
                if (bt_rx_ready == 0)
                  {

                    if ((received == start) && (line_flag==1))
                      { // start '$'
                        line_flag = 0;                          // New line has begun
                        ptr_write = bt_rx_buffer;                  // Begin at start of buffer
                        bt_rx_len = 0;
//#ifdef DEBUG
//                        debug_pgm(PSTR("NMEA $"));
//#endif
                      }
                    if (line_flag == 0)
                      {                                         // Are we receiving a line?
                            *ptr_write = received;              // Add current byte
                            bt_rx_len++;

                            // GPS Datensatzende

                                   if (received == end)
                                     {  // End of MK-GPS or NMEA-line?
                                       line_flag = 1;            // Yes, start new line
                                       bt_rx_ready = 1;             // Lock buffer until line has been processed

//#ifdef DEBUG
//                        debug_pgm(PSTR("NMEA End"));
//#endif
                                      return true;
                                     }
                      }

                     ptr_write++;
                    if(bt_rx_len == RXD_BUFFER_SIZE) line_flag = 1; // Line too long? Try again
                  }//if (bt_rx_ready == 0)

                }
#ifdef DEBUG
                        debug_pgm(PSTR("bt_receiveNMEA: Timeout"));
#endif

        return false;
}






//
////--------------------------------------------------------------
//uint16_t bt_receiveNMEA2(void)
//{
//
//        char received;
//        static uint8_t line_flag = 1;
//        static char* ptr_write = bt_rx_buffer;
//        uint16_t timeout_ms=2000;
//
////        while_timeout(true, timeout_ms) {
//while(1){
//
//        uart_receive();
//           if (fifo_is_empty(&in_fifo))
//               continue;
//        fifo_read(&in_fifo, &received);
//        USART_putc(received);
////        _delay_ms(1);
//    }
////
//return true;
//}



#endif
#endif
#endif