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