Turtle functionality in RemoteControl

lib/APPRemoteControl/src/App.cpp

@@ -63,6 +63,7 @@
 static void App_cmdChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData);
 static void App_motorSpeedSetpointsChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData);
 static void App_statusChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData);
+static void App_turtleChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData);
 
 /******************************************************************************
  * Local Variables
@@ -287,6 +288,7 @@ bool App::setupSerialMuxProt()
     m_smpServer.subscribeToChannel(COMMAND_CHANNEL_NAME, App_cmdChannelCallback);
     m_smpServer.subscribeToChannel(SPEED_SETPOINT_CHANNEL_NAME, App_motorSpeedSetpointsChannelCallback);
     m_smpServer.subscribeToChannel(STATUS_CHANNEL_NAME, App_statusChannelCallback);
+    m_smpServer.subscribeToChannel(TURTLE_CHANNEL_NAME, App_turtleChannelCallback);
 
     /* Channel creation. */
     m_serialMuxProtChannelIdRemoteCtrlRsp =
@@ -384,3 +386,33 @@ void App_statusChannelCallback(const uint8_t* payload, const uint8_t payloadSize
         application->systemStatusCallback(currentStatus->status);
     }
 }
+
+/**
+ * Receives Turtle speed setpoints over SerialMuxProt channel.
+ *
+ * @param[in] payload       Linear and angular speed setpoints in a TurtleSpeed structure.
+ * @param[in] payloadSize   Size of the TurtleSpeed structure.
+ * @param[in] userData      Instance of App class.
+ */
+void App_turtleChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData)
+{
+    (void)userData;
+    if ((nullptr != payload) && (TURTLE_CHANNEL_DLC == payloadSize))
+    {
+        const TurtleSpeed* turtleSpeedData = reinterpret_cast<const TurtleSpeed*>(payload);
+        DifferentialDrive& diffDrive       = DifferentialDrive::getInstance();
+        int16_t            angularSpeed    = static_cast<int16_t>(turtleSpeedData->angular);
+
+        /* Convert to [steps/s] */
+        int16_t centerSpeed = Util::millimetersPerSecondToStepsPerSecond(turtleSpeedData->linearCenter);
+
+        /* Linear speed is set first. Overwrites all speed setpoints. */
+        diffDrive.setLinearSpeed(centerSpeed);
+
+        /* Angular speed is set on-top of the linear speed. Must be called after setLinearSpeed(). */
+        diffDrive.setAngularSpeed(angularSpeed);
+
+        /* Turtle expects no initial data. Can be called without side-effects when no longer in StartupState. */
+        StartupState::getInstance().notifyInitialDataIsSet();
+    }
+}

lib/APPRemoteControl/src/SerialMuxChannels.h

@@ -82,6 +82,12 @@
 /** DLC of Line Sensor Channel */
 #define LINE_SENSOR_CHANNEL_DLC (sizeof(LineSensorData))
 
+/** Name of the Channel to send Turtle Speeds. */
+#define TURTLE_CHANNEL_NAME "TURTLE"
+
+/** DLC of Turtle Channel */
+#define TURTLE_CHANNEL_DLC (sizeof(TurtleSpeed))
+
 /******************************************************************************
  * Types and Classes
  *****************************************************************************/
@@ -205,6 +211,13 @@ typedef struct _LineSensorData
     uint16_t lineSensorData[5U]; /**< Line sensor data [digits] normalized to max 1000 digits. */
 } __attribute__((packed)) LineSensorData;
 
+/** Struct of the "Turtle" channel payload. */
+typedef struct _TurtleSpeed
+{
+    int32_t linearCenter; /**< Linear speed of the vehicle center. [mm/s] */
+    int32_t angular;      /**< Angular speed. [mrad/s] */
+} __attribute__((packed)) TurtleSpeed;
+
 /******************************************************************************
  * Functions
  *****************************************************************************/

lib/MainNative/src/main.cpp

@@ -35,6 +35,7 @@
 #include <stdio.h>
 #include <getopt.h>
 #include <string.h>
+#include <unistd.h>
 #include <Arduino.h>
 #include <Board.h>
 #include <RobotDeviceNames.h>
@@ -77,6 +78,7 @@ static void          systemDelay(unsigned long ms);
 static const struct option LONG_OPTIONS[] = {{"help", no_argument, nullptr, 0},
                                              {"serialRxCh", required_argument, nullptr, 0},
                                              {"serialTxCh", required_argument, nullptr, 0},
+                                             {"cwd",        required_argument, nullptr, 0},
                                              {nullptr, no_argument, nullptr, 0}}; /* Marks the end. */
 
 /** Program argument default value of the robot name. */
@@ -263,6 +265,10 @@ static int handleCommandLineArguments(PrgArguments& prgArguments, int argc, char
             {
                 prgArguments.serialTxChannel = optarg;
             }
+            else if (0 == strcmp(LONG_OPTIONS[optionIndex].name, "cwd"))
+            {
+                chdir(optarg);
+            }
             else
             {
                 status = -1;
@@ -307,6 +313,7 @@ static int handleCommandLineArguments(PrgArguments& prgArguments, int argc, char
         printf("\t--serialTxCh <CHANNEL>\t\tSet serial tx channel (ZumoComSystem)."); /* Serial txchannel */
         printf(" Default: %s\n", PRG_ARG_SERIAL_TX_CH_DEFAULT); /* Serial tx channel default value */
         printf("\t-v\t\t\tVerbose mode. Default: Disabled\n");  /* Flag */
+        printf("\t--cwd <dir>\t\tSpecify working directory.");  /* Set process working directory */
     }
 
     return status;

lib/Service/src/Speedometer.cpp

@@ -98,7 +98,7 @@ void Speedometer::process()
 
         if (currentDrivingDirection != m_lastDirectionRight)
         {
-            resetLeft = true;
+            resetRight = true;
         }
 
         m_lastDirectionRight = currentDrivingDirection;