|
|
|
|
|
|
|
|
|
|
|
#include "uart_communication.h" |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
LFR_UART::LFR_UART() : fileDescriptor(-1) { |
|
|
|
|
|
this->openSerialPort(); |
|
|
|
|
|
this->configureSerialPort(); |
|
|
|
|
|
} |
|
|
|
|
|
LFR_UART::~LFR_UART() { |
|
|
|
|
|
this->closeSerialPort(); |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
void LFR_UART::openFile(const char *fileName) { |
|
|
|
|
|
this->fileDescriptor = open(fileName, O_RDWR | O_NONBLOCK); |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
int LFR_UART::writeDataToFile(uint8_t *buff, uint32_t bufferLength) { |
|
|
|
|
|
return write(this->fileDescriptor, buff, bufferLength); |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
int LFR_UART::readFromFile(uint8_t *buff, uint32_t bufferLength) { |
|
|
|
|
|
return read(this->fileDescriptor, buff, bufferLength); |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
int LFR_UART::closeFile() { |
|
|
|
|
|
return close(this->fileDescriptor); |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
uint8_t LFR_UART::doubleToByte(double in){ |
|
|
|
|
|
/* |
|
|
|
|
|
* Map the range of -1.0 to 1.0 as double to the range of a byte. |
|
|
|
|
|
* |
|
|
|
|
|
* -1 -> 0 |
|
|
|
|
|
* 0.0 -> 127 |
|
|
|
|
|
* 1.0 -> 254 |
|
|
|
|
|
* Not using the full range upto 254 to hit the 0.0 |
|
|
|
|
|
*/ |
|
|
|
|
|
double minDouble = -1.0; |
|
|
|
|
|
double maxDouble = 1.0; |
|
|
|
|
|
double rangeDouble = maxDouble - minDouble; |
|
|
|
|
|
|
|
|
|
|
|
double minByte = 0.0; |
|
|
|
|
|
double maxByte = 255.0; |
|
|
|
|
|
double rangeByte = maxByte - minByte; |
|
|
|
|
|
|
|
|
|
|
|
double progress = in - minDouble; |
|
|
|
|
|
double progressPercent = progress/rangeDouble; |
|
|
|
|
|
|
|
|
|
|
|
double inputInByteRange = minByte + progressPercent*rangeByte; |
|
|
|
|
|
return uint8_t(inputInByteRange); |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
double LFR_UART::byteToDouble(uint8_t in){ |
|
|
|
|
|
double minDouble = -1.0; |
|
|
|
|
|
double maxDouble = 1.0; |
|
|
|
|
|
double rangeDouble = maxDouble - minDouble; |
|
|
|
|
|
|
|
|
|
|
|
double minByte = 0.0; |
|
|
|
|
|
double maxByte = 255.0; |
|
|
|
|
|
double rangeByte = maxByte - minByte; |
|
|
|
|
|
|
|
|
|
|
|
double progress = double(in) - minByte; |
|
|
|
|
|
double progressPercent = progress/rangeByte; |
|
|
|
|
|
|
|
|
|
|
|
double inputInDoubleRange = minDouble + progressPercent*rangeDouble; |
|
|
|
|
|
return inputInDoubleRange; |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
void LFR_UART::sendTelegram(double wheel1, double wheel2, double wheel3, double wheel4){ |
|
|
|
|
|
if(wheel1 > 1.0 || wheel2 > 1.0 || wheel3 > 1.0 || wheel4 > 1.0){ |
|
|
|
|
|
throw CommunicatorException("Wheel value must not be greater than 1.0"); |
|
|
|
|
|
} |
|
|
|
|
|
if(wheel1 < -1.0 || wheel2 < -1.0 || wheel3 < -1.0 || wheel4 < -1.0){ |
|
|
|
|
|
throw CommunicatorException("Wheel value must not be smaller than -1.0"); |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
uint8_t wheel1B = this->doubleToByte(wheel1); |
|
|
|
|
|
uint8_t wheel2B = this->doubleToByte(wheel2); |
|
|
|
|
|
uint8_t wheel3B = this->doubleToByte(wheel3); |
|
|
|
|
|
uint8_t wheel4B = this->doubleToByte(wheel4); |
|
|
|
|
|
|
|
|
|
|
|
uint8_t checksum = wheel1B^wheel2B^wheel3B^wheel4B; |
|
|
|
|
|
|
|
|
|
|
|
uint8_t telegram_buffer[5] = {wheel1B, wheel2B, wheel3B, wheel4B, checksum}; |
|
|
|
|
|
uint32_t telegram_length = 5; |
|
|
|
|
|
this->writeDataToFile(telegram_buffer, telegram_length); |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
bool LFR_UART::readTelegram(double* buffer){ |
|
|
|
|
|
uint8_t tmp_buffer[5] = {0, 0, 0, 0, 0}; |
|
|
|
|
|
uint32_t telegram_length = 5; |
|
|
|
|
|
this->readFromFile(tmp_buffer, telegram_length); |
|
|
|
|
|
|
|
|
|
|
|
//std::cout << "Read from file: " << std::bitset<8>(tmp_buffer[0]) << ", " << std::bitset<8>(tmp_buffer[1]) << ", " << std::bitset<8>(tmp_buffer[2]) << ", " << std::bitset<8>(tmp_buffer[3]) << ", " << std::bitset<8>(tmp_buffer[4]) << std::endl; |
|
|
|
|
|
|
|
|
|
|
|
if (std::bitset<8>(tmp_buffer[0]^tmp_buffer[1]^tmp_buffer[2]^tmp_buffer[3]) != std::bitset<8>(tmp_buffer[4])){ |
|
|
|
|
|
return false; |
|
|
|
|
|
} |
|
|
|
|
|
else { |
|
|
|
|
|
buffer[0] = this->byteToDouble(tmp_buffer[0]); |
|
|
|
|
|
buffer[1] = this->byteToDouble(tmp_buffer[1]); |
|
|
|
|
|
buffer[2] = this->byteToDouble(tmp_buffer[2]); |
|
|
|
|
|
buffer[3] = this->byteToDouble(tmp_buffer[3]); |
|
|
|
|
|
} |
|
|
|
|
|
return true; |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
void LFR_UART::openSerialPort() { |
|
|
|
|
|
openFile(this->serialPortPath); |
|
|
|
|
|
if(this->fileDescriptor < 0) { |
|
|
|
|
|
throw CommunicatorException("LFR_UART couldn't open the serial port."); |
|
|
|
|
|
exit(EXIT_FAILURE); |
|
|
|
|
|
} |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
void LFR_UART::configureSerialPort() { |
|
|
|
|
|
if(tcgetattr(this->fileDescriptor, &this->tty)) { |
|
|
|
|
|
throw CommunicatorException("LFR_UART couldn't get the port attributes."); |
|
|
|
|
|
exit(EXIT_FAILURE); |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
cfsetispeed(&this->tty,B115200); |
|
|
|
|
|
cfsetospeed(&this->tty,B115200); |
|
|
|
|
|
|
|
|
|
|
|
cfmakeraw(&this->tty); |
|
|
|
|
|
|
|
|
|
|
|
if(tcsetattr(this->fileDescriptor,TCSANOW,&this->tty)) { |
|
|
|
|
|
throw CommunicatorException("LFR_UART couldn't set the port attributes."); |
|
|
|
|
|
exit(EXIT_FAILURE); |
|
|
|
|
|
} |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
void LFR_UART::closeSerialPort() { |
|
|
|
|
|
this->closeFile(); |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
int main(void) { |
|
|
|
|
|
printf("Starting the loopback application...\r\n"); |
|
|
|
|
|
|
|
|
|
|
|
LFR_UART uartCommunicator; |
|
|
|
|
|
uartCommunicator.sendTelegram(-1.0, -0.99, 0.99, 1.0); |
|
|
|
|
|
double buffer[4] = {0.0, 0.0, 0.0, 0.0}; |
|
|
|
|
|
sleep(1); |
|
|
|
|
|
if(uartCommunicator.readTelegram(buffer)){ |
|
|
|
|
|
for(int i = 0; i < 4; i++) { |
|
|
|
|
|
std::cout << buffer[i] << " "; |
|
|
|
|
|
} |
|
|
|
|
|
} |
|
|
|
|
|
else { |
|
|
|
|
|
std::cout<<"ne"; |
|
|
|
|
|
} |
|
|
|
|
|
return 0; |
|
|
|
|
|
} |