X-Git-Url: http://git.smho.de/gw/?p=owTools.git;a=blobdiff_plain;f=src%2FowARDUINOInterface.cpp;h=4ec1911da92b82e212390298d476e7305e0243ff;hp=33d2814db817d6abf02f9d5810cb4989eff19ac3;hb=1a6465a924428af072a8eb5e75ee547c394f4d8e;hpb=039b202e5c68834801d23e22eecc7cae2879ea83 diff --git a/src/owARDUINOInterface.cpp b/src/owARDUINOInterface.cpp index 33d2814..4ec1911 100644 --- a/src/owARDUINOInterface.cpp +++ b/src/owARDUINOInterface.cpp @@ -1,489 +1,560 @@ -// Copyright (c) 2017, Tobias Mueller tm(at)tm3d.de -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are -// met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the -// distribution. -// * All advertising materials mentioning features or use of this -// software must display the following acknowledgement: This product -// includes software developed by tm3d.de and its contributors. -// * Neither the name of tm3d.de nor the names of its contributors may -// be used to endorse or promote products derived from this software -// without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR -// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT -// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, -// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT -// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY -// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "owARDUINOInterface.h" - -#define MODE_DATA 0xE1 -#define MODE_COMMAND 0xE3 - -// Baud rate bits -#define PARMSET_9600 0x00 -#define PARMSET_19200 0x02 -#define PARMSET_57600 0x04 -#define PARMSET_115200 0x06 - - - -#define COM_IDENTIFER 1 -#define COM_RESET 2 -#define COM_SEARCH_INIT 3 -#define COM_SEARCH_NEXT 4 -#define COM_BLOCK 5 -#define COM_SBYTE 6 -#define COM_RBYTE 7 -#define COM_SBIT 8 -#define COM_RBIT 9 - -//--------------------------------------------------------------------------- -// Description: -// flush the rx and tx buffers -// -// 'portnum' - number 0 to MAX_PORTNUM-1. This number was provided to -// OpenCOM to indicate the port number. -// -void owARDUINOInterface::FlushCOM() { - tcflush(fd, TCIOFLUSH); -} - - -int owARDUINOInterface::OpenCOM(uint8_t comnr) { - struct termios t; // see man termios - declared as above - int rc; - //int fd; - char port_zstr[100]; - if (com_init) return fd; - - sprintf(port_zstr,"/dev/ttyUSB%i",comnr); - - fd = open(port_zstr, O_RDWR|O_NONBLOCK| O_NOCTTY ); - if (fd<0) - { - log->set(OWLOG_ERROR,"ERROR open Com %s return %i",port_zstr,fd); - return fd; - } - rc = tcgetattr (fd, &t); - if (rc < 0) - { - int tmp; - tmp = errno; - close(fd); - errno = tmp; - log->set(OWLOG_ERROR,"OWERROR_SYSTEM_RESOURCE_INIT_FAILED %s",port_zstr); - return rc; // changed (2.00), used to return rc; - } - - cfsetospeed(&t, B9600); - cfsetispeed (&t, B9600); - - // Get terminal parameters. (2.00) removed raw - tcgetattr(fd,&t); - // Save original settings. - origterm = t; - - t.c_cflag &= ~PARENB; // Make 8n1 - t.c_cflag &= ~CSTOPB; - t.c_cflag &= ~CSIZE; - t.c_cflag |= CS8; - - t.c_cflag &= ~CRTSCTS; // no flow control - t.c_cc[VMIN] = 1; // read doesn't block - t.c_cc[VTIME] = 5; // 0.5 seconds read timeout - t.c_cflag |= CREAD | CLOCAL; // turn on READ & ignore ctrl lines - t.c_cflag &= ~CRTSCTS; // no flow control - t.c_iflag &= ~(IXON | IXOFF | IXANY);// turn off s/w flow ctrl -/* Make raw */ - cfmakeraw(&t); - tcflush(fd,TCIOFLUSH); - - rc = tcsetattr(fd, TCSAFLUSH, &t); - - if (rc < 0) - { - int tmp; - tmp = errno; - close(fd); - errno = tmp; - log->set(OWLOG_ERROR,"OWERROR_SYSTEM_RESOURCE_INIT_FAILED %s",port_zstr); - return rc; // changed (2.00), used to return rc; - } - com_init=1; - return fd; // changed (2.00), used to return fd; -} - - -//--------------------------------------------------------------------------- -// Closes the connection to the port. -// -// 'portnum' - number 0 to MAX_PORTNUM-1. This number was provided to -// OpenCOM to indicate the port number. -// -void owARDUINOInterface::CloseCOM() -{ - // restore tty settings - tcsetattr(fd, TCSAFLUSH, &origterm); - FlushCOM(); - close(fd); - com_init=0; - -} - - -//-------------------------------------------------------------------------- -// Write an array of bytes to the COM port, verify that it was -// sent out. Assume that baud rate has been set. -// -// 'portnum' - number 0 to MAX_PORTNUM-1. This number provided will -// be used to indicate the port number desired when calling -// all other functions in this library. -// Returns 1 for success and 0 for failure -// -int owARDUINOInterface::WriteCOM( int outlen, uint8_t *outbuf) -{ - long count = outlen; - int i=0; - for (int k=0;kset(OWLOG_ERROR,"Read Error on Serial"); - - return cnt; - } - } - else { - log->set(OWLOG_ERROR,"Read Error on Serial (select)"); - return cnt; - } - } - - - // success, so return desired length - return inlen; -} - - - - -//-------------------------------------------------------------------------- -// Description: -// Send a break on the com port for at least 2 ms -// -// 'portnum' - number 0 to MAX_PORTNUM-1. This number was provided to -// OpenCOM to indicate the port number. -// -void owARDUINOInterface::BreakCOM() -{ - int duration = 0; // see man termios break may be - tcsendbreak(fd, duration); // too long -} - - -//-------------------------------------------------------------------------- -// Set the baud rate on the com port. -// -// 'portnum' - number 0 to MAX_PORTNUM-1. This number was provided to -// OpenCOM to indicate the port number. -// 'new_baud' - new baud rate defined as -// PARMSET_9600 0x00 -// PARMSET_19200 0x02 -// PARMSET_57600 0x04 -// PARMSET_115200 0x06 -// -void owARDUINOInterface::SetBaudCOM( uint8_t new_baud) -{ - struct termios t; - int rc; - speed_t baud; - - // read the attribute structure - rc = tcgetattr(fd, &t); - if (rc < 0) - { - close(fd); - log->set(OWLOG_ERROR,"Error on Serial (set Boudrate)"); - - return; - } - - // convert parameter to linux baud rate - switch(new_baud) - { - case PARMSET_9600: - baud = B9600; - break; - case PARMSET_19200: - baud = B19200; - break; - case PARMSET_57600: - baud = B57600; - break; - case PARMSET_115200: - baud = B115200; - break; - default: - baud = B9600; - break; - } - - // set baud in structure - cfsetospeed(&t, baud); - cfsetispeed(&t, baud); - - // change baud on port - rc = tcsetattr(fd, TCSAFLUSH, &t); - if (rc < 0) { - log->set(OWLOG_ERROR,"Error on Serial (set Boudrate)"); - - close(fd); - } -} - - - - - - - - - - - - - - - - - -int owARDUINOInterface::InitAdapter(uint8_t nr) { - // attempt to open the communications port - if ((fd = OpenCOM(nr)) < 0) - { - log->set(OWLOG_ERROR,"OWERROR_OPENCOM_FAILED"); - return -1; - } - sleep(2); - uint8_t readbuffer[20],sendpacket[20]; - uint8_t sendlen=0; - sendpacket[sendlen++]=0x01; - sendpacket[sendlen++]=0x00; - sendpacket[sendlen++]=0x03; - if (WriteCOM(sendlen,sendpacket)) { - if ((sendlen=ReadCOM(3,readbuffer)) == 3) { - - // printf("%02X %02X %02X\n",readbuffer[0],readbuffer[1],readbuffer[2] ); - - - - } else { - //printf("%i\n",sendlen); - log->set(OWLOG_ERROR,"OWERROR_READCOM_FAILED"); - } - } else log->set(OWLOG_ERROR,"OWERROR_WRITECOM_FAILED"); - - com_init=1; - - - return 1; -} - - -int owARDUINOInterface::owFirst() { - uint8_t readbuffer[20],sendpacket[20]; - uint8_t sendlen=0; - sendpacket[sendlen++]=0x03; - sendpacket[sendlen++]=0x00; - sendpacket[sendlen++]=0x08; - if (WriteCOM(sendlen,sendpacket)) { - if ((sendlen=ReadCOM(8,readbuffer)) == 8) { - if (readbuffer[0]!=0) { - for (int i=0;i<8;i++) ROM_NO[i]=readbuffer[i]; - return 1; - } - } - } - return 0; - -} -int owARDUINOInterface::owNext() { - uint8_t readbuffer[20],sendpacket[20]; - uint8_t sendlen=0; - sendpacket[sendlen++]=0x04; - sendpacket[sendlen++]=0x00; - sendpacket[sendlen++]=0x08; - if (WriteCOM(sendlen,sendpacket)) { - if ((sendlen=ReadCOM(8,readbuffer)) == 8) { - if (readbuffer[0]!=0) { - for (int i=0;i<8;i++) ROM_NO[i]=readbuffer[i]; - return 1; - } - } - } - return 0; - -} - - -void owARDUINOInterface::ReleaseAdapter() { - CloseCOM(); -} - - -int owARDUINOInterface::Reset() { - uint8_t readbuffer[5],sendpacket[5]; - uint8_t sendlen=0; - /* size_t l; - - if ((l=read(fd,readbuffer,5))>0) { - printf("in buf %i bytes %02X %02X %02X %02X %02X\n",l,readbuffer[0],readbuffer[1],readbuffer[2],readbuffer[3],readbuffer[4]); - }*/ - sendlen=0; - sendpacket[sendlen++]=0x02; - sendpacket[sendlen++]=0x00; - sendpacket[sendlen++]=0x01; - if (WriteCOM(sendlen,sendpacket)) { - if ((sendlen=ReadCOM(1,readbuffer)) == 1) { - // printf("Reset %i\n",readbuffer[0]); - if (readbuffer[0]) return 1; - - } - } - - return 0; -} - - -uint8_t owARDUINOInterface::sendrecivByte(uint8_t byte) { - - uint8_t readbuffer[5],sendpacket[5]; - uint8_t sendlen=0; -/* size_t l; - if ((l=read(fd,readbuffer,5))>0) { - printf("in buf %i bytes %02X %02X %02X %02X %02X\n",l,readbuffer[0],readbuffer[1],readbuffer[2],readbuffer[3],readbuffer[4]); - }*/ - if (byte!=0xFF) { - sendlen=0; - sendpacket[sendlen++]=COM_SBYTE; - sendpacket[sendlen++]=0x01; - sendpacket[sendlen++]=0x01; - sendpacket[sendlen++]=byte; - if (WriteCOM(sendlen,sendpacket)) { - if ((sendlen=ReadCOM(1,readbuffer)) == 1) { - // printf("Send %02X %02X \n",byte,readbuffer[0]); - - return byte; - } - } - } else { - sendlen=0; - sendpacket[sendlen++]=COM_RBYTE; - sendpacket[sendlen++]=0x00; - sendpacket[sendlen++]=0x01; - if (WriteCOM(sendlen,sendpacket)) { - if ((sendlen=ReadCOM(1,readbuffer)) == 1) { - // printf("Recive %02X\n",readbuffer[0]); - return readbuffer[0]; - } - } - - } - - return 0; -} -uint8_t owARDUINOInterface::sendrecivBit(uint8_t bit) { - return 0; -} - -int owARDUINOInterface::Communicate(std::vector *data, int scount, int rcount) { - int i=0; - data->resize(scount); - uint8_t readbuffer[128],sendpacket[128+3]; - uint8_t sendlen=0; - sendpacket[sendlen++]=COM_BLOCK; - sendpacket[sendlen++]=scount; - sendpacket[sendlen++]=rcount; - for(uint8_t v:*data) { - sendpacket[sendlen++]=v; - (*data)[i]=v;i++; - } - if (WriteCOM(sendlen,sendpacket)) { - if ((sendlen=ReadCOM(rcount,readbuffer)) == rcount) { - for(i=0;ipush_back(readbuffer[i]); - } - //printf("\n"); - } - } - - - return 0; -} - +// Copyright (c) 2017, Tobias Mueller tm(at)tm3d.de +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the +// distribution. +// * All advertising materials mentioning features or use of this +// software must display the following acknowledgement: This product +// includes software developed by tm3d.de and its contributors. +// * Neither the name of tm3d.de nor the names of its contributors may +// be used to endorse or promote products derived from this software +// without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR +// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, +// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY +// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifdef LINUX +#include +#include +#include +#include +#include +#endif +#include +#include +#include +#include +#include +#include "owARDUINOInterface.h" + + +#define COM_IDENTIFER 1 +#define COM_RESET 2 +#define COM_SEARCH_INIT 3 +#define COM_SEARCH_NEXT 4 +#define COM_BLOCK 5 +#define COM_SBYTE 6 +#define COM_RBYTE 7 +#define COM_SBIT 8 +#define COM_RBIT 9 + +//--------------------------------------------------------------------------- +// Description: +// flush the rx and tx buffers +// +// 'portnum' - number 0 to MAX_PORTNUM-1. This number was provided to +// OpenCOM to indicate the port number. +// +void owARDUINOInterface::FlushCOM() { +#ifdef LINUX + tcflush(fd, TCIOFLUSH); +#endif +#ifdef WIN + PurgeComm(fd, PURGE_TXABORT | PURGE_RXABORT | + PURGE_TXCLEAR | PURGE_RXCLEAR); +#endif +} + +#ifdef LINUX +int +#endif +#ifdef WIN +HANDLE +#endif +owARDUINOInterface::OpenCOM(uint8_t comnr) +{ + char port_zstr[100]; +#ifdef LINUX + struct termios t; // see man termios - declared as above + int rc; + //int fd; + + if (com_init) return fd; + + sprintf(port_zstr,"/dev/ttyUSB%i",comnr); + + fd = open(port_zstr, O_RDWR|O_NONBLOCK| O_NOCTTY ); + if (fd<0) + { + log->set(OWLOG_ERROR,"ERROR open Com %s return %i",port_zstr,fd); + return fd; + } + rc = tcgetattr (fd, &t); + if (rc < 0) + { + int tmp; + tmp = errno; + close(fd); + errno = tmp; + log->set(OWLOG_ERROR,"OWERROR_SYSTEM_RESOURCE_INIT_FAILED %s",port_zstr); + return rc; // changed (2.00), used to return rc; + } + + cfsetospeed(&t, B9600); + cfsetispeed (&t, B9600); + + // Get terminal parameters. (2.00) removed raw + tcgetattr(fd,&t); + // Save original settings. + origterm = t; + + t.c_cflag &= ~PARENB; // Make 8n1 + t.c_cflag &= ~CSTOPB; + t.c_cflag &= ~CSIZE; + t.c_cflag |= CS8; + + t.c_cflag &= ~CRTSCTS; // no flow control + t.c_cc[VMIN] = 1; // read doesn't block + t.c_cc[VTIME] = 5; // 0.5 seconds read timeout + t.c_cflag |= CREAD | CLOCAL; // turn on READ & ignore ctrl lines + t.c_cflag &= ~CRTSCTS; // no flow control + t.c_iflag &= ~(IXON | IXOFF | IXANY);// turn off s/w flow ctrl +/* Make raw */ + cfmakeraw(&t); + tcflush(fd,TCIOFLUSH); + + rc = tcsetattr(fd, TCSAFLUSH, &t); + + if (rc < 0) + { + int tmp; + tmp = errno; + close(fd); + errno = tmp; + log->set(OWLOG_ERROR,"OWERROR_SYSTEM_RESOURCE_INIT_FAILED %s",port_zstr); + return rc; // changed (2.00), used to return rc; + } + com_init=1; + return fd; // changed (2.00), used to return fd; +#endif +#ifdef WIN + short fRetVal; + COMMTIMEOUTS CommTimeOuts; + DCB dcb; + int comnr1 = comnr; + sprintf_s(port_zstr,100, "COM%d", comnr1); + if (fd <= 0) { + if ((fd = CreateFileA(port_zstr, GENERIC_READ | GENERIC_WRITE, + 0, + NULL, // no security attrs + OPEN_EXISTING, + FILE_FLAG_OVERLAPPED, // overlapped I/O + NULL)) == (HANDLE)-1) { + fd = 0; + log->set(OWLOG_ERROR, "ERROR open Com %s return %i", port_zstr, fd); + return (FALSE); + } else { + // get any early notifications + SetCommMask(fd, EV_RXCHAR | EV_TXEMPTY | EV_ERR | EV_BREAK); + SetupComm(fd, 2048, 2048); + // purge any information in the buffer + PurgeComm(fd, PURGE_TXABORT | PURGE_RXABORT | PURGE_TXCLEAR | PURGE_RXCLEAR); + // set up for overlapped non-blocking I/O + CommTimeOuts.ReadIntervalTimeout = 0; + CommTimeOuts.ReadTotalTimeoutMultiplier = 20; + CommTimeOuts.ReadTotalTimeoutConstant = 40; + CommTimeOuts.WriteTotalTimeoutMultiplier = 20; + CommTimeOuts.WriteTotalTimeoutConstant = 40; + SetCommTimeouts(fd, &CommTimeOuts); + // setup the com port + GetCommState(fd, &dcb); + dcb.BaudRate = CBR_9600; // current baud rate + dcb.fBinary = TRUE; // binary mode, no EOF check + dcb.fParity = FALSE; // enable parity checking + dcb.fOutxCtsFlow = FALSE; // CTS output flow control + dcb.fOutxDsrFlow = FALSE; // DSR output flow control + dcb.fDtrControl = FALSE; // DTR flow control type + dcb.fDsrSensitivity = FALSE; // DSR sensitivity + dcb.fTXContinueOnXoff = FALSE; // XOFF continues Tx + dcb.fOutX = FALSE; // XON/XOFF out flow control + dcb.fInX = FALSE; // XON/XOFF in flow control + dcb.fErrorChar = FALSE; // enable error replacement + dcb.fNull = FALSE; // enable null stripping + dcb.fRtsControl = FALSE; // RTS flow control + dcb.fAbortOnError = FALSE; // abort reads/writes on error + dcb.XonLim = 0; // transmit XON threshold + dcb.XoffLim = 0; // transmit XOFF threshold + dcb.ByteSize = 8; // number of bits/byte, 4-8 + dcb.Parity = NOPARITY; // 0-4=no,odd,even,mark,space + dcb.StopBits = ONESTOPBIT; // 0,1,2 = 1, 1.5, 2 + dcb.XonChar = 0; // Tx and Rx XON character + dcb.XoffChar = 0; // Tx and Rx XOFF character + dcb.ErrorChar = 0; // error replacement character + dcb.EofChar = 0; // end of input character + dcb.EvtChar = 0; // received event character + fRetVal = SetCommState(fd, &dcb); + + } + // check if successfull + if (!fRetVal) { + CloseHandle(fd); + fd = 0; + log->set(OWLOG_ERROR, "OWERROR_SYSTEM_RESOURCE_INIT_FAILED %s", port_zstr); + } + return (fd); + } + return 0; +#endif +} + + +//--------------------------------------------------------------------------- +// Closes the connection to the port. +// +// 'portnum' - number 0 to MAX_PORTNUM-1. This number was provided to +// OpenCOM to indicate the port number. +// +void owARDUINOInterface::CloseCOM() +{ +#ifdef LINUX + // restore tty settings + tcsetattr(fd, TCSAFLUSH, &origterm); + FlushCOM(); + close(fd); +#endif +#ifdef WIN + // disable event notification and wait for thread + // to halt + SetCommMask(fd, 0); + + PurgeComm(fd, PURGE_TXABORT | PURGE_RXABORT | PURGE_TXCLEAR | PURGE_RXCLEAR); + CloseHandle(fd); + fd = 0; +#endif + com_init = 0; + +} + + +//-------------------------------------------------------------------------- +// Write an array of bytes to the COM port, verify that it was +// sent out. Assume that baud rate has been set. +// +// 'portnum' - number 0 to MAX_PORTNUM-1. This number provided will +// be used to indicate the port number desired when calling +// all other functions in this library. +// Returns 1 for success and 0 for failure +// +int owARDUINOInterface::WriteCOM( int outlen, uint8_t *outbuf) { +#ifdef LINUX + long count = outlen; + int i=0; + for (int k=0;kset(OWLOG_ERROR, "SERIAL_ERROR_IO_PENDING"); + WaitForSingleObject(osWrite.hEvent, to); + // verify all is written correctly + + fWriteStat = GetOverlappedResult(fd, &osWrite, &dwBytesWritten, FALSE); + } + // check results of write + if (!fWriteStat || (dwBytesWritten != (DWORD)outlen)) + return 0; + else + return 1; +#endif +} + + +//-------------------------------------------------------------------------- +// Read an array of bytes to the COM port, verify that it was +// sent out. Assume that baud rate has been set. +// +// 'portnum' - number 0 to MAX_PORTNUM-1. This number was provided to +// OpenCOM to indicate the port number. +// 'outlen' - number of bytes to write to COM port +// 'outbuf' - pointer ot an array of bytes to write +// +// Returns: TRUE(1) - success +// FALSE(0) - failure +// +int owARDUINOInterface::ReadCOM( int inlen, uint8_t *inbuf) +{ +#ifdef LINUX + fd_set filedescr; + struct timeval tval; + int cnt; + + // loop to wait until each byte is available and read it + for (cnt = 0; cnt < inlen; cnt++) + { + // set a descriptor to wait for a character available + FD_ZERO(&filedescr); + FD_SET(fd,&filedescr); + // set timeout to 10ms + tval.tv_sec = 10; + tval.tv_usec = 10000; + + // if byte available read or return bytes read + if (select(fd+1,&filedescr,NULL,NULL,&tval) != 0) + { + if (read(fd,&inbuf[cnt],1) != 1) { + log->set(OWLOG_ERROR,"Read Error on Serial"); + + return cnt; + } + } + else { + log->set(OWLOG_ERROR,"Read Error on Serial (select)"); + return cnt; + } + } + + + // success, so return desired length + return inlen; +#endif +#ifdef WIN + DWORD dwLength = 0; + BOOL fReadStat; + DWORD ler = 0, to; + OVERLAPPED osReader = { 0 }; + // calculate a timeout + to = 20 * inlen + 60; + // read + osReader.hEvent = CreateEvent(NULL, TRUE, FALSE, NULL); + ResetEvent(osReader.hEvent); + fReadStat = ReadFile(fd, (LPSTR)&inbuf[0], inlen, &dwLength, &osReader); + //Sleep(100); + // check for an error + if (!fReadStat) { + ler = GetLastError(); + // log->set(OWLOG_ERROR, "Read Error on Serial"); + } + // if not done writing then wait + int e = 0; + while (!fReadStat && ler == ERROR_IO_PENDING) { + // wait until everything is read + // log->set(OWLOG_ERROR, "Read Error on Serial"); + // printf("try Read %i\n", e); e++; + WaitForSingleObject(osReader.hEvent, to); + // verify all is read correctly + fReadStat = GetOverlappedResult(fd, &osReader, &dwLength, FALSE); + //printf("%i %i\n", dwLength, inlen); + if (dwLength == inlen) break; + if (e == 3) break; + printf("repeat\n"); + + } + // check results + if (fReadStat) + return dwLength; + else + return 0; +#endif +} + + +/* + +//-------------------------------------------------------------------------- +// Description: +// Send a break on the com port for at least 2 ms +// +// 'portnum' - number 0 to MAX_PORTNUM-1. This number was provided to +// OpenCOM to indicate the port number. +// +void owARDUINOInterface::BreakCOM() +{ + int duration = 0; // see man termios break may be + tcsendbreak(fd, duration); // too long +} + +*/ + + + + + + + + + +int owARDUINOInterface::InitAdapter(uint8_t nr) { + // attempt to open the communications port + if ((fd = OpenCOM(nr)) < 0) + { + log->set(OWLOG_ERROR,"OWERROR_OPENCOM_FAILED"); + return -1; + } +#ifdef WIN + Sleep(2000); +#endif +#ifdef LINUX + sleep(2); +#endif + uint8_t readbuffer[20],sendpacket[20]; + uint8_t sendlen=0; + sendpacket[sendlen++]=0x01; + sendpacket[sendlen++]=0x00; + sendpacket[sendlen++]=0x03; + if (WriteCOM(sendlen,sendpacket)) { + if ((sendlen=ReadCOM(3,readbuffer)) == 3) { + // printf("%02X %02X %02X\n",readbuffer[0],readbuffer[1],readbuffer[2] ); + } else { + log->set(OWLOG_ERROR,"OWERROR_READCOM_FAILED"); + } + } else log->set(OWLOG_ERROR,"OWERROR_WRITECOM_FAILED"); + com_init=1; + return 1; +} + + +int owARDUINOInterface::owFirst() { + uint8_t readbuffer[20],sendpacket[20]; + uint8_t sendlen=0; + sendpacket[sendlen++]=0x03; + sendpacket[sendlen++]=0x00; + sendpacket[sendlen++]=0x08; + if (WriteCOM(sendlen,sendpacket)) { + if ((sendlen=ReadCOM(8,readbuffer)) == 8) { + if (readbuffer[0]!=0) { + for (int i=0;i<8;i++) ROM_NO[i]=readbuffer[i]; + return 1; + } + } + } + return 0; + +} +int owARDUINOInterface::owNext() { + uint8_t readbuffer[20],sendpacket[20]; + uint8_t sendlen=0; + sendpacket[sendlen++]=0x04; + sendpacket[sendlen++]=0x00; + sendpacket[sendlen++]=0x08; + if (WriteCOM(sendlen,sendpacket)) { + if ((sendlen=ReadCOM(8,readbuffer)) == 8) { + if (readbuffer[0]!=0) { + for (int i=0;i<8;i++) ROM_NO[i]=readbuffer[i]; + return 1; + } + } + } + return 0; + +} + + +void owARDUINOInterface::ReleaseAdapter() { + CloseCOM(); +} + + +int owARDUINOInterface::Reset() { + uint8_t readbuffer[5],sendpacket[5]; + uint8_t sendlen=0; + sendlen=0; + sendpacket[sendlen++]=0x02; + sendpacket[sendlen++]=0x00; + sendpacket[sendlen++]=0x01; + if (WriteCOM(sendlen,sendpacket)) { + if ((sendlen=ReadCOM(1,readbuffer)) == 1) { + if (readbuffer[0]) return 1; + } + } + + return 0; +} + + +uint8_t owARDUINOInterface::sendrecivByte(uint8_t byte) { + + uint8_t readbuffer[5],sendpacket[5]; + uint8_t sendlen=0; + if (byte!=0xFF) { + sendlen=0; + sendpacket[sendlen++]=COM_SBYTE; + sendpacket[sendlen++]=0x01; + sendpacket[sendlen++]=0x01; + sendpacket[sendlen++]=byte; + if (WriteCOM(sendlen,sendpacket)) { + if ((sendlen=ReadCOM(1,readbuffer)) == 1) { + return byte; + } + } + } else { + sendlen=0; + sendpacket[sendlen++]=COM_RBYTE; + sendpacket[sendlen++]=0x00; + sendpacket[sendlen++]=0x01; + if (WriteCOM(sendlen,sendpacket)) { + if ((sendlen=ReadCOM(1,readbuffer)) == 1) { + return readbuffer[0]; + } + } + + } + + return 0; +} +uint8_t owARDUINOInterface::sendrecivBit(uint8_t bit) { + return 0; +} + +int owARDUINOInterface::Communicate(std::vector *data, int scount, int rcount) { + int i=0; + data->resize(scount); + uint8_t readbuffer[128],sendpacket[128+3]; + uint8_t sendlen=0; + sendpacket[sendlen++]=COM_BLOCK; + sendpacket[sendlen++]=scount; + sendpacket[sendlen++]=rcount; + for(uint8_t v:*data) { + sendpacket[sendlen++]=v; + (*data)[i]=v;i++; + } + + if (WriteCOM(sendlen,sendpacket)) { + if ((sendlen=ReadCOM(rcount,readbuffer)) == rcount) { + for(i=0;ipush_back(readbuffer[i]); + } + } else { + for (i = 0; ipush_back(0xFF); + } + } + return 0; +} +