diff --git a/lib/APPRemoteControl/src/App.cpp b/lib/APPRemoteControl/src/App.cpp index 28675f2..823ec6f 100644 --- a/lib/APPRemoteControl/src/App.cpp +++ b/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(payload); + DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + int16_t angularSpeed = static_cast(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(); + } +} \ No newline at end of file diff --git a/lib/APPRemoteControl/src/SerialMuxChannels.h b/lib/APPRemoteControl/src/SerialMuxChannels.h index 29838c3..6e49cac 100644 --- a/lib/APPRemoteControl/src/SerialMuxChannels.h +++ b/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 *****************************************************************************/