diff --git a/lib/RemoteControl/App.cpp b/lib/RemoteControl/App.cpp index 11bbc951..33cd3c89 100644 --- a/lib/RemoteControl/App.cpp +++ b/lib/RemoteControl/App.cpp @@ -41,6 +41,7 @@ #include #include #include +#include "SerialMuxChannels.h" /****************************************************************************** * Compiler Switches @@ -65,18 +66,6 @@ static void App_motorSpeedsChannelCallback(const uint8_t* payload, const uint8_t * Local Variables *****************************************************************************/ -/* Initialize channel name for receiving commands. */ -const char* App::CH_NAME_CMD = "REMOTE_CMD"; - -/* Initialize channel name for sending command responses. */ -const char* App::CH_NAME_RSP = "REMOTE_RSP"; - -/* Initialize channel name for receiving commands. */ -const char* App::CH_NAME_MOTOR_SPEEDS = "MOT_SPEEDS"; - -/* Initialize channel name for sending line sensors data. */ -const char* App::CH_NAME_LINE_SENSORS = "LINE_SENS"; - /** Only in remote control state its possible to control the robot. */ static bool gIsRemoteCtrlActive = false; @@ -87,23 +76,21 @@ static bool gIsRemoteCtrlActive = false; void App::setup() { Board::getInstance().init(); - ILineSensors& lineSensors = Board::getInstance().getLineSensors(); - uint8_t maxLineSensors = lineSensors.getNumLineSensors(); - uint8_t lineSensorChannelDlc = maxLineSensors * sizeof(uint16_t); m_systemStateMachine.setState(&StartupState::getInstance()); m_controlInterval.start(DIFFERENTIAL_DRIVE_CONTROL_PERIOD); m_sendLineSensorsDataInterval.start(SEND_LINE_SENSORS_DATA_PERIOD); /* Remote control commands/responses */ - m_smpServer.subscribeToChannel(CH_NAME_CMD, App_cmdChannelCallback); - m_smpChannelIdRemoteCtrlRsp = m_smpServer.createChannel(CH_NAME_RSP, sizeof(RemoteCtrlState::RspId)); + m_smpServer.subscribeToChannel(COMMAND_CHANNEL_NAME, App_cmdChannelCallback); + m_smpChannelIdRemoteCtrlRsp = + m_smpServer.createChannel(COMMAND_RESPONSE_CHANNEL_NAME, COMMAND_RESPONSE_CHANNEL_DLC); /* Receiving linear motor speed left/right */ - m_smpServer.subscribeToChannel(CH_NAME_MOTOR_SPEEDS, App_motorSpeedsChannelCallback); + m_smpServer.subscribeToChannel(SPEED_SETPOINT_CHANNEL_NAME, App_motorSpeedsChannelCallback); /* Providing line sensor data */ - m_smpChannelIdLineSensors = m_smpServer.createChannel(CH_NAME_LINE_SENSORS, lineSensorChannelDlc); + m_smpChannelIdLineSensors = m_smpServer.createChannel(LINE_SENSOR_CHANNEL_NAME, LINE_SENSOR_CHANNEL_DLC); } void App::loop() @@ -163,9 +150,9 @@ void App::sendRemoteControlResponses() /* Send only on change. */ if (remoteControlRspId != m_lastRemoteControlRspId) { - const uint8_t* payload = reinterpret_cast(&remoteControlRspId); + const CommandResponse* payload = reinterpret_cast(&remoteControlRspId); - (void)m_smpServer.sendData(m_smpChannelIdRemoteCtrlRsp, payload, sizeof(remoteControlRspId)); + (void)m_smpServer.sendData(m_smpChannelIdRemoteCtrlRsp, reinterpret_cast(&payload), sizeof(payload)); m_lastRemoteControlRspId = remoteControlRspId; } @@ -177,17 +164,19 @@ void App::sendLineSensorsData() const uint8_t maxLineSensors = lineSensors.getNumLineSensors(); const uint16_t* lineSensorValues = lineSensors.getSensorValues(); uint8_t lineSensorIdx = 0U; - uint8_t payload[maxLineSensors * sizeof(uint16_t)]; + LineSensorData payload; - while (maxLineSensors > lineSensorIdx) + if (LINE_SENSOR_CHANNEL_DLC == maxLineSensors * sizeof(uint16_t)) { - Util::uint16ToByteArray(&payload[lineSensorIdx * sizeof(uint16_t)], sizeof(uint16_t), - lineSensorValues[lineSensorIdx]); + while (maxLineSensors > lineSensorIdx) + { + payload.lineSensorData[lineSensorIdx] = lineSensorValues[lineSensorIdx]; - ++lineSensorIdx; + ++lineSensorIdx; + } } - (void)m_smpServer.sendData(m_smpChannelIdLineSensors, payload, sizeof(payload)); + (void)m_smpServer.sendData(m_smpChannelIdLineSensors, reinterpret_cast(&payload), sizeof(payload)); } /****************************************************************************** @@ -222,16 +211,9 @@ static void App_cmdChannelCallback(const uint8_t* payload, const uint8_t payload */ static void App_motorSpeedsChannelCallback(const uint8_t* payload, const uint8_t payloadSize) { - if ((nullptr != payload) && ((2U * sizeof(uint16_t)) == payloadSize) && (true == gIsRemoteCtrlActive)) + if ((nullptr != payload) && (SPEED_SETPOINT_CHANNEL_DLC == payloadSize) && (true == gIsRemoteCtrlActive)) { - int16_t linearSpeedLeft; - int16_t linearSpeedRight; - bool convResultLSL = Util::byteArrayToInt16(&payload[0U * sizeof(int16_t)], sizeof(int16_t), linearSpeedLeft); - bool convResultLSR = Util::byteArrayToInt16(&payload[1U * sizeof(int16_t)], sizeof(int16_t), linearSpeedRight); - - if ((true == convResultLSL) && (true == convResultLSR)) - { - DifferentialDrive::getInstance().setLinearSpeed(linearSpeedLeft, linearSpeedRight); - } + const SpeedData* motorSpeedData = reinterpret_cast(payload); + DifferentialDrive::getInstance().setLinearSpeed(motorSpeedData->left, motorSpeedData->right); } } diff --git a/lib/RemoteControl/App.h b/lib/RemoteControl/App.h index 7e3c7813..f856c3ac 100644 --- a/lib/RemoteControl/App.h +++ b/lib/RemoteControl/App.h @@ -99,18 +99,6 @@ class App /** Sending Data period in ms. */ static const uint32_t SEND_LINE_SENSORS_DATA_PERIOD = 20; - /** SerialMuxProt channel name for receiving commands. */ - static const char* CH_NAME_CMD; - - /** SerialMuxProt channel name for sending command responses. */ - static const char* CH_NAME_RSP; - - /** SerialMuxProt channel name for receiving motor sppeds. */ - static const char* CH_NAME_MOTOR_SPEEDS; - - /** SerialMuxProt channel name for sending line sensors data. */ - static const char* CH_NAME_LINE_SENSORS; - /** The system state machine. */ StateMachine m_systemStateMachine;