From 0fabdcfa9abe05f7dad744a435b72c1d4bd992c5 Mon Sep 17 00:00:00 2001 From: greyes Date: Sun, 5 Nov 2023 14:07:23 +0100 Subject: [PATCH 1/4] Removed Bytearray Utils. SMP Payloads are sent using little-endian structs --- lib/Service/Util.cpp | 93 --------------------------------- lib/Service/Util.h | 121 +++++++++++-------------------------------- 2 files changed, 29 insertions(+), 185 deletions(-) diff --git a/lib/Service/Util.cpp b/lib/Service/Util.cpp index 4ff4d4f1..a0a7434d 100644 --- a/lib/Service/Util.cpp +++ b/lib/Service/Util.cpp @@ -150,99 +150,6 @@ void Util::intToStr(char* str, size_t size, int32_t value) } } -void Util::int16ToByteArray(uint8_t* buffer, size_t size, int16_t value) -{ - if ((nullptr != buffer) && (sizeof(int16_t) <= size)) - { - buffer[0U] = ((value >> 8U) & 0xFF); - buffer[1U] = ((value >> 0U) & 0xFF); - } -} - -void Util::uint16ToByteArray(uint8_t* buffer, size_t size, uint16_t value) -{ - if ((nullptr != buffer) && (sizeof(uint16_t) <= size)) - { - buffer[0U] = ((value >> 8U) & 0xFF); - buffer[1U] = ((value >> 0U) & 0xFF); - } -} - -void Util::int32ToByteArray(uint8_t* buffer, size_t size, int32_t value) -{ - if ((nullptr != buffer) && (sizeof(int32_t) <= size)) - { - uint16_t hiBytes = ((value >> 16U) & 0xFFFF); - uint16_t lowBytes = ((value >> 0U) & 0xFFFF); - - buffer[0U] = ((hiBytes >> 8U) & 0xFF); - buffer[1U] = ((hiBytes >> 0U) & 0xFF); - buffer[2U] = ((lowBytes >> 8U) & 0xFF); - buffer[3U] = ((lowBytes >> 0U) & 0xFF); - } -} - -void Util::uint32ToByteArray(uint8_t* buffer, size_t size, uint32_t value) -{ - if ((nullptr != buffer) && (sizeof(uint32_t) <= size)) - { - uint16_t hiBytes = ((value >> 16U) & 0xFFFF); - uint16_t lowBytes = ((value >> 0U) & 0xFFFF); - - buffer[0U] = ((hiBytes >> 8U) & 0xFF); - buffer[1U] = ((hiBytes >> 0U) & 0xFF); - buffer[2U] = ((lowBytes >> 8U) & 0xFF); - buffer[3U] = ((lowBytes >> 0U) & 0xFF); - } -} - -bool Util::byteArrayToInt16(const uint8_t* buffer, size_t size, int16_t& value) -{ - bool isSuccess = false; - - if ((nullptr != buffer) && (sizeof(int16_t) <= size)) - { - value = ((static_cast(buffer[0U]) << 8U) & 0xFF00) | - ((static_cast(buffer[1U]) << 0U) & 0x00FF) ; - - isSuccess = true; - } - - return isSuccess; -} - -bool Util::byteArrayToUint16(const uint8_t* buffer, size_t size, uint16_t& value) -{ - bool isSuccess = false; - - if ((nullptr != buffer) && (sizeof(uint16_t) <= size)) - { - value = ((static_cast(buffer[0U]) << 8U) & 0xFF00) | - ((static_cast(buffer[1U]) << 0U) & 0x00FF) ; - - isSuccess = true; - } - - return isSuccess; -} - -bool Util::byteArrayToUint32(const uint8_t* buffer, size_t size, uint32_t& value) -{ - bool isSuccess = false; - - if ((nullptr != buffer) && (sizeof(uint32_t) <= size)) - { - value = ((static_cast(buffer[0U]) << 24U) & 0xFF000000) | - ((static_cast(buffer[1U]) << 16U) & 0x00FF0000) | - ((static_cast(buffer[2U]) << 8U) & 0x0000FF00) | - ((static_cast(buffer[3U]) << 0U) & 0x000000FF) ; - - isSuccess = true; - } - - return isSuccess; -} - /****************************************************************************** * Local Functions *****************************************************************************/ diff --git a/lib/Service/Util.h b/lib/Service/Util.h index 8e39b72b..da44f7ab 100644 --- a/lib/Service/Util.h +++ b/lib/Service/Util.h @@ -52,98 +52,35 @@ namespace Util { -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/****************************************************************************** - * Functions - *****************************************************************************/ - -/** - * Unsigned integer to string, without preceeding zeros. - * - * @param[out] str Destination string - * @param[in] size Size of the destination string in byte - * @param[in] value Value - */ -void uintToStr(char* str, size_t size, uint32_t value); - -/** - * Signed integer to string, without preceeding zeros. - * - * @param[out] str Destination string - * @param[in] size Size of the destination string in byte - * @param[in] value Value - */ -void intToStr(char* str, size_t size, int32_t value); - -/** - * Signed 16-bit integer to byte array. - * Endianness: Big endian - * @param[out] buffer Destination array - * @param[in] size Size of the destination buffer in byte - * @param[in] value Value - */ -void int16ToByteArray(uint8_t* buffer, size_t size, int16_t value); - -/** - * Unsigned 16-bit integer to byte array. - * Endianness: Big endian - * @param[out] buffer Destination array - * @param[in] size Size of the destination buffer in byte - * @param[in] value Value - */ -void uint16ToByteArray(uint8_t* buffer, size_t size, uint16_t value); - -/** - * Signed 32-bit integer to byte array. - * Endianness: Big endian - * @param[out] buffer Destination array - * @param[in] size Size of the destination buffer in byte - * @param[in] value Value - */ -void int32ToByteArray(uint8_t* buffer, size_t size, int32_t value); - -/** - * Unsigned 32-bit integer to byte array. - * Endianness: Big endian. - * @param[out] buffer Destination array. - * @param[in] size Size of the destination buffer in byte. - * @param[in] value Value. - */ -void uint32ToByteArray(uint8_t* buffer, size_t size, uint32_t value); - -/** - * Big endian byte array to int16_t. - * @param[in] buffer Source Array. - * @param[in] size Size of source array. - * @param[out] value Destination integer. - * @returns true if succesfully parsed. Otherwise, false. - */ -bool byteArrayToInt16(const uint8_t* buffer, size_t size, int16_t& value); - -/** - * Big endian byte array to uint16_t. - * @param[in] buffer Source Array. - * @param[in] size Size of source array. - * @param[out] value Destination integer. - * @returns true if succesfully parsed. Otherwise, false. - */ -bool byteArrayToUint16(const uint8_t* buffer, size_t size, uint16_t& value); - -/** - * Big endian byte array to uint32_t. - * @param[in] buffer Source Array. - * @param[in] size Size of source array. - * @param[out] value Destination integer. - * @returns true if succesfully parsed. Otherwise, false. - */ -bool byteArrayToUint32(const uint8_t* buffer, size_t size, uint32_t& value); + /****************************************************************************** + * Macros + *****************************************************************************/ + + /****************************************************************************** + * Types and Classes + *****************************************************************************/ + + /****************************************************************************** + * Functions + *****************************************************************************/ + + /** + * Unsigned integer to string, without preceeding zeros. + * + * @param[out] str Destination string + * @param[in] size Size of the destination string in byte + * @param[in] value Value + */ + void uintToStr(char* str, size_t size, uint32_t value); + + /** + * Signed integer to string, without preceeding zeros. + * + * @param[out] str Destination string + * @param[in] size Size of the destination string in byte + * @param[in] value Value + */ + void intToStr(char* str, size_t size, int32_t value); } // namespace Util From 4423c329da00a7bae753bb738b40b2c3f04629a6 Mon Sep 17 00:00:00 2001 From: greyes Date: Sun, 5 Nov 2023 14:07:55 +0100 Subject: [PATCH 2/4] Updated AppRemoteControl to use SerialMuxChannels --- lib/APPRemoteControl/App.cpp | 55 ++++-------- lib/APPRemoteControl/App.h | 19 +--- lib/APPRemoteControl/SerialMuxChannels.h | 105 +++++++++++++++++++++++ 3 files changed, 126 insertions(+), 53 deletions(-) create mode 100644 lib/APPRemoteControl/SerialMuxChannels.h diff --git a/lib/APPRemoteControl/App.cpp b/lib/APPRemoteControl/App.cpp index 11bbc951..ae896655 100644 --- a/lib/APPRemoteControl/App.cpp +++ b/lib/APPRemoteControl/App.cpp @@ -65,18 +65,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 +75,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 +149,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 +163,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 +210,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/APPRemoteControl/App.h b/lib/APPRemoteControl/App.h index 7e3c7813..cf8acd9b 100644 --- a/lib/APPRemoteControl/App.h +++ b/lib/APPRemoteControl/App.h @@ -46,7 +46,7 @@ #include #include #include - +#include "SerialMuxChannels.h" #include "RemoteCtrlState.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; @@ -123,10 +111,9 @@ class App /** * SerialMuxProt Server Instance * - * @tparam tMaxChannels set to 10, as App does not require - * more channels for external communication. + * @tparam tMaxChannels set to MAX_CHANNELS, defined in SerialMuxChannels.h. */ - SerialMuxProtServer<10U> m_smpServer; + SerialMuxProtServer m_smpServer; /** Channel id sending remote control command responses. */ uint8_t m_smpChannelIdRemoteCtrlRsp; diff --git a/lib/APPRemoteControl/SerialMuxChannels.h b/lib/APPRemoteControl/SerialMuxChannels.h new file mode 100644 index 00000000..d37cccf2 --- /dev/null +++ b/lib/APPRemoteControl/SerialMuxChannels.h @@ -0,0 +1,105 @@ +/* MIT License + * + * Copyright (c) 2023 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Channel structure definition for the SerialMuxProt. + * @author Gabryel Reyes + */ + +#ifndef SERIAL_MUX_CHANNELS_H_ +#define SERIAL_MUX_CHANNELS_H_ + +/****************************************************************************** + * Includes + *****************************************************************************/ + +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/** Maximum number of SerialMuxProt Channels. */ +#define MAX_CHANNELS (10U) + +/** Name of Channel to send Commands to. */ +#define COMMAND_CHANNEL_NAME "CMD" + +/** DLC of Command Channel. */ +#define COMMAND_CHANNEL_DLC (sizeof(Command)) + +/** Name of Channel to receive Command Responses from. */ +#define COMMAND_RESPONSE_CHANNEL_NAME "CMD_RSP" + +/** DLC of Command Response Channel. */ +#define COMMAND_RESPONSE_CHANNEL_DLC (sizeof(CommandResponse)) + +/** Name of Channel to send Motor Speed Setpoints to. */ +#define SPEED_SETPOINT_CHANNEL_NAME "SPEED_SET" + +/** DLC of Speedometer Channel */ +#define SPEED_SETPOINT_CHANNEL_DLC (sizeof(SpeedData)) + +/** Name of the Channel to receive Line Sensor Data from. */ +#define LINE_SENSOR_CHANNEL_NAME "LINE_SENS" + +/** DLC of Line Sensor Channel */ +#define LINE_SENSOR_CHANNEL_DLC (sizeof(LineSensorData)) + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** Struct of the "Command" channel payload. */ +typedef struct _Command +{ + uint8_t commandId; +} __attribute__((packed)) Command; + +/** Struct of the "Command Response" channel payload. */ +typedef struct _CommandResponse +{ + uint8_t response; +} __attribute__((packed)) CommandResponse; + +/** Struct of the "Speed" channel payload. */ +typedef struct _SpeedData +{ + int16_t left; + int16_t right; +} __attribute__((packed)) SpeedData; + +/** Struct of the "Line Sensor" channel payload. */ +typedef struct _LineSensorData +{ + uint16_t lineSensorData[5U]; +} __attribute__((packed)) LineSensorData; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* SERIAL_MUX_CHANNELS_H_ */ \ No newline at end of file From 0e09a649cfa8edff5197d4c6344e2ba6b45d8140 Mon Sep 17 00:00:00 2001 From: greyes Date: Sun, 5 Nov 2023 14:18:45 +0100 Subject: [PATCH 3/4] Removed old Utils from AppConvoyLeader --- lib/APPConvoyLeader/App.cpp | 12 ++---------- 1 file changed, 2 insertions(+), 10 deletions(-) diff --git a/lib/APPConvoyLeader/App.cpp b/lib/APPConvoyLeader/App.cpp index 9dee3894..ca797b2c 100644 --- a/lib/APPConvoyLeader/App.cpp +++ b/lib/APPConvoyLeader/App.cpp @@ -114,20 +114,12 @@ void App::loop() void App::reportPosition() { - int32_t xPos; - int32_t yPos; - uint8_t outBuf[POSITION_CHANNEL_DLC]; - - Odometry::getInstance().getPosition(xPos, yPos); - - Util::int32ToByteArray(&outBuf[0U], (sizeof(outBuf) - sizeof(int32_t)), xPos); - Util::int32ToByteArray(&outBuf[4U], (sizeof(outBuf) - sizeof(int32_t)), yPos); - - m_smpServer.sendData(POSITION_CHANNEL, outBuf, sizeof(outBuf)); + ; /* Do nothing. */ } void App::positionCallback(const uint8_t* payload, const uint8_t payloadSize) { + ; /* Do nothing. */ } /****************************************************************************** From 31ccdf4af308d44144e34456ed905a18c2784793 Mon Sep 17 00:00:00 2001 From: greyes Date: Sun, 5 Nov 2023 14:44:32 +0100 Subject: [PATCH 4/4] Updated documentation. Closes #20 --- README.md | 6 +++++- doc/configuration/README.md | 13 +++++++++++++ 2 files changed, 18 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index e82d8f76..fbef3a89 100644 --- a/README.md +++ b/README.md @@ -51,7 +51,11 @@ The simulation is based on the open source robot simulator *Webots*. The applica * Windows: ```%WEBOTS_HOME%\lib\controller``` 3. Install the native compiler toolchain: * Linux: Install the gcc toolchain, depended on your distribution. - * Windows: Install the [MSYS2](https://www.msys2.org) toolchain and follow the instructions there. + * Windows + * Install the [MSYS2](https://www.msys2.org) toolchain. + * Open MSYS2 shell. + * Update package database: ```pacman -Sy pacman``` + * Install GCC: ```pacman -Sy mingw-w64-ucrt-x86_64-gcc``` ## The Webots library To adapt the HAL to the simulation, some sourcecode files from Webots are necessary. Currently there is no Webots library in the platformio registry available. Therefore a local library is created during the build. Ensure that that Webots is already installed, before you try to build it! diff --git a/doc/configuration/README.md b/doc/configuration/README.md index 2823b58a..24d037a1 100644 --- a/doc/configuration/README.md +++ b/doc/configuration/README.md @@ -1,6 +1,7 @@ # Radon Ulzer - Line Follower - [Configuration Management](#configuration-management) +- [ArduinoNative](#arduinonative) - [Issues, Ideas And Bugs](#issues-ideas-and-bugs) - [License](#license) - [Contribution](#contribution) @@ -9,6 +10,18 @@ ![config](http://www.plantuml.com/plantuml/proxy?cache=no&src=https://raw.githubusercontent.com/BlueAndi/RadonUlzer/main/doc/configuration/uml/configuration.plantuml) +# PlatformIO Environments +As seen in the diagram before, the working environments are defined in the platformio.ini file, and these are used by PlatformIO to build the respective applications. To build an specific application, there are three possible methods: + +- On the left task bar of VS Code, choose the PlatformIO extension symbol, and open de drop-down menu of the desired application. +- Choose the desired application on the bottom task bar using the "Switch PlatformIO Project Environment" button. +- Use the command line: ```pio run -e ``` + +# ArduinoNative +In order to maintain compatibility between target and simulation, some interfaces from the Arduino Core have been adapted and/or stubbed into the ArduinoNative library. Code stubs for Stream, Serial, millis(), delay() and other similar Arduino functionalities have been implemented here. + +It is important to note that in the case of the simulation, the main entry point of the program is found in Arduino.cpp. + # Issues, Ideas And Bugs If you have further ideas or you found some bugs, great! Create a [issue](https://github.com/BlueAndi/RadonUlzer/issues) or if you are able and willing to fix it by yourself, clone the repository and create a pull request.