From fb6b5b9e464a80881846445ffee0d29ad496bfab Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Tue, 8 Feb 2022 15:58:17 +0100
Subject: [PATCH] bugfixes for test code

---
 linux/boardtest/UartTestClass.cpp | 40 +++++++++++++++++++++++--------
 1 file changed, 30 insertions(+), 10 deletions(-)

diff --git a/linux/boardtest/UartTestClass.cpp b/linux/boardtest/UartTestClass.cpp
index 8cdf3ce7..d39bcfdf 100644
--- a/linux/boardtest/UartTestClass.cpp
+++ b/linux/boardtest/UartTestClass.cpp
@@ -1,3 +1,4 @@
+#include <fsfw/tasks/TaskFactory.h>
 #include "UartTestClass.h"
 #if defined(RASPBERRY_PI)
 #include "rpiConfig.h"
@@ -12,6 +13,7 @@
 #include "fsfw/globalfunctions/CRC.h"
 #include "fsfw/globalfunctions/arrayprinter.h"
 #include "fsfw/serviceinterface.h"
+#include "fsfw/globalfunctions/DleEncoder.h"
 #include "mission/devices/devicedefinitions/SCEXDefinitions.h"
 
 #define GPS_REPLY_WIRETAPPING 0
@@ -161,20 +163,38 @@ void UartTestClass::scexInit() {
 }
 
 void UartTestClass::scexPeriodic() {
+  auto dleEncoder = DleEncoder();
+  std::array<uint8_t, 128> tmpCmdBuf = {};
   // Send ping command
-  cmdBuf[0] = scex::CMD_PING;
+  tmpCmdBuf[0] = scex::CMD_PING;
   // 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;
+  tmpCmdBuf[1] = 1;
+  tmpCmdBuf[2] = 1;
   uint16_t userDataLen = 0;
-  cmdBuf[3] = (userDataLen >> 8) & 0xff;
-  cmdBuf[4] = userDataLen & 0xff;
-  uint16_t crc = CRC::crc16ccitt(cmdBuf.data(), 5);
-  cmdBuf[5] = (crc >> 8) & 0xff;
-  cmdBuf[6] = crc & 0xff;
-  size_t bytesWritten = write(serialPort, cmdBuf.data(), 7);
-  if (bytesWritten != 7) {
+  tmpCmdBuf[3] = (userDataLen >> 8) & 0xff;
+  tmpCmdBuf[4] = userDataLen & 0xff;
+  uint16_t crc = CRC::crc16ccitt(tmpCmdBuf.data(), 5);
+  tmpCmdBuf[5] = (crc >> 8) & 0xff;
+  tmpCmdBuf[6] = crc & 0xff;
+
+  size_t encodedLen = 0;
+  ReturnValue_t result = dleEncoder.encode(tmpCmdBuf.data(), 7, cmdBuf.data(),
+          cmdBuf.size(), &encodedLen, true);
+  if(result != HasReturnvaluesIF::RETURN_OK) {
+      sif::warning << "UartTestClass::scexInit: Encoding failed" << std::endl;
+      return;
+  }
+  arrayprinter::print(cmdBuf.data(), 9);
+  size_t bytesWritten = write(serialPort, cmdBuf.data(), encodedLen);
+
+  if (bytesWritten != encodedLen) {
+    sif::warning << "Sending ping command to solar experiment failed" << std::endl;
+  }
+
+  TaskFactory::delayTask(20);
+  bytesWritten = write(serialPort, cmdBuf.data(), encodedLen);
+  if (bytesWritten != encodedLen) {
     sif::warning << "Sending ping command to solar experiment failed" << std::endl;
   }