Add Utility/Fixedpoint module

Tests/CMakeLists.txt

@@ -15,7 +15,8 @@ add_executable(tests
     packet.cpp
     parser.cpp
     button.cpp
-    rand.cpp)
+    rand.cpp
+    fixedpoint.cpp)
 
 target_link_libraries(tests
     PUBLIC

Tests/fixedpoint.cpp

@@ -0,0 +1,232 @@
+//
+// Created by erki on 30.04.21.
+//
+
+#include <catch2/catch.hpp>
+
+#include "utility_fixedpoint.hpp"
+
+using FP15 = Peripherals::FixedPoint<std::uint32_t, 15>;
+
+TEST_CASE("FP constructors work appropriately.", "[utility],[fixed point]")
+{
+  SECTION("Empty FP constructs with data 0.")
+  {
+    FP15 fp;
+
+    REQUIRE(fp.data == 0);
+  }
+
+  SECTION("FP with integral data constructors appropriately.")
+  {
+    FP15 fp(1);
+
+    REQUIRE(fp.data == (1 << 15));
+  }
+
+  SECTION("FP assignment operators work appropriately.")
+  {
+    FP15 fp;
+    fp = 15;
+
+    REQUIRE(fp.data == (15 << 15));
+  }
+
+  SECTION("FP with floating point data constructors works appropriately.")
+  {
+    FP15 fp(1.5f);
+
+    REQUIRE(fp.data == ((1 << 15) | (1 << 14)));
+
+    FP15 fp2(1.5);
+
+    REQUIRE(fp2.data == ((1 << 15) | (1 << 14)));
+  }
+
+  SECTION("FP assignment operators work with floating point data appropriately.")
+  {
+    FP15 fp;
+    fp = 1.5f;
+
+    REQUIRE(fp.data == ((1 << 15) | (1 << 14)));
+
+    fp = 1.5;
+
+    REQUIRE(fp.data == ((1 << 15) | (1 << 14)));
+  }
+}
+
+TEST_CASE("FP comparison operators work appropriately.", "[utility],[fixed point]")
+{
+  SECTION("FPs compare equal with other FPs appropriately.")
+  {
+    const FP15 fp1(15);
+    const FP15 fp2(15);
+
+    REQUIRE(fp1 == fp2);
+
+    const FP15 fp3(32);
+
+    REQUIRE(fp1 != fp3);
+  }
+
+  SECTION("FPs compare equal with integers appropriately.")
+  {
+    const FP15 fp1(15);
+
+    REQUIRE(fp1 == 15);
+    REQUIRE(fp1 != 32);
+  }
+
+  SECTION("FPs compare equal with floats appropriately.")
+  {
+    const FP15 fp1(1.45f);
+
+    REQUIRE(fp1 == 1.45f);
+    REQUIRE(fp1 == 1.45);
+  }
+
+  SECTION("FPs compare lt and gt with other FPs appropriately.")
+  {
+    const FP15 fp(1.45f);
+    const FP15 fp2(1.4525f);
+    const FP15 fp3(1.4475f);
+
+    REQUIRE(fp < fp2);
+    REQUIRE(fp > fp3);
+    REQUIRE(fp <= fp2);
+    REQUIRE(fp >= fp3);
+
+    REQUIRE(fp <= fp);
+    REQUIRE(fp >= fp);
+    REQUIRE(!(fp < fp));
+    REQUIRE(!(fp > fp));
+  }
+
+  SECTION("FPs compare lt and gt with integers appropriately.")
+  {
+    const FP15 fp(15);
+
+    REQUIRE(fp < 16);
+    REQUIRE(fp > 14);
+    REQUIRE(!(fp < 15));
+    REQUIRE(!(fp > 15));
+
+    REQUIRE(fp <= 15);
+    REQUIRE(fp <= 16);
+    REQUIRE(fp >= 15);
+    REQUIRE(fp >= 14);
+  }
+
+  SECTION("FPs compare lt and gt with floating point numbers appropriately.")
+  {
+    const FP15 fp(45.75f);
+
+    REQUIRE(fp < 45.80f);
+    REQUIRE(fp > 45.70f);
+    REQUIRE(!(fp < 45.75f));
+    REQUIRE(!(fp > 45.75f));
+
+    REQUIRE(fp <= 45.75f);
+    REQUIRE(fp <= 45.80f);
+    REQUIRE(fp >= 45.75f);
+    REQUIRE(fp >= 45.70f);
+  }
+}
+
+TEST_CASE("FP arithmetic operators work appropriately.", "[utility],[fixed point]")
+{
+  SECTION("FPs add with other FPs appropriately.")
+  {
+    const FP15 fp1(45.75f);
+    const FP15 fp2(0.25f);
+
+    REQUIRE((fp1 + fp2) == FP15(46));
+  }
+
+  SECTION("FPs subtract from other FPs appropriately.")
+  {
+    const FP15 fp1(45.75f);
+    const FP15 fp2(0.75f);
+
+    REQUIRE((fp1 - fp2) == FP15(45));
+  }
+
+  SECTION("FPs add with arithmetic types appropriately.")
+  {
+    const FP15 fp(45.75f);
+
+    REQUIRE((fp + 10) == FP15(55.75f));
+    REQUIRE((fp + 0.25f) == FP15(46));
+  }
+
+  SECTION("FPs subtract from arithmetic types appropriately.")
+  {
+    const FP15 fp(45.75f);
+
+    REQUIRE((fp - 10) == FP15(35.75f));
+    REQUIRE((fp - 0.75f) == FP15(45));
+  }
+}
+
+TEMPLATE_TEST_CASE("FP converting back to integral values works appropriately.", "[utility],[fixed point]",
+                   std::int8_t, std::int16_t, std::int32_t)
+{
+  using FP = Peripherals::FixedPoint<TestType, 4>;
+
+  FP fp(4);
+
+  REQUIRE(fp.template toValue<TestType>() == 4);
+}
+
+TEMPLATE_TEST_CASE("FP converting back to floating point values works appropriately.", "[utility],[fixed point]",
+                   float, double)
+{
+  FP15 fp(TestType(4.45));
+
+  REQUIRE(TestType(fp) == Approx(TestType(4.45)));
+}
+
+TEST_CASE("FP multiplication works appropriately.", "[utility],[fixed point]")
+{
+  SECTION("FP multiplies with other FPs appropriately.")
+  {
+    FP15 fp1(0.25);
+    FP15 fp2(0.1);
+
+    FP15 fp3 = fp1 * fp2;
+
+    REQUIRE(fp3 == FP15(0.025));
+  }
+
+  SECTION("FP multiplies with real values appropriately.")
+  {
+    FP15 fp1(0.25);
+
+    FP15 fp2 = fp1 * 0.1;
+
+    REQUIRE(fp2 == FP15(0.025));
+  }
+}
+
+TEST_CASE("FP division works appropriately.", "[utility],[fixed point]")
+{
+  SECTION("FP divides with other FPs appropriately.")
+  {
+    FP15 fp1(0.25);
+    FP15 fp2(0.1);
+
+    FP15 fp3 = fp1 / fp2;
+
+    REQUIRE(fp3.data == FP15(2.5).data - 5);// Rounding errors, yay!
+  }
+
+  SECTION("FP divides with real values appropriately.")
+  {
+    FP15 fp1(0.25);
+
+    FP15 fp3 = fp1 / 0.1;
+
+    REQUIRE(fp3.data == FP15(2.5).data - 5);// Rounding errors, yay!
+  }
+}

Utility/Inc/utility_fixedpoint.hpp

@@ -0,0 +1,292 @@
+//
+// Created by erki on 30.04.21.
+//
+
+#ifndef SKULLC_UTILITY_FIXEDPOINT_HPP
+#define SKULLC_UTILITY_FIXEDPOINT_HPP
+
+#include <type_traits>
+
+namespace Peripherals
+{
+
+namespace Details
+{
+
+template<typename T>
+struct NextRankingInteger
+{
+};
+
+template<>
+struct NextRankingInteger<std::int16_t>
+{
+  using T = std::int32_t;
+};
+
+template<>
+struct NextRankingInteger<std::int32_t>
+{
+  using T = std::int64_t;
+};
+
+template<>
+struct NextRankingInteger<std::int64_t>
+{
+  using T = std::int64_t;
+};
+
+template<>
+struct NextRankingInteger<std::uint16_t>
+{
+  using T = std::uint32_t;
+};
+
+template<>
+struct NextRankingInteger<std::uint32_t>
+{
+  using T = std::uint64_t;
+};
+
+template<>
+struct NextRankingInteger<std::uint64_t>
+{
+  using T = std::uint64_t;
+};
+
+}// namespace Details
+
+template<typename U, std::size_t D>
+struct FixedPoint
+{
+  using underlying = U;
+  using isSigned = std::is_signed<underlying>;
+  constexpr static std::size_t fractionalBits = D;
+
+  underlying data;
+
+  constexpr FixedPoint()
+      : data{0}
+  {}
+
+  template<typename T>
+  constexpr explicit FixedPoint(const T& value)
+  {
+    static_assert(std::is_arithmetic_v<T>, "T is not an arithmetic type.");
+    *this = FixedPoint<U, D>::fromValue(value);
+  }
+
+  template<typename T>
+  constexpr FixedPoint& operator=(const T& value)
+  {
+    static_assert(std::is_arithmetic_v<T>, "T is not an arithmetic type.");
+
+    *this = FixedPoint<U, D>::fromValue(value);
+
+    return *this;
+  }
+
+  constexpr bool operator==(const FixedPoint<U, D>& o) const
+  {
+    return data == o.data;
+  }
+
+  template<typename T>
+  constexpr bool operator==(const T& value) const
+  {
+    static_assert(std::is_arithmetic_v<T>, "T is not an integral type.");
+    const auto other = FixedPoint<U, D>(value);
+    return *this == other;
+  }
+
+  template<typename T>
+  constexpr bool operator!=(const T& value) const
+  {
+    static_assert(std::is_arithmetic_v<T>, "T is not an integral type.");
+    const auto other = FixedPoint<U, D>(value);
+    return *this != other;
+  }
+
+  constexpr bool operator!=(const FixedPoint<U, D>& o) const
+  {
+    return !operator==(o);
+  }
+
+  constexpr bool operator<(const FixedPoint<U, D>& o) const
+  {
+    return data < o.data;
+  }
+
+  constexpr bool operator<=(const FixedPoint<U, D>& o) const
+  {
+    return data <= o.data;
+  }
+
+  constexpr bool operator>(const FixedPoint<U, D>& o) const
+  {
+    return data > o.data;
+  }
+
+  constexpr bool operator>=(const FixedPoint<U, D>& o) const
+  {
+    return data >= o.data;
+  }
+
+  template<typename T>
+  constexpr bool operator<(const T& value) const
+  {
+    const auto o = fromValue(value);
+    return data < o.data;
+  }
+
+  template<typename T>
+  constexpr bool operator<=(const T& value) const
+  {
+    const auto o = fromValue(value);
+    return data <= o.data;
+  }
+
+  template<typename T>
+  constexpr bool operator>(const T& value) const
+  {
+    const auto o = fromValue(value);
+    return data > o.data;
+  }
+
+  template<typename T>
+  constexpr bool operator>=(const T& value) const
+  {
+    const auto o = fromValue(value);
+    return data >= o.data;
+  }
+
+  friend constexpr FixedPoint<U, D> operator+(const FixedPoint<U, D>& a, const FixedPoint<U, D>& b)
+  {
+    auto clone = a;
+    clone.data += b.data;
+
+    return clone;
+  }
+
+  friend constexpr FixedPoint<U, D> operator-(const FixedPoint<U, D>& a, const FixedPoint<U, D>& b)
+  {
+    auto clone = a;
+    clone.data -= b.data;
+
+    return clone;
+  }
+
+  template<typename T>
+  friend constexpr FixedPoint<U, D> operator+(const FixedPoint<U, D>& a, const T& b)
+  {
+    static_assert(std::is_arithmetic_v<T>, "T is not an arithmetic type.");
+    return a + FixedPoint<U, D>(b);
+  }
+
+  template<typename T>
+  friend constexpr FixedPoint<U, D> operator-(const FixedPoint<U, D>& a, const T& b)
+  {
+    static_assert(std::is_arithmetic_v<T>, "T is not an arithmetic type.");
+    return a - FixedPoint<U, D>(b);
+  }
+
+  friend constexpr FixedPoint<U, D> operator*(const FixedPoint<U, D>& a, const FixedPoint<U, D>& b)
+  {
+    // Ref: https://spin.atomicobject.com/2012/03/15/simple-fixed-point-math/
+    // Ref: https://en.wikipedia.org/wiki/Q_(number_format)
+    using T = typename Details::NextRankingInteger<U>::T;
+
+    const T axb = T(a.data) * T(b.data);
+    const T round_value = axb >> (fractionalBits - 1);
+
+    auto dest = FixedPoint<U, D>();
+    dest.data = (axb / (underlying(1) << fractionalBits)) + (round_value & 1);
+    return dest;
+  }
+
+  friend constexpr FixedPoint<U, D> operator/(const FixedPoint<U, D>& a, const FixedPoint<U, D>& b)
+  {
+    // Ref: https://en.wikipedia.org/wiki/Q_(number_format)
+    using T = typename Details::NextRankingInteger<U>::T;
+
+    T temp = T(a.data) << fractionalBits;
+
+    // rounding
+    if ((temp >= 0 && b.data >= 0) || (temp < 0 && b.data < 0))
+      temp += b.data / 2;
+    else
+      temp -= b.data / 2;
+
+    auto dest = FixedPoint<U, D>();
+    dest.data = temp / b.data;
+    return dest;
+  }
+
+  template<typename T>
+  friend constexpr FixedPoint<U, D> operator*(const FixedPoint<U, D>& a, const T& b)
+  {
+    static_assert(std::is_arithmetic_v<T>, "T is not an arithmetic type.");
+    return a * FixedPoint<U, D>(b);
+  }
+
+  template<typename T>
+  friend constexpr FixedPoint<U, D> operator/(const FixedPoint<U, D>& a, const T& b)
+  {
+    static_assert(std::is_arithmetic_v<T>, "T is not an arithmetic type.");
+    return a / FixedPoint<U, D>(b);
+  }
+
+  template<typename T>
+  static constexpr FixedPoint<U, D> fromValue(const T& value)
+  {
+    static_assert(std::is_integral_v<T>, "T is not integral value.");
+    auto fp = FixedPoint<U, D>();
+
+    fp.data = value << D;
+
+    return fp;
+  }
+
+  constexpr static FixedPoint<U, D> fromValue(const float& value)
+  {
+    auto fp = FixedPoint<U, D>();
+
+    fp.data = std::round(value * (underlying(1) << fractionalBits));
+
+    return fp;
+  }
+
+  constexpr static FixedPoint<U, D> fromValue(const double& value)
+  {
+    auto fp = FixedPoint<U, D>();
+
+    fp.data = std::round(value * (underlying(1) << fractionalBits));
+
+    return fp;
+  }
+
+  template<typename T>
+  constexpr T toValue() const
+  {
+    static_assert(std::is_integral_v<T>, "T is not an integral type.");
+    return data >> fractionalBits;
+  }
+
+  constexpr explicit operator float() const
+  {
+    return float(data) / float(underlying(1) << fractionalBits);
+  }
+
+  constexpr explicit operator double() const
+  {
+    return double(data) / double(underlying(1) << fractionalBits);
+  }
+};
+
+using Q7 = FixedPoint<std::int8_t, 7>;
+using Q15 = FixedPoint<std::int16_t, 15>;
+using Q31 = FixedPoint<std::int32_t, 31>;
+
+}// namespace Peripherals
+
+#endif//SKULLC_UTILITY_FIXEDPOINT_HPP