Subversion Repositories Projects

#include "main.h"
#include "uart.h"

int uart0_computer = -1;
int uart1_kopter = -1;
int uart2_flow = -1;
int uart3_gps = -1;

//>> Storing Chars one by one
//------------------------------------------------------------------------------------------------------
u8 get_char(serial_data_struct* pdata_package){
        if(pdata_package->count--)
        {
                pdata_package->c = pdata_package->txrxdata[pdata_package->position++];
                return(1);
        }else{return(0);}
}

//>> Searching for initiation-character and storing message
//------------------------------------------------------------------------------------------------------
u8 collect_data(serial_data_struct* pdata_package){
        if(pdata_package->c == '#')
        {
                pdata_package->collecting = 1;
        }
        if(pdata_package->collecting)
        {
                pdata_package->collecting_data[pdata_package->position_package++] = pdata_package->c;
                if(pdata_package->c == '\r'){
                        return(0);
                }
        }
        return(1);
}

//>> Searching for initiation-character and storing message (for Flow Control)
//------------------------------------------------------------------------------------------------------
u8 collect_data_flow(serial_data_struct* pdata_package){
        if(pdata_package->c == 254)
        {
                pdata_package->collecting = 1;
        }
        if(pdata_package->collecting)
        {
                pdata_package->collecting_data[pdata_package->position_package++] = pdata_package->c;
                if(pdata_package->position_package == pdata_package->length_payload + 8){
                        return(0);
                }
        }
        return(1);
}

//>> Searching for initiation-character and storing message (for GPS)
//------------------------------------------------------------------------------------------------------
u8 collect_data_gps(serial_data_struct* pdata_package){
        if(pdata_package->c == '$')
        {
                pdata_package->collecting = 1;
        }
        if(pdata_package->collecting)
        {
                if(pdata_package->c == ','){
                        pdata_package->position_package_gps++;
                        pdata_package->position_package = 0;
                }else{
                        pdata_package->gps_data[pdata_package->position_package_gps][pdata_package->position_package++] = pdata_package->c;
                }
                if(pdata_package->c == '\r'){
                        return(0);
                }
        }
        return(1);
}

float tmpflow = 0.0;
//>> Receiving Data from the Optical Flow Module and processing it
//------------------------------------------------------------------------------------------------------
void receive_data_from_flow(serial_data_struct* pdata_package_flow){
        if (uart2_flow != -1)
        {
                int rx_length = read(uart2_flow, (void*)pdata_package_flow->txrxdata, 1024);
                if (rx_length > 0)
                {
                        pdata_package_flow->cmdLength = rx_length;

                        pdata_package_flow->count = rx_length;
                        pdata_package_flow->position = 0;
                        while(get_char(pdata_package_flow))
                        {
                                if(!collect_data_flow(pdata_package_flow))
                                {
                                        pdata_package_flow->collecting = 0;
                                        pdata_package_flow->dataLength = pdata_package_flow->position_package;
                                        pdata_package_flow->position_package = 0;
                                //|----------------------------Process Flow Data----------------------------|//
                                //|                                                                                                                                             |//    
                                        process_flow_data(pdata_package_flow);
                                        //perform_flight_correction();
                                //|                                                                                                                                             |//
                                //|----------------------------Process Flow Data----------------------------|//
                                        memset(pdata_package_flow->collecting_data, 0, 1023);
                                }else{
                                        if(pdata_package_flow->position_package == 6){
                                                pdata_package_flow->length_payload = pdata_package_flow->collecting_data[1];
                                                pdata_package_flow->msg_id = pdata_package_flow->collecting_data[5];
                                        }
                                        if(pdata_package_flow->collecting_data[0] != 254){
                                                memset(pdata_package_flow->collecting_data, 0, 1023);
                                                memset(pdata_package_flow->txrxdata, 0, 1023);
                                                pdata_package_flow->collecting = 0;
                                                pdata_package_flow->position_package = 0;
                                                pdata_package_flow->msg_id = 0;
                                        }
                                }
                        }
                memset(pdata_package_flow->txrxdata, 0, 1023);
                       
                }
        }else{
                printf("FLOW RX Error\n");
        }
}

//>> Receiving Data from the MikroKopter-tool und transmitting it to the kopter
//------------------------------------------------------------------------------------------------------
void receive_data_from_computer(serial_data_struct* pdata_package_receive_computer, u8 psendThrough){
        if (uart0_computer != -1)
        {
                memset(pdata_package_receive_computer->txrxdata, 0, 1023);
                int rx_length = read(uart0_computer, (void*)pdata_package_receive_computer->txrxdata, 256);             //Filestream, buffer to store in, number of bytes to read (max)
                if (rx_length > 0)
                {
                        pdata_package_receive_computer->cmdLength = rx_length;
                        transmit_data(TO_KOPTER, pdata_package_receive_computer);

                        if(!psendThrough){
                        pdata_package_receive_computer->count = rx_length;
                        pdata_package_receive_computer->position = 0;
                        while(get_char(pdata_package_receive_computer))
                        {
                                if(!collect_data(pdata_package_receive_computer))
                                {
                                        pdata_package_receive_computer->collecting = 0;
                                        pdata_package_receive_computer->dataLength = pdata_package_receive_computer->position_package;
                                        pdata_package_receive_computer->position_package = 0;
                                        u16 crc = 0;
                                        u8      crc1, crc2;
                                        for (int i = 0; i < pdata_package_receive_computer->dataLength - 3; i++)
                                        {
                                                crc += pdata_package_receive_computer->collecting_data[i];
                                        }
                                        crc %= 4096;
                                        crc1 = '=' + crc / 64;
                                        crc2 = '=' + crc % 64;
                                        if(crc1 == pdata_package_receive_computer->collecting_data[pdata_package_receive_computer->dataLength - 3] && crc2 == pdata_package_receive_computer->collecting_data[pdata_package_receive_computer->dataLength - 2])
                                        {
                                                analyze_data_package(pdata_package_receive_computer);
                                                //>> CRC OK
                                                pdata_package_receive_computer->position_package = 0;
                                        }else{
                                                //>> CRC NOT OK
                                                printf("RX Checksum Error\n");
                                                pdata_package_receive_computer->position_package = 0;
                                                memset(pdata_package_receive_computer->collecting_data, 0, 1023);
                                        }
                                }
                        }
                        }
                }
        }else{
                printf("RX Error\n");
        }
}

//>> Receiving Data from the Kopter and collecting serial frame if needed
//------------------------------------------------------------------------------------------------------
void receive_data_from_kopter(serial_data_struct* pdata_package_receive_kopter){
        if (uart1_kopter != -1)
        {
                memset(pdata_package_receive_kopter->txrxdata, 0, 1023);
                int rx_length = read(uart1_kopter, (void*)pdata_package_receive_kopter->txrxdata, 256);         //Filestream, buffer to store in, number of bytes to read (max)
                if (rx_length > 0)
                {
                        pdata_package_receive_kopter->cmdLength = rx_length;
                        pdata_package_receive_kopter->count = rx_length;
                        pdata_package_receive_kopter->position = 0;
                        while(get_char(pdata_package_receive_kopter))
                        {
                                if(!collect_data(pdata_package_receive_kopter))
                                {
                                        pdata_package_receive_kopter->collecting = 0;
                                        pdata_package_receive_kopter->dataLength = pdata_package_receive_kopter->position_package;
                                        pdata_package_receive_kopter->position_package = 0;
                                        u16 crc = 0;
                                        u8      crc1, crc2;
                                        for (int i = 0; i < pdata_package_receive_kopter->dataLength - 3; i++)
                                        {
                                                crc += pdata_package_receive_kopter->collecting_data[i];
                                        }
                                        crc %= 4096;
                                        crc1 = '=' + crc / 64;
                                        crc2 = '=' + crc % 64;
                                        if(crc1 == pdata_package_receive_kopter->collecting_data[pdata_package_receive_kopter->dataLength - 3] && crc2 == pdata_package_receive_kopter->collecting_data[pdata_package_receive_kopter->dataLength - 2])
                                        {
                                                analyze_data_package(pdata_package_receive_kopter);
                                                //>> CRC OK
                                                pdata_package_receive_kopter->position_package = 0;
                                        }else{
                                                //>> CRC NOT OK
                                                printf("RX Checksum Error\n");
                                                pdata_package_receive_kopter->position_package = 0;
                                                memset(pdata_package_receive_kopter->collecting_data, 0, 1023);
                                        }
                                }
                        }
                        transmit_data(TO_COMPUTER, pdata_package_receive_kopter);
                }
        }else{
                printf("KOPTER RX Error\n");
        }
}

//>> Receiving Data from the Optical Flow Module and processing it
//------------------------------------------------------------------------------------------------------
void receive_data_from_gps(serial_data_struct* pdata_package_gps){
        if (uart3_gps != -1)
        {
                int rx_length = read(uart3_gps, (void*)pdata_package_gps->txrxdata, 1024);
                if (rx_length > 0)
                {
                        pdata_package_gps->cmdLength = rx_length;
                        pdata_package_gps->count = rx_length;
                        pdata_package_gps->position = 0;
                        while(get_char(pdata_package_gps))
                        {
                                if(!collect_data_gps(pdata_package_gps))
                                {
                                        pdata_package_gps->collecting = 0;
                                        pdata_package_gps->dataLength = pdata_package_gps->position_package_gps;
                                        pdata_package_gps->position_package = 0;
                                        pdata_package_gps->position_package_gps = 0;
                                //|----------------------------Process Flow Data----------------------------|//
                                //|                            
                                        for (int i = 0; i < pdata_package_gps->dataLength; ++i)
                                        {
                                                process_gps_data(pdata_package_gps);
                                                pdata_package_gps->position_package++;
                                        }
                                        pdata_package_gps->position_package = 0;
                                               
                                //|                                                                                                                                             |//
                                //|----------------------------Process Flow Data----------------------------|//
                                        memset(pdata_package_gps->gps_data, 0, 30*20*sizeof(pdata_package_gps->gps_data[0][0]));
                                }
                        }
                memset(pdata_package_gps->txrxdata, 0, 1023);           }
        }else{
                printf("GPS RX Error\n");
        }
}


//>> Transmitting data
//------------------------------------------------------------------------------------------------------
void transmit_data(int uart_filestream, serial_data_struct* pdata_package_transmit){
        if (uart_filestream == 1)
        {
                uart_filestream = uart0_computer;
                /*
                printf("TO COMPUTER\n");
                printf("%s\n", pdata_package_transmit->txrxdata);
                */
        }else if(uart_filestream == 2)
        {
                uart_filestream = uart1_kopter;
                /*
                printf("TO KOPTER\n");
                printf("%s\n", pdata_package_transmit->txrxdata);
               
                for (int i = 0; i < pdata_package_transmit->cmdLength; ++i)
                {
                        printf("%d ", pdata_package_transmit->txrxdata[i]);
                       
                }
                printf("\n");
                */
        }else if(uart_filestream == 3){
                uart_filestream = uart2_flow;
        }
        if (uart_filestream != -1)
        {
                int count = write(uart_filestream, pdata_package_transmit->txrxdata, pdata_package_transmit->cmdLength);       
                if (count < 0)         
                {
                        //printf("UART TX error: %d\n", uart_filestream);
                }
        }
}

//>> Adding address and command ID, encoding and checksum
//------------------------------------------------------------------------------------------------------
void create_serial_frame(u8 address, u8 cmdID, u16 cmdLength, serial_data_struct* pdata_package_create){
        pdata_package_create->txrxdata[0] = '\r';
        pdata_package_create->txrxdata[1] = '#';
        pdata_package_create->txrxdata[2] = address + 'a';
        pdata_package_create->txrxdata[3] = cmdID;
        //encode data
        u8 a,b,c;
        int counter = 0;
        int u = 4;
        while(cmdLength){
                if(cmdLength)
                        {
                                cmdLength--;
                                a = pdata_package_create->data[counter++];
                        }else{a = 0; counter++;};
                if(cmdLength)
                        {
                                cmdLength--;
                                b = pdata_package_create->data[counter++];
                        }else{b = 0; counter++;};
                if(cmdLength)
                        {
                                cmdLength--;
                                c = pdata_package_create->data[counter++];
                        }else{c = 0; counter++;};
                pdata_package_create->txrxdata[u++] = '=' + (a >> 2);
                pdata_package_create->txrxdata[u++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4));
                pdata_package_create->txrxdata[u++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6));
                pdata_package_create->txrxdata[u++] = '=' + (c & 0x3f);
        }      
        //add Checksum
        u16 crc = 0;
        u8      crc1, crc2;
        for (int i = 1; i < u; i++)
        {
                crc += pdata_package_create->txrxdata[i];
        }
        crc %= 4096;
        crc1 = '=' + crc / 64;
        crc2 = '=' + crc % 64;
        pdata_package_create->txrxdata[u++] = crc1;
        pdata_package_create->txrxdata[u++] = crc2;
        pdata_package_create->txrxdata[u++] = '\r';

        pdata_package_create->cmdLength = u;
}

//>> Decoding Data and retrieving address and cmdID
//------------------------------------------------------------------------------------------------------
void collect_serial_frame(serial_data_struct* pdata_package_collect){
        pdata_package_collect->position_package++;                                                                                                                                                                                      //first character: #
        pdata_package_collect->address = pdata_package_collect->collecting_data[pdata_package_collect->position_package++] - 'a';       //address
        pdata_package_collect->cmdID = pdata_package_collect->collecting_data[pdata_package_collect->position_package++];                       //cmdID
        u8 a, b, c, d;
        u16 datacpy = 0;
        while(pdata_package_collect->position_package < pdata_package_collect->dataLength-3){
                a = pdata_package_collect->collecting_data[pdata_package_collect->position_package++] - '=';
                b = pdata_package_collect->collecting_data[pdata_package_collect->position_package++] - '=';
                c = pdata_package_collect->collecting_data[pdata_package_collect->position_package++] - '=';
                d = pdata_package_collect->collecting_data[pdata_package_collect->position_package++] - '=';

                pdata_package_collect->data[datacpy++] = (a << 2) | (b >> 4);
                pdata_package_collect->data[datacpy++] = ((b & 0x0f) << 4) | (c >> 2);
                pdata_package_collect->data[datacpy++] = ((c & 0x03) << 6) | d;
        }
        pdata_package_collect->dataLength = datacpy;
        pdata_package_collect->position_package = 0;
       
/*
        printf("%s\n", pdata_package_collect->collecting_data);
       
        for (int i = 0; i < datacpy; ++i)
        {
                printf("|%d|: %d ", i, pdata_package_collect->data[i]);
        }

        printf("%s\n", pdata_package_collect->data);
*/     
}


//>> Initializing serial interface
//------------------------------------------------------------------------------------------------------
void uart_init(){
        uart0_computer = open(SERIAL_COMPUTER, O_RDWR | O_NOCTTY | O_NDELAY);
        uart1_kopter = open(SERIAL_KOPTER,O_RDWR | O_NOCTTY | O_NDELAY);
        uart2_flow = open(SERIAL_FLOW,O_RDWR | O_NOCTTY | O_NDELAY);
        uart3_gps = open(SERIAL_GPS,O_RDWR | O_NOCTTY | O_NDELAY);
        if (uart0_computer == -1)
        {
                printf("Error - Unable to open UART0 (Computer)!\n");
        }
        if (uart1_kopter == -1)
        {
                printf("Error - Unable to open UART1 (Kopter)!\n");
        }
        if (uart2_flow == -1)
        {
                printf("Error - Unable to open UART2 (FLOW)!\n");
        }
        if (uart3_gps == -1)
        {
                printf("Error - Unable to open UART2 (GPS)!\n");
        }
        //UART Options
        struct termios options;
        tcgetattr(uart0_computer, &options);
        options.c_cflag = B57600 | CS8 | CLOCAL | CREAD;               
        options.c_iflag = IGNPAR;
        options.c_oflag = 0;
        options.c_lflag = 0;
        tcflush(uart0_computer, TCIFLUSH);
        tcsetattr(uart0_computer, TCSANOW, &options);
        tcflush(uart1_kopter, TCIFLUSH);
        tcsetattr(uart1_kopter, TCSANOW, &options);
        tcflush(uart2_flow, TCIFLUSH);
        tcsetattr(uart2_flow, TCSANOW, &options);
        //UART Options
       
        struct termios options_flow;
        tcgetattr(uart3_gps, &options_flow);
        options_flow.c_cflag = B9600 | CS8 | CLOCAL | CREAD;           
        options.c_iflag = IGNPAR;
        options_flow.c_oflag = 0;
        options_flow.c_lflag = 0;
        tcflush(uart3_gps, TCIFLUSH);
        tcsetattr(uart3_gps, TCSANOW, &options_flow);
}