Obbart/RoboGlue_QT
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
Files
13a494256298ff2b332403394f86731e33151b6f
RoboGlue_QT/libURcom/clientMessage.cpp
Emanuele b843b8439e Risolto problema in libURcom 
package lenght mismatch per il pacchetto tipo 8 AdditionalInfo,
aggiunto uchar foo alla fine per far sparire l'errore.. chi sa perchè?
2019-10-29 10:39:35 +01:00

457 lines
13 KiB
C++
Raw Blame History

/*
* clientMessage.cpp
*
* Created on: Jan 16, 2019
* Author: emanuele
*/
#include "clientMessage.h"
namespace subpackage {
clientSubpackage::clientSubpackage() {
}
clientSubpackage::~clientSubpackage() {
}
uint32_t clientSubpackage::parse() {
return 0;
}
versionSub::~versionSub() {
}
configurationSub::~configurationSub() {
}
calibrationSub::~calibrationSub() {
}
robotModeSub::~robotModeSub() {
}
forceModeSub::~forceModeSub() {
}
kineInfoSub::~kineInfoSub() {
}
jointDataSub::~jointDataSub() {
}
cartesianInfoSub::~cartesianInfoSub() {
}
additionalInfoSub::~additionalInfoSub() {
}
toolDataSub::~toolDataSub() {
}
toolModeSub::~toolModeSub() {
}
toolComSub::~toolComSub() {
}
masterBoardSub::~masterBoardSub() {
}
uint32_t subpackage::clientSubpackage::getUInt32(unsigned char** dataPointer) {
uint32_t d;
memcpy(&d, *dataPointer, sizeof(uint32_t));
*dataPointer+=sizeof(uint32_t);
return big_to_native(d);
}
uint64_t subpackage::clientSubpackage::getUInt64(unsigned char** dataPointer) {
uint64_t d;
memcpy(&d, *dataPointer, sizeof(uint64_t));
*dataPointer+=sizeof(uint64_t);
return big_to_native(d);
}
int32_t subpackage::clientSubpackage::getInt32(unsigned char** dataPointer) {
int32_t d;
memcpy(&d, *dataPointer, sizeof(int32_t));
*dataPointer+=sizeof(int32_t);
return big_to_native(d);
}
double subpackage::clientSubpackage::getDouble(unsigned char** dataPointer) {
union{
uint64_t i;
double dd;
} d;
memcpy(&d, *dataPointer, sizeof(double));
d.i=big_to_native(d.i);
*dataPointer+=sizeof(double);
return d.dd;
}
float subpackage::clientSubpackage::getFloat(unsigned char** dataPointer) {
union{
uint32_t i;
float dd;
} d;
memcpy(&d, *dataPointer, sizeof(float));
d.i=big_to_native(d.i);
*dataPointer+=sizeof(float);
return d.dd;
}
char subpackage::clientSubpackage::getChar(unsigned char** dataPointer) {
char val=**dataPointer;
*dataPointer+=1;
return val;
}
unsigned char subpackage::clientSubpackage::getUChar(unsigned char** dataPointer) {
unsigned char val=**dataPointer;
*dataPointer+=1;
return val;
}
void clientSubpackage::copyString(std::string **out, unsigned char** in, char len) {
char* s=NULL;
if (*out != NULL)
delete(out);
s = (char*)malloc(len+1);
strncpy(s, (const char*)*in, len);
s[(unsigned char)len]='\0';
*out = new std::string(s);
free(s);
*in+=len;
}
//////////////////////////////////////////////////////////////////////////////
/////////////// Overload of Parse Method /////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
uint32_t subpackage::robotModeSub::parse() {
robotModeData_t *v = &dataOut->robotMode;
unsigned char* begin = dataIn;
uint32_t size;
size = v->packageSize = this->getInt32(&dataIn);
v->packageType = this->getUChar(&dataIn);
v->timestamp = this->getUInt64(&dataIn);
v->isRealRobotConnected = this->getUChar(&dataIn);
v->isRealRobotEnabled = this->getUChar(&dataIn);
v->isRobotPowerOn = this->getUChar(&dataIn);
v->isEmergencyStopped = this->getUChar(&dataIn);
v->isProtectiveStopped = this->getUChar(&dataIn);
v->isProgramRunning = this->getUChar(&dataIn);
v->isProgramPaused = this->getUChar(&dataIn);
v->robotMode = this->getUChar(&dataIn);
v->controlMode = this->getUChar(&dataIn);
v->targetSpeedFraction = this->getDouble(&dataIn);
v->speedScaling = this->getDouble(&dataIn);
v->targetSpeedFractionLimit = this->getDouble(&dataIn);
v->internalValue = this->getUChar(&dataIn);
// check if read data is aligned with package size
if (dataIn-begin != size) throw dataIn-begin-size;
return dataIn-begin;
}
uint32_t subpackage::jointDataSub::parse() {
jointData_t *v = &dataOut->jointData;
unsigned char* begin = dataIn;
uint32_t size;
uint32_t c;
size = v->packageSize = this->getInt32(&dataIn);
v->packageType = this->getUChar(&dataIn);
for (c=0; c<JOINT_NUMBER; c++){
v->jointParam[c].actual = this->getDouble(&dataIn);
v->jointParam[c].target = this->getDouble(&dataIn);
v->jointParam[c].speedActual = this->getDouble(&dataIn);
v->jointParam[c].current = this->getFloat(&dataIn);
v->jointParam[c].voltage = this->getFloat(&dataIn);
v->jointParam[c].motorTemp = this->getFloat(&dataIn);
v->jointParam[c].nc = this->getFloat(&dataIn);
v->jointParam[c].mode = this->getUChar(&dataIn);
}
// check if read data is aligned with package size
if (dataIn-begin != size) throw dataIn-begin-size;
return dataIn-begin;
}
uint32_t subpackage::cartesianInfoSub::parse() {
cartesianInfo_t *v = &dataOut->cartesianInfo;
unsigned char* begin = dataIn;
uint32_t size;
uint32_t c;
size = v->packageSize = this->getInt32(&dataIn);
v->packageType = this->getUChar(&dataIn);
for (c=0; c<JOINT_NUMBER; c++){
v->cartPosition.pos[c] = this->getDouble(&dataIn);
}
for (c=0; c<JOINT_NUMBER; c++){
v->cartTCPoffset.ofst[c] = this->getDouble(&dataIn);
}
// check if read data is aligned with package size
if (dataIn-begin != size) throw dataIn-begin-size;
return dataIn-begin;
}
uint32_t subpackage::masterBoardSub::parse() {
masterBoardData_t *v = &dataOut->masterBoard;
unsigned char* begin = dataIn;
uint32_t size;
size = v->packageSize = this->getInt32(&dataIn);
v->packageType = this->getUChar(&dataIn);
v->digitalInputBits = this->getUInt32(&dataIn);
v->digitalOutputBits = this->getUInt32(&dataIn);
v->analogInputRange0 = this->getUChar(&dataIn);
v->analogInputRange1 = this->getUChar(&dataIn);
v->analogInput0 = this->getDouble(&dataIn);
v->analogInput1 = this->getDouble(&dataIn);
v->analogOutputDomain0 = this->getChar(&dataIn);
v->analogOutputDomain1 = this->getChar(&dataIn);
v->analogOutput0 = this->getDouble(&dataIn);
v->analogOutput1 = this->getDouble(&dataIn);
v->masterBoardTemperature = this->getFloat(&dataIn);
v->robotVoltage48V = this->getFloat(&dataIn);
v->robotCurrent = this->getFloat(&dataIn);
v->masterIOCurrent = this->getFloat(&dataIn);
v->safetyMode = this->getUChar(&dataIn);
v->InReducedMode = this->getUChar(&dataIn);
v->euromap67InterfaceInstalled = this->getChar(&dataIn);
#ifdef EUROMAP
v->euromapInputBits = this->getUInt32(&dataIn);
v->euromapOutputBits = this->getUInt32(&dataIn);
v->euromapVoltage24V = this->getFloat(&dataIn);
v->euromapCurrent = this->getFloat(&dataIn);
#endif
v->nc = this->getUInt32(&dataIn);
v->operationalModeSelectorInput = this->getUChar(&dataIn);
v->threePositionEnblingDeviceInput = this->getUChar(&dataIn);
// check if read data is aligned with package size
if (dataIn-begin != size) throw dataIn-begin-size;
return dataIn-begin;
}
uint32_t subpackage::toolDataSub::parse() {
toolData_t *v = &dataOut->toolData;
unsigned char* begin = dataIn;
uint32_t size;
size = v->packageSize = this->getInt32(&dataIn);
v->packageType = this->getUChar(&dataIn);
v->analogInputRange0 = this->getUChar(&dataIn);
v->analogInputRange1 = this->getUChar(&dataIn);
v->analogInput0 = this->getDouble(&dataIn);
v->analogInput1 = this->getDouble(&dataIn);
v->toolVoltage48V = this->getFloat(&dataIn);
v->toolOutputVoltage = this->getUChar(&dataIn);
v->toolCurrent = this->getFloat(&dataIn);
v->toolTemperature = this->getFloat(&dataIn);
v->toolMode = this->getUChar(&dataIn);
// check if read data is aligned with package size
if (dataIn-begin != size) throw dataIn-begin-size;
return dataIn-begin;
}
uint32_t subpackage::kineInfoSub::parse() {
kinematicsInfo_t *v = &dataOut->kineInfo;
kineDH_t *k=&dataOut->kineInfo.kinematics;
unsigned char* begin = dataIn;
uint32_t size;
uint32_t c;
size = v->packageSize = this->getInt32(&dataIn);
v->packageType = this->getUChar(&dataIn);
for(c=0; c<JOINT_NUMBER; c++){
v->checksum[c]=this->getUInt32(&dataIn);
}
for(c=0; c<JOINT_NUMBER; c++){
k->DHtheta[c]=this->getDouble(&dataIn);
}
for(c=0; c<JOINT_NUMBER; c++){
k->DHa[c]=this->getDouble(&dataIn);
}
for(c=0; c<JOINT_NUMBER; c++){
k->DHd[c]=this->getDouble(&dataIn);
}
for(c=0; c<JOINT_NUMBER; c++){
k->DHalpha[c]=this->getDouble(&dataIn);
}
v->calibration_status = this->getUInt32(&dataIn);
// check if read data is aligned with package size
if (dataIn-begin != size) throw dataIn-begin-size;
return dataIn-begin;
}
uint32_t subpackage::configurationSub::parse() {
configurationData_t *v = &dataOut->configuration;
kineDH_t *k=&dataOut->configuration.kinematics;
unsigned char* begin = dataIn;
uint32_t size;
uint32_t c;
size = v->packageSize = this->getInt32(&dataIn);
v->packageType = this->getUChar(&dataIn);
for(c=0; c<JOINT_NUMBER; c++){
v->jointPositionLimits[c].min=this->getDouble(&dataIn);
v->jointPositionLimits[c].max=this->getDouble(&dataIn);
}
for(c=0; c<JOINT_NUMBER; c++){
v->jointSpeedLimits[c].min=this->getDouble(&dataIn);
v->jointSpeedLimits[c].max=this->getDouble(&dataIn);
}
v->vJointDefault=this->getDouble(&dataIn);
v->aJointDefault=this->getDouble(&dataIn);
v->vToolDefault=this->getDouble(&dataIn);
v->aToolDefault=this->getDouble(&dataIn);
v->eqRadius=this->getDouble(&dataIn);
for(c=0; c<JOINT_NUMBER; c++){
k->DHa[c]=this->getDouble(&dataIn);
}
for(c=0; c<JOINT_NUMBER; c++){
k->DHd[c]=this->getDouble(&dataIn);
}
for(c=0; c<JOINT_NUMBER; c++){
k->DHalpha[c]=this->getDouble(&dataIn);
}
for(c=0; c<JOINT_NUMBER; c++){
k->DHtheta[c]=this->getDouble(&dataIn);
}
v->masterboardVersion=this->getUInt32(&dataIn);
v->controllerBoxType=this->getInt32(&dataIn);
v->robotType=this->getInt32(&dataIn);
v->robotSubType=this->getInt32(&dataIn);
// check if read data is aligned with package size
if (dataIn-begin != size) throw dataIn-begin-size;
return dataIn-begin;
}
uint32_t subpackage::forceModeSub::parse() {
forceModeData_t *v = &dataOut->forceMode;
unsigned char* begin = dataIn;
uint32_t size;
uint32_t c;
size = v->packageSize = this->getInt32(&dataIn);
v->packageType = this->getUChar(&dataIn);
for(c=0; c<JOINT_NUMBER; c++){
v->forcePosition.force[c] = this->getDouble(&dataIn);
}
v->robotDexterity = this->getDouble(&dataIn);
// check if read data is aligned with package size
if (dataIn-begin != size) throw dataIn-begin-size;
return dataIn-begin;
}
uint32_t subpackage::additionalInfoSub::parse() {
additionalInfo_t *v = &dataOut->additionalInfo;
unsigned char* begin = dataIn;
uint32_t size;
size = v->packageSize = this->getInt32(&dataIn);
v->packageType = this->getUChar(&dataIn);
v->feedriveButtonPressed = this->getUChar(&dataIn);
v->feedriveButtonEnabled = this->getUChar(&dataIn);
v->ioEnableFeedrive = this->getUChar(&dataIn);
v->foo = this->getUChar(&dataIn);
// check if read data is aligned with package size
if (dataIn-begin != size) throw dataIn-begin-size;
return dataIn-begin;
}
uint32_t subpackage::calibrationSub::parse() {
calibrationData_t *v = &dataOut->calibrationData;
unsigned char* begin = dataIn;
uint32_t size;
uint32_t c;
size = v->packageSize = this->getInt32(&dataIn);
v->packageType = this->getUChar(&dataIn);
for(c=0; c<JOINT_NUMBER; c++){
v->fcalData.force[c] = this->getDouble(&dataIn);
}
// check if read data is aligned with package size
if (dataIn-begin != size) throw dataIn-begin-size;
return dataIn-begin;
}
uint32_t subpackage::toolComSub::parse() {
toolCommunicationInfo_t *v = &dataOut->toolCom;
unsigned char* begin = dataIn;
uint32_t size;
size = v->packageSize = this->getInt32(&dataIn);
v->packageType = this->getUChar(&dataIn);
v->communicationIsEnabled = this->getUChar(&dataIn);
v->baudRate = this->getInt32(&dataIn);
v->parity = this->getInt32(&dataIn);
v->stopBits = this->getInt32(&dataIn);
v->rxIdleChars = this->getFloat(&dataIn);
v->txIdleChars = this->getFloat(&dataIn);
// check if read data is aligned with package size
if (dataIn-begin != size) throw dataIn-begin-size;
return dataIn-begin;
}
uint32_t subpackage::toolModeSub::parse() {
toolModeInfo_t *v = &dataOut->toolMode;
unsigned char* begin = dataIn;
uint32_t size;
size = v->packageSize = this->getInt32(&dataIn);
v->packageType = this->getUChar(&dataIn);
v->outputMode = this->getChar(&dataIn);
v->digitalModeOut0 = this->getUChar(&dataIn);
v->digitalModeOut1 = this->getUChar(&dataIn);
// check if read data is aligned with package size
if (dataIn-begin != size) throw dataIn-begin-size;
return dataIn-begin;
}
uint32_t subpackage::versionSub::parse() {
versionMessage_t *v = &dataOut->versonMessage;
unsigned char* begin = dataIn;
uint32_t size;
char nameSize;
size = v->packageSize = this->getInt32(&dataIn);
v->packageType = this->getUChar(&dataIn);
v->timestamp = this->getUInt64(&dataIn);
v->source = this->getChar(&dataIn);
v->robotMessageType = this->getChar(&dataIn);
nameSize = v->projectNameSize = this->getChar(&dataIn);
this->copyString(&v->projectName, &dataIn, nameSize);
v->majorVersion = this->getUChar(&dataIn);
v->minorVersion = this->getUChar(&dataIn);
v->bugFixVersion = this->getUChar(&dataIn);
v->buildNumber = this->getUChar(&dataIn);
nameSize = (begin+v->packageSize-dataIn);
this->copyString(&v->buildDate, &dataIn, nameSize);
// check if read data is aligned with package size
if (dataIn-begin != size) throw dataIn-begin-size;
return dataIn-begin;
}
} /* namespace clientmessage */
Reference in New Issue View Git Blame Copy Permalink