540 lines
15 KiB
C++
540 lines
15 KiB
C++
/*
|
|
* Generated by erpcgen 1.14.0 on Thu Apr 23 10:54:10 2026.
|
|
*
|
|
* AUTOGENERATED - DO NOT EDIT
|
|
*/
|
|
|
|
|
|
#include "servo_service_server.hpp"
|
|
#if ERPC_ALLOCATION_POLICY == ERPC_ALLOCATION_POLICY_DYNAMIC
|
|
#include <new>
|
|
#include "erpc_port.h"
|
|
#endif
|
|
#include "erpc_manually_constructed.hpp"
|
|
|
|
#if 11400 != ERPC_VERSION_NUMBER
|
|
#error "The generated shim code version is different to the rest of eRPC code."
|
|
#endif
|
|
|
|
using namespace erpc;
|
|
using namespace std;
|
|
using namespace erpcShim;
|
|
|
|
#if ERPC_NESTED_CALLS_DETECTION
|
|
extern bool nestingDetection;
|
|
#endif
|
|
|
|
|
|
//! @brief Function to read struct ServoCmd
|
|
static void read_ServoCmd_struct(erpc::Codec * codec, ServoCmd * data);
|
|
|
|
//! @brief Function to read struct list_ServoCmd_1_t
|
|
static void read_list_ServoCmd_1_t_struct(erpc::Codec * codec, list_ServoCmd_1_t * data);
|
|
|
|
//! @brief Function to read struct list_float_1_t
|
|
static void read_list_float_1_t_struct(erpc::Codec * codec, list_float_1_t * data);
|
|
|
|
|
|
// Read struct ServoCmd function implementation
|
|
static void read_ServoCmd_struct(erpc::Codec * codec, ServoCmd * data)
|
|
{
|
|
if(NULL == data)
|
|
{
|
|
return;
|
|
}
|
|
|
|
{
|
|
uint32_t id_len;
|
|
char * id_local;
|
|
codec->readString(id_len, &id_local);
|
|
data->id = (char*) erpc_malloc((id_len + 1) * sizeof(char));
|
|
if ((data->id == NULL) || (id_local == NULL))
|
|
{
|
|
codec->updateStatus(kErpcStatus_MemoryError);
|
|
}
|
|
else
|
|
{
|
|
memcpy(data->id, id_local, id_len);
|
|
(data->id)[id_len] = 0;
|
|
}
|
|
}
|
|
|
|
codec->read(data->angle_rad);
|
|
}
|
|
|
|
// Read struct list_ServoCmd_1_t function implementation
|
|
static void read_list_ServoCmd_1_t_struct(erpc::Codec * codec, list_ServoCmd_1_t * data)
|
|
{
|
|
if(NULL == data)
|
|
{
|
|
return;
|
|
}
|
|
|
|
codec->startReadList(data->elementsCount);
|
|
data->elements = (ServoCmd *) erpc_malloc(data->elementsCount * sizeof(ServoCmd));
|
|
if ((data->elements == NULL) && (data->elementsCount > 0))
|
|
{
|
|
codec->updateStatus(kErpcStatus_MemoryError);
|
|
}
|
|
for (uint32_t listCount = 0U; listCount < data->elementsCount; ++listCount)
|
|
{
|
|
read_ServoCmd_struct(codec, &(data->elements[listCount]));
|
|
}
|
|
}
|
|
|
|
// Read struct list_float_1_t function implementation
|
|
static void read_list_float_1_t_struct(erpc::Codec * codec, list_float_1_t * data)
|
|
{
|
|
if(NULL == data)
|
|
{
|
|
return;
|
|
}
|
|
|
|
codec->startReadList(data->elementsCount);
|
|
data->elements = (float *) erpc_malloc(data->elementsCount * sizeof(float));
|
|
if ((data->elements == NULL) && (data->elementsCount > 0))
|
|
{
|
|
codec->updateStatus(kErpcStatus_MemoryError);
|
|
}
|
|
for (uint32_t listCount = 0U; listCount < data->elementsCount; ++listCount)
|
|
{
|
|
codec->read(data->elements[listCount]);
|
|
}
|
|
}
|
|
|
|
|
|
//! @brief Function to free space allocated inside struct ServoCmd
|
|
static void free_ServoCmd_struct(ServoCmd * data);
|
|
|
|
//! @brief Function to free space allocated inside struct list_ServoCmd_1_t
|
|
static void free_list_ServoCmd_1_t_struct(list_ServoCmd_1_t * data);
|
|
|
|
//! @brief Function to free space allocated inside struct list_float_1_t
|
|
static void free_list_float_1_t_struct(list_float_1_t * data);
|
|
|
|
|
|
// Free space allocated inside struct ServoCmd function implementation
|
|
static void free_ServoCmd_struct(ServoCmd * data)
|
|
{
|
|
erpc_free(data->id);
|
|
}
|
|
|
|
// Free space allocated inside struct list_ServoCmd_1_t function implementation
|
|
static void free_list_ServoCmd_1_t_struct(list_ServoCmd_1_t * data)
|
|
{
|
|
for (uint32_t listCount = 0; listCount < data->elementsCount; ++listCount)
|
|
{
|
|
free_ServoCmd_struct(&data->elements[listCount]);
|
|
}
|
|
|
|
erpc_free(data->elements);
|
|
}
|
|
|
|
// Free space allocated inside struct list_float_1_t function implementation
|
|
static void free_list_float_1_t_struct(list_float_1_t * data)
|
|
{
|
|
erpc_free(data->elements);
|
|
}
|
|
|
|
|
|
|
|
servo_service_service::servo_service_service(servo_service_interface *_servo_service_interface)
|
|
: erpc::Service(servo_service_interface::m_serviceId)
|
|
, m_handler(_servo_service_interface)
|
|
{
|
|
}
|
|
|
|
servo_service_service::~servo_service_service()
|
|
{
|
|
}
|
|
|
|
// return service interface handler.
|
|
servo_service_interface* servo_service_service::getHandler(void)
|
|
{
|
|
return m_handler;
|
|
}
|
|
|
|
// Call the correct server shim based on method unique ID.
|
|
erpc_status_t servo_service_service::handleInvocation(uint32_t methodId, uint32_t sequence, Codec * codec, MessageBufferFactory *messageFactory, Transport * transport)
|
|
{
|
|
erpc_status_t erpcStatus;
|
|
switch (methodId)
|
|
{
|
|
case servo_service_interface::m_setConstraintsId:
|
|
{
|
|
erpcStatus = setConstraints_shim(codec, messageFactory, transport, sequence);
|
|
break;
|
|
}
|
|
|
|
case servo_service_interface::m_setPositionGainId:
|
|
{
|
|
erpcStatus = setPositionGain_shim(codec, messageFactory, transport, sequence);
|
|
break;
|
|
}
|
|
|
|
case servo_service_interface::m_setModeId:
|
|
{
|
|
erpcStatus = setMode_shim(codec, messageFactory, transport, sequence);
|
|
break;
|
|
}
|
|
|
|
case servo_service_interface::m_setUpdatePeriodMsId:
|
|
{
|
|
erpcStatus = setUpdatePeriodMs_shim(codec, messageFactory, transport, sequence);
|
|
break;
|
|
}
|
|
|
|
case servo_service_interface::m_moveId:
|
|
{
|
|
erpcStatus = move_shim(codec, messageFactory, transport, sequence);
|
|
break;
|
|
}
|
|
|
|
case servo_service_interface::m_moveJId:
|
|
{
|
|
erpcStatus = moveJ_shim(codec, messageFactory, transport, sequence);
|
|
break;
|
|
}
|
|
|
|
default:
|
|
{
|
|
erpcStatus = kErpcStatus_InvalidArgument;
|
|
break;
|
|
}
|
|
}
|
|
|
|
return erpcStatus;
|
|
}
|
|
|
|
// Server shim for setConstraints of servo_service interface.
|
|
erpc_status_t servo_service_service::setConstraints_shim(Codec * codec, MessageBufferFactory *messageFactory, Transport * transport, uint32_t sequence)
|
|
{
|
|
erpc_status_t err = kErpcStatus_Success;
|
|
|
|
char * id = NULL;
|
|
float max_velocity_rad;
|
|
float max_acceleration_rad;
|
|
float max_jerk_rad;
|
|
bool result;
|
|
|
|
// startReadMessage() was already called before this shim was invoked.
|
|
|
|
{
|
|
uint32_t id_len;
|
|
char * id_local;
|
|
codec->readString(id_len, &id_local);
|
|
id = (char*) erpc_malloc((id_len + 1) * sizeof(char));
|
|
if ((id == NULL) || (id_local == NULL))
|
|
{
|
|
codec->updateStatus(kErpcStatus_MemoryError);
|
|
}
|
|
else
|
|
{
|
|
memcpy(id, id_local, id_len);
|
|
(id)[id_len] = 0;
|
|
}
|
|
}
|
|
|
|
codec->read(max_velocity_rad);
|
|
|
|
codec->read(max_acceleration_rad);
|
|
|
|
codec->read(max_jerk_rad);
|
|
|
|
err = codec->getStatus();
|
|
if (err == kErpcStatus_Success)
|
|
{
|
|
// Invoke the actual served function.
|
|
#if ERPC_NESTED_CALLS_DETECTION
|
|
nestingDetection = true;
|
|
#endif
|
|
result = m_handler->setConstraints(id, max_velocity_rad, max_acceleration_rad, max_jerk_rad);
|
|
#if ERPC_NESTED_CALLS_DETECTION
|
|
nestingDetection = false;
|
|
#endif
|
|
|
|
// preparing MessageBuffer for serializing data
|
|
err = messageFactory->prepareServerBufferForSend(codec->getBufferRef(), transport->reserveHeaderSize());
|
|
}
|
|
|
|
if (err == kErpcStatus_Success)
|
|
{
|
|
// preparing codec for serializing data
|
|
codec->reset(transport->reserveHeaderSize());
|
|
|
|
// Build response message.
|
|
codec->startWriteMessage(message_type_t::kReplyMessage, servo_service_interface::m_serviceId, servo_service_interface::m_setConstraintsId, sequence);
|
|
|
|
codec->write(result);
|
|
|
|
err = codec->getStatus();
|
|
}
|
|
|
|
erpc_free(id);
|
|
|
|
return err;
|
|
}
|
|
|
|
// Server shim for setPositionGain of servo_service interface.
|
|
erpc_status_t servo_service_service::setPositionGain_shim(Codec * codec, MessageBufferFactory *messageFactory, Transport * transport, uint32_t sequence)
|
|
{
|
|
erpc_status_t err = kErpcStatus_Success;
|
|
|
|
char * id = NULL;
|
|
float position_gain;
|
|
bool result;
|
|
|
|
// startReadMessage() was already called before this shim was invoked.
|
|
|
|
{
|
|
uint32_t id_len;
|
|
char * id_local;
|
|
codec->readString(id_len, &id_local);
|
|
id = (char*) erpc_malloc((id_len + 1) * sizeof(char));
|
|
if ((id == NULL) || (id_local == NULL))
|
|
{
|
|
codec->updateStatus(kErpcStatus_MemoryError);
|
|
}
|
|
else
|
|
{
|
|
memcpy(id, id_local, id_len);
|
|
(id)[id_len] = 0;
|
|
}
|
|
}
|
|
|
|
codec->read(position_gain);
|
|
|
|
err = codec->getStatus();
|
|
if (err == kErpcStatus_Success)
|
|
{
|
|
// Invoke the actual served function.
|
|
#if ERPC_NESTED_CALLS_DETECTION
|
|
nestingDetection = true;
|
|
#endif
|
|
result = m_handler->setPositionGain(id, position_gain);
|
|
#if ERPC_NESTED_CALLS_DETECTION
|
|
nestingDetection = false;
|
|
#endif
|
|
|
|
// preparing MessageBuffer for serializing data
|
|
err = messageFactory->prepareServerBufferForSend(codec->getBufferRef(), transport->reserveHeaderSize());
|
|
}
|
|
|
|
if (err == kErpcStatus_Success)
|
|
{
|
|
// preparing codec for serializing data
|
|
codec->reset(transport->reserveHeaderSize());
|
|
|
|
// Build response message.
|
|
codec->startWriteMessage(message_type_t::kReplyMessage, servo_service_interface::m_serviceId, servo_service_interface::m_setPositionGainId, sequence);
|
|
|
|
codec->write(result);
|
|
|
|
err = codec->getStatus();
|
|
}
|
|
|
|
erpc_free(id);
|
|
|
|
return err;
|
|
}
|
|
|
|
// Server shim for setMode of servo_service interface.
|
|
erpc_status_t servo_service_service::setMode_shim(Codec * codec, MessageBufferFactory *messageFactory, Transport * transport, uint32_t sequence)
|
|
{
|
|
erpc_status_t err = kErpcStatus_Success;
|
|
|
|
RpcMotionMode mode;
|
|
int32_t _tmp_local_i32;
|
|
bool result;
|
|
|
|
// startReadMessage() was already called before this shim was invoked.
|
|
|
|
codec->read(_tmp_local_i32);
|
|
mode = static_cast<RpcMotionMode>(_tmp_local_i32);
|
|
|
|
err = codec->getStatus();
|
|
if (err == kErpcStatus_Success)
|
|
{
|
|
// Invoke the actual served function.
|
|
#if ERPC_NESTED_CALLS_DETECTION
|
|
nestingDetection = true;
|
|
#endif
|
|
result = m_handler->setMode(mode);
|
|
#if ERPC_NESTED_CALLS_DETECTION
|
|
nestingDetection = false;
|
|
#endif
|
|
|
|
// preparing MessageBuffer for serializing data
|
|
err = messageFactory->prepareServerBufferForSend(codec->getBufferRef(), transport->reserveHeaderSize());
|
|
}
|
|
|
|
if (err == kErpcStatus_Success)
|
|
{
|
|
// preparing codec for serializing data
|
|
codec->reset(transport->reserveHeaderSize());
|
|
|
|
// Build response message.
|
|
codec->startWriteMessage(message_type_t::kReplyMessage, servo_service_interface::m_serviceId, servo_service_interface::m_setModeId, sequence);
|
|
|
|
codec->write(result);
|
|
|
|
err = codec->getStatus();
|
|
}
|
|
|
|
return err;
|
|
}
|
|
|
|
// Server shim for setUpdatePeriodMs of servo_service interface.
|
|
erpc_status_t servo_service_service::setUpdatePeriodMs_shim(Codec * codec, MessageBufferFactory *messageFactory, Transport * transport, uint32_t sequence)
|
|
{
|
|
erpc_status_t err = kErpcStatus_Success;
|
|
|
|
uint32_t ms;
|
|
bool result;
|
|
|
|
// startReadMessage() was already called before this shim was invoked.
|
|
|
|
codec->read(ms);
|
|
|
|
err = codec->getStatus();
|
|
if (err == kErpcStatus_Success)
|
|
{
|
|
// Invoke the actual served function.
|
|
#if ERPC_NESTED_CALLS_DETECTION
|
|
nestingDetection = true;
|
|
#endif
|
|
result = m_handler->setUpdatePeriodMs(ms);
|
|
#if ERPC_NESTED_CALLS_DETECTION
|
|
nestingDetection = false;
|
|
#endif
|
|
|
|
// preparing MessageBuffer for serializing data
|
|
err = messageFactory->prepareServerBufferForSend(codec->getBufferRef(), transport->reserveHeaderSize());
|
|
}
|
|
|
|
if (err == kErpcStatus_Success)
|
|
{
|
|
// preparing codec for serializing data
|
|
codec->reset(transport->reserveHeaderSize());
|
|
|
|
// Build response message.
|
|
codec->startWriteMessage(message_type_t::kReplyMessage, servo_service_interface::m_serviceId, servo_service_interface::m_setUpdatePeriodMsId, sequence);
|
|
|
|
codec->write(result);
|
|
|
|
err = codec->getStatus();
|
|
}
|
|
|
|
return err;
|
|
}
|
|
|
|
// Server shim for move of servo_service interface.
|
|
erpc_status_t servo_service_service::move_shim(Codec * codec, MessageBufferFactory *messageFactory, Transport * transport, uint32_t sequence)
|
|
{
|
|
erpc_status_t err = kErpcStatus_Success;
|
|
|
|
list_ServoCmd_1_t *cmds = NULL;
|
|
cmds = (list_ServoCmd_1_t *) erpc_malloc(sizeof(list_ServoCmd_1_t));
|
|
if (cmds == NULL)
|
|
{
|
|
codec->updateStatus(kErpcStatus_MemoryError);
|
|
}
|
|
int32_t result;
|
|
|
|
// startReadMessage() was already called before this shim was invoked.
|
|
|
|
read_list_ServoCmd_1_t_struct(codec, cmds);
|
|
|
|
err = codec->getStatus();
|
|
if (err == kErpcStatus_Success)
|
|
{
|
|
// Invoke the actual served function.
|
|
#if ERPC_NESTED_CALLS_DETECTION
|
|
nestingDetection = true;
|
|
#endif
|
|
result = m_handler->move(cmds);
|
|
#if ERPC_NESTED_CALLS_DETECTION
|
|
nestingDetection = false;
|
|
#endif
|
|
|
|
// preparing MessageBuffer for serializing data
|
|
err = messageFactory->prepareServerBufferForSend(codec->getBufferRef(), transport->reserveHeaderSize());
|
|
}
|
|
|
|
if (err == kErpcStatus_Success)
|
|
{
|
|
// preparing codec for serializing data
|
|
codec->reset(transport->reserveHeaderSize());
|
|
|
|
// Build response message.
|
|
codec->startWriteMessage(message_type_t::kReplyMessage, servo_service_interface::m_serviceId, servo_service_interface::m_moveId, sequence);
|
|
|
|
codec->write(result);
|
|
|
|
err = codec->getStatus();
|
|
}
|
|
|
|
if (cmds)
|
|
{
|
|
free_list_ServoCmd_1_t_struct(cmds);
|
|
}
|
|
erpc_free(cmds);
|
|
|
|
return err;
|
|
}
|
|
|
|
// Server shim for moveJ of servo_service interface.
|
|
erpc_status_t servo_service_service::moveJ_shim(Codec * codec, MessageBufferFactory *messageFactory, Transport * transport, uint32_t sequence)
|
|
{
|
|
erpc_status_t err = kErpcStatus_Success;
|
|
|
|
list_float_1_t *angles_rad = NULL;
|
|
angles_rad = (list_float_1_t *) erpc_malloc(sizeof(list_float_1_t));
|
|
if (angles_rad == NULL)
|
|
{
|
|
codec->updateStatus(kErpcStatus_MemoryError);
|
|
}
|
|
int32_t result;
|
|
|
|
// startReadMessage() was already called before this shim was invoked.
|
|
|
|
read_list_float_1_t_struct(codec, angles_rad);
|
|
|
|
err = codec->getStatus();
|
|
if (err == kErpcStatus_Success)
|
|
{
|
|
// Invoke the actual served function.
|
|
#if ERPC_NESTED_CALLS_DETECTION
|
|
nestingDetection = true;
|
|
#endif
|
|
result = m_handler->moveJ(angles_rad);
|
|
#if ERPC_NESTED_CALLS_DETECTION
|
|
nestingDetection = false;
|
|
#endif
|
|
|
|
// preparing MessageBuffer for serializing data
|
|
err = messageFactory->prepareServerBufferForSend(codec->getBufferRef(), transport->reserveHeaderSize());
|
|
}
|
|
|
|
if (err == kErpcStatus_Success)
|
|
{
|
|
// preparing codec for serializing data
|
|
codec->reset(transport->reserveHeaderSize());
|
|
|
|
// Build response message.
|
|
codec->startWriteMessage(message_type_t::kReplyMessage, servo_service_interface::m_serviceId, servo_service_interface::m_moveJId, sequence);
|
|
|
|
codec->write(result);
|
|
|
|
err = codec->getStatus();
|
|
}
|
|
|
|
if (angles_rad)
|
|
{
|
|
free_list_float_1_t_struct(angles_rad);
|
|
}
|
|
erpc_free(angles_rad);
|
|
|
|
return err;
|
|
}
|