OTP Transform PDU i/o
This commit is contained in:
parent
914be1f3f9
commit
03347a5e21
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -109,14 +109,6 @@ public:
|
|||
if (stream_ && stream_->good())
|
||||
data->iStream(stream_);
|
||||
}
|
||||
template<class T>
|
||||
void createDataBlock() {
|
||||
auto block = new PDU::Block<T>();
|
||||
if (stream_)
|
||||
block->iStream(stream_);
|
||||
block->setParent(shared_from_this());
|
||||
data = block;
|
||||
}
|
||||
|
||||
protected:
|
||||
Stream stream_;
|
||||
|
|
|
|||
|
|
@ -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 <chrono>
|
||||
|
||||
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<module_data>(); // data
|
||||
|
||||
auto d = static_cast<module_data*>(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<point_data>(); // data
|
||||
|
||||
auto d = static_cast<point_data*>(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<transform_data>(); // data
|
||||
|
||||
auto d = static_cast<transform_data*>(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<uint64_t>());
|
||||
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<std::chrono::microseconds>(d);
|
||||
uint64_t m = us.count();
|
||||
s << m;
|
||||
return s;
|
||||
}
|
||||
|
||||
|
||||
} // namespace OTP::Transform
|
||||
|
|
@ -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 <chrono>
|
||||
|
||||
namespace OTP::Transform {
|
||||
|
||||
using Timestamp = std::chrono::time_point<std::chrono::system_clock>;
|
||||
|
||||
|
||||
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<Module::pdu> 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<Point::pdu> 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
|
||||
Loading…
Reference in New Issue