402 lines
14 KiB
C++
402 lines
14 KiB
C++
#include "UartTestClass.h"
|
|
|
|
#include <errno.h> // Error integer and strerror() function
|
|
#include <fcntl.h> // Contains file controls like O_RDWR
|
|
#include <fsfw/tasks/TaskFactory.h>
|
|
#include <fsfw_hal/linux/serial/SerialCookie.h>
|
|
#include <linux/devices/ScexDleParser.h>
|
|
#include <linux/devices/ScexHelper.h>
|
|
#include <linux/devices/ScexUartReader.h>
|
|
#include <unistd.h> // write(), read(), close()
|
|
|
|
#include <random>
|
|
#include <string>
|
|
|
|
#include "OBSWConfig.h"
|
|
#include "fsfw/globalfunctions/CRC.h"
|
|
#include "fsfw/globalfunctions/DleEncoder.h"
|
|
#include "fsfw/globalfunctions/arrayprinter.h"
|
|
#include "fsfw/serviceinterface.h"
|
|
#include "mission/devices/devicedefinitions/ScexDefinitions.h"
|
|
|
|
#define GPS_REPLY_WIRETAPPING 0
|
|
|
|
#ifndef RPI_TEST_GPS_HANDLER
|
|
#define RPI_TEST_GPS_HANDLER 0
|
|
#endif
|
|
|
|
using namespace returnvalue;
|
|
|
|
UartTestClass::UartTestClass(object_id_t objectId) : TestTask(objectId) {
|
|
mode = TestModes::SCEX;
|
|
scexMode = ScexModes::SIMPLE;
|
|
// No one-cell and all-cell support implemented yet
|
|
currCmd = scex::Cmds::PING;
|
|
if (scexMode == ScexModes::SIMPLE) {
|
|
auto encodingBuf = new std::array<uint8_t, 4096>;
|
|
DleParser::BufPair encodingBufPair{encodingBuf->data(), encodingBuf->size()};
|
|
auto decodedBuf = new std::array<uint8_t, 4096>;
|
|
DleParser::BufPair decodingBufPair{decodedBuf->data(), decodedBuf->size()};
|
|
// TODO: Code changes but this test class has not, might not work like this anymore
|
|
dleParser = new ScexDleParser(*(new SimpleRingBuffer(4096, true)), dleEncoder, encodingBufPair,
|
|
decodingBufPair);
|
|
} else {
|
|
reader = new ScexUartReader(objects::SCEX_UART_READER);
|
|
}
|
|
}
|
|
|
|
ReturnValue_t UartTestClass::initialize() {
|
|
if (mode == TestModes::GPS) {
|
|
gpsInit();
|
|
} else if (mode == TestModes::SCEX) {
|
|
scexInit();
|
|
}
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
ReturnValue_t UartTestClass::performOneShotAction() { return returnvalue::OK; }
|
|
|
|
ReturnValue_t UartTestClass::performPeriodicAction() {
|
|
if (mode == TestModes::GPS) {
|
|
gpsPeriodic();
|
|
} else if (mode == TestModes::SCEX) {
|
|
scexPeriodic();
|
|
}
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
void UartTestClass::gpsInit() {
|
|
#if RPI_TEST_GPS_HANDLER == 1
|
|
int result = lwgps_init(&gpsData);
|
|
if (result == 0) {
|
|
sif::warning << "UartTestClass::gpsInit: lwgps_init error: " << result << std::endl;
|
|
}
|
|
|
|
/* Get file descriptor */
|
|
serialPort = open("/dev/serial0", O_RDWR);
|
|
if (serialPort < 0) {
|
|
sif::warning << "UartTestClass::gpsInit: 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 << "UartTestClass::gpsInit: 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_HANDLER == 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::gpsPeriodic: read call failed with error [" << errno << ", "
|
|
<< strerror(errno) << "]" << std::endl;
|
|
break;
|
|
} else if (bytesRead >= static_cast<int>(recBuf.size())) {
|
|
sif::debug << "UartTestClass::gpsPeriodic: "
|
|
"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::gpsPeriodic: 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
|
|
}
|
|
|
|
void UartTestClass::scexInit() {
|
|
if (scexMode == ScexModes::SIMPLE) {
|
|
scexSimpleInit();
|
|
} else {
|
|
if (reader == nullptr) {
|
|
sif::warning << "UartTestClass::scexInit: Reader invalid" << std::endl;
|
|
return;
|
|
}
|
|
#if defined(RASPBERRY_PI)
|
|
std::string devname = "/dev/serial0";
|
|
#else
|
|
std::string devname = "/dev/ul-scex";
|
|
#endif
|
|
uartCookie = new UartCookie(this->getObjectId(), devname, UartBaudRate::RATE_57600, 4096);
|
|
reader->setDebugMode(false);
|
|
ReturnValue_t result = reader->initializeInterface(uartCookie);
|
|
if (result != OK) {
|
|
sif::warning << "UartTestClass::scexInit: Initializing SCEX reader "
|
|
"UART IF failed"
|
|
<< std::endl;
|
|
}
|
|
}
|
|
}
|
|
|
|
void UartTestClass::scexPeriodic() {
|
|
using namespace std;
|
|
using namespace scex;
|
|
|
|
if (scexMode == ScexModes::SIMPLE) {
|
|
scexSimplePeriodic();
|
|
} else {
|
|
if (reader == nullptr) {
|
|
return;
|
|
}
|
|
if (not cmdSent) {
|
|
size_t len = 0;
|
|
prepareScexCmd(currCmd, false, cmdBuf.data(), &len);
|
|
reader->sendMessage(uartCookie, cmdBuf.data(), len);
|
|
cmdSent = true;
|
|
cmdDone = false;
|
|
}
|
|
if (cmdSent and not cmdDone) {
|
|
uint8_t* decodedPacket = nullptr;
|
|
size_t len = 0;
|
|
do {
|
|
ReturnValue_t result = reader->readReceivedMessage(uartCookie, &decodedPacket, &len);
|
|
if (len == 0) {
|
|
break;
|
|
}
|
|
ScexHelper helper;
|
|
const uint8_t* helperPtr = decodedPacket;
|
|
result = helper.deSerialize(&helperPtr, &len);
|
|
if (result == ScexHelper::INVALID_CRC) {
|
|
sif::warning << "UartTestClass::scexPeriodic: CRC invalid" << std::endl;
|
|
}
|
|
sif::info << helper << endl;
|
|
|
|
// ping
|
|
// if ping cmd
|
|
if (helper.getCmd() == PING) {
|
|
ofstream out("/tmp/scex-ping.bin", ofstream::binary);
|
|
if (out.bad()) {
|
|
sif::warning << "bad" << std::endl;
|
|
}
|
|
out << helper;
|
|
}
|
|
// fram
|
|
if (helper.getCmd() == FRAM) {
|
|
if (not fileNameSet) {
|
|
fileId = random_string(6);
|
|
fileName = "/tmp/scex-fram_" + fileId + ".bin";
|
|
fileNameSet = true;
|
|
}
|
|
if (helper.getPacketCounter() == 1) {
|
|
// countdown starten
|
|
finishCountdown.resetTimer();
|
|
ofstream out(fileName,
|
|
ofstream::binary); // neues file anlegen
|
|
} else {
|
|
ofstream out(fileName,
|
|
ofstream::binary | ofstream::app); // an bestehendes file appenden
|
|
out << helper;
|
|
}
|
|
|
|
if (finishCountdown.hasTimedOut()) {
|
|
triggerEvent(scex::EXPERIMENT_TIMEDOUT, currCmd, 0);
|
|
reader->finish();
|
|
sif::warning << "UartTestClass::scexPeriodic: Reader timeout" << endl;
|
|
cmdDone = true;
|
|
fileNameSet = false;
|
|
}
|
|
}
|
|
|
|
if (helper.getPacketCounter() == helper.getTotalPacketCounter()) {
|
|
reader->finish();
|
|
sif::info << "UartTestClass::scexPeriodic: Reader is finished" << endl;
|
|
cmdDone = true;
|
|
fileNameSet = false;
|
|
if (helper.getCmd() == scex::Cmds::PING) {
|
|
cmdSent = false;
|
|
fileNameSet = true; // to not generate everytime new file
|
|
}
|
|
}
|
|
} while (len > 0);
|
|
}
|
|
}
|
|
}
|
|
|
|
void UartTestClass::scexSimpleInit() {
|
|
#if defined(RASPBERRY_PI)
|
|
std::string devname = "/dev/serial0";
|
|
#else
|
|
std::string devname = "/dev/ul-scex";
|
|
#endif
|
|
/* Get file descriptor */
|
|
serialPort = open(devname.c_str(), O_RDWR);
|
|
if (serialPort < 0) {
|
|
sif::warning << "UartTestClass::scexSimpleInit: Open call failed with error [" << errno << ", "
|
|
<< strerror(errno) << std::endl;
|
|
return;
|
|
}
|
|
// 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 non-canonical mode and clear echo flag
|
|
tty.c_lflag &= ~(ICANON | ECHO);
|
|
|
|
// Non-blocking mode, read until either line is 0.1 second idle or maximum of 255 bytes are
|
|
// received in one go
|
|
tty.c_cc[VTIME] = 0; // In units of 0.1 seconds
|
|
tty.c_cc[VMIN] = 0; // Read up to 255 bytes
|
|
|
|
// Q7S UART Lite has fixed baud rate. For other linux systems, set baud rate here.
|
|
#if !defined(XIPHOS_Q7S)
|
|
if (cfsetispeed(&tty, B57600) != 0) {
|
|
sif::warning << "UartTestClass::scexSimpleInit: Setting baud rate failed" << std::endl;
|
|
}
|
|
#endif
|
|
|
|
if (tcsetattr(serialPort, TCSANOW, &tty) != 0) {
|
|
sif::warning << "UartTestClass::scexSimpleInit: tcsetattr call failed with error [" << errno
|
|
<< ", " << strerror(errno) << std::endl;
|
|
}
|
|
// Flush received and unread data
|
|
tcflush(serialPort, TCIOFLUSH);
|
|
}
|
|
|
|
void UartTestClass::scexSimplePeriodic() {
|
|
using namespace scex;
|
|
ReturnValue_t result = OK;
|
|
if (not cmdSent) {
|
|
// Flush received and unread data
|
|
tcflush(serialPort, TCIFLUSH);
|
|
uint8_t tmpCmdBuf[32] = {};
|
|
size_t len = 0;
|
|
sif::info << "UartTestClass::scexSimplePeriodic: Sending command to SCEX" << std::endl;
|
|
prepareScexCmd(currCmd, false, tmpCmdBuf, &len);
|
|
result = dleEncoder.encode(tmpCmdBuf, len, cmdBuf.data(), cmdBuf.size(), &encodedLen, true);
|
|
if (result != OK) {
|
|
sif::warning << "UartTestClass::scexSimplePeriodic: Encoding failed" << std::endl;
|
|
return;
|
|
}
|
|
if (result != 0) {
|
|
return;
|
|
};
|
|
size_t bytesWritten = write(serialPort, cmdBuf.data(), encodedLen);
|
|
if (bytesWritten != encodedLen) {
|
|
sif::warning
|
|
<< "UartTestClass::scexSimplePeriodic: Sending command to solar experiment failed"
|
|
<< std::endl;
|
|
}
|
|
cmdSent = true;
|
|
cmdDone = false;
|
|
}
|
|
if (not cmdDone) {
|
|
// Read back reply immediately
|
|
int bytesRead = 0;
|
|
do {
|
|
bytesRead = read(serialPort, reinterpret_cast<void*>(recBuf.data()),
|
|
static_cast<unsigned int>(recBuf.size()));
|
|
if (bytesRead == 0) {
|
|
sif::warning << "UartTestClass::scexSimplePeriodic: Reading SCEX: Timeout or no bytes read"
|
|
<< std::endl;
|
|
} else if (bytesRead < 0) {
|
|
sif::warning << "UartTestClass::scexSimplePeriodic: read call failed with error [" << errno
|
|
<< ", " << strerror(errno) << "]" << std::endl;
|
|
break;
|
|
} else if (bytesRead >= static_cast<int>(recBuf.size())) {
|
|
sif::debug << "UartTestClass::scexSimplePeriodic: recv buffer might not be large "
|
|
"enough, bytes read:"
|
|
<< bytesRead << std::endl;
|
|
} else if (bytesRead > 0) {
|
|
dleParser->passData(recBuf.data(), bytesRead);
|
|
if (currCmd == Cmds::PING) {
|
|
cmdDone = true;
|
|
cmdSent = false;
|
|
}
|
|
}
|
|
} while (bytesRead > 0);
|
|
}
|
|
}
|
|
|
|
int UartTestClass::prepareScexCmd(scex::Cmds cmd, bool tempCheck, uint8_t* cmdBuf, size_t* len) {
|
|
using namespace scex;
|
|
// Send command
|
|
cmdBuf[0] = scex::createCmdByte(cmd, false);
|
|
// These two fields are the packet counter and the total packet count. Those are 1 and 1 for each
|
|
// telecommand so far
|
|
cmdBuf[1] = 1;
|
|
cmdBuf[2] = 1;
|
|
uint16_t userDataLen = 0;
|
|
cmdBuf[3] = (userDataLen >> 8) & 0xff;
|
|
cmdBuf[4] = userDataLen & 0xff;
|
|
uint16_t crc = CRC::crc16ccitt(cmdBuf, 5);
|
|
cmdBuf[5] = (crc >> 8) & 0xff;
|
|
cmdBuf[6] = crc & 0xff;
|
|
*len = 7;
|
|
return 0;
|
|
}
|
|
|
|
void UartTestClass::handleFoundDlePacket(uint8_t* packet, size_t len) {
|
|
sif::info << "UartTestClass::handleFoundDlePacket: Detected DLE encoded packet with decoded size "
|
|
<< len << std::endl;
|
|
}
|
|
|
|
std::string UartTestClass::random_string(std::string::size_type length) {
|
|
static auto& chrs =
|
|
"0123456789"
|
|
"abcdefghijklmnopqrstuvwxyz"
|
|
"ABCDEFGHIJKLMNOPQRSTUVWXYZ";
|
|
|
|
thread_local static std::mt19937 rg{std::random_device{}()};
|
|
thread_local static std::uniform_int_distribution<std::string::size_type> pick(0,
|
|
sizeof(chrs) - 2);
|
|
|
|
std::string s;
|
|
|
|
s.reserve(length);
|
|
|
|
while (length--) s += chrs[pick(rg)];
|
|
|
|
return s;
|
|
}
|