From 30c5cf8f89ff7df0a3ec514d4e871eb690b42f1d Mon Sep 17 00:00:00 2001
From: gabryelreyes <gabryel.reyes@newtec.de>
Date: Fri, 20 Oct 2023 15:17:14 +0200
Subject: [PATCH] Removed debug functions. Commited by error in
 f9d2e4cd9e7a559aa80ed0f376309a7400ba9291

---
 lib/RemoteControl/App.cpp | 59 +++++++++------------------------------
 lib/RemoteControl/App.h   |  2 +-
 2 files changed, 14 insertions(+), 47 deletions(-)

diff --git a/lib/RemoteControl/App.cpp b/lib/RemoteControl/App.cpp
index b6fd2964..11bbc951 100644
--- a/lib/RemoteControl/App.cpp
+++ b/lib/RemoteControl/App.cpp
@@ -104,8 +104,6 @@ void App::setup()
 
     /* Providing line sensor data */
     m_smpChannelIdLineSensors = m_smpServer.createChannel(CH_NAME_LINE_SENSORS, lineSensorChannelDlc);
-
-    (void)m_smpServer.createChannel("DEBUG", (6U * sizeof(int16_t)));
 }
 
 void App::loop()
@@ -170,57 +168,26 @@ void App::sendRemoteControlResponses()
         (void)m_smpServer.sendData(m_smpChannelIdRemoteCtrlRsp, payload, sizeof(remoteControlRspId));
 
         m_lastRemoteControlRspId = remoteControlRspId;
-
-        uint8_t payloadSize         = (6U * sizeof(int16_t));
-        uint8_t buffer[payloadSize] = {0U};
-        Util::int16ToByteArray(&buffer[0U * sizeof(int16_t)], sizeof(int16_t),
-                               DifferentialDrive::getInstance().getMaxMotorSpeed());
-        (void)m_smpServer.sendData("DEBUG", buffer, payloadSize);
     }
 }
 
 void App::sendLineSensorsData() const
 {
-    // ILineSensors&   lineSensors      = Board::getInstance().getLineSensors();
-    // uint8_t         maxLineSensors   = lineSensors.getNumLineSensors();
-    // const uint16_t* lineSensorValues = lineSensors.getSensorValues();
-    // uint8_t         lineSensorIdx    = 0U;
-    // uint8_t         payload[maxLineSensors * sizeof(uint16_t)];
-
-    // while (maxLineSensors > lineSensorIdx)
-    // {
-    //     Util::uint16ToByteArray(&payload[lineSensorIdx * sizeof(uint16_t)], sizeof(uint16_t),
-    //                             lineSensorValues[lineSensorIdx]);
-
-    //     ++lineSensorIdx;
-    // }
-
-    // (void)m_smpServer.sendData(m_smpChannelIdLineSensors, payload, sizeof(payload));
-
-    uint8_t payloadSize         = (12U);
-    uint8_t buffer[payloadSize] = {0U};
-
-    int16_t linearSetpointLeft;
-    int16_t linearSetpointRight;
-
-    DifferentialDrive::getInstance().getLinearSpeed(linearSetpointLeft, linearSetpointRight);
-
-    Util::int16ToByteArray(&buffer[0U * sizeof(int16_t)], sizeof(int16_t),
-                           DifferentialDrive::getInstance().getLinearSpeed());
-    Util::int16ToByteArray(&buffer[1U * sizeof(int16_t)], sizeof(int16_t),
-                           Speedometer::getInstance().getLinearSpeedCenter());
-    Util::int16ToByteArray(&buffer[2U * sizeof(int16_t)], sizeof(int16_t), linearSetpointLeft);
-    Util::int16ToByteArray(&buffer[3U * sizeof(int16_t)], sizeof(int16_t),
-                           Speedometer::getInstance().getLinearSpeedLeft());
-    Util::int16ToByteArray(&buffer[4U * sizeof(int16_t)], sizeof(int16_t), linearSetpointRight);
-    Util::int16ToByteArray(&buffer[5U * sizeof(int16_t)], sizeof(int16_t),
-                           Speedometer::getInstance().getLinearSpeedRight());
-
-    IMotors& motors = Board::getInstance().getMotors();
-    if (0 == motors.getLeftSpeed())
+    ILineSensors&   lineSensors      = Board::getInstance().getLineSensors();
+    uint8_t         maxLineSensors   = lineSensors.getNumLineSensors();
+    const uint16_t* lineSensorValues = lineSensors.getSensorValues();
+    uint8_t         lineSensorIdx    = 0U;
+    uint8_t         payload[maxLineSensors * sizeof(uint16_t)];
+
+    while (maxLineSensors > lineSensorIdx)
     {
-        (void)m_smpServer.sendData("DEBUG", buffer, payloadSize);
+        Util::uint16ToByteArray(&payload[lineSensorIdx * sizeof(uint16_t)], sizeof(uint16_t),
+                                lineSensorValues[lineSensorIdx]);
+
+        ++lineSensorIdx;
     }
+
+    (void)m_smpServer.sendData(m_smpChannelIdLineSensors, payload, sizeof(payload));
 }
 
 /******************************************************************************
diff --git a/lib/RemoteControl/App.h b/lib/RemoteControl/App.h
index b72abde5..7e3c7813 100644
--- a/lib/RemoteControl/App.h
+++ b/lib/RemoteControl/App.h
@@ -97,7 +97,7 @@ class App
     static const uint32_t DIFFERENTIAL_DRIVE_CONTROL_PERIOD = 5;
 
     /** Sending Data period in ms. */
-    static const uint32_t SEND_LINE_SENSORS_DATA_PERIOD = 1000;
+    static const uint32_t SEND_LINE_SENSORS_DATA_PERIOD = 20;
 
     /** SerialMuxProt channel name for receiving commands. */
     static const char* CH_NAME_CMD;