115 lines
3.4 KiB
C++
115 lines
3.4 KiB
C++
#include "NetworkSync.h"
|
|
|
|
#ifdef RC_DEBUG
|
|
#include <Arduino.h>
|
|
#endif
|
|
|
|
void NetworkSync::SendVector3(unsigned char *data, int startIndex, Vector3 v) {
|
|
SendSingle100(data, startIndex, v.x);
|
|
SendSingle100(data, startIndex + 4, v.y);
|
|
SendSingle100(data, startIndex + 8, v.z);
|
|
}
|
|
|
|
void NetworkSync::SendSingle100(unsigned char *data, int startIndex,
|
|
float value) {
|
|
// Sends a float with truncated 2 decimal precision
|
|
Int32 intValue = value * 100;
|
|
SendInt32(data, startIndex, intValue);
|
|
// for (unsigned char ix = 0; ix < 4; ix++) {
|
|
// data[startIndex + ix] = ((unsigned char *)&intValue)[ix];
|
|
// }
|
|
}
|
|
|
|
void NetworkSync::SendInt32(unsigned char *data, int startIndex, Int32 value) {
|
|
for (unsigned char ix = 0; ix < 4; ix++) {
|
|
data[startIndex + ix] = ((unsigned char *)&value)[ix];
|
|
}
|
|
}
|
|
|
|
void NetworkSync::PublishTrackedObjects(SendBuffer sendBuffer,
|
|
TrackedObject **objects) {
|
|
for (unsigned char objIx = 0; objIx < Perception::maxObjectCount; objIx++) {
|
|
TrackedObject *obj = objects[objIx];
|
|
if (obj == nullptr)
|
|
continue;
|
|
// if (obj->sensor->type == Thing::ExternalType)
|
|
// continue;
|
|
|
|
// tmp
|
|
obj->id = objIx;
|
|
PublishTrackedObject(sendBuffer, obj);
|
|
}
|
|
}
|
|
|
|
void NetworkSync::PublishTrackedObject(SendBuffer sendBuffer,
|
|
TrackedObject *object) {
|
|
Vector2 worldPosition2 = Vector2::Rotate(
|
|
Vector2::forward * object->position.distance, -object->position.angle);
|
|
Vector3 worldPosition3 = Vector3(worldPosition2.x, 0, worldPosition2.y);
|
|
|
|
#ifdef RC_DEBUG
|
|
Serial.print("Send Pose ");
|
|
Serial.print((int)object->id);
|
|
Serial.print(" Position ");
|
|
Serial.print(worldPosition3.x);
|
|
Serial.print(", ");
|
|
Serial.print(worldPosition3.y);
|
|
Serial.print(", ");
|
|
Serial.println(worldPosition3.z);
|
|
#else
|
|
UInt16 bufferSize = 3 + 12;
|
|
UInt8 buffer[bufferSize] = {
|
|
PoseMsg,
|
|
object->id,
|
|
Pose_Position,
|
|
};
|
|
SendVector3(buffer, 3, worldPosition3);
|
|
sendBuffer(buffer, bufferSize);
|
|
#endif
|
|
}
|
|
|
|
void NetworkSync::SendPoseMsg(SendBuffer sendBuffer, Roboid *roboid) {
|
|
Polar velocity = roboid->propulsion->GetVelocity();
|
|
Vector2 worldVelocity2 =
|
|
Vector2::Rotate(Vector2::forward * velocity.distance, velocity.angle);
|
|
Vector3 worldVelocity3 = Vector3(worldVelocity2.x, 0, worldVelocity2.y);
|
|
|
|
float angularVelocity = roboid->propulsion->GetAngularVelocity();
|
|
Vector3 worldAngularVelocity = Vector3(0, angularVelocity, 0);
|
|
|
|
#ifdef RC_DEBUG
|
|
Serial.print("Send Pose 0 LinearVelocity ");
|
|
Serial.print(worldVelocity3.x);
|
|
Serial.print(", ");
|
|
Serial.print(worldVelocity3.y);
|
|
Serial.print(", ");
|
|
Serial.print(worldVelocity3.z);
|
|
Serial.print(" AngularVelocity ");
|
|
Serial.print(worldAngularVelocity.x);
|
|
Serial.print(", ");
|
|
Serial.print(worldAngularVelocity.y);
|
|
Serial.print(", ");
|
|
Serial.println(worldAngularVelocity.z);
|
|
#else
|
|
const unsigned int bufferSize = 3 + 12 + 12;
|
|
unsigned char buffer[bufferSize] = {
|
|
PoseMsg,
|
|
0, // objectId;
|
|
Pose_LinearVelocity | Pose_AngularVelocity,
|
|
};
|
|
SendVector3(buffer, 3, worldVelocity3);
|
|
SendVector3(buffer, 15, worldAngularVelocity);
|
|
|
|
sendBuffer(buffer, bufferSize);
|
|
#endif
|
|
}
|
|
|
|
void NetworkSync::SendDestroyObject(SendBuffer sendBuffer, TrackedObject *obj) {
|
|
#ifdef RC_DEBUG
|
|
Serial.print("Send Destroy ");
|
|
Serial.println((int)obj->id);
|
|
#else
|
|
unsigned char buffer[2] = {DestroyMsg, (unsigned char)obj->id};
|
|
sendBuffer(buffer, 2);
|
|
#endif
|
|
} |