From 03347a5e216179c45dbfcef8b5e60714a86140ec Mon Sep 17 00:00:00 2001 From: Kevin Matz Date: Fri, 20 Aug 2021 11:34:26 -0400 Subject: [PATCH] OTP Transform PDU i/o --- CMakeLists.txt | 2 + otp/pdu.h | 8 - otp/transform.cpp | 420 ++++++++++++++++++++++++++++++++++++++++++++++ otp/transform.h | 277 ++++++++++++++++++++++++++++++ 4 files changed, 699 insertions(+), 8 deletions(-) create mode 100644 otp/transform.cpp create mode 100644 otp/transform.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 73fd509..3d6027c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -64,6 +64,8 @@ set(SOURCE_FILES otp/otp.h otp/pdu.h otp/pdu.cpp + otp/transform.h + otp/transform.cpp rdm/controller.h rdm/controller.cpp rdm/device.h diff --git a/otp/pdu.h b/otp/pdu.h index 2d75bc7..459cc9d 100644 --- a/otp/pdu.h +++ b/otp/pdu.h @@ -109,14 +109,6 @@ public: if (stream_ && stream_->good()) data->iStream(stream_); } - template - void createDataBlock() { - auto block = new PDU::Block(); - if (stream_) - block->iStream(stream_); - block->setParent(shared_from_this()); - data = block; - } protected: Stream stream_; diff --git a/otp/transform.cpp b/otp/transform.cpp new file mode 100644 index 0000000..003371e --- /dev/null +++ b/otp/transform.cpp @@ -0,0 +1,420 @@ +/* + otp/transform.h + + Copyright (c) 2021 Kevin Matz (kevin.matz@gmail.com) + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in all + copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. +*/ + +#include "otp.h" +#include "transform.h" + +#include + +namespace OTP::Transform { +namespace Point { + +/** + * @brief address::iStream + * @param stream + */ +void address::iStream(PDU::Stream stream) +{ + *stream >> system; + *stream >> group; + *stream >> point; +} + + +/** + * @brief address::oStream + * @param stream + */ +void address::oStream(PDU::Stream stream) const +{ + *stream << system; + *stream << group; + *stream << point; +} + + +namespace Module { + +/** + * @brief Transform::Point::Module::pdu::iStream + * @param stream + */ +void pdu::iStream(PDU::Stream stream) +{ + OTP::PDU::pdu::iStream(stream); // vector and length + createData(); // data + + auto d = static_cast(data); + + module_id id; + id.manufacturer = vector; + id.number = d->number; + + if (id.manufacturer == ESTA_MANUFACTURER_ID) + { + switch (id.number) + { + case Position: + d->mod = new position_module(); + break; + case Position_VA: + d->mod = new position_va_module(); + break; + case Rotation: + d->mod = new rotation_module(); + break; + case Rotation_VA: + d->mod = new position_va_module(); + break; + case Scale: + d->mod = new scale_module(); + break; + case Reference_Frame: + d->mod = new reference_frame_module(); + break; + default: + return; + } + *stream_ >> *d->mod; + } +} + + +/** + * @brief position_module::iStream + * @param stream + */ +void position_module::iStream(PDU::Stream stream) +{ + *stream >> options; + *stream >> x; + *stream >> y; + *stream >> z; +} + + +/** + * @brief position_module::oStream + * @param stream + */ +void position_module::oStream(PDU::Stream stream) const +{ + *stream << options; + *stream << x; + *stream << y; + *stream << z; +} + + +/** + * @brief position_va_module::iStream + * @param stream + */ +void position_va_module::iStream(PDU::Stream stream) +{ + *stream >> Vx; + *stream >> Vy; + *stream >> Vz; + *stream >> Ax; + *stream >> Ay; + *stream >> Az; +} + + +/** + * @brief position_va_module::oStream + * @param stream + */ +void position_va_module::oStream(PDU::Stream stream) const +{ + *stream << Vx; + *stream << Vy; + *stream << Vz; + *stream << Ax; + *stream << Ay; + *stream << Az; +} + + +/** + * @brief rotation_module::iStream + * @param stream + */ +void rotation_module::iStream(PDU::Stream stream) +{ + *stream >> Rx; + *stream >> Ry; + *stream >> Rz; +} + + +/** + * @brief rotation_module::oStream + * @param stream + */ +void rotation_module::oStream(PDU::Stream stream) const +{ + *stream << Rx; + *stream << Ry; + *stream << Rz; +} + + +/** + * @brief rotation_va_module::iStream + * @param stream + */ +void rotation_va_module::iStream(PDU::Stream stream) +{ + *stream >> Vrx; + *stream >> Vry; + *stream >> Vrz; + *stream >> Arx; + *stream >> Ary; + *stream >> Arz; +} + + +/** + * @brief rotation_va_module::oStream + * @param stream + */ +void rotation_va_module::oStream(PDU::Stream stream) const +{ + *stream << Vrx; + *stream << Vry; + *stream << Vrz; + *stream << Arx; + *stream << Ary; + *stream << Arz; +} + + +/** + * @brief scale_module::iStream + * @param stream + */ +void scale_module::iStream(PDU::Stream stream) +{ + *stream >> scale_x; + *stream >> scale_y; + *stream >> scale_z; +} + + +/** + * @brief scale_module::oStream + * @param stream + */ +void scale_module::oStream(PDU::Stream stream) const +{ + *stream << scale_x; + *stream << scale_y; + *stream << scale_z; +} + + +/** + * @brief module_data::~module_data + */ +module_data::~module_data() +{ + delete mod; +} + + +/** + * @brief module_data::streamSize + * @return + */ +size_t module_data::streamSize() const +{ + return 2; // + module values +} + + +/** + * @brief module_data::iStream + * @param stream + */ +void module_data::iStream(PDU::Stream stream) +{ + *stream >> number; +} + + +/** + * @brief module_data::oStream + * @param stream + */ +void module_data::oStream(PDU::Stream stream) const +{ + *stream << number; +} + + +} // namespace Module + + +/** + * @brief Transform::Point::pdu::iStream + * @param stream + */ +void pdu::iStream(PDU::Stream stream) +{ + OTP::PDU::pdu::iStream(stream); // vector and length + createData(); // data + + auto d = static_cast(data); + d->modules.setParent(shared_from_this()); +} + + +/** + * @brief point_data::streamSize + * @return + */ +size_t point_data::streamSize() const +{ + return 20 + modules.streamSize(); +} + + +/** + * @brief point_data::iStream + * @param stream + */ +void point_data::iStream(PDU::Stream stream) +{ + *stream >> priority; + *stream >> group; + *stream >> point; + *stream >> timestamp; + *stream >> options; + *stream >> reserved; + *stream >> modules; +} + + +/** + * @brief point_data::oStream + * @param stream + */ +void point_data::oStream(PDU::Stream stream) const +{ + *stream << priority; + *stream << group; + *stream << point; + *stream << timestamp; + *stream << options; + *stream << reserved; + *stream << modules; +} + +} // namespace Point + + +/** + * @brief Transform::pdu::iStream + * @param stream + */ +void pdu::iStream(PDU::Stream stream) +{ + OTP::PDU::pdu::iStream(stream); // vector and length + createData(); // data + + auto d = static_cast(data); + d->points.setParent(shared_from_this()); +} + + +/** + * @brief transform_data::streamSize + * @return + */ +size_t transform_data::streamSize() const +{ + return 14 + points.streamSize(); +} + + +/** + * @brief transform_data::iStream + * @param stream + */ +void transform_data::iStream(PDU::Stream stream) +{ + *stream >> system; + *stream >> timestamp; + *stream >> options; + *stream >> reserved; + *stream >> points; +} + + +/** + * @brief transform_data::oStream + * @param stream + */ +void transform_data::oStream(PDU::Stream stream) const +{ + *stream << system; + *stream << std::chrono::system_clock::now(); + *stream << options; + *stream << reserved; + *stream << points; +} + + +/** + * @brief operator >> + * @param s + * @param t + * @return + */ +ACN::PDU::pdu_stream& operator>>(ACN::PDU::pdu_stream& s, OTP::Transform::Timestamp& t) +{ + const auto us = std::chrono::microseconds(s.readType()); + t = OTP::Transform::Timestamp(us); + return s; +} + + +/** + * @brief operator << + * @param s + * @param t + * @return + */ +ACN::PDU::pdu_stream& operator<<(ACN::PDU::pdu_stream&s, const OTP::Transform::Timestamp& t) +{ + const auto d = t.time_since_epoch(); + const auto us = std::chrono::duration_cast(d); + uint64_t m = us.count(); + s << m; + return s; +} + + +} // namespace OTP::Transform diff --git a/otp/transform.h b/otp/transform.h new file mode 100644 index 0000000..6e1e149 --- /dev/null +++ b/otp/transform.h @@ -0,0 +1,277 @@ +/* + otp/transform.h + + Copyright (c) 2021 Kevin Matz (kevin.matz@gmail.com) + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in all + copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. +*/ +#pragma once + +#include "pdu.h" + +#include + +namespace OTP::Transform { + +using Timestamp = std::chrono::time_point; + + +namespace Point { + +struct address + : PDU::pdu_stream_object +{ + uint8_t system; + uint16_t group; + uint32_t point; + + size_t streamSize() const override { return 7; } + void iStream(PDU::Stream) override; + void oStream(PDU::Stream) const override; +}; + + +namespace Module { + +/** + * @brief Figure 16-1: Module Identifier Structure + */ +struct module_id +{ + uint16_t manufacturer; + uint16_t number; +}; + +/// Table 16-1: Standard Module Types +static const uint16_t Position = 0x0001; //!< the current position of a Point in all three linear directions. +static const uint16_t Position_VA = 0x0002; //!< the positional velocity and acceleration of a Point. +static const uint16_t Rotation = 0x0003; //!< the current rotation of the Point using intrinsic Euler rotation +static const uint16_t Rotation_VA = 0x0004; //!< the rotational velocity and acceleration of the Point +static const uint16_t Scale = 0x0005; //!< the unitless, absolute scale of the Point +static const uint16_t Reference_Frame = 0x0006; //!< the Address of the Reference Frame of the Point. + + +/** + * @brief The OTP::Transform::Point::Module::pdu class + */ +class pdu + : public OTP::PDU::pdu +{ +public: + void iStream(PDU::Stream) override; +}; + + +struct module : PDU::pdu_stream_object {}; + + +/** + * @brief The position_module struct + */ +struct position_module + : module +{ + union { + uint8_t options; + struct { + uint8_t o_reserved : 7; + bool scaling : 1; + }; + }; + int32_t x; + int32_t y; + int32_t z; + + size_t streamSize() const override { return 13; } + void iStream(PDU::Stream) override; + void oStream(PDU::Stream) const override; +}; + + +/** + * @brief The position_va_module struct + */ +struct position_va_module + : module +{ + int32_t Vx; + int32_t Vy; + int32_t Vz; + int32_t Ax; + int32_t Ay; + int32_t Az; + + size_t streamSize() const override { return 24; } + void iStream(PDU::Stream) override; + void oStream(PDU::Stream) const override; +}; + + +/** + * @brief The rotation_module struct + */ +struct rotation_module + : module +{ + int32_t Rx; + int32_t Ry; + int32_t Rz; + + size_t streamSize() const override { return 12; } + void iStream(PDU::Stream) override; + void oStream(PDU::Stream) const override; +}; + + +/** + * @brief The rotation_va_module struct + */ +struct rotation_va_module + : module +{ + int32_t Vrx; + int32_t Vry; + int32_t Vrz; + int32_t Arx; + int32_t Ary; + int32_t Arz; + + size_t streamSize() const override { return 24; } + void iStream(PDU::Stream) override; + void oStream(PDU::Stream) const override; +}; + + +/** + * @brief The scale_module struct + */ +struct scale_module + : module +{ + int32_t scale_x; + int32_t scale_y; + int32_t scale_z; + + size_t streamSize() const override { return 12; } + void iStream(PDU::Stream) override; + void oStream(PDU::Stream) const override; +}; + + +/** + * @brief The reference_frame_module struct + */ +struct reference_frame_module + : module +{ + address point; + + size_t streamSize() const override { return point.streamSize(); } + void iStream(PDU::Stream s) override { *s >> point; } + void oStream(PDU::Stream s) const override { *s << point; } +}; + + +struct module_data + : OTP::PDU::pdu_data +{ + ~module_data(); + + uint16_t number; + module * mod = nullptr; + + size_t streamSize() const override; + void iStream(PDU::Stream) override; + void oStream(PDU::Stream) const override; + +}; + +} // namespace Module + + +/** + * @brief The OTP::Transform::Point::pdu class + */ +class pdu + : public OTP::PDU::pdu +{ +public: + void iStream(PDU::Stream) override; +}; + + +struct point_data + : OTP::PDU::pdu_data +{ + uint8_t priority = 100; + uint16_t group; + uint32_t point; + Timestamp timestamp; + union { + uint8_t options; + struct { + uint8_t f_reserved : 8; + }; + }; + uint32_t reserved; + PDU::Block modules; + + size_t streamSize() const override; + void iStream(PDU::Stream) override; + void oStream(PDU::Stream) const override; +}; + + +} // namespace Point + + +/** + * @brief The OTP::Transform::pdu class + */ +class pdu + : public OTP::PDU::pdu +{ +public: + void iStream(PDU::Stream) override; +}; + +struct transform_data + : OTP::PDU::pdu_data +{ + uint8_t system; + Timestamp timestamp; + union { + uint8_t options; + struct { + uint8_t f_reserved : 7; + bool full_set : 1; + }; + }; + uint32_t reserved; + PDU::Block points; + + size_t streamSize() const override; + void iStream(PDU::Stream) override; + void oStream(PDU::Stream) const override; +}; + +ACN::PDU::pdu_stream& operator>>(ACN::PDU::pdu_stream&, OTP::Transform::Timestamp&); +ACN::PDU::pdu_stream& operator<<(ACN::PDU::pdu_stream&, const OTP::Transform::Timestamp&); + +} // namespace OTP::Transform