eive-obsw/bsp_hosted/comIF/ArduinoComIF.cpp

#include <bsp_hosted/comIF/ArduinoComIF.h>
#include <bsp_hosted/comIF/ArduinoCookie.h>
#include <fsfw/globalfunctions/DleEncoder.h>
#include <fsfw/globalfunctions/CRC.h>
#include <fsfw/serviceinterface/ServiceInterfaceStream.h>

// This only works on Linux
#ifdef LINUX
#include <termios.h>
#elif WIN32
#include <windows.h>
#include <strsafe.h>
#endif

#include <cstring>

ArduinoComIF::ArduinoComIF(object_id_t setObjectId, bool promptComIF,
        const char *serialDevice):
		rxBuffer(MAX_PACKET_SIZE * MAX_NUMBER_OF_SPI_DEVICES*10, true),
		SystemObject(setObjectId) {
#ifdef LINUX
	initialized = false;
	serialPort = ::open("/dev/ttyUSB0", O_RDWR);

	if (serialPort < 0) {
		//configuration error
		printf("Error %i from open: %s\n", errno, strerror(errno));
		return;
	}

	struct termios tty;
	memset(&tty, 0, sizeof tty);

	// Read in existing settings, and handle any error
	if (tcgetattr(serialPort, &tty) != 0) {
		printf("Error %i from tcgetattr: %s\n", errno, strerror(errno));
		return;
	}

	tty.c_cflag &= ~PARENB; // Clear parity bit, disabling parity
	tty.c_cflag &= ~CSTOPB; // Clear stop field, only one stop bit used in communication
	tty.c_cflag |= CS8; // 8 bits per byte
	tty.c_cflag &= ~CRTSCTS; // Disable RTS/CTS hardware flow control
	tty.c_lflag &= ~ICANON; //Disable Canonical Mode
	tty.c_oflag &= ~OPOST; // Prevent special interpretation of output bytes (e.g. newline chars)
	tty.c_oflag &= ~ONLCR; // Prevent conversion of newline to carriage return/line feed
	tty.c_cc[VTIME] = 0;    // Non Blocking
	tty.c_cc[VMIN] = 0;

	cfsetispeed(&tty, B9600); //Baudrate

	if (tcsetattr(serialPort, TCSANOW, &tty) != 0) {
		//printf("Error %i from tcsetattr: %s\n", errno, strerror(errno));
		return;
	}

	initialized = true;
#elif WIN32
	DCB serialParams = { 0 };

	// we need to ask the COM port from the user.
	if(promptComIF) {
	    sif::info << "Please enter the COM port (c to cancel): " << std::flush;
	    std::string comPort;
	    while(hCom == INVALID_HANDLE_VALUE) {

	        std::getline(std::cin, comPort);
	        if(comPort[0] == 'c') {
	            break;
	        }
	        const TCHAR *pcCommPort = comPort.c_str();
	        hCom = CreateFileA(pcCommPort,        //port name
	                GENERIC_READ | GENERIC_WRITE, //Read/Write
	                0,                            // No Sharing
	                NULL,                         // No Security
	                OPEN_EXISTING,// Open existing port only
	                0,            // Non Overlapped I/O
	                NULL);        // Null for Comm Devices

	        if (hCom == INVALID_HANDLE_VALUE)
	        {
	            if(GetLastError() == 2) {
	                sif::error << "COM Port does not found!" << std::endl;
	            }
	            else {
	                TCHAR err[128];
	                FormatMessage(FORMAT_MESSAGE_FROM_SYSTEM, NULL,
	                        GetLastError(),
	                        MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT),
	                        err, sizeof(err), NULL);
	                //  Handle the error.
	                sif::info << "CreateFileA Error code: " << GetLastError()
	                         << std::endl;
	                sif::error << err << std::flush;
	            }
	            sif::info << "Please enter a valid COM port: " << std::flush;
	        }
	    }

	}


	serialParams.DCBlength = sizeof(serialParams);
	if(baudRate == 9600) {
	    serialParams.BaudRate = CBR_9600;
	}
	if(baudRate == 115200) {
	    serialParams.BaudRate = CBR_115200;
	}
	else {
	    serialParams.BaudRate = baudRate;
	}

	serialParams.ByteSize = 8;
	serialParams.Parity = NOPARITY;
	serialParams.StopBits = ONESTOPBIT;
	SetCommState(hCom, &serialParams);

	COMMTIMEOUTS timeout = { 0 };
	// This will set the read operation to be blocking until data is received
	// and then read continuously until there is a gap of one millisecond.
	timeout.ReadIntervalTimeout = 1;
	timeout.ReadTotalTimeoutConstant = 0;
	timeout.ReadTotalTimeoutMultiplier = 0;
	timeout.WriteTotalTimeoutConstant = 0;
	timeout.WriteTotalTimeoutMultiplier = 0;
	SetCommTimeouts(hCom, &timeout);
	// Serial port should now be read for operations.
#endif
}

ArduinoComIF::~ArduinoComIF() {
#ifdef LINUX
	::close(serialPort);
#elif WIN32
	CloseHandle(hCom);
#endif
}
ReturnValue_t ArduinoComIF::initializeInterface(CookieIF * cookie) {
    return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t ArduinoComIF::sendMessage(CookieIF *cookie, const uint8_t *data,
		size_t len) {
	ArduinoCookie *arduinoCookie = dynamic_cast<ArduinoCookie*>(cookie);
	if (arduinoCookie == nullptr) {
		return INVALID_COOKIE_TYPE;
	}

	return sendMessage(arduinoCookie->command, arduinoCookie->address, data,
			len);
}

ReturnValue_t ArduinoComIF::getSendSuccess(CookieIF *cookie) {
	return RETURN_OK;
}

ReturnValue_t ArduinoComIF::requestReceiveMessage(CookieIF *cookie,
        size_t requestLen) {
	return RETURN_OK;
}

ReturnValue_t ArduinoComIF::readReceivedMessage(CookieIF *cookie,
		uint8_t **buffer, size_t *size) {

	handleSerialPortRx();

	ArduinoCookie *arduinoCookie = dynamic_cast<ArduinoCookie*>(cookie);
	if (arduinoCookie == nullptr) {
		return INVALID_COOKIE_TYPE;
	}

	*buffer = arduinoCookie->replyBuffer.data();
	*size = arduinoCookie->receivedDataLen;
	return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t ArduinoComIF::sendMessage(uint8_t command,
		uint8_t address, const uint8_t *data, size_t dataLen) {
	if (dataLen > UINT16_MAX) {
		return TOO_MUCH_DATA;
	}

	//being conservative here
	uint8_t sendBuffer[(dataLen + 6) * 2 + 2];

	sendBuffer[0] = DleEncoder::STX_CHAR;

	uint8_t *currentPosition = sendBuffer + 1;
	size_t remainingLen = sizeof(sendBuffer) - 1;
	size_t encodedLen = 0;

	ReturnValue_t result = DleEncoder::encode(&command, 1, currentPosition,
			remainingLen, &encodedLen, false);
	if (result != RETURN_OK) {
		return result;
	}
	currentPosition += encodedLen;
	remainingLen -= encodedLen; //DleEncoder will never return encodedLen > remainingLen

	result = DleEncoder::encode(&address, 1, currentPosition, remainingLen,
			&encodedLen, false);
	if (result != RETURN_OK) {
		return result;
	}
	currentPosition += encodedLen;
	remainingLen -= encodedLen; //DleEncoder will never return encodedLen > remainingLen

	uint8_t temporaryBuffer[2];

	//note to Lukas: yes we _could_ use Serialize here, but for 16 bit it is a bit too much...
	temporaryBuffer[0] = dataLen >> 8; //we checked dataLen above
	temporaryBuffer[1] = dataLen;

	result = DleEncoder::encode(temporaryBuffer, 2, currentPosition,
			remainingLen, &encodedLen, false);
	if (result != RETURN_OK) {
		return result;
	}
	currentPosition += encodedLen;
	remainingLen -= encodedLen; //DleEncoder will never return encodedLen > remainingLen

	//encoding the actual data
	result = DleEncoder::encode(data, dataLen, currentPosition, remainingLen,
			&encodedLen, false);
	if (result != RETURN_OK) {
		return result;
	}
	currentPosition += encodedLen;
	remainingLen -= encodedLen; //DleEncoder will never return encodedLen > remainingLen

	uint16_t crc = CRC::crc16ccitt(&command, 1);
	crc = CRC::crc16ccitt(&address, 1, crc);
	//fortunately the length is still there
	crc = CRC::crc16ccitt(temporaryBuffer, 2, crc);
	crc = CRC::crc16ccitt(data, dataLen, crc);

	temporaryBuffer[0] = crc >> 8;
	temporaryBuffer[1] = crc;

	result = DleEncoder::encode(temporaryBuffer, 2, currentPosition,
			remainingLen, &encodedLen, false);
	if (result != RETURN_OK) {
		return result;
	}
	currentPosition += encodedLen;
	remainingLen -= encodedLen; //DleEncoder will never return encodedLen > remainingLen

	if (remainingLen > 0) {
		*currentPosition = DleEncoder::ETX_CHAR;
	}
	remainingLen -= 1;

	encodedLen = sizeof(sendBuffer) - remainingLen;

#ifdef LINUX
	ssize_t writtenlen = write(serialPort, sendBuffer, encodedLen);
	if (writtenlen < 0) {
		//we could try to find out what happened...
		return RETURN_FAILED;
	}
	if (writtenlen != encodedLen) {
		//the OS failed us, we do not try to block until everything is written, as
		//we can not block the whole system here
		return RETURN_FAILED;
	}
	return RETURN_OK;
#elif WIN32
	return HasReturnvaluesIF::RETURN_OK;
#endif
}

void ArduinoComIF::handleSerialPortRx() {
#ifdef LINUX
	uint32_t availableSpace = rxBuffer.availableWriteSpace();

	uint8_t dataFromSerial[availableSpace];

	ssize_t bytesRead = read(serialPort, dataFromSerial,
			sizeof(dataFromSerial));

	if (bytesRead < 0) {
		return;
	}

	rxBuffer.writeData(dataFromSerial, bytesRead);

	uint8_t dataReceivedSoFar[rxBuffer.maxSize()];

	uint32_t dataLenReceivedSoFar = 0;

	rxBuffer.readData(dataReceivedSoFar, sizeof(dataReceivedSoFar), true,
			&dataLenReceivedSoFar);

	//look for STX
	size_t firstSTXinRawData = 0;
	while ((firstSTXinRawData < dataLenReceivedSoFar)
			&& (dataReceivedSoFar[firstSTXinRawData] != DleEncoder::STX)) {
		firstSTXinRawData++;
	}

	if (dataReceivedSoFar[firstSTXinRawData] != DleEncoder::STX) {
		//there is no STX in our data, throw it away...
		rxBuffer.deleteData(dataLenReceivedSoFar);
		return;
	}

	uint8_t packet[MAX_PACKET_SIZE];
	size_t packetLen = 0;

	size_t readSize = 0;

	ReturnValue_t result = DleEncoder::decode(
			dataReceivedSoFar + firstSTXinRawData,
			dataLenReceivedSoFar - firstSTXinRawData, &readSize, packet,
			sizeof(packet), &packetLen);

	size_t toDelete = firstSTXinRawData;
	if (result == HasReturnvaluesIF::RETURN_OK) {
		handlePacket(packet, packetLen);

		// after handling the packet, we can delete it from the raw stream,
		// it has been copied to packet
		toDelete += readSize;
	}

	//remove Data which was processed
	rxBuffer.deleteData(toDelete);
#elif WIN32
#endif
}

void ArduinoComIF::setBaudrate(uint32_t baudRate) {
    this->baudRate = baudRate;
}

void ArduinoComIF::handlePacket(uint8_t *packet, size_t packetLen) {
	uint16_t crc = CRC::crc16ccitt(packet, packetLen);
	if (crc != 0) {
		//CRC error
		return;
	}

	uint8_t command = packet[0];
	uint8_t address = packet[1];

	uint16_t size = (packet[2] << 8) + packet[3];

	if (size != packetLen - 6) {
		//Invalid Length
		return;
	}

	switch (command) {
	case ArduinoCookie::SPI: {
		//ArduinoCookie **itsComplicated;
		auto findIter = spiMap.find(address);
		if (findIter == spiMap.end()) {
			//we do no know this address
			return;
		}
		ArduinoCookie& cookie = findIter->second;
		if (packetLen > cookie.maxReplySize + 6) {
			packetLen = cookie.maxReplySize + 6;
		}
		std::memcpy(cookie.replyBuffer.data(), packet + 4, packetLen - 6);
		cookie.receivedDataLen = packetLen - 6;
	}
		break;
	default:
		return;
	}
}