16#ifndef DYNAMIXEL_PROTOCOL_HPP
17#define DYNAMIXEL_PROTOCOL_HPP
28namespace dynamixel::protocol {
51inline constexpr std::array<uint8_t, 4>
kHeaderV2{0xFF, 0xFF, 0xFD, 0x00};
71constexpr std::array<uint16_t, 256>
MakeTable() noexcept {
72 std::array<uint16_t, 256> table{};
73 for (uint32_t i = 0U; i < 256U; ++i) {
74 uint32_t crc = i << 8U;
75 for (uint32_t bit = 0U; bit < 8U; ++bit) {
76 crc = ((crc & 0x8000U) != 0U) ? (((crc << 1U) ^ 0x8005U) & 0xFFFFU) : ((crc << 1U) & 0xFFFFU);
78 table[i] =
static_cast<uint16_t
>(crc);
92[[nodiscard]]
constexpr uint16_t
Crc16(uint16_t crc, std::span<const uint8_t> data)
noexcept {
94 for (
const uint8_t
byte : data) {
95 const uint32_t index = ((acc >> 8U) ^
static_cast<uint32_t
>(
byte)) & 0xFFU;
96 acc = ((acc << 8U) & 0xFFFFU) ^ static_cast<uint32_t>(detail_crc::kTable[index]);
98 return static_cast<uint16_t
>(acc);
106[[nodiscard]]
constexpr uint8_t
Checksum1(std::span<const uint8_t> data)
noexcept {
108 for (
const uint8_t
byte : data) {
109 sum +=
static_cast<uint32_t
>(byte);
111 return static_cast<uint8_t
>(~sum & 0xFFU);
131 : buf_(buffer), protocol_(protocol) {
145 raw(
static_cast<uint8_t
>(instruction));
146 params_start_ = len_;
153 void Put(uint8_t
byte)
noexcept {
159 if (
byte == 0xFF_u8) {
160 run_ = (run_ < 2U) ? (run_ + 1U) : 2U;
161 }
else if ((
byte == 0xFD_u8) && (run_ == 2U)) {
174 Put(
static_cast<uint8_t
>(
static_cast<uint32_t
>(value) & 0xFFU));
175 Put(
static_cast<uint8_t
>((
static_cast<uint32_t
>(value) >> 8U) & 0xFFU));
182 void PutBytes(std::span<const uint8_t> data)
noexcept {
183 for (
const uint8_t
byte : data) {
192 [[nodiscard]] std::size_t
Finish() noexcept {
194 const std::size_t length = (len_ - params_start_) + 3U;
195 if ((length > 0xFFFFU) || ((len_ + 2U) > buf_.size()) || overflow_) {
198 buf_[5] =
static_cast<uint8_t
>(length & 0xFFU);
199 buf_[6] =
static_cast<uint8_t
>((length >> 8U) & 0xFFU);
200 const uint16_t crc =
Crc16(0_u16, std::span<const uint8_t>{buf_.data(), len_});
201 buf_[len_] =
static_cast<uint8_t
>(
static_cast<uint32_t
>(crc) & 0xFFU);
202 buf_[len_ + 1U] =
static_cast<uint8_t
>((
static_cast<uint32_t
>(crc) >> 8U) & 0xFFU);
205 const std::size_t params = len_ - params_start_;
206 if ((params >
kMaxParamsV1) || ((len_ + 1U) > buf_.size()) || overflow_) {
209 buf_[3] =
static_cast<uint8_t
>(params + 2U);
210 buf_[len_] =
Checksum1(std::span<const uint8_t>{buf_.data() + 2, len_ - 2U});
215 void raw(uint8_t
byte)
noexcept {
216 if (len_ < buf_.size()) {
224 std::span<uint8_t> buf_;
227 std::size_t params_start_{0};
229 bool overflow_{
false};
233inline constexpr std::size_t
kBadStuffing =
static_cast<std::size_t
>(-1);
240[[nodiscard]]
constexpr std::size_t
Destuff(std::span<uint8_t> params)
noexcept {
243 for (std::size_t i = 0; i < params.size(); ++i) {
244 const uint8_t
byte = params[i];
247 if (
byte == 0xFF_u8) {
248 run = (run < 2U) ? (run + 1U) : 2U;
249 }
else if ((
byte == 0xFD_u8) && (run == 2U)) {
251 if (((i + 1U) >= params.size()) || (params[i + 1U] != 0xFD_u8)) {
279namespace detail_scan {
286constexpr bool MatchesV2(std::span<const uint8_t> b, std::size_t i)
noexcept {
287 return (b[i] == 0xFF_u8) && (b[i + 1U] == 0xFF_u8) && (b[i + 2U] == 0xFD_u8) &&
288 (b[i + 3U] == 0x00_u8);
300 std::size_t left)
noexcept {
302 for (std::size_t k = 0; k < left; ++k) {
304 if (bytes[pos + k] != expect) {
320 std::size_t left)
noexcept {
326 return (bytes[pos] == 0xFF_u8) && (bytes[pos + 1U] == 0xFF_u8) &&
327 ((left < 3U) || (bytes[pos + 2U] != 0xFF_u8));
341 std::size_t left, std::size_t max_frame, std::size_t& total)
noexcept {
343 const std::size_t length =
static_cast<std::size_t
>(bytes[pos + 5U]) |
344 (
static_cast<std::size_t
>(bytes[pos + 6U]) << 8U);
346 const bool wrong_instruction =
347 (left >= 8U) && (bytes[pos + 7U] !=
static_cast<uint8_t
>(Instruction::Status));
348 return (length >= 4U) && (total <= max_frame) && !wrong_instruction;
350 const std::size_t length = bytes[pos + 3U];
352 return (length >= 2U) && (total <= max_frame) && (bytes[pos + 2U] <=
kMaxIdV1);
362 const std::size_t total = frame.size();
364 const uint16_t crc =
Crc16(0_u16, frame.first(total - 2U));
365 return (frame[total - 2U] ==
static_cast<uint8_t
>(
static_cast<uint32_t
>(crc) & 0xFFU)) &&
366 (frame[total - 1U] ==
static_cast<uint8_t
>((
static_cast<uint32_t
>(crc) >> 8U) & 0xFFU));
368 return frame[total - 1U] ==
Checksum1(frame.subspan(2U, total - 3U));
387 std::size_t max_frame)
noexcept {
389 const std::size_t header = (protocol ==
Protocol::V2) ? 4U : 2U;
390 const std::size_t min_for_len = (protocol ==
Protocol::V2) ? 7U : 4U;
392 while (pos < bytes.size()) {
393 const std::size_t left = bytes.size() - pos;
395 if ((left < header) && detail_scan::IsPartialHeader(protocol, bytes, pos, left)) {
396 result.discard = pos;
397 result.need = header - left;
400 if ((left < header) || !detail_scan::IsCandidate(protocol, bytes, pos, left)) {
401 result.garbage += 1U;
405 if (left < min_for_len) {
406 result.discard = pos;
407 result.need = min_for_len - left;
410 std::size_t total = 0;
411 if (!detail_scan::IsPlausible(protocol, bytes, pos, left, max_frame, total)) {
412 result.garbage += 1U;
417 result.discard = pos;
418 result.need = total - left;
421 if (detail_scan::IsChecked(protocol, bytes.subspan(pos, total))) {
423 result.discard = pos;
424 result.frame_size = total;
427 result.crc_drops += 1U;
430 result.discard = pos;
451 view.error = frame[8];
452 view.params = frame.subspan(9U, frame.size() - 11U);
455 view.error = frame[4];
456 view.params = frame.subspan(5U, frame.size() - 6U);
466[[nodiscard]]
constexpr uint32_t
LoadLe(std::span<const uint8_t> data)
noexcept {
467 if (data.empty() || (data.size() > 4U)) {
471 for (std::size_t i = data.size(); i > 0U; --i) {
472 value = (value << 8U) | static_cast<uint32_t>(data[i - 1U]);
483[[nodiscard]]
constexpr int32_t
DecodeValue(
Kind kind, std::span<const uint8_t> data)
noexcept {
484 const uint32_t raw =
LoadLe(data);
485 const uint32_t bits =
static_cast<uint32_t
>(data.size()) * 8U;
488 if ((bits < 32U) && ((raw & (1U << (bits - 1U))) != 0U)) {
489 return static_cast<int32_t
>(raw) -
static_cast<int32_t
>(1U << bits);
491 return static_cast<int32_t
>(raw);
493 const auto magnitude =
static_cast<int32_t
>(raw & 0x3FFU);
494 return ((raw & 0x400U) != 0U) ? -magnitude : magnitude;
497 return static_cast<int32_t
>(raw) - 2048;
499 return static_cast<int32_t
>(raw);
503 return static_cast<int32_t
>(raw);
515 std::span<uint8_t, 4> out)
noexcept {
516 if ((size != 1U) && (size != 2U) && (size != 4U)) {
519 const int64_t v = value;
520 const uint32_t bits =
static_cast<uint32_t
>(size) * 8U;
521 const auto range =
static_cast<int64_t
>(uint64_t{1} << bits);
522 const int64_t half = range / 2;
526 if ((v < 0) || (v >= range)) {
529 raw =
static_cast<uint32_t
>(v);
532 if ((v < -half) || (v >= half)) {
535 raw =
static_cast<uint32_t
>(
static_cast<uint64_t
>(v) & 0xFFFFFFFFULL);
538 if ((v < -1023) || (v > 1023)) {
541 raw = (v < 0) ? (static_cast<uint32_t>(-v) | 0x400U) :
static_cast<uint32_t
>(v);
544 if (((v + 2048) < 0) || ((v + 2048) >= range)) {
547 raw =
static_cast<uint32_t
>(v + 2048);
552 for (std::size_t i = 0; i < size; ++i) {
553 out[i] =
static_cast<uint8_t
>((raw >> (8U *
static_cast<uint32_t
>(i))) & 0xFFU);
559static_assert(Crc16(0_u16, std::array<uint8_t, 8>{0xFF, 0xFF, 0xFD, 0x00, 0x01, 0x03, 0x00,
562static_assert(Checksum1(std::array<uint8_t, 6>{0x01, 0x05, 0x03, 0x0C, 0x64, 0xAA}) == 0xDCU);
Builds one instruction packet in a caller buffer, with byte stuffing for protocol 2....
Definition dynamixel_protocol.hpp:120
void PutLe16(uint16_t value) noexcept
Append a 16-bit value, low byte first.
Definition dynamixel_protocol.hpp:173
std::size_t Finish() noexcept
Complete the packet: length and CRC or checksum.
Definition dynamixel_protocol.hpp:192
FrameWriter(std::span< uint8_t > buffer, Protocol protocol, uint8_t id, Instruction instruction) noexcept
Start a packet.
Definition dynamixel_protocol.hpp:129
void PutBytes(std::span< const uint8_t > data) noexcept
Append bytes.
Definition dynamixel_protocol.hpp:182
void Put(uint8_t byte) noexcept
Append one parameter byte (stuffed in protocol 2.0).
Definition dynamixel_protocol.hpp:153
Compile-time helpers of the DYNAMIXEL driver: typed integer literals, exact hook matching,...
constexpr std::size_t Destuff(std::span< uint8_t > params) noexcept
Remove the stuffed bytes of received protocol 2.0 parameters, in place.
Definition dynamixel_protocol.hpp:240
constexpr bool MatchesV2(std::span< const uint8_t > b, std::size_t i) noexcept
Whether the four bytes at i are the protocol 2.0 header FF FF FD 00.
Definition dynamixel_protocol.hpp:286
constexpr std::size_t kOverheadV1
Fixed bytes of a protocol 1.0 packet around its parameters (prefix and checksum).
Definition dynamixel_protocol.hpp:61
constexpr std::size_t kMaxParamsV1
Definition dynamixel_protocol.hpp:64
constexpr bool EncodeValue(Kind kind, std::size_t size, int32_t value, std::span< uint8_t, 4 > out) noexcept
Encode a value into a register's raw bytes.
Definition dynamixel_protocol.hpp:514
Instruction
Instruction codes of both protocols (the e-Manual instruction tables).
Definition dynamixel_protocol.hpp:31
@ BulkRead
Bulk Read (both, different parameter layouts).
@ RegWrite
Reg Write (both).
@ Backup
Control Table Backup (protocol 2.0).
@ BulkWrite
Bulk Write (protocol 2.0).
@ SyncRead
Sync Read (protocol 2.0).
@ FastBulkRead
Fast Bulk Read (protocol 2.0).
@ FactoryReset
Factory Reset (both; protocol 2.0 has an option byte).
@ FastSyncRead
Fast Sync Read (protocol 2.0).
@ Clear
Clear (protocol 2.0).
@ SyncWrite
Sync Write (both).
@ Status
Status (return) packet (protocol 2.0).
constexpr std::size_t kPrefixV2
Definition dynamixel_protocol.hpp:54
constexpr bool IsCandidate(Protocol protocol, std::span< const uint8_t > bytes, std::size_t pos, std::size_t left) noexcept
Whether a full header starts at pos.
Definition dynamixel_protocol.hpp:319
constexpr bool IsPartialHeader(Protocol protocol, std::span< const uint8_t > bytes, std::size_t pos, std::size_t left) noexcept
Whether the left bytes at pos (fewer than a header) are the start of one.
Definition dynamixel_protocol.hpp:299
constexpr int32_t DecodeValue(Kind kind, std::span< const uint8_t > data) noexcept
Decode a register's raw bytes.
Definition dynamixel_protocol.hpp:483
constexpr uint16_t Crc16(uint16_t crc, std::span< const uint8_t > data) noexcept
Continue a protocol 2.0 CRC-16 over more bytes.
Definition dynamixel_protocol.hpp:92
constexpr ScanResult Scan(Protocol protocol, std::span< const uint8_t > bytes, std::size_t max_frame) noexcept
Find the first valid status packet in received bytes, skipping garbage and resynchronising after a da...
Definition dynamixel_protocol.hpp:386
constexpr std::size_t kPrefixV1
Definition dynamixel_protocol.hpp:57
constexpr std::size_t kOverheadV2
Fixed bytes of a protocol 2.0 packet around its parameters (prefix and CRC).
Definition dynamixel_protocol.hpp:59
constexpr std::array< uint8_t, 4 > kHeaderV2
First three bytes of a protocol 2.0 header (the fourth is the reserved 0x00).
Definition dynamixel_protocol.hpp:51
constexpr uint8_t Checksum1(std::span< const uint8_t > data) noexcept
The protocol 1.0 checksum of the bytes from the ID to the last parameter.
Definition dynamixel_protocol.hpp:106
constexpr std::size_t kBadStuffing
Returned by Destuff() for parameters that are not validly stuffed.
Definition dynamixel_protocol.hpp:233
constexpr StatusView Split(Protocol protocol, std::span< uint8_t > frame) noexcept
Split a valid status packet found by Scan().
Definition dynamixel_protocol.hpp:447
constexpr uint32_t LoadLe(std::span< const uint8_t > data) noexcept
Read a little-endian unsigned value of 1 to 4 bytes.
Definition dynamixel_protocol.hpp:466
constexpr std::array< uint16_t, 256 > kTable
The table MakeTable() built.
Definition dynamixel_protocol.hpp:83
constexpr bool IsPlausible(Protocol protocol, std::span< const uint8_t > bytes, std::size_t pos, std::size_t left, std::size_t max_frame, std::size_t &total) noexcept
The size a candidate packet claims, and whether that claim is plausible.
Definition dynamixel_protocol.hpp:340
constexpr std::array< uint16_t, 256 > MakeTable() noexcept
The CRC-16 lookup table, generated at build time from the polynomial 0x8005.
Definition dynamixel_protocol.hpp:71
constexpr bool IsChecked(Protocol protocol, std::span< const uint8_t > frame) noexcept
Whether the CRC-16 or checksum of a complete packet matches.
Definition dynamixel_protocol.hpp:361
Control tables of the DYNAMIXEL servo families, the model catalogue and the unit scales.
Public types of the DYNAMIXEL driver: errors, Result<T>, protocol enumerations, replies,...
constexpr uint8_t kMaxIdV1
The largest valid servo ID of protocol 1.0.
Definition dynamixel_types.hpp:363
Protocol
The packet format of the bus: one per DYNAMIXEL object, because every servo on a bus shares it.
Definition dynamixel_types.hpp:182
Kind
How the raw bytes of a register are read as a number.
Definition dynamixel_tables.hpp:59
@ SignMag10
Protocol 1.0 speed, load, torque: bits 9..0 magnitude, bit 10 direction (CW).
@ Offset2048
MX-64/106 current: value - 2048 counts.
@ Signed
Little-endian two's complement of the register width.
@ Unsigned
Little-endian unsigned.
What Scan() found in the receive buffer.
Definition dynamixel_protocol.hpp:264
std::size_t need
Smallest number of further bytes that can make progress.
Definition dynamixel_protocol.hpp:274
std::size_t crc_drops
Candidates dropped for a wrong CRC or checksum.
Definition dynamixel_protocol.hpp:275
std::size_t discard
Bytes before the packet (or all useless bytes) to drop from the front.
Definition dynamixel_protocol.hpp:271
Kind
The outcome.
Definition dynamixel_protocol.hpp:266
@ Frame
A valid status packet starts at discard, frame_size bytes long.
@ NeedMore
No complete valid status packet yet; read need more bytes.
Kind kind
The outcome.
Definition dynamixel_protocol.hpp:270
std::size_t garbage
Bytes skipped that did not start a candidate.
Definition dynamixel_protocol.hpp:276
std::size_t frame_size
Size of the valid packet when kind is Frame.
Definition dynamixel_protocol.hpp:273
The fields of a located status packet; the spans point into the receive buffer.
Definition dynamixel_protocol.hpp:435
std::span< uint8_t > params
The parameters, still stuffed for protocol 2.0.
Definition dynamixel_protocol.hpp:438
uint8_t id
The ID field.
Definition dynamixel_protocol.hpp:436
uint8_t error
The error byte.
Definition dynamixel_protocol.hpp:437