Skip to content

Commit 48ab5ff

Browse files
stefanvogelstefanvogel
authored
RemoteControl APP improvements (#231)
* fix(APPRemoteControl): add ROBOT_SET channel and fix initial pose handling * chore: remove default PIO envs * docs: fix typos * fix(APPRemoteControl): improve logs
1 parent 02dcc40 commit 48ab5ff

6 files changed

Lines changed: 49 additions & 24 deletions

File tree

‎doc/architecture/uml/LogicalView/Platoon/V2VCommManagerClass.puml‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -23,11 +23,11 @@ package "PlatoonService" as serv {
2323
package "HAL" as hal {
2424
class "MQTT Client" as mqtt {
2525
+ publish(String topic, String payload) : bool
26-
+ subcribe(String topic, TopicCallback callback) : bool
26+
+ subscribe(String topic, TopicCallback callback) : bool
2727
}
2828
}
2929

3030
app *--> VCM : <<use>>
3131
VCM ..> mqtt : <<use>>
3232

33-
@enduml
33+
@enduml

‎lib/APPRemoteControl/src/App.cpp‎

Lines changed: 31 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -178,7 +178,7 @@ void App::loop()
178178
/* Process SerialMuxProt. */
179179
m_smpServer.process(millis());
180180

181-
if (false == m_initialDataSent)
181+
if ((false == m_initialDataSent) && (true == m_smpServer.isSynced()))
182182
{
183183
SettingsHandler& settings = SettingsHandler::getInstance();
184184
Command cmd = {SMPChannelPayload::CMD_ID_SET_INIT_POS, settings.getInitialXPosition(),
@@ -189,6 +189,10 @@ void App::loop()
189189
LOG_DEBUG("Initial vehicle data sent.");
190190
m_initialDataSent = true;
191191
}
192+
else
193+
{
194+
LOG_WARNING("Failed to send initial vehicle data.");
195+
}
192196
}
193197

194198
if ((true == m_statusTimer.isTimeout()) && (true == m_smpServer.isSynced()))
@@ -231,14 +235,16 @@ bool App::setupSerialMuxProtServer()
231235
m_serialMuxProtChannelIdRemoteCtrl = m_smpServer.createChannel(COMMAND_CHANNEL_NAME, COMMAND_CHANNEL_DLC);
232236
m_serialMuxProtChannelIdMotorSpeeds =
233237
m_smpServer.createChannel(MOTOR_SPEED_SETPOINT_CHANNEL_NAME, MOTOR_SPEED_SETPOINT_CHANNEL_DLC);
238+
m_serialMuxProtChannelIdRobotSpeeds =
239+
m_smpServer.createChannel(ROBOT_SPEED_SETPOINT_CHANNEL_NAME, ROBOT_SPEED_SETPOINT_CHANNEL_DLC);
234240
m_serialMuxProtChannelIdStatus = m_smpServer.createChannel(STATUS_CHANNEL_NAME, STATUS_CHANNEL_DLC);
235241

236242
m_smpServer.subscribeToChannel(COMMAND_RESPONSE_CHANNEL_NAME, App_cmdRspChannelCallback);
237243
m_smpServer.subscribeToChannel(LINE_SENSOR_CHANNEL_NAME, App_lineSensorChannelCallback);
238244
m_smpServer.subscribeToChannel(CURRENT_VEHICLE_DATA_CHANNEL_NAME, App_currentVehicleChannelCallback);
239245

240246
if ((0U == m_serialMuxProtChannelIdRemoteCtrl) || (0U == m_serialMuxProtChannelIdMotorSpeeds) ||
241-
(0U == m_serialMuxProtChannelIdStatus))
247+
(0U == m_serialMuxProtChannelIdRobotSpeeds) || (0U == m_serialMuxProtChannelIdStatus))
242248
{
243249
LOG_ERROR("Failed to create SerialMuxProt channels.");
244250
}
@@ -278,13 +284,13 @@ bool App::setupMqtt(const String& clientId, const String& brokerAddr, uint16_t b
278284
else if (false == m_mqttClient.subscribe(TOPIC_NAME_CMD, true,
279285
[this](const String& payload) { cmdTopicCallback(payload); }))
280286
{
281-
LOG_FATAL("Could not subcribe to MQTT topic: %s.", TOPIC_NAME_CMD);
287+
LOG_FATAL("Could not subscribe to MQTT topic: %s.", TOPIC_NAME_CMD);
282288
}
283289
/* Subscribe to Motor Speeds Topic. */
284290
else if (false == m_mqttClient.subscribe(TOPIC_NAME_MOTOR_SPEEDS, true,
285291
[this](const String& payload) { motorSpeedsTopicCallback(payload); }))
286292
{
287-
LOG_FATAL("Could not subcribe to MQTT topic: %s.", TOPIC_NAME_MOTOR_SPEEDS);
293+
LOG_FATAL("Could not subscribe to MQTT topic: %s.", TOPIC_NAME_MOTOR_SPEEDS);
288294
}
289295
else
290296
{
@@ -341,10 +347,25 @@ void App::cmdTopicCallback(const String& payload)
341347
break;
342348

343349
case 6U:
344-
cmd.commandId = SMPChannelPayload::CmdId::CMD_ID_SET_INIT_POS;
345-
LOG_WARNING("Setting initial position is not supported.");
346-
isValid = false;
350+
{
351+
JsonVariantConst xPos = jsonPayload["X"];
352+
JsonVariantConst yPos = jsonPayload["Y"];
353+
JsonVariantConst heading = jsonPayload["HEADING"];
354+
355+
if (xPos.isNull() || yPos.isNull() || heading.isNull())
356+
{
357+
LOG_WARNING("CMD_ID_SET_INIT_POS requires X, Y and HEADING fields.");
358+
isValid = false;
359+
}
360+
else
361+
{
362+
cmd.commandId = SMPChannelPayload::CmdId::CMD_ID_SET_INIT_POS;
363+
cmd.xPos = xPos.as<int32_t>();
364+
cmd.yPos = yPos.as<int32_t>();
365+
cmd.orientation = heading.as<int32_t>();
366+
}
347367
break;
368+
}
348369

349370
default:
350371
isValid = false;
@@ -353,16 +374,16 @@ void App::cmdTopicCallback(const String& payload)
353374

354375
if (false == isValid)
355376
{
356-
LOG_ERROR("Invalid command ID %d.", cmdId);
377+
LOG_ERROR("Got invalid payload in command with ID %d.", cmdId);
357378
}
358379
else if (true == m_smpServer.sendData(m_serialMuxProtChannelIdRemoteCtrl, reinterpret_cast<uint8_t*>(&cmd),
359380
sizeof(cmd)))
360381
{
361-
LOG_DEBUG("Command %d sent.", cmd.commandId);
382+
LOG_DEBUG("Command with ID %d successfully sent.", cmd.commandId);
362383
}
363384
else
364385
{
365-
LOG_WARNING("Failed to send command %d.", cmd.commandId);
386+
LOG_WARNING("Failed to send command with ID %d.", cmd.commandId);
366387
}
367388
}
368389
else

‎lib/APPRemoteControl/src/App.h‎

Lines changed: 7 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -70,6 +70,7 @@ class App
7070
m_smpServer(Board::getInstance().getRobot().getStream()),
7171
m_serialMuxProtChannelIdRemoteCtrl(0U),
7272
m_serialMuxProtChannelIdMotorSpeeds(0U),
73+
m_serialMuxProtChannelIdRobotSpeeds(0U),
7374
m_serialMuxProtChannelIdStatus(0U),
7475
m_mqttClient(),
7576
m_initialDataSent(false),
@@ -109,13 +110,16 @@ class App
109110
/** MQTT topic name for receiving motor speeds. */
110111
static const char* TOPIC_NAME_MOTOR_SPEEDS;
111112

112-
/** SerialMuxProt Channel id for sending remote control commands. */
113+
/** SerialMuxProt Channel ID for sending remote control commands. */
113114
uint8_t m_serialMuxProtChannelIdRemoteCtrl;
114115

115-
/** SerialMuxProt Channel id for sending motor speeds. */
116+
/** SerialMuxProt Channel ID for sending motor speeds. */
116117
uint8_t m_serialMuxProtChannelIdMotorSpeeds;
117118

118-
/** SerialMuxProt Channel id for sending system status. */
119+
/** SerialMuxProt Channel ID for sending robot speed setpoints (linear + angular). */
120+
uint8_t m_serialMuxProtChannelIdRobotSpeeds;
121+
122+
/** SerialMuxProt Channel ID for sending system status. */
119123
uint8_t m_serialMuxProtChannelIdStatus;
120124

121125
/**

‎lib/PlatoonService/src/V2VCommManager.cpp‎

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -611,12 +611,12 @@ bool V2VCommManager::setupCommonTopics(uint8_t platoonId, uint8_t vehicleId)
611611
/* Subscribe to Input Topic. */
612612
if (false == m_mqttClient.subscribe(m_waypointInputTopic, false, lambdaWaypointInputTopicCallback))
613613
{
614-
LOG_ERROR("Could not subcribe to MQTT Topic: %s.", m_waypointInputTopic.c_str());
614+
LOG_ERROR("Could not subscribe to MQTT Topic: %s.", m_waypointInputTopic.c_str());
615615
}
616616
/* Subscribe to emergency topic. */
617617
else if (false == m_mqttClient.subscribe(m_emergencyTopic, false, lambdaWaypointInputTopicCallback))
618618
{
619-
LOG_ERROR("Could not subcribe to MQTT Topic: %s.", m_emergencyTopic);
619+
LOG_ERROR("Could not subscribe to MQTT Topic: %s.", m_emergencyTopic);
620620
}
621621
else
622622
{
@@ -667,7 +667,7 @@ bool V2VCommManager::setupHeartbeatTopics(uint8_t platoonId, uint8_t vehicleId)
667667
/* Subscribe to platoon heartbeat Topic. */
668668
if (false == m_mqttClient.subscribe(m_platoonHeartbeatTopic, false, lambdaHeartbeatInputTopicCallback))
669669
{
670-
LOG_ERROR("Could not subcribe to MQTT Topic: %s.", m_platoonHeartbeatTopic.c_str());
670+
LOG_ERROR("Could not subscribe to MQTT Topic: %s.", m_platoonHeartbeatTopic.c_str());
671671
}
672672
{
673673
isSuccessful = true;
@@ -700,17 +700,17 @@ bool V2VCommManager::setupLeaderTopics()
700700
/* Subscribe to Vehicles Heartbeat Topics. */
701701
else if (false == m_mqttClient.subscribe(m_heartbeatResponseTopic, false, lambdaEventCallback))
702702
{
703-
LOG_ERROR("Could not subcribe to MQTT Topic: %s.", m_platoonHeartbeatTopic.c_str());
703+
LOG_ERROR("Could not subscribe to MQTT Topic: %s.", m_platoonHeartbeatTopic.c_str());
704704
}
705705
/* Subscribe to Last Vehicle Feedback Topic. */
706706
else if (false == m_mqttClient.subscribe(m_feedbackTopic, false, lambdaEventCallback))
707707
{
708-
LOG_ERROR("Could not subcribe to MQTT Topic: %s.", m_feedbackTopic.c_str());
708+
LOG_ERROR("Could not subscribe to MQTT Topic: %s.", m_feedbackTopic.c_str());
709709
}
710710
/* Subscribe to IVS Topic. */
711711
else if (false == m_mqttClient.subscribe(m_ivsTopic, false, lambdaEventCallback))
712712
{
713-
LOG_ERROR("Could not subcribe to MQTT Topic: %s.", m_ivsTopic.c_str());
713+
LOG_ERROR("Could not subscribe to MQTT Topic: %s.", m_ivsTopic.c_str());
714714
}
715715
else
716716
{
@@ -867,4 +867,4 @@ bool V2VCommManager::sendPlatoonLength(const int32_t length) const
867867

868868
/******************************************************************************
869869
* Local Functions
870-
*****************************************************************************/
870+
*****************************************************************************/

‎lib/Utilities/src/SimpleTimer.hpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -192,7 +192,7 @@ class SimpleTimer
192192

193193
/**
194194
* Get current duration in ms, till the timer was started.
195-
* It is independed of whether the timer is stopped or timeout.
195+
* It is independent of whether the timer is stopped or timeout.
196196
*
197197
* @return Current duration in ms
198198
*/

‎platformio.ini‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,7 @@
1212
; PlatformIO specific configurations
1313
; *****************************************************************************
1414
[platformio]
15-
default_envs = ConvoyLeaderTarget, ConvoyLeaderSim
15+
;default_envs = ConvoyLeaderTarget, ConvoyLeaderSim
1616

1717
extra_configs =
1818
config/buildmode.ini

0 commit comments

Comments
 (0)