diff --git a/protocol/enttec/dmx-usb-pro/pro.cpp b/protocol/enttec/dmx-usb-pro/pro.cpp index 3e60155..bdc9606 100644 --- a/protocol/enttec/dmx-usb-pro/pro.cpp +++ b/protocol/enttec/dmx-usb-pro/pro.cpp @@ -23,8 +23,6 @@ */ #include "dmx.h" -#include -#include #include "pro.h" namespace ENTTEC::Pro { @@ -215,21 +213,13 @@ void MsgRecievedDMXChanged::oStream(std::shared_ptr stream) const void MsgGetWidgetSerialReply::iStream(std::shared_ptr stream) { - const uint8_t NIBBLE_BIT = CHAR_BIT / 2; - auto bcd = stream->readType(); - serial = bcd & 0xf; - for(size_t i = 1; i * NIBBLE_BIT < sizeof(bcd) * CHAR_BIT; i++) - serial += ((bcd >> (i * NIBBLE_BIT)) & 0xf) * pow(10,i); + serial = BCDtoDecimal(stream->readType()); }; void MsgGetWidgetSerialReply::oStream(std::shared_ptr stream) const { - const uint8_t NIBBLE_BIT = CHAR_BIT / 2; - uint32_t bcd = serial & 0xf; - for(size_t i = 1; i * NIBBLE_BIT < sizeof(bcd) * CHAR_BIT; i++) - bcd += ((uint32_t)(serial / pow(10,i)) & 0xf) << (i * NIBBLE_BIT); - *stream << bcd; + *stream << DecimalToBCD(serial); }; diff --git a/protocol/enttec/dmx-usb-pro/pro.h b/protocol/enttec/dmx-usb-pro/pro.h index 3a55d5a..63caa93 100644 --- a/protocol/enttec/dmx-usb-pro/pro.h +++ b/protocol/enttec/dmx-usb-pro/pro.h @@ -25,8 +25,10 @@ #include +#include #include #include +#include #include #include @@ -418,4 +420,39 @@ struct MsgSendRDMDiscovery uint8_t request[38]; //!< DISC_UNIQUE_BRANCH RDM request packet to send. }; + +/** + * @brief BCDtoDecimal + * @param bcd + * @return + */ +template +T BCDtoDecimal(const T bcd) +{ + static_assert(std::is_integral::value, "Integer type required."); + const uint_fast8_t NIBBLE_BIT = CHAR_BIT / 2; + T decimal = bcd & 0xf; + for(size_t i = 1; i * NIBBLE_BIT < sizeof(bcd) * CHAR_BIT; i++) + decimal += ((bcd >> (i * NIBBLE_BIT)) & 0xf) * pow(10,i); + return decimal; +} + + +/** + * @brief DecimalToBCD + * @param decimal + * @return + */ +template +T DecimalToBCD(const T decimal) +{ + static_assert(std::is_integral::value, "Integer type required."); + const uint_fast8_t NIBBLE_BIT = CHAR_BIT / 2; + T bcd = decimal & 0xf; + for(size_t i = 1; i * NIBBLE_BIT < sizeof(bcd) * CHAR_BIT; i++) + bcd |= ((decimal / (uint)pow(10,i)) % 10) << (i * NIBBLE_BIT); + return bcd; +} + + } // namespace ENTTEC::Pro