Robin Mueller
e142408bb8
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
123 lines
4.3 KiB
C++
123 lines
4.3 KiB
C++
#include "UartTestClass.h"
|
|
#if defined(RASPBERRY_PI)
|
|
#include "rpiConfig.h"
|
|
#elif defined(XIPHOS_Q7S)
|
|
#include "q7sConfig.h"
|
|
#endif
|
|
|
|
#include <errno.h> // Error integer and strerror() function
|
|
#include <fcntl.h> // Contains file controls like O_RDWR
|
|
#include <unistd.h> // write(), read(), close()
|
|
|
|
#include "fsfw/serviceinterface/ServiceInterface.h"
|
|
#include "lwgps/lwgps.h"
|
|
|
|
#define GPS_REPLY_WIRETAPPING 0
|
|
|
|
UartTestClass::UartTestClass(object_id_t objectId) : TestTask(objectId) {}
|
|
|
|
ReturnValue_t UartTestClass::initialize() {
|
|
if (mode == TestModes::GPS) {
|
|
gpsInit();
|
|
}
|
|
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|
|
ReturnValue_t UartTestClass::performOneShotAction() {
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|
|
ReturnValue_t UartTestClass::performPeriodicAction() {
|
|
if(mode == TestModes::GPS) {
|
|
gpsPeriodic();
|
|
}
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|
|
void UartTestClass::gpsInit() {
|
|
#if RPI_TEST_GPS_DEVICE == 1
|
|
int result = lwgps_init(&gpsData);
|
|
if (result == 0) {
|
|
sif::warning << "lwgps_init error: " << result << std::endl;
|
|
}
|
|
|
|
/* Get file descriptor */
|
|
serialPort = open("/dev/serial0", O_RDWR);
|
|
if (serialPort < 0) {
|
|
sif::warning << "open call failed with error [" << errno << ", " << strerror(errno)
|
|
<< std::endl;
|
|
}
|
|
/* Setting up UART parameters */
|
|
tty.c_cflag &= ~PARENB; // Clear parity bit
|
|
tty.c_cflag &= ~CSTOPB; // Clear stop field, only one stop bit used in communication
|
|
tty.c_cflag &= ~CSIZE; // Clear all the size bits
|
|
tty.c_cflag |= CS8; // 8 bits per byte
|
|
tty.c_cflag &= ~CRTSCTS; // Disable RTS/CTS hardware flow control
|
|
tty.c_cflag |= CREAD | CLOCAL; // Turn on READ & ignore ctrl lines (CLOCAL = 1)
|
|
// Use canonical mode for GPS device
|
|
tty.c_lflag |= ICANON;
|
|
tty.c_lflag &= ~ECHO; // Disable echo
|
|
tty.c_lflag &= ~ECHOE; // Disable erasure
|
|
tty.c_lflag &= ~ECHONL; // Disable new-line echo
|
|
tty.c_lflag &= ~ISIG; // Disable interpretation of INTR, QUIT and SUSP
|
|
tty.c_iflag &= ~(IXON | IXOFF | IXANY); // Turn off s/w flow ctrl
|
|
tty.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR |
|
|
ICRNL); // Disable any special handling of received bytes
|
|
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
|
|
|
|
// Non-blocking mode
|
|
tty.c_cc[VTIME] = 0;
|
|
tty.c_cc[VMIN] = 0;
|
|
|
|
cfsetispeed(&tty, B9600);
|
|
cfsetospeed(&tty, B9600);
|
|
if (tcsetattr(serialPort, TCSANOW, &tty) != 0) {
|
|
sif::warning << "tcsetattr call failed with error [" << errno << ", " << strerror(errno)
|
|
<< std::endl;
|
|
;
|
|
}
|
|
// Flush received and unread data. Those are old NMEA strings which are not relevant anymore
|
|
tcflush(serialPort, TCIFLUSH);
|
|
#endif
|
|
}
|
|
|
|
void UartTestClass::gpsPeriodic() {
|
|
#if RPI_TEST_GPS_DEVICE == 1
|
|
int bytesRead = 0;
|
|
do {
|
|
bytesRead = read(serialPort, reinterpret_cast<void*>(recBuf.data()),
|
|
static_cast<unsigned int>(recBuf.size()));
|
|
if (bytesRead < 0) {
|
|
sif::warning << "UartTestClass::performPeriodicAction: read call failed with error [" << errno
|
|
<< ", " << strerror(errno) << "]" << std::endl;
|
|
break;
|
|
} else if (bytesRead >= static_cast<int>(recBuf.size())) {
|
|
sif::debug << "UartTestClass::performPeriodicAction: "
|
|
"recv buffer might not be large enough"
|
|
<< std::endl;
|
|
} else if (bytesRead > 0) {
|
|
// pass data to lwgps for processing
|
|
#if GPS_REPLY_WIRETAPPING == 1
|
|
sif::info << recBuf.data() << std::endl;
|
|
#endif
|
|
int result = lwgps_process(&gpsData, recBuf.data(), bytesRead);
|
|
if (result == 0) {
|
|
sif::warning << "UartTestClass::performPeriodicAction: lwgps_process error" << std::endl;
|
|
}
|
|
recvCnt++;
|
|
if (recvCnt == 6) {
|
|
recvCnt = 0;
|
|
sif::info << "GPS Data" << std::endl;
|
|
// Print messages
|
|
printf("Valid status: %d\n", gpsData.is_valid);
|
|
printf("Latitude: %f degrees\n", gpsData.latitude);
|
|
printf("Longitude: %f degrees\n", gpsData.longitude);
|
|
printf("Altitude: %f meters\n", gpsData.altitude);
|
|
}
|
|
}
|
|
} while (bytesRead > 0);
|
|
#endif
|
|
}
|