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

Improved accuracy, fixed field order in sending sperical

Browse Source
This commit is contained in:
Pascal Serrarens 2024-12-14 12:41:14 +01:00
parent 6518cb62be
commit b190b6830c
6 changed files with 125 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
CoreThing.cpp
View File

@ -8,31 +8,31 @@ CoreThing::CoreThing(
this->type = thingType;
this->networkId = networkId;
this->Init();
// CoreThings::Add(this);
CoreThing::Add(this);
}
void CoreThing::Init() {}
// CoreThing *CoreThings::allThings[256] = {nullptr};
CoreThing *CoreThing::allThings[256] = {nullptr};
// CoreThing *CoreThings::Get(unsigned char networkId, unsigned char thingId) {
// for (unsigned char ix = 0; ix < 256; ix++) {
// CoreThing *thing = allThings[ix];
// if (thing == nullptr)
// continue;
// if (thing->networkId == networkId && thing->id == thingId)
// return thing;
// }
// return nullptr;
// }
CoreThing *CoreThing::Get(unsigned char networkId, unsigned char thingId) {
for (unsigned char ix = 0; ix < 256; ix++) {
CoreThing *thing = allThings[ix];
if (thing == nullptr)
continue;
if (thing->networkId == networkId && thing->id == thingId)
return thing;
}
return nullptr;
}
// bool CoreThings::Add(CoreThing *newThing) {
// for (unsigned char ix = 0; ix < 256; ix++) {
// CoreThing *thing = allThings[ix];
// if (thing == nullptr) {
// allThings[ix] = newThing;
// return true;
// }
// }
// return false;
// }
bool CoreThing::Add(CoreThing *newThing) {
for (unsigned char ix = 0; ix < 256; ix++) {
CoreThing *thing = allThings[ix];
if (thing == nullptr) {
allThings[ix] = newThing;
return true;
}
}
return false;
}

18
CoreThing.h
View File

@ -15,27 +15,19 @@ public:
const char *modelUrl;
// protected Sensor sensor;
static CoreThing *allThings[];
public:
CoreThing(
// Participant *client,
unsigned char networkId, unsigned char thingId,
unsigned char thingType = 0);
static CoreThing *Get(unsigned char networkId, unsigned char thingId);
static bool Add(CoreThing *thing);
protected:
virtual void Init();
// Things
};
// class CoreThings {
// friend class CoreThing;
// public:
// static CoreThing *allThings[];
// public:
// static CoreThing *Get(unsigned char networkId, unsigned char thingId);
// static bool Add(CoreThing *thing);
// };
} // namespace Passer::Control

37
LowLevelMessages.cpp
View File

@ -2,6 +2,12 @@
#include "../float16/float16.h"
void LowLevelMessages::SendAngle8(unsigned char *buffer, unsigned char *ix,
const float angle) {
Angle8 packedAngle2 = Angle8::Degrees(angle);
buffer[(*ix)++] = packedAngle2.GetBinary();
}
void LowLevelMessages::SendFloat16(unsigned char *buffer, unsigned char *ix,
float value) {
float16 value16 = float16(value);
@ -9,4 +15,33 @@ void LowLevelMessages::SendFloat16(unsigned char *buffer, unsigned char *ix,
buffer[(*ix)++] = (binary >> 8) & 0xFF;
buffer[(*ix)++] = binary & 0xFF;
}
}
void Passer::Control::LowLevelMessages::SendSpherical16(unsigned char *buffer,
unsigned char *ix,
Spherical16 s) {
SendFloat16(buffer, ix, s.distance);
SendAngle8(buffer, ix, s.direction.horizontal.InDegrees());
SendAngle8(buffer, ix, s.direction.vertical.InDegrees());
}
void Passer::Control::LowLevelMessages::SendQuat32(unsigned char *buffer,
unsigned char *ix,
SwingTwist16 rotation) {
Quaternion q = rotation.ToQuaternion();
unsigned char qx = (char)(q.x * 127 + 128);
unsigned char qy = (char)(q.y * 127 + 128);
unsigned char qz = (char)(q.z * 127 + 128);
unsigned char qw = (char)(q.w * 255);
if (q.w < 0) {
qx = -qx;
qy = -qy;
qz = -qz;
qw = -qw;
}
// Serial.printf(" (%d) %d:%d:%d:%d ", startIndex, qx, qy, qz, qw);
buffer[(*ix)++] = qx;
buffer[(*ix)++] = qy;
buffer[(*ix)++] = qz;
buffer[(*ix)++] = qw;
}

7
LowLevelMessages.h
View File

@ -1,11 +1,18 @@
#include "../LinearAlgebra/Spherical.h"
#include "../LinearAlgebra/SwingTwist.h"
namespace Passer::Control {
class LowLevelMessages {
public:
static void SendAngle8(unsigned char *buffer, unsigned char *ix,
const float angle);
static void SendFloat16(unsigned char *buffer, unsigned char *ix,
float value);
static void SendSpherical16(unsigned char *buffer, unsigned char *ix,
Spherical16 s);
static void SendQuat32(unsigned char *buffer, unsigned char *ix,
SwingTwist16 q);
};
} // namespace Passer::Control

29
Messages.cpp
View File

@ -130,4 +130,33 @@ unsigned char ModelUrlMsg::Serialize(unsigned char *buffer) {
}
// Model Url
#pragma endregion
#pragma region PoseMsg
#include <Arduino.h>
PoseMsg::PoseMsg(unsigned char networkId, unsigned char thingId,
unsigned char poseType, Spherical16 position,
SwingTwist16 orientation) {
this->networkId = networkId;
this->thingId = thingId;
this->position = position;
this->orientation = orientation;
this->poseType = poseType;
}
unsigned char PoseMsg::Serialize(unsigned char *buffer) {
unsigned char ix = 0;
buffer[ix++] = PoseMsg::id;
buffer[ix++] = this->networkId;
buffer[ix++] = this->thingId;
buffer[ix++] = this->poseType;
LowLevelMessages::SendSpherical16(buffer, &ix, this->position);
printf("send spherical %f %f\n", this->position.distance,
this->position.direction.horizontal.InDegrees());
LowLevelMessages::SendQuat32(buffer, &ix, this->orientation);
return ix;
}
// Pose
#pragma endregion

26
Messages.h
View File

@ -1,4 +1,6 @@
#pragma once
#include "../LinearAlgebra/Spherical.h"
#include "../LinearAlgebra/SwingTwist.h"
#include "../float16/float16.h"
#include "CoreThing.h"
#include "Participant.h"
@ -67,8 +69,10 @@ public:
class ModelUrlMsg : public IMessage {
public:
static const unsigned char id = 0x90;
unsigned char networkId;
unsigned char thingId;
float scale;
unsigned char urlLength;
const char *url;
@ -79,5 +83,27 @@ public:
virtual unsigned char Serialize(unsigned char *buffer) override;
};
class PoseMsg : public IMessage {
public:
static const unsigned char id = 0x10;
unsigned char length = 4 + 4 + 4;
unsigned char networkId;
unsigned char thingId;
unsigned char poseType;
const unsigned char Pose_Position = 0x01;
const unsigned char Pose_Orientation = 0x02;
Spherical16 position;
SwingTwist16 orientation;
PoseMsg(unsigned char networkId, unsigned char thingId,
unsigned char poseType, Spherical16 position,
SwingTwist16 orientation);
virtual unsigned char Serialize(unsigned char *buffer) override;
};
} // namespace Passer::Control
using namespace Passer::Control;