Subversion Repositories FlightCtrl

/* First version by Bjoern Biesenbach <bjoern@bjoern-b.de> */
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <termios.h>
#include <stdlib.h>

#include "serial.h"
#include "main.h"

static int fd; // File descriptor for serial port

int open_port(char *port)
{
        int fds; /* File descriptor for the port */
       
        fds = open(port, O_RDWR | O_NOCTTY | O_NDELAY);
        if (fds == -1)
        {
                /*
                * Could not open the port.
                */
                perror("open_port: Unable to open serial port!");
                exit(1);
        }
        else
        fcntl(fds, F_SETFL, 0);
       
        return (fds);
}
void AddCRC(unsigned int wieviele)
{
 unsigned int tmpCRC = 0,i;
 for(i = 0; i < wieviele;i++)
  {
   tmpCRC += TxBuffer[i];
  }
   tmpCRC %= 4096;
   TxBuffer[i++] = '=' + tmpCRC / 64;
   TxBuffer[i++] = '=' + tmpCRC % 64;
   TxBuffer[i++] = '\r';
}



void sende()
{
        unsigned char tmp_tx;
        int i=0;
        while(TxBuffer[i] !='\r' && i<150)
        {
                write(fd,&TxBuffer[i],1);
                i++;
        }
        write(fd,"\r",1);
}

void SendOutData(unsigned char cmd,unsigned char modul, unsigned char *buffer, int len)
{
 unsigned int pt = 0,i;

 TxBuffer[pt++] = '#';           // Startzeichen
 TxBuffer[pt++] = modul+'a';     // Adresse (a=0; b=1,...)
 TxBuffer[pt++] = cmd;           // Commando
 if(len)
 for(i = 0;i < len; i+=3)
 {
    TxBuffer[pt++] = '=' + (buffer[i] >> 2);
    TxBuffer[pt++] = '=' + (((buffer[i] & 0x03) << 4) | ((buffer[i+1] & 0xf0) >> 4));
    TxBuffer[pt++] = '=' + ((unsigned char) ((buffer[i+1] & 0x0f) << 2) | ((buffer[i+2] & 0xc0) >> 6));
    TxBuffer[pt++] = '=' + ((unsigned char) buffer[i+2] & 0x3f);
  }
 AddCRC(pt);
 sende();
}

void initSerial(char *port)
{
        struct termios options;
        fd=open_port(port);
        tcgetattr(fd, &options);
        cfsetispeed(&options, B57600);
        cfsetospeed(&options, B57600);
        options.c_cflag |= (CLOCAL | CREAD);
        tcsetattr(fd, TCSANOW, &options);
        fcntl(fd, F_SETFL, FNDELAY);
}