From e2ce4ba4e2ad4c02e8ece5592adbd3d9e231f2a0 Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Mon, 15 Jan 2024 14:43:16 +0100 Subject: [PATCH 1/6] Set position initial vehicle data from DCS --- lib/APPRemoteControl/App.cpp | 27 ++++++++++++++++++++++++ lib/APPRemoteControl/SerialMuxChannels.h | 6 ++++++ lib/Service/Odometry.h | 13 ++++++++++++ 3 files changed, 46 insertions(+) diff --git a/lib/APPRemoteControl/App.cpp b/lib/APPRemoteControl/App.cpp index 30d44cee..f445a0bf 100644 --- a/lib/APPRemoteControl/App.cpp +++ b/lib/APPRemoteControl/App.cpp @@ -62,6 +62,7 @@ static void App_cmdChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData); static void App_motorSpeedsChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData); +static void App_initialDataChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData); /****************************************************************************** * Local Variables @@ -94,6 +95,10 @@ void App::setup() /* Providing line sensor data */ m_smpChannelIdLineSensors = m_smpServer.createChannel(LINE_SENSOR_CHANNEL_NAME, LINE_SENSOR_CHANNEL_DLC); + /* Receive initial vehicle data. */ + m_smpServer.subscribeToChannel(INITIAL_VEHICLE_DATA_CHANNEL_DLC_CHANNEL_NAME, App_initialDataChannelCallback); + + /* Providing current vehicle data. */ m_smpChannelIdCurrentVehicleData = m_smpServer.createChannel(CURRENT_VEHICLE_DATA_CHANNEL_DLC_CHANNEL_NAME, CURRENT_VEHICLE_DATA_CHANNEL_DLC); @@ -268,3 +273,25 @@ static void App_motorSpeedsChannelCallback(const uint8_t* payload, const uint8_t DifferentialDrive::getInstance().setLinearSpeed(motorSpeedData->left, motorSpeedData->right); } } + +/** + * Receives initial data over SerialMuxProt channel. Triggers sending the max speed. + * + * @param[in] payload Initial vehicle data. Position coordinates, orientation, and motor speeds + * @param[in] payloadSize Size of VehicleData struct. + * @param[in] userData User data + */ +static void App_initialDataChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData) +{ + (void)userData; + if ((nullptr != payload) && (INITIAL_VEHICLE_DATA_CHANNEL_DLC == payloadSize) && (true == gIsRemoteCtrlActive)) + { + const VehicleData* vehicleData = reinterpret_cast(payload); + Odometry& odometry = Odometry::getInstance(); + + odometry.clearPosition(); + odometry.clearMileage(); + odometry.setPosition(vehicleData->xPos, vehicleData->yPos); + odometry.setOrientation(vehicleData->orientation); + } +} diff --git a/lib/APPRemoteControl/SerialMuxChannels.h b/lib/APPRemoteControl/SerialMuxChannels.h index 83b4954c..b9ba078f 100644 --- a/lib/APPRemoteControl/SerialMuxChannels.h +++ b/lib/APPRemoteControl/SerialMuxChannels.h @@ -75,6 +75,12 @@ /** DLC of Current Vehicle Data Channel */ #define CURRENT_VEHICLE_DATA_CHANNEL_DLC (sizeof(VehicleData)) +/** Name of Channel to send Initial Vehicle Data to. */ +#define INITIAL_VEHICLE_DATA_CHANNEL_DLC_CHANNEL_NAME "INIT_DATA" + +/** DLC of Initial Vehicle Data Channel */ +#define INITIAL_VEHICLE_DATA_CHANNEL_DLC (sizeof(VehicleData)) + /****************************************************************************** * Types and Classes *****************************************************************************/ diff --git a/lib/Service/Odometry.h b/lib/Service/Odometry.h index 493ceb5e..06c7091d 100644 --- a/lib/Service/Odometry.h +++ b/lib/Service/Odometry.h @@ -126,6 +126,19 @@ class Odometry posY = m_posY; } + /** + * Set absolute position in coordinate system. + * The x- and y-axis unit is mm. + * + * @param[in] posX x-coordinate + * @param[in] posY y-coordinate + */ + void setPosition(int32_t posX, int32_t posY) + { + m_posX = posX; + m_posY = posY; + } + /** * Set the orientation. * Use this to align the Y axis to north. From 5aaa376dd1b1aa602d88858c18cac7c60c11be9e Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Mon, 15 Jan 2024 16:01:03 +0100 Subject: [PATCH 2/6] Expanded definition of remote command respose --- lib/APPRemoteControl/App.cpp | 15 ++++++++------- lib/APPRemoteControl/App.h | 4 ++-- lib/APPRemoteControl/RemoteCtrlState.h | 23 +++++++++++++---------- lib/APPRemoteControl/SerialMuxChannels.h | 4 +++- 4 files changed, 26 insertions(+), 20 deletions(-) diff --git a/lib/APPRemoteControl/App.cpp b/lib/APPRemoteControl/App.cpp index f445a0bf..6de37e35 100644 --- a/lib/APPRemoteControl/App.cpp +++ b/lib/APPRemoteControl/App.cpp @@ -177,16 +177,17 @@ void App::loop() void App::sendRemoteControlResponses() { - RemoteCtrlState::RspId remoteControlRspId = RemoteCtrlState::getInstance().getCmdRsp(); + CommandResponse remoteControlRspId = RemoteCtrlState::getInstance().getCmdRsp(); /* Send only on change. */ - if (remoteControlRspId != m_lastRemoteControlRspId) + if ((remoteControlRspId.responseId != m_lastRemoteControlRspId.responseId) || + (remoteControlRspId.commandId != m_lastRemoteControlRspId.commandId)) { - const CommandResponse* payload = reinterpret_cast(&remoteControlRspId); - - (void)m_smpServer.sendData(m_smpChannelIdRemoteCtrlRsp, reinterpret_cast(&payload), sizeof(payload)); - - m_lastRemoteControlRspId = remoteControlRspId; + if (true == m_smpServer.sendData(m_smpChannelIdRemoteCtrlRsp, reinterpret_cast(&remoteControlRspId), + sizeof(remoteControlRspId))) + { + m_lastRemoteControlRspId = remoteControlRspId; + } } } diff --git a/lib/APPRemoteControl/App.h b/lib/APPRemoteControl/App.h index 5200b9a9..986aec5b 100644 --- a/lib/APPRemoteControl/App.h +++ b/lib/APPRemoteControl/App.h @@ -73,7 +73,7 @@ class App m_smpChannelIdRemoteCtrlRsp(0U), m_smpChannelIdLineSensors(0U), m_smpChannelIdCurrentVehicleData(0U), - m_lastRemoteControlRspId(RemoteCtrlState::RSP_ID_OK) + m_lastRemoteControlRspId() { } @@ -136,7 +136,7 @@ class App uint8_t m_smpChannelIdCurrentVehicleData; /** Last remote control response id */ - RemoteCtrlState::RspId m_lastRemoteControlRspId; + CommandResponse m_lastRemoteControlRspId; /* Not allowed. */ App(const App& app); /**< Copy construction of an instance. */ diff --git a/lib/APPRemoteControl/RemoteCtrlState.h b/lib/APPRemoteControl/RemoteCtrlState.h index 039d4769..e8e07443 100644 --- a/lib/APPRemoteControl/RemoteCtrlState.h +++ b/lib/APPRemoteControl/RemoteCtrlState.h @@ -45,6 +45,7 @@ *****************************************************************************/ #include #include +#include "SerialMuxChannels.h" /****************************************************************************** * Macros @@ -64,7 +65,8 @@ class RemoteCtrlState : public IState CMD_ID_IDLE = 0, /**< Nothing to do. */ CMD_ID_START_LINE_SENSOR_CALIB, /**< Start line sensor calibration. */ CMD_ID_START_MOTOR_SPEED_CALIB, /**< Start motor speed calibration. */ - CMD_ID_REINIT_BOARD /**< Re-initialize the board. Required for webots simulation. */ + CMD_ID_REINIT_BOARD, /**< Re-initialize the board. Required for webots simulation. */ + CMD_ID_GET_MAX_SPEED, /**< Get maximum speed. */ } CmdId; @@ -118,8 +120,8 @@ class RemoteCtrlState : public IState { if (CMD_ID_IDLE == m_cmdId) { - m_cmdId = cmdId; - m_rspId = RSP_ID_PENDING; + m_cmdId = cmdId; + m_cmdRsp.responseId = RSP_ID_PENDING; } } @@ -128,20 +130,20 @@ class RemoteCtrlState : public IState * * @return Command response */ - RspId getCmdRsp() const + CommandResponse getCmdRsp() { - return m_rspId; + return m_cmdRsp; } protected: private: - CmdId m_cmdId; /**< Current pending command. */ - RspId m_rspId; /**< Current command response. */ + CmdId m_cmdId; /**< Current pending command. */ + CommandResponse m_cmdRsp; /**< Command response */ /** * Default constructor. */ - RemoteCtrlState() : m_cmdId(CMD_ID_IDLE), m_rspId(RSP_ID_OK) + RemoteCtrlState() : m_cmdId(CMD_ID_IDLE), m_cmdRsp() { } @@ -162,8 +164,9 @@ class RemoteCtrlState : public IState */ void finishCommand(RspId rsp) { - m_rspId = rsp; - m_cmdId = CMD_ID_IDLE; + m_cmdRsp.commandId = m_cmdId; + m_cmdRsp.responseId = rsp; + m_cmdId = CMD_ID_IDLE; } }; diff --git a/lib/APPRemoteControl/SerialMuxChannels.h b/lib/APPRemoteControl/SerialMuxChannels.h index b9ba078f..f80afba0 100644 --- a/lib/APPRemoteControl/SerialMuxChannels.h +++ b/lib/APPRemoteControl/SerialMuxChannels.h @@ -94,7 +94,9 @@ typedef struct _Command /** Struct of the "Command Response" channel payload. */ typedef struct _CommandResponse { - uint8_t response; /**< Response to the command */ + uint8_t commandId; /**< Command ID */ + uint8_t responseId; /**< Response to the command */ + int16_t value; /**< Value of the response */ } __attribute__((packed)) CommandResponse; /** Struct of the "Speed" channel payload. */ From d2a45a7d338b7bb6879a5c34884fce8d29192f4e Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Mon, 15 Jan 2024 16:01:23 +0100 Subject: [PATCH 3/6] Added CMD_ID_GET_MAX_SPEED --- lib/APPRemoteControl/RemoteCtrlState.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/lib/APPRemoteControl/RemoteCtrlState.cpp b/lib/APPRemoteControl/RemoteCtrlState.cpp index 9d908055..93852add 100644 --- a/lib/APPRemoteControl/RemoteCtrlState.cpp +++ b/lib/APPRemoteControl/RemoteCtrlState.cpp @@ -109,6 +109,11 @@ void RemoteCtrlState::process(StateMachine& sm) finishCommand(RSP_ID_OK); break; + case CMD_ID_GET_MAX_SPEED: + m_cmdRsp.value = DifferentialDrive::getInstance().getMaxMotorSpeed(); + finishCommand(RSP_ID_OK); + break; + default: break; } From 77462b04c229f36118bcdc20f2d9a2e1d3ae9f9b Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Tue, 16 Jan 2024 11:39:36 +0100 Subject: [PATCH 4/6] Fixed review findings --- lib/APPRemoteControl/App.cpp | 2 +- lib/APPRemoteControl/App.h | 2 +- lib/APPRemoteControl/RemoteCtrlState.cpp | 2 +- lib/APPRemoteControl/SerialMuxChannels.h | 5 ++++- 4 files changed, 7 insertions(+), 4 deletions(-) diff --git a/lib/APPRemoteControl/App.cpp b/lib/APPRemoteControl/App.cpp index 6de37e35..b1b8a5b5 100644 --- a/lib/APPRemoteControl/App.cpp +++ b/lib/APPRemoteControl/App.cpp @@ -276,7 +276,7 @@ static void App_motorSpeedsChannelCallback(const uint8_t* payload, const uint8_t } /** - * Receives initial data over SerialMuxProt channel. Triggers sending the max speed. + * Receives initial data over SerialMuxProt channel. * * @param[in] payload Initial vehicle data. Position coordinates, orientation, and motor speeds * @param[in] payloadSize Size of VehicleData struct. diff --git a/lib/APPRemoteControl/App.h b/lib/APPRemoteControl/App.h index 986aec5b..f849c76a 100644 --- a/lib/APPRemoteControl/App.h +++ b/lib/APPRemoteControl/App.h @@ -73,7 +73,7 @@ class App m_smpChannelIdRemoteCtrlRsp(0U), m_smpChannelIdLineSensors(0U), m_smpChannelIdCurrentVehicleData(0U), - m_lastRemoteControlRspId() + m_lastRemoteControlRspId{RemoteCtrlState::CMD_ID_IDLE, RemoteCtrlState::RSP_ID_OK, 0} { } diff --git a/lib/APPRemoteControl/RemoteCtrlState.cpp b/lib/APPRemoteControl/RemoteCtrlState.cpp index 93852add..3bb505c1 100644 --- a/lib/APPRemoteControl/RemoteCtrlState.cpp +++ b/lib/APPRemoteControl/RemoteCtrlState.cpp @@ -110,7 +110,7 @@ void RemoteCtrlState::process(StateMachine& sm) break; case CMD_ID_GET_MAX_SPEED: - m_cmdRsp.value = DifferentialDrive::getInstance().getMaxMotorSpeed(); + m_cmdRsp.maxMotorSpeed = DifferentialDrive::getInstance().getMaxMotorSpeed(); finishCommand(RSP_ID_OK); break; diff --git a/lib/APPRemoteControl/SerialMuxChannels.h b/lib/APPRemoteControl/SerialMuxChannels.h index f80afba0..64e7d7ba 100644 --- a/lib/APPRemoteControl/SerialMuxChannels.h +++ b/lib/APPRemoteControl/SerialMuxChannels.h @@ -96,7 +96,10 @@ typedef struct _CommandResponse { uint8_t commandId; /**< Command ID */ uint8_t responseId; /**< Response to the command */ - int16_t value; /**< Value of the response */ + union + { + int16_t maxMotorSpeed; /**< Value of the response */ + }; } __attribute__((packed)) CommandResponse; /** Struct of the "Speed" channel payload. */ From 940fd1bc6003ec2ccff464c9e10ab5ba75fc1c8a Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Tue, 16 Jan 2024 11:42:55 +0100 Subject: [PATCH 5/6] Fixed response comment --- lib/APPRemoteControl/SerialMuxChannels.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/APPRemoteControl/SerialMuxChannels.h b/lib/APPRemoteControl/SerialMuxChannels.h index 64e7d7ba..fd55b8f7 100644 --- a/lib/APPRemoteControl/SerialMuxChannels.h +++ b/lib/APPRemoteControl/SerialMuxChannels.h @@ -98,7 +98,7 @@ typedef struct _CommandResponse uint8_t responseId; /**< Response to the command */ union { - int16_t maxMotorSpeed; /**< Value of the response */ + int16_t maxMotorSpeed; /**< Max speed. */ }; } __attribute__((packed)) CommandResponse; From 632bca1ac78b30e07bd478c01f0a6124f9a5d1e7 Mon Sep 17 00:00:00 2001 From: gabryelreyes Date: Tue, 16 Jan 2024 11:50:57 +0100 Subject: [PATCH 6/6] Added missing Doxygen comment --- lib/APPRemoteControl/SerialMuxChannels.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/lib/APPRemoteControl/SerialMuxChannels.h b/lib/APPRemoteControl/SerialMuxChannels.h index fd55b8f7..cd00708f 100644 --- a/lib/APPRemoteControl/SerialMuxChannels.h +++ b/lib/APPRemoteControl/SerialMuxChannels.h @@ -96,6 +96,8 @@ typedef struct _CommandResponse { uint8_t commandId; /**< Command ID */ uint8_t responseId; /**< Response to the command */ + + /** Response Payload. */ union { int16_t maxMotorSpeed; /**< Max speed. */