From ae81b80dc4e07e54537d5b5c0eb219440fa7232e Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Mon, 16 Dec 2024 12:47:47 +0100 Subject: [PATCH] Control Core fixes --- CMakeLists.txt | 57 ++++++++++ CoreThing.cpp | 26 +++-- CoreThing.h | 45 ++++++-- LowLevelMessages.cpp | 4 +- LowLevelMessages.h | 6 +- Messages.cpp | 45 +++----- Messages.h | 12 ++- Participant.h | 6 +- float16.cpp | 243 +++++++++++++++++++++++++++++++++++++++++++ float16.h | 75 +++++++++++++ test/CMakeLists.txt | 36 +++++++ test/dummy_test.cc | 9 ++ 12 files changed, 502 insertions(+), 62 deletions(-) create mode 100644 CMakeLists.txt create mode 100644 float16.cpp create mode 100644 float16.h create mode 100644 test/CMakeLists.txt create mode 100644 test/dummy_test.cc diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..e5b51a3 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,57 @@ +cmake_minimum_required(VERSION 3.13) # CMake version check +if(ESP_PLATFORM) + idf_component_register( + SRC_DIRS "." + INCLUDE_DIRS "." + ) +else() + project(ControlCore) + + set(CMAKE_CXX_STANDARD 11) # Enable c++11 standard + set(CMAKE_POSITION_INDEPENDENT_CODE ON) + + add_compile_definitions(GTEST) + include(FetchContent) + FetchContent_Declare( + googletest + DOWNLOAD_EXTRACT_TIMESTAMP ON + URL https://github.com/google/googletest/archive/refs/heads/main.zip + ) + + # For Windows: Prevent overriding the parent project's compiler/linker settings + set(gtest_force_shared_crt ON CACHE BOOL "" FORCE) + FetchContent_MakeAvailable(googletest) + + include_directories(.) + add_library(ControlCore STATIC + "CoreThing.cpp" + "LowLevelMessages.cpp" + "Messages.cpp" + "Participant.cpp" + "float16.cpp" + ) + + enable_testing() + + file(GLOB_RECURSE test_srcs test/*_test.cc) + add_executable( + ControlCoreTest + ${test_srcs} + ) + target_link_libraries( + ControlCoreTest + gtest_main + ControlCore + ) + + if(MSVC) + target_compile_options(ControlCoreTest PRIVATE /W4 /WX) + else() + target_compile_options(ControlCoreTest PRIVATE -Wall -Wextra -Wpedantic -Werror) + endif() + + + include(GoogleTest) + gtest_discover_tests(ControlCoreTest) +endif() + diff --git a/CoreThing.cpp b/CoreThing.cpp index c33602e..26a8740 100644 --- a/CoreThing.cpp +++ b/CoreThing.cpp @@ -1,18 +1,25 @@ #include "CoreThing.h" -CoreThing::CoreThing( - // Participant *client, - unsigned char networkId, unsigned char thingId, unsigned char thingType) { - // this->client = client; - this->id = thingId; +#include + +CoreThing::CoreThing(unsigned char networkId, unsigned char thingType) { this->type = thingType; this->networkId = networkId; this->Init(); - CoreThing::Add(this); + + int thingId = CoreThing::Add(this); + if (thingId < 0) { + std::cout << "ERROR: Thing store is full\n"; + this->id = 0; // what to do when we cannot store any more things? + } else + this->id = thingId; } void CoreThing::Init() {} +void CoreThing::SetName(const char *name) { this->name = name; } + +// All things CoreThing *CoreThing::allThings[256] = {nullptr}; CoreThing *CoreThing::Get(unsigned char networkId, unsigned char thingId) { @@ -26,13 +33,14 @@ CoreThing *CoreThing::Get(unsigned char networkId, unsigned char thingId) { return nullptr; } -bool CoreThing::Add(CoreThing *newThing) { +int 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 ix; } } - return false; + return -1; } \ No newline at end of file diff --git a/CoreThing.h b/CoreThing.h index 025f372..0ab5d05 100644 --- a/CoreThing.h +++ b/CoreThing.h @@ -1,32 +1,57 @@ #pragma once -namespace Passer::Control { +namespace Passer { +namespace Control { class CoreThing { public: // Participant *client; unsigned char networkId; + /// @char The id of the thing unsigned char id; // CoreThing *parent; + /// @brief The type of Thing unsigned char type; - const char *name; - const char *modelUrl; + const char *name = nullptr; + const char *modelUrl = nullptr; // protected Sensor sensor; - static CoreThing *allThings[]; + /// @brief Basic Thing types + enum class Type { + Undetermined, + // Sensor, + Switch, + DistanceSensor, + DirectionalSensor, + TemperatureSensor, + // Motor, + ControlledMotor, + UncontrolledMotor, + Servo, + // Other + Humanoid, + ExternalSensor, + }; public: - CoreThing( - // Participant *client, - unsigned char networkId, unsigned char thingId, - unsigned char thingType = 0); + CoreThing(unsigned char networkId = 0, + unsigned char thingType = (unsigned char)Type::Undetermined); + + void SetName(const char *name); + + virtual void SendBytes(unsigned char *buffer, unsigned char *ix) {}; + + // All things +private: + static CoreThing *allThings[]; static CoreThing *Get(unsigned char networkId, unsigned char thingId); - static bool Add(CoreThing *thing); + static int Add(CoreThing *thing); protected: virtual void Init(); }; -} // namespace Passer::Control +} // namespace Control +} // namespace Passer using namespace Passer::Control; \ No newline at end of file diff --git a/LowLevelMessages.cpp b/LowLevelMessages.cpp index 247368f..f49537f 100644 --- a/LowLevelMessages.cpp +++ b/LowLevelMessages.cpp @@ -1,6 +1,6 @@ #include "LowLevelMessages.h" -#include "../float16/float16.h" +#include "float16.h" void LowLevelMessages::SendAngle8(unsigned char *buffer, unsigned char *ix, const float angle) { @@ -32,7 +32,7 @@ float LowLevelMessages::ReceiveFloat16(const unsigned char *buffer, f.setBinary(value); *startIndex = ix; - return f.toDouble(); + return (float)f.toFloat(); } void LowLevelMessages::SendSpherical16(unsigned char *buffer, unsigned char *ix, diff --git a/LowLevelMessages.h b/LowLevelMessages.h index 65dcb9b..313b38d 100644 --- a/LowLevelMessages.h +++ b/LowLevelMessages.h @@ -1,7 +1,8 @@ #include "../LinearAlgebra/Spherical.h" #include "../LinearAlgebra/SwingTwist.h" -namespace Passer::Control { +namespace Passer { +namespace Control { class LowLevelMessages { public: @@ -26,5 +27,6 @@ public: unsigned char *ix); }; -} // namespace Passer::Control +} // namespace Control +} // namespace Passer using namespace Passer::Control; \ No newline at end of file diff --git a/Messages.cpp b/Messages.cpp index e5562b5..5278400 100644 --- a/Messages.cpp +++ b/Messages.cpp @@ -237,52 +237,33 @@ unsigned char PoseMsg::Serialize(unsigned char *buffer) { #pragma region CustomMsg CustomMsg::CustomMsg(unsigned char *buffer) { - unsigned char ix; + unsigned char ix = 1; this->networkId = buffer[ix++]; this->thingId = buffer[ix++]; - this->dataSize = buffer[ix++]; this->data = buffer + ix; // This is only valid because the code ensures the the msg // lifetime is shorter than the buffer lifetime... } -CustomMsg::CustomMsg(unsigned char networkId, unsigned char thingId, - unsigned char *data, unsigned char dataSize) { +CustomMsg::CustomMsg(unsigned char networkId, CoreThing *thing) { this->networkId = networkId; - this->thingId = thingId; - this->dataSize = dataSize; - this->data = data; + this->thingId = thing->id; + this->thing = thing; } +#include unsigned char CustomMsg::Serialize(unsigned char *buffer) { - unsigned char ix = 0; - buffer[ix++] = this->id; - buffer[ix++] = this->networkId; - buffer[ix++] = this->thingId; - for (int dataIx = 0; dataIx < this->dataSize; dataIx++) - buffer[ix++] = this->data[dataIx]; + unsigned char ix = this->length; + this->thing->SendBytes(buffer, &ix); + if (ix <= this->length) // in this case, no data is actually sent + return 0; + + buffer[0] = this->id; + buffer[1] = this->networkId; + buffer[2] = this->thingId; return ix; } -// void CustomMsg::Deserialize(unsigned char *buffer) { -// unsigned char ix; -// this->networkId = buffer[ix++]; -// this->thingId = buffer[ix++]; -// this->dataSize = buffer[ix++]; -// this->data = buffer + ix; // challenging: point directly into the buffer! - -// // this->data = new unsigned char[this->dataSize]; // memory leak! -// // for (unsigned char dataIx = 0; dataIx < this->dataSize; dataIx++) -// // this->data[dataIx] = buffer[ix++]; -// } - -// bool CustomMsg::Send(Participant *participant, unsigned char networkId, -// unsigned char thingId, unsigned char *data, -// unsigned char dataSize) { -// CustomMsg msg = CustomMsg(networkId, thingId, data, dataSize); -// return msg.Send(participant); -// } - CustomMsg CustomMsg::Receive(unsigned char *buffer, unsigned char bufferSize) { CustomMsg msg = CustomMsg(buffer); return msg; diff --git a/Messages.h b/Messages.h index 8436455..18c8e9a 100644 --- a/Messages.h +++ b/Messages.h @@ -2,10 +2,11 @@ #include "../LinearAlgebra/Spherical.h" #include "../LinearAlgebra/SwingTwist.h" -#include "../float16/float16.h" #include "CoreThing.h" +#include "float16.h" -namespace Passer::Control { +namespace Passer { +namespace Control { class Participant; @@ -133,19 +134,20 @@ public: unsigned char networkId; unsigned char thingId; + CoreThing *thing; unsigned char dataSize; unsigned char *data; CustomMsg(unsigned char *buffer); - CustomMsg(unsigned char networkId, unsigned char thingId, unsigned char *data, - unsigned char dataSize); + CustomMsg(unsigned char networkId, CoreThing *thing); virtual unsigned char Serialize(unsigned char *buffer) override; static CustomMsg Receive(unsigned char *buffer, unsigned char bufferSize); }; -} // namespace Passer::Control +} // namespace Control +} // namespace Passer using namespace Passer::Control; \ No newline at end of file diff --git a/Participant.h b/Participant.h index ca033d7..b709dcd 100644 --- a/Participant.h +++ b/Participant.h @@ -2,7 +2,8 @@ #include "Messages.h" -namespace Passer::Control { +namespace Passer { +namespace Control { class Participant { public: @@ -21,5 +22,6 @@ protected: virtual void ProcessCustomMsg(CustomMsg msg); }; -} // namespace Passer::Control +} // namespace Control +} // namespace Passer using namespace Passer::Control; \ No newline at end of file diff --git a/float16.cpp b/float16.cpp new file mode 100644 index 0000000..c8c46d7 --- /dev/null +++ b/float16.cpp @@ -0,0 +1,243 @@ +// +// FILE: float16.cpp +// AUTHOR: Rob Tillaart +// VERSION: 0.1.8 +// PURPOSE: library for Float16s for Arduino +// URL: http://en.wikipedia.org/wiki/Half-precision_floating-point_format + +#include "float16.h" +// #include +#include + +// CONSTRUCTOR +float16::float16(float f) { _value = f32tof16(f); } + +// PRINTING +// size_t float16::printTo(Print& p) const +// { +// double d = this->f16tof32(_value); +// return p.print(d, _decimals); +// } + +float float16::toFloat() const { return f16tof32(_value); } + +////////////////////////////////////////////////////////// +// +// EQUALITIES +// +bool float16::operator==(const float16 &f) { return (_value == f._value); } + +bool float16::operator!=(const float16 &f) { return (_value != f._value); } + +bool float16::operator>(const float16 &f) { + if ((_value & 0x8000) && (f._value & 0x8000)) + return _value < f._value; + if (_value & 0x8000) + return false; + if (f._value & 0x8000) + return true; + return _value > f._value; +} + +bool float16::operator>=(const float16 &f) { + if ((_value & 0x8000) && (f._value & 0x8000)) + return _value <= f._value; + if (_value & 0x8000) + return false; + if (f._value & 0x8000) + return true; + return _value >= f._value; +} + +bool float16::operator<(const float16 &f) { + if ((_value & 0x8000) && (f._value & 0x8000)) + return _value > f._value; + if (_value & 0x8000) + return true; + if (f._value & 0x8000) + return false; + return _value < f._value; +} + +bool float16::operator<=(const float16 &f) { + if ((_value & 0x8000) && (f._value & 0x8000)) + return _value >= f._value; + if (_value & 0x8000) + return true; + if (f._value & 0x8000) + return false; + return _value <= f._value; +} + +////////////////////////////////////////////////////////// +// +// NEGATION +// +float16 float16::operator-() { + float16 f16; + f16.setBinary(_value ^ 0x8000); + return f16; +} + +////////////////////////////////////////////////////////// +// +// MATH +// +float16 float16::operator+(const float16 &f) { + return float16(this->toFloat() + f.toFloat()); +} + +float16 float16::operator-(const float16 &f) { + return float16(this->toFloat() - f.toFloat()); +} + +float16 float16::operator*(const float16 &f) { + return float16(this->toFloat() * f.toFloat()); +} + +float16 float16::operator/(const float16 &f) { + return float16(this->toFloat() / f.toFloat()); +} + +float16 &float16::operator+=(const float16 &f) { + *this = this->toFloat() + f.toFloat(); + return *this; +} + +float16 &float16::operator-=(const float16 &f) { + *this = this->toFloat() - f.toFloat(); + return *this; +} + +float16 &float16::operator*=(const float16 &f) { + *this = this->toFloat() * f.toFloat(); + return *this; +} + +float16 &float16::operator/=(const float16 &f) { + *this = this->toFloat() / f.toFloat(); + return *this; +} + +////////////////////////////////////////////////////////// +// +// MATH HELPER FUNCTIONS +// +int float16::sign() { + if (_value & 0x8000) + return -1; + if (_value & 0xFFFF) + return 1; + return 0; +} + +bool float16::isZero() { return ((_value & 0x7FFF) == 0x0000); } + +bool float16::isNaN() { + if ((_value & 0x7C00) != 0x7C00) + return false; + if ((_value & 0x03FF) == 0x0000) + return false; + return true; +} + +bool float16::isInf() { return ((_value == 0x7C00) || (_value == 0xFC00)); } + +////////////////////////////////////////////////////////// +// +// CORE CONVERSION +// +float float16::f16tof32(uint16_t _value) const { + uint16_t sgn, man; + int exp; + float f; + + sgn = (_value & 0x8000) > 0; + exp = (_value & 0x7C00) >> 10; + man = (_value & 0x03FF); + + // ZERO + if ((_value & 0x7FFF) == 0) { + return sgn ? -0.0f : 0.0f; + } + // NAN & INF + if (exp == 0x001F) { + if (man == 0) + return sgn ? -INFINITY : INFINITY; + else + return NAN; + } + + // SUBNORMAL/NORMAL + if (exp == 0) + f = 0; + else + f = 1; + + // PROCESS MANTISSE + for (int i = 9; i >= 0; i--) { + f *= 2; + if (man & (1 << i)) + f = f + 1; + } + f = f * powf(2.0f, (float)(exp - 25)); + if (exp == 0) { + f = f * powf(2.0f, -13); // 5.96046447754e-8; + } + return sgn ? -f : f; +} + +uint16_t float16::f32tof16(float f) const { + uint32_t t = *(uint32_t *)&f; + // man bits = 10; but we keep 11 for rounding + uint16_t man = (t & 0x007FFFFF) >> 12; + int16_t exp = (t & 0x7F800000) >> 23; + bool sgn = (t & 0x80000000); + + // handle 0 + if ((t & 0x7FFFFFFF) == 0) { + return sgn ? 0x8000 : 0x0000; + } + // denormalized float32 does not fit in float16 + if (exp == 0x00) { + return sgn ? 0x8000 : 0x0000; + } + // handle infinity & NAN + if (exp == 0x00FF) { + if (man) + return 0xFE00; // NAN + return sgn ? 0xFC00 : 0x7C00; // -INF : INF + } + + // normal numbers + exp = exp - 127 + 15; + // overflow does not fit => INF + if (exp > 30) { + return sgn ? 0xFC00 : 0x7C00; // -INF : INF + } + // subnormal numbers + if (exp < -38) { + return sgn ? 0x8000 : 0x0000; // -0 or 0 ? just 0 ? + } + if (exp <= 0) // subnormal + { + man >>= (exp + 14); + // rounding + man++; + man >>= 1; + if (sgn) + return 0x8000 | man; + return man; + } + + // normal + // TODO rounding + exp <<= 10; + man++; + man >>= 1; + if (sgn) + return 0x8000 | exp | man; + return exp | man; +} + +// -- END OF FILE -- diff --git a/float16.h b/float16.h new file mode 100644 index 0000000..02fddb3 --- /dev/null +++ b/float16.h @@ -0,0 +1,75 @@ +#pragma once +// +// FILE: float16.h +// AUTHOR: Rob Tillaart +// VERSION: 0.1.8 +// PURPOSE: Arduino library to implement float16 data type. +// half-precision floating point format, +// used for efficient storage and transport. +// URL: https://github.com/RobTillaart/float16 + +// #include "Arduino.h" +#include + +#define FLOAT16_LIB_VERSION (F("0.1.8")) + +typedef uint16_t __fp16; + +class float16 { +public: + // Constructors + float16(void) { _value = 0x0000; }; + float16(float f); + float16(const float16 &f) { _value = f._value; }; + + // Conversion + float toFloat(void) const; + // access the 2 byte representation. + uint16_t getBinary() { return _value; }; + void setBinary(uint16_t u) { _value = u; }; + + // Printable + // size_t printTo(Print &p) const; + void setDecimals(uint8_t d) { _decimals = d; }; + uint8_t getDecimals() { return _decimals; }; + + // equalities + bool operator==(const float16 &f); + bool operator!=(const float16 &f); + + bool operator>(const float16 &f); + bool operator>=(const float16 &f); + bool operator<(const float16 &f); + bool operator<=(const float16 &f); + + // negation + float16 operator-(); + + // basic math + float16 operator+(const float16 &f); + float16 operator-(const float16 &f); + float16 operator*(const float16 &f); + float16 operator/(const float16 &f); + + float16 &operator+=(const float16 &f); + float16 &operator-=(const float16 &f); + float16 &operator*=(const float16 &f); + float16 &operator/=(const float16 &f); + + // math helper functions + int sign(); // 1 = positive 0 = zero -1 = negative. + bool isZero(); + bool isNaN(); + bool isInf(); + + // CORE CONVERSION + // should be private but for testing... + float f16tof32(uint16_t) const; + uint16_t f32tof16(float) const; + +private: + uint8_t _decimals = 4; + __fp16 _value; +}; + +// -- END OF FILE -- diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt new file mode 100644 index 0000000..38cbe51 --- /dev/null +++ b/test/CMakeLists.txt @@ -0,0 +1,36 @@ +cmake_minimum_required(VERSION 3.13) # CMake version check +Project(ControlCoreTest) + +set(CMAKE_CXX_STANDARD 11) # Enable c++11 standard +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +add_compile_definitions(GTEST) +include(FetchContent) +FetchContent_Declare( + googletest + DOWNLOAD_EXTRACT_TIMESTAMP ON + URL https://github.com/google/googletest/archive/refs/heads/main.zip +) + +# For Windows: Prevent overriding the parent project's compiler/linker settings +set(gtest_force_shared_crt ON CACHE BOOL "" FORCE) +FetchContent_MakeAvailable(googletest) + +include_directories( + . + .. +) +enable_testing() + +add_executable( + ControlCoreTest + "dummy_test.cc" +) + +target_link_libraries( + ControlCoreTest + gtest_main +) + +include(GoogleTest) +gtest_discover_tests(ControlCoreTest) diff --git a/test/dummy_test.cc b/test/dummy_test.cc new file mode 100644 index 0000000..0947327 --- /dev/null +++ b/test/dummy_test.cc @@ -0,0 +1,9 @@ +#if GTEST + +// #include +// not supported using Visual Studio 2022 compiler... +#include + +TEST(Dummy, Dummytest) {} + +#endif