44 lines
1.5 KiB
C++
44 lines
1.5 KiB
C++
#include "PoseMsg.h"
|
|
#include "LowLevelMessages.h"
|
|
|
|
PoseMsg::PoseMsg(unsigned char networkId, unsigned char thingId,
|
|
unsigned char poseType, Spherical16 position,
|
|
SwingTwist16 orientation, Spherical16 linearVelocity,
|
|
Spherical16 angularVelocity) {
|
|
this->networkId = networkId;
|
|
this->thingId = thingId;
|
|
|
|
this->poseType = poseType;
|
|
this->position = position;
|
|
this->orientation = orientation;
|
|
this->linearVelocity = linearVelocity;
|
|
this->angularVelocity = angularVelocity;
|
|
}
|
|
PoseMsg::PoseMsg(const char *buffer) {
|
|
unsigned char ix = 1; // First byte is msg id
|
|
this->networkId = buffer[ix++];
|
|
this->thingId = buffer[ix++];
|
|
this->poseType = buffer[ix++];
|
|
this->position = LowLevelMessages::ReceiveSpherical16(buffer, &ix);
|
|
this->orientation = LowLevelMessages::ReceiveQuat32(buffer, &ix);
|
|
}
|
|
|
|
PoseMsg::~PoseMsg() {}
|
|
|
|
unsigned char PoseMsg::Serialize(char *buffer) {
|
|
unsigned char ix = 0;
|
|
buffer[ix++] = PoseMsg::id;
|
|
buffer[ix++] = this->networkId;
|
|
buffer[ix++] = this->thingId;
|
|
buffer[ix++] = this->poseType;
|
|
if ((this->poseType & Pose_Position) != 0)
|
|
LowLevelMessages::SendSpherical16(buffer, &ix, this->position);
|
|
if ((this->poseType & Pose_Orientation) != 0)
|
|
LowLevelMessages::SendQuat32(buffer, &ix, this->orientation);
|
|
if ((this->poseType & Pose_LinearVelocity) != 0)
|
|
LowLevelMessages::SendSpherical16(buffer, &ix, this->linearVelocity);
|
|
if ((this->poseType & Pose_AngularVelocity) != 0)
|
|
LowLevelMessages::SendSpherical16(buffer, &ix, this->angularVelocity);
|
|
return ix;
|
|
}
|