Skip to content

Commit

Permalink
Merge branch 'feature/removeBytearrayUtils' into HeadingCalculation
Browse files Browse the repository at this point in the history
  • Loading branch information
gabryelreyes committed Nov 1, 2023
2 parents e20ee44 + 4a45316 commit dab863c
Show file tree
Hide file tree
Showing 5 changed files with 150 additions and 234 deletions.
56 changes: 19 additions & 37 deletions lib/RemoteControl/App.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,7 @@
#include <Odometry.h>
#include <Board.h>
#include <Util.h>
#include "SerialMuxChannels.h"

/******************************************************************************
* Compiler Switches
Expand All @@ -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;

Expand All @@ -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()
Expand Down Expand Up @@ -163,9 +150,9 @@ void App::sendRemoteControlResponses()
/* Send only on change. */
if (remoteControlRspId != m_lastRemoteControlRspId)
{
const uint8_t* payload = reinterpret_cast<const uint8_t*>(&remoteControlRspId);
const CommandResponse* payload = reinterpret_cast<const CommandResponse*>(&remoteControlRspId);

(void)m_smpServer.sendData(m_smpChannelIdRemoteCtrlRsp, payload, sizeof(remoteControlRspId));
(void)m_smpServer.sendData(m_smpChannelIdRemoteCtrlRsp, reinterpret_cast<uint8_t*>(&payload), sizeof(payload));

m_lastRemoteControlRspId = remoteControlRspId;
}
Expand All @@ -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<uint8_t*>(&payload), sizeof(payload));
}

/******************************************************************************
Expand Down Expand Up @@ -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<const SpeedData*>(payload);
DifferentialDrive::getInstance().setLinearSpeed(motorSpeedData->left, motorSpeedData->right);
}
}
12 changes: 0 additions & 12 deletions lib/RemoteControl/App.h
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down
102 changes: 102 additions & 0 deletions lib/RemoteControl/SerialMuxChannels.h
Original file line number Diff line number Diff line change
@@ -0,0 +1,102 @@
/* MIT License
*
* Copyright (c) 2023 Andreas Merkle <[email protected]>
*
* 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 <[email protected]>
*/

#ifndef SERIAL_MUX_CHANNELS_H_
#define SERIAL_MUX_CHANNELS_H_

/******************************************************************************
* Includes
*****************************************************************************/

#include <Arduino.h>

/******************************************************************************
* Macros
*****************************************************************************/

/** 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_ */
93 changes: 0 additions & 93 deletions lib/Service/Util.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<int16_t>(buffer[0U]) << 8U) & 0xFF00) |
((static_cast<int16_t>(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<uint16_t>(buffer[0U]) << 8U) & 0xFF00) |
((static_cast<uint16_t>(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<uint32_t>(buffer[0U]) << 24U) & 0xFF000000) |
((static_cast<uint32_t>(buffer[1U]) << 16U) & 0x00FF0000) |
((static_cast<uint32_t>(buffer[2U]) << 8U) & 0x0000FF00) |
((static_cast<uint32_t>(buffer[3U]) << 0U) & 0x000000FF) ;

isSuccess = true;
}

return isSuccess;
}

/******************************************************************************
* Local Functions
*****************************************************************************/
Loading

0 comments on commit dab863c

Please sign in to comment.