diff --git a/doc/architecture/uml/LogicalView/Platoon/V2VCommManagerClass.puml b/doc/architecture/uml/LogicalView/Platoon/V2VCommManagerClass.puml index d4b2b6ef..70f7f0e9 100644 --- a/doc/architecture/uml/LogicalView/Platoon/V2VCommManagerClass.puml +++ b/doc/architecture/uml/LogicalView/Platoon/V2VCommManagerClass.puml @@ -23,11 +23,11 @@ package "PlatoonService" as serv { package "HAL" as hal { class "MQTT Client" as mqtt { + publish(String topic, String payload) : bool - + subcribe(String topic, TopicCallback callback) : bool + + subscribe(String topic, TopicCallback callback) : bool } } app *--> VCM : <> VCM ..> mqtt : <> -@enduml \ No newline at end of file +@enduml diff --git a/lib/APPRemoteControl/src/App.cpp b/lib/APPRemoteControl/src/App.cpp index eeb6a0db..a95df11c 100644 --- a/lib/APPRemoteControl/src/App.cpp +++ b/lib/APPRemoteControl/src/App.cpp @@ -178,7 +178,7 @@ void App::loop() /* Process SerialMuxProt. */ m_smpServer.process(millis()); - if (false == m_initialDataSent) + if ((false == m_initialDataSent) && (true == m_smpServer.isSynced())) { SettingsHandler& settings = SettingsHandler::getInstance(); Command cmd = {SMPChannelPayload::CMD_ID_SET_INIT_POS, settings.getInitialXPosition(), @@ -189,6 +189,10 @@ void App::loop() LOG_DEBUG("Initial vehicle data sent."); m_initialDataSent = true; } + else + { + LOG_WARNING("Failed to send initial vehicle data."); + } } if ((true == m_statusTimer.isTimeout()) && (true == m_smpServer.isSynced())) @@ -231,6 +235,8 @@ bool App::setupSerialMuxProtServer() m_serialMuxProtChannelIdRemoteCtrl = m_smpServer.createChannel(COMMAND_CHANNEL_NAME, COMMAND_CHANNEL_DLC); m_serialMuxProtChannelIdMotorSpeeds = m_smpServer.createChannel(MOTOR_SPEED_SETPOINT_CHANNEL_NAME, MOTOR_SPEED_SETPOINT_CHANNEL_DLC); + m_serialMuxProtChannelIdRobotSpeeds = + m_smpServer.createChannel(ROBOT_SPEED_SETPOINT_CHANNEL_NAME, ROBOT_SPEED_SETPOINT_CHANNEL_DLC); m_serialMuxProtChannelIdStatus = m_smpServer.createChannel(STATUS_CHANNEL_NAME, STATUS_CHANNEL_DLC); m_smpServer.subscribeToChannel(COMMAND_RESPONSE_CHANNEL_NAME, App_cmdRspChannelCallback); @@ -238,7 +244,7 @@ bool App::setupSerialMuxProtServer() m_smpServer.subscribeToChannel(CURRENT_VEHICLE_DATA_CHANNEL_NAME, App_currentVehicleChannelCallback); if ((0U == m_serialMuxProtChannelIdRemoteCtrl) || (0U == m_serialMuxProtChannelIdMotorSpeeds) || - (0U == m_serialMuxProtChannelIdStatus)) + (0U == m_serialMuxProtChannelIdRobotSpeeds) || (0U == m_serialMuxProtChannelIdStatus)) { LOG_ERROR("Failed to create SerialMuxProt channels."); } @@ -278,13 +284,13 @@ bool App::setupMqtt(const String& clientId, const String& brokerAddr, uint16_t b else if (false == m_mqttClient.subscribe(TOPIC_NAME_CMD, true, [this](const String& payload) { cmdTopicCallback(payload); })) { - LOG_FATAL("Could not subcribe to MQTT topic: %s.", TOPIC_NAME_CMD); + LOG_FATAL("Could not subscribe to MQTT topic: %s.", TOPIC_NAME_CMD); } /* Subscribe to Motor Speeds Topic. */ else if (false == m_mqttClient.subscribe(TOPIC_NAME_MOTOR_SPEEDS, true, [this](const String& payload) { motorSpeedsTopicCallback(payload); })) { - LOG_FATAL("Could not subcribe to MQTT topic: %s.", TOPIC_NAME_MOTOR_SPEEDS); + LOG_FATAL("Could not subscribe to MQTT topic: %s.", TOPIC_NAME_MOTOR_SPEEDS); } else { @@ -341,10 +347,25 @@ void App::cmdTopicCallback(const String& payload) break; case 6U: - cmd.commandId = SMPChannelPayload::CmdId::CMD_ID_SET_INIT_POS; - LOG_WARNING("Setting initial position is not supported."); - isValid = false; + { + JsonVariantConst xPos = jsonPayload["X"]; + JsonVariantConst yPos = jsonPayload["Y"]; + JsonVariantConst heading = jsonPayload["HEADING"]; + + if (xPos.isNull() || yPos.isNull() || heading.isNull()) + { + LOG_WARNING("CMD_ID_SET_INIT_POS requires X, Y and HEADING fields."); + isValid = false; + } + else + { + cmd.commandId = SMPChannelPayload::CmdId::CMD_ID_SET_INIT_POS; + cmd.xPos = xPos.as(); + cmd.yPos = yPos.as(); + cmd.orientation = heading.as(); + } break; + } default: isValid = false; @@ -353,16 +374,16 @@ void App::cmdTopicCallback(const String& payload) if (false == isValid) { - LOG_ERROR("Invalid command ID %d.", cmdId); + LOG_ERROR("Got invalid payload in command with ID %d.", cmdId); } else if (true == m_smpServer.sendData(m_serialMuxProtChannelIdRemoteCtrl, reinterpret_cast(&cmd), sizeof(cmd))) { - LOG_DEBUG("Command %d sent.", cmd.commandId); + LOG_DEBUG("Command with ID %d successfully sent.", cmd.commandId); } else { - LOG_WARNING("Failed to send command %d.", cmd.commandId); + LOG_WARNING("Failed to send command with ID %d.", cmd.commandId); } } else diff --git a/lib/APPRemoteControl/src/App.h b/lib/APPRemoteControl/src/App.h index 4683665c..b8e8f938 100644 --- a/lib/APPRemoteControl/src/App.h +++ b/lib/APPRemoteControl/src/App.h @@ -70,6 +70,7 @@ class App m_smpServer(Board::getInstance().getRobot().getStream()), m_serialMuxProtChannelIdRemoteCtrl(0U), m_serialMuxProtChannelIdMotorSpeeds(0U), + m_serialMuxProtChannelIdRobotSpeeds(0U), m_serialMuxProtChannelIdStatus(0U), m_mqttClient(), m_initialDataSent(false), @@ -109,13 +110,16 @@ class App /** MQTT topic name for receiving motor speeds. */ static const char* TOPIC_NAME_MOTOR_SPEEDS; - /** SerialMuxProt Channel id for sending remote control commands. */ + /** SerialMuxProt Channel ID for sending remote control commands. */ uint8_t m_serialMuxProtChannelIdRemoteCtrl; - /** SerialMuxProt Channel id for sending motor speeds. */ + /** SerialMuxProt Channel ID for sending motor speeds. */ uint8_t m_serialMuxProtChannelIdMotorSpeeds; - /** SerialMuxProt Channel id for sending system status. */ + /** SerialMuxProt Channel ID for sending robot speed setpoints (linear + angular). */ + uint8_t m_serialMuxProtChannelIdRobotSpeeds; + + /** SerialMuxProt Channel ID for sending system status. */ uint8_t m_serialMuxProtChannelIdStatus; /** diff --git a/lib/PlatoonService/src/V2VCommManager.cpp b/lib/PlatoonService/src/V2VCommManager.cpp index 909e04da..a26b3868 100644 --- a/lib/PlatoonService/src/V2VCommManager.cpp +++ b/lib/PlatoonService/src/V2VCommManager.cpp @@ -611,12 +611,12 @@ bool V2VCommManager::setupCommonTopics(uint8_t platoonId, uint8_t vehicleId) /* Subscribe to Input Topic. */ if (false == m_mqttClient.subscribe(m_waypointInputTopic, false, lambdaWaypointInputTopicCallback)) { - LOG_ERROR("Could not subcribe to MQTT Topic: %s.", m_waypointInputTopic.c_str()); + LOG_ERROR("Could not subscribe to MQTT Topic: %s.", m_waypointInputTopic.c_str()); } /* Subscribe to emergency topic. */ else if (false == m_mqttClient.subscribe(m_emergencyTopic, false, lambdaWaypointInputTopicCallback)) { - LOG_ERROR("Could not subcribe to MQTT Topic: %s.", m_emergencyTopic); + LOG_ERROR("Could not subscribe to MQTT Topic: %s.", m_emergencyTopic); } else { @@ -667,7 +667,7 @@ bool V2VCommManager::setupHeartbeatTopics(uint8_t platoonId, uint8_t vehicleId) /* Subscribe to platoon heartbeat Topic. */ if (false == m_mqttClient.subscribe(m_platoonHeartbeatTopic, false, lambdaHeartbeatInputTopicCallback)) { - LOG_ERROR("Could not subcribe to MQTT Topic: %s.", m_platoonHeartbeatTopic.c_str()); + LOG_ERROR("Could not subscribe to MQTT Topic: %s.", m_platoonHeartbeatTopic.c_str()); } { isSuccessful = true; @@ -700,17 +700,17 @@ bool V2VCommManager::setupLeaderTopics() /* Subscribe to Vehicles Heartbeat Topics. */ else if (false == m_mqttClient.subscribe(m_heartbeatResponseTopic, false, lambdaEventCallback)) { - LOG_ERROR("Could not subcribe to MQTT Topic: %s.", m_platoonHeartbeatTopic.c_str()); + LOG_ERROR("Could not subscribe to MQTT Topic: %s.", m_platoonHeartbeatTopic.c_str()); } /* Subscribe to Last Vehicle Feedback Topic. */ else if (false == m_mqttClient.subscribe(m_feedbackTopic, false, lambdaEventCallback)) { - LOG_ERROR("Could not subcribe to MQTT Topic: %s.", m_feedbackTopic.c_str()); + LOG_ERROR("Could not subscribe to MQTT Topic: %s.", m_feedbackTopic.c_str()); } /* Subscribe to IVS Topic. */ else if (false == m_mqttClient.subscribe(m_ivsTopic, false, lambdaEventCallback)) { - LOG_ERROR("Could not subcribe to MQTT Topic: %s.", m_ivsTopic.c_str()); + LOG_ERROR("Could not subscribe to MQTT Topic: %s.", m_ivsTopic.c_str()); } else { @@ -867,4 +867,4 @@ bool V2VCommManager::sendPlatoonLength(const int32_t length) const /****************************************************************************** * Local Functions - *****************************************************************************/ \ No newline at end of file + *****************************************************************************/ diff --git a/lib/Utilities/src/SimpleTimer.hpp b/lib/Utilities/src/SimpleTimer.hpp index 8bee845f..2431049d 100644 --- a/lib/Utilities/src/SimpleTimer.hpp +++ b/lib/Utilities/src/SimpleTimer.hpp @@ -192,7 +192,7 @@ class SimpleTimer /** * Get current duration in ms, till the timer was started. - * It is independed of whether the timer is stopped or timeout. + * It is independent of whether the timer is stopped or timeout. * * @return Current duration in ms */ diff --git a/platformio.ini b/platformio.ini index cc04220a..7d71fe9d 100644 --- a/platformio.ini +++ b/platformio.ini @@ -12,7 +12,7 @@ ; PlatformIO specific configurations ; ***************************************************************************** [platformio] -default_envs = ConvoyLeaderTarget, ConvoyLeaderSim +;default_envs = ConvoyLeaderTarget, ConvoyLeaderSim extra_configs = config/buildmode.ini