RoboidControl/RoboidControl-cpp
RoboidControl/RoboidControl-cpp
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Improve AngleOf usage

Browse Source
This commit is contained in:
Pascal Serrarens 2024-07-31 11:44:43 +02:00
parent 42d64a6afe
commit 00f40fea98
2 changed files with 63 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
LinearAlgebra

@ -1 +1 @@
Subproject commit c70c079efc4fcfe9e9b2c29cd82f67faa63c827f
Subproject commit b81b77b1c95de8fda430a0bd428dd551c89db380

112
NetworkSync.cpp
View File

@ -7,21 +7,24 @@
#endif
#include "LinearAlgebra/Angle8.h"
#include "LinearAlgebra/AngleUsing.h"
#include "LinearAlgebra/Spherical.h"
#include "float16/float16.h"
#include <string.h>
NetworkSync::NetworkSync(Roboid *roboid) { this->roboid = roboid; }
NetworkSync::NetworkSync(Roboid* roboid) {
this->roboid = roboid;
}
void NetworkSync::ReceiveMessage(Roboid *roboid, unsigned char bytecount) {
void NetworkSync::ReceiveMessage(Roboid* roboid, unsigned char bytecount) {
networkPerception->ProcessPacket(roboid, buffer, bytecount);
switch (buffer[0]) {
case NetworkIdMsg:
// this->networkId = NetworkIdMsg;
ReceiveNetworkId();
break;
case NetworkIdMsg:
// this->networkId = NetworkIdMsg;
ReceiveNetworkId();
break;
}
}
@ -37,7 +40,7 @@ void NetworkSync::ReceiveNetworkId() {
PublishRelativeThing(roboid->GetChild(0), true);
}
void NetworkSync::NewObject(InterestingThing *thing) {
void NetworkSync::NewObject(InterestingThing* thing) {
if (thing == nullptr || thing->networkId != 0x00)
return;
@ -54,8 +57,8 @@ void NetworkSync::NewObject(InterestingThing *thing) {
// PublishTrackedObject(roboid, obj);
}
void NetworkSync::PublishRelativeThing(Thing *thing, bool recurse) {
Thing *parentThing = thing->GetParent();
void NetworkSync::PublishRelativeThing(Thing* thing, bool recurse) {
Thing* parentThing = thing->GetParent();
unsigned char ix = 0;
buffer[ix++] = RelativePoseMsg;
@ -72,13 +75,13 @@ void NetworkSync::PublishRelativeThing(Thing *thing, bool recurse) {
// delay(1000);
if (recurse) {
Thing *child = thing->GetChild(0);
Thing* child = thing->GetChild(0);
if (child != nullptr)
PublishRelativeThing(child, true);
}
}
void NetworkSync::PublishModel(Roboid *roboid) {
void NetworkSync::PublishModel(Roboid* roboid) {
if (roboid->modelUrl == nullptr)
return;
@ -87,11 +90,11 @@ void NetworkSync::PublishModel(Roboid *roboid) {
return;
unsigned char ix = 0;
buffer[ix++] = 0x90; // modelMsg
buffer[ix++] = 0x00; // objectId
Spherical s = Spherical::zero; //(roboid->modelPosition);
buffer[ix++] = 0x90; // modelMsg
buffer[ix++] = 0x00; // objectId
Spherical s = Spherical::zero; //(roboid->modelPosition);
SendSpherical(buffer, &ix, s);
SendFloat16(buffer, &ix, 1); // roboid->modelScale);
SendFloat16(buffer, &ix, 1); // roboid->modelScale);
buffer[ix++] = len;
for (int urlIx = 0; urlIx < len; urlIx++)
@ -100,7 +103,7 @@ void NetworkSync::PublishModel(Roboid *roboid) {
SendBuffer(ix);
}
void NetworkSync::PublishModel(Thing *thing) {
void NetworkSync::PublishModel(Thing* thing) {
if (thing->modelUrl == nullptr)
return;
@ -109,11 +112,11 @@ void NetworkSync::PublishModel(Thing *thing) {
return;
unsigned char ix = 0;
buffer[ix++] = 0x90; // modelMsg
buffer[ix++] = thing->id; // objectId
Spherical s = Spherical::zero; // Spherical(thing->modelPosition);
buffer[ix++] = 0x90; // modelMsg
buffer[ix++] = thing->id; // objectId
Spherical s = Spherical::zero; // Spherical(thing->modelPosition);
SendSpherical(buffer, &ix, s);
SendFloat16(buffer, &ix, 1); // thing->modelScale);
SendFloat16(buffer, &ix, 1); // thing->modelScale);
buffer[ix++] = len;
for (int urlIx = 0; urlIx < len; urlIx++)
@ -122,7 +125,7 @@ void NetworkSync::PublishModel(Thing *thing) {
SendBuffer(ix);
}
void NetworkSync::DestroyObject(InterestingThing *thing) {
void NetworkSync::DestroyObject(InterestingThing* thing) {
unsigned char ix = 0;
buffer[ix++] = DestroyMsg;
buffer[ix++] = thing->id;
@ -133,7 +136,7 @@ void NetworkSync::DestroyObject(InterestingThing *thing) {
#endif
}
void NetworkSync::SendPose(Roboid *roboid, bool recurse) {
void NetworkSync::SendPose(Roboid* roboid, bool recurse) {
unsigned char ix = 0;
buffer[ix++] = PoseMsg;
buffer[ix++] = 0x00;
@ -143,13 +146,13 @@ void NetworkSync::SendPose(Roboid *roboid, bool recurse) {
SendBuffer(ix);
if (recurse) {
Thing *child = roboid->GetChild(0);
Thing* child = roboid->GetChild(0);
if (child != nullptr)
SendPose(child, true);
}
}
void NetworkSync::SendPose(Thing *thing, bool recurse) {
void NetworkSync::SendPose(Thing* thing, bool recurse) {
unsigned char ix = 0;
buffer[ix++] = PoseMsg;
buffer[ix++] = thing->id;
@ -160,7 +163,7 @@ void NetworkSync::SendPose(Thing *thing, bool recurse) {
if (recurse) {
// delay(10);
Thing *child = thing->GetChild(0);
Thing* child = thing->GetChild(0);
if (child != nullptr)
SendPose(child, true);
}
@ -169,7 +172,7 @@ void NetworkSync::SendPose(Thing *thing, bool recurse) {
void NetworkSync::PublishClient() {
unsigned char ix = 0;
buffer[ix++] = ClientMsg;
buffer[ix++] = 0; // No network ID
buffer[ix++] = 0; // No network ID
SendBuffer(ix);
#ifdef RC_DEBUG
@ -177,11 +180,11 @@ void NetworkSync::PublishClient() {
#endif
}
void NetworkSync::PublishTrackedObjects(Roboid *roboid,
InterestingThing **objects) {
void NetworkSync::PublishTrackedObjects(Roboid* roboid,
InterestingThing** objects) {
int n = 0;
for (unsigned char objIx = 0; objIx < Perception::maxObjectCount; objIx++) {
InterestingThing *obj = objects[objIx];
InterestingThing* obj = objects[objIx];
if (obj == nullptr)
continue;
@ -192,8 +195,8 @@ void NetworkSync::PublishTrackedObjects(Roboid *roboid,
}
}
void NetworkSync::PublishTrackedObject(Roboid *roboid,
InterestingThing *object) {
void NetworkSync::PublishTrackedObject(Roboid* roboid,
InterestingThing* object) {
if (object == nullptr || object->updated == false ||
object->networkId != 0x00) {
return;
@ -211,8 +214,8 @@ void NetworkSync::PublishTrackedObject(Roboid *roboid,
Quaternion worldOrientation = roboidOrientation * object->orientation;
unsigned char ix = 0;
buffer[ix++] = PoseMsg; // Position2DMsg;
buffer[ix++] = object->id; // objectId;
buffer[ix++] = PoseMsg; // Position2DMsg;
buffer[ix++] = object->id; // objectId;
buffer[ix++] = Pose_Position | Pose_Orientation;
// SendPolar(buffer, &ix, polar); // 3 bytes
SendVector3(buffer, &ix, worldPosition);
@ -227,7 +230,7 @@ void NetworkSync::PublishTrackedObject(Roboid *roboid,
#endif
}
void NetworkSync::SendPoseMsg(Buffer sendBuffer, Roboid *roboid) {
void NetworkSync::SendPoseMsg(Buffer sendBuffer, Roboid* roboid) {
Polar velocity = roboid->propulsion->GetVelocity();
Vector2 worldVelocity2 =
Vector2::Rotate(Vector2::forward * velocity.distance, velocity.angle);
@ -253,7 +256,7 @@ void NetworkSync::SendPoseMsg(Buffer sendBuffer, Roboid *roboid) {
const unsigned int bufferSize = 3 + 12 + 12;
unsigned char buffer[bufferSize] = {
PoseMsg,
0, // objectId;
0, // objectId;
Pose_LinearVelocity | Pose_AngularVelocity,
};
unsigned char ix = 3;
@ -264,7 +267,7 @@ void NetworkSync::SendPoseMsg(Buffer sendBuffer, Roboid *roboid) {
#endif
}
void NetworkSync::SendDestroyObject(Buffer sendBuffer, InterestingThing *obj) {
void NetworkSync::SendDestroyObject(Buffer sendBuffer, InterestingThing* obj) {
#ifdef RC_DEBUG
Serial.print("Send Destroy ");
Serial.println((int)obj->id);
@ -274,7 +277,7 @@ void NetworkSync::SendDestroyObject(Buffer sendBuffer, InterestingThing *obj) {
#endif
}
void NetworkSync::SendInvestigateThing(InterestingThing *thing) {
void NetworkSync::SendInvestigateThing(InterestingThing* thing) {
#ifdef RC_DEBUG
printf("Investigate [%d/%d]\n", thing->networkId, thing->id);
#endif
@ -285,7 +288,7 @@ void NetworkSync::SendInvestigateThing(InterestingThing *thing) {
SendBuffer(ix);
}
void NetworkSync::SendText(const char *s) {
void NetworkSync::SendText(const char* s) {
unsigned char length;
for (length = 0; length < 253; length++) {
if (s[length] == '\0')
@ -318,7 +321,8 @@ void NetworkSync::SendInt(const int x) {
// Low-level functions
void NetworkSync::SendVector3(unsigned char *data, unsigned char *startIndex,
void NetworkSync::SendVector3(unsigned char* data,
unsigned char* startIndex,
const Vector3 v) {
SendSingle100(data, *startIndex, v.Right());
(*startIndex) += 4;
@ -328,7 +332,8 @@ void NetworkSync::SendVector3(unsigned char *data, unsigned char *startIndex,
(*startIndex) += 4;
}
void NetworkSync::SendQuaternion(unsigned char *data, const int startIndex,
void NetworkSync::SendQuaternion(unsigned char* data,
const int startIndex,
const Quaternion q) {
Vector3 angles = Quaternion::ToAngles(q);
int ix = startIndex;
@ -337,13 +342,15 @@ void NetworkSync::SendQuaternion(unsigned char *data, const int startIndex,
SendAngle8(data, ix++, angles.Forward());
}
void NetworkSync::SendPolar(unsigned char *data, unsigned char *startIndex,
void NetworkSync::SendPolar(unsigned char* data,
unsigned char* startIndex,
Polar p) {
SendAngle8(data, *startIndex, (const float)p.angle);
SendSingle100(data, (*startIndex) + 1, p.distance);
}
void NetworkSync::SendSpherical(unsigned char *data, unsigned char *startIndex,
void NetworkSync::SendSpherical(unsigned char* data,
unsigned char* startIndex,
Spherical s) {
SendAngle8(data, (*startIndex)++, s.horizontalAngle);
SendAngle8(data, (*startIndex)++, s.verticalAngle);
@ -351,7 +358,8 @@ void NetworkSync::SendSpherical(unsigned char *data, unsigned char *startIndex,
SendFloat16(data, startIndex, s.distance);
}
void NetworkSync::SendQuat32(unsigned char *data, unsigned char *startIndex,
void NetworkSync::SendQuat32(unsigned char* data,
unsigned char* startIndex,
const Quaternion q) {
unsigned char qx = (char)(q.x * 127 + 128);
unsigned char qy = (char)(q.y * 127 + 128);
@ -370,10 +378,11 @@ void NetworkSync::SendQuat32(unsigned char *data, unsigned char *startIndex,
data[(*startIndex)++] = qw;
}
void NetworkSync::SendAngle8(unsigned char *data, unsigned int startIndex,
void NetworkSync::SendAngle8(unsigned char* data,
unsigned int startIndex,
const float angle) {
AngleUsing<signed char> packedAngle = AngleUsing<signed char>(angle);
data[startIndex] = packedAngle.GetValue();
AngleOf<signed char> packedAngle2 = AngleOf<signed char>(angle);
data[startIndex] = packedAngle2.GetBinary();
}
// void NetworkSync::SendAngle16(unsigned char *data, unsigned int startIndex,
@ -397,7 +406,8 @@ void NetworkSync::SendAngle8(unsigned char *data, unsigned int startIndex,
// // data[startIndex + 3]);
// }
void NetworkSync::SendSingle100(unsigned char *data, unsigned int startIndex,
void NetworkSync::SendSingle100(unsigned char* data,
unsigned int startIndex,
float value) {
// Sends a float with truncated 2 decimal precision
Int32 intValue = value * 100;
@ -406,7 +416,8 @@ void NetworkSync::SendSingle100(unsigned char *data, unsigned int startIndex,
// data[startIndex + ix] = ((unsigned char *)&intValue)[ix];
// }
}
void NetworkSync::SendFloat16(unsigned char *data, unsigned char *startIndex,
void NetworkSync::SendFloat16(unsigned char* data,
unsigned char* startIndex,
float value) {
float16 value16 = float16(value);
short binary = value16.getBinary();
@ -415,10 +426,11 @@ void NetworkSync::SendFloat16(unsigned char *data, unsigned char *startIndex,
data[(*startIndex)++] = binary & 0xFF;
}
void NetworkSync::SendInt32(unsigned char *data, unsigned int startIndex,
void NetworkSync::SendInt32(unsigned char* data,
unsigned int startIndex,
Int32 value) {
for (unsigned char ix = 0; ix < 4; ix++) {
data[startIndex++] = ((unsigned char *)&value)[ix];
data[startIndex++] = ((unsigned char*)&value)[ix];
}
}