278 lines
5.8 KiB
C
278 lines
5.8 KiB
C
|
/*
|
||
|
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
|