DYNAMIXEL Driver 0.1.0
API reference
Loading...
Searching...
No Matches
dynamixel_protocol.hpp
Go to the documentation of this file.
1
16#ifndef DYNAMIXEL_PROTOCOL_HPP
17#define DYNAMIXEL_PROTOCOL_HPP
18
19#include <array>
20#include <cstddef>
21#include <cstdint>
22#include <span>
23
24#include "dynamixel_detail.hpp"
25#include "dynamixel_tables.hpp"
26#include "dynamixel_types.hpp"
27
28namespace dynamixel::protocol {
29
31enum class Instruction : uint8_t {
32 Ping = 0x01,
33 Read = 0x02,
34 Write = 0x03,
35 RegWrite = 0x04,
36 Action = 0x05,
37 FactoryReset = 0x06,
38 Reboot = 0x08,
39 Clear = 0x10,
40 Backup = 0x20,
41 Status = 0x55,
42 SyncRead = 0x82,
43 SyncWrite = 0x83,
44 FastSyncRead = 0x8A,
45 BulkRead = 0x92,
46 BulkWrite = 0x93,
47 FastBulkRead = 0x9A,
48};
49
51inline constexpr std::array<uint8_t, 4> kHeaderV2{0xFF, 0xFF, 0xFD, 0x00};
54inline constexpr std::size_t kPrefixV2 = 8;
57inline constexpr std::size_t kPrefixV1 = 5;
59inline constexpr std::size_t kOverheadV2 = kPrefixV2 + 2U;
61inline constexpr std::size_t kOverheadV1 = kPrefixV1 + 1U;
64inline constexpr std::size_t kMaxParamsV1 = 253;
65
66namespace detail_crc {
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);
77 }
78 table[i] = static_cast<uint16_t>(crc);
79 }
80 return table;
81}
83inline constexpr std::array<uint16_t, 256> kTable = MakeTable();
84} // namespace detail_crc
85
92[[nodiscard]] constexpr uint16_t Crc16(uint16_t crc, std::span<const uint8_t> data) noexcept {
93 uint32_t acc = crc;
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]);
97 }
98 return static_cast<uint16_t>(acc);
99}
100
106[[nodiscard]] constexpr uint8_t Checksum1(std::span<const uint8_t> data) noexcept {
107 uint32_t sum = 0U;
108 for (const uint8_t byte : data) {
109 sum += static_cast<uint32_t>(byte);
110 }
111 return static_cast<uint8_t>(~sum & 0xFFU);
112}
113
121public:
129 FrameWriter(std::span<uint8_t> buffer, Protocol protocol, uint8_t id,
130 Instruction instruction) noexcept
131 : buf_(buffer), protocol_(protocol) {
132 if (protocol == Protocol::V2) {
133 for (const uint8_t byte : kHeaderV2) {
134 raw(byte);
135 }
136 raw(id);
137 raw(0_u8); // length low, set by Finish()
138 raw(0_u8); // length high
139 } else {
140 raw(0xFF_u8);
141 raw(0xFF_u8);
142 raw(id);
143 raw(0_u8); // length, set by Finish()
144 }
145 raw(static_cast<uint8_t>(instruction));
146 params_start_ = len_;
147 }
148
153 void Put(uint8_t byte) noexcept {
154 raw(byte);
155 if (protocol_ != Protocol::V2) {
156 return;
157 }
158 // Stuffing: after FF FF FD of the parameters one more FD follows (e-Manual, protocol 2.0).
159 if (byte == 0xFF_u8) {
160 run_ = (run_ < 2U) ? (run_ + 1U) : 2U;
161 } else if ((byte == 0xFD_u8) && (run_ == 2U)) {
162 raw(0xFD_u8);
163 run_ = 0U;
164 } else {
165 run_ = 0U;
166 }
167 }
168
173 void PutLe16(uint16_t value) noexcept {
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));
176 }
177
182 void PutBytes(std::span<const uint8_t> data) noexcept {
183 for (const uint8_t byte : data) {
184 Put(byte);
185 }
186 }
187
192 [[nodiscard]] std::size_t Finish() noexcept {
193 if (protocol_ == Protocol::V2) {
194 const std::size_t length = (len_ - params_start_) + 3U;
195 if ((length > 0xFFFFU) || ((len_ + 2U) > buf_.size()) || overflow_) {
196 return 0U;
197 }
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);
203 return len_ + 2U;
204 }
205 const std::size_t params = len_ - params_start_;
206 if ((params > kMaxParamsV1) || ((len_ + 1U) > buf_.size()) || overflow_) {
207 return 0U;
208 }
209 buf_[3] = static_cast<uint8_t>(params + 2U);
210 buf_[len_] = Checksum1(std::span<const uint8_t>{buf_.data() + 2, len_ - 2U});
211 return len_ + 1U;
212 }
213
214private:
215 void raw(uint8_t byte) noexcept {
216 if (len_ < buf_.size()) {
217 buf_[len_] = byte;
218 ++len_;
219 } else {
220 overflow_ = true;
221 }
222 }
223
224 std::span<uint8_t> buf_;
225 Protocol protocol_;
226 std::size_t len_{0};
227 std::size_t params_start_{0};
228 uint32_t run_{0};
229 bool overflow_{false};
230};
231
233inline constexpr std::size_t kBadStuffing = static_cast<std::size_t>(-1);
234
240[[nodiscard]] constexpr std::size_t Destuff(std::span<uint8_t> params) noexcept {
241 std::size_t out = 0;
242 uint32_t run = 0U;
243 for (std::size_t i = 0; i < params.size(); ++i) {
244 const uint8_t byte = params[i];
245 params[out] = byte;
246 ++out;
247 if (byte == 0xFF_u8) {
248 run = (run < 2U) ? (run + 1U) : 2U;
249 } else if ((byte == 0xFD_u8) && (run == 2U)) {
250 // FF FF FD must be followed by the stuffed FD, which is dropped.
251 if (((i + 1U) >= params.size()) || (params[i + 1U] != 0xFD_u8)) {
252 return kBadStuffing;
253 }
254 ++i;
255 run = 0U;
256 } else {
257 run = 0U;
258 }
259 }
260 return out;
261}
262
266 enum class Kind : uint8_t {
267 NeedMore = 0,
268 Frame = 1,
269 };
271 std::size_t discard{
272 0};
273 std::size_t frame_size{0};
274 std::size_t need{1};
275 std::size_t crc_drops{0};
276 std::size_t garbage{0};
277};
278
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);
289}
290
299constexpr bool IsPartialHeader(Protocol protocol, std::span<const uint8_t> bytes, std::size_t pos,
300 std::size_t left) noexcept {
301 bool partial = true;
302 for (std::size_t k = 0; k < left; ++k) {
303 const uint8_t expect = (protocol == Protocol::V2) ? kHeaderV2[k] : 0xFF_u8;
304 if (bytes[pos + k] != expect) {
305 partial = false;
306 }
307 }
308 return partial;
309}
310
319constexpr bool IsCandidate(Protocol protocol, std::span<const uint8_t> bytes, std::size_t pos,
320 std::size_t left) noexcept {
321 if (protocol == Protocol::V2) {
322 return MatchesV2(bytes, pos);
323 }
324 // Protocol 1.0: FF FF then an ID that is not 0xFF (0xFF FF FF would be a longer run of header
325 // bytes).
326 return (bytes[pos] == 0xFF_u8) && (bytes[pos + 1U] == 0xFF_u8) &&
327 ((left < 3U) || (bytes[pos + 2U] != 0xFF_u8));
328}
329
340constexpr bool IsPlausible(Protocol protocol, std::span<const uint8_t> bytes, std::size_t pos,
341 std::size_t left, std::size_t max_frame, std::size_t& total) noexcept {
342 if (protocol == Protocol::V2) {
343 const std::size_t length = static_cast<std::size_t>(bytes[pos + 5U]) |
344 (static_cast<std::size_t>(bytes[pos + 6U]) << 8U);
345 total = 7U + length;
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;
349 }
350 const std::size_t length = bytes[pos + 3U];
351 total = 4U + length;
352 return (length >= 2U) && (total <= max_frame) && (bytes[pos + 2U] <= kMaxIdV1);
353}
354
361constexpr bool IsChecked(Protocol protocol, std::span<const uint8_t> frame) noexcept {
362 const std::size_t total = frame.size();
363 if (protocol == Protocol::V2) {
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));
367 }
368 return frame[total - 1U] == Checksum1(frame.subspan(2U, total - 3U));
369}
370} // namespace detail_scan
371
386[[nodiscard]] constexpr ScanResult Scan(Protocol protocol, std::span<const uint8_t> bytes,
387 std::size_t max_frame) noexcept {
388 ScanResult result{};
389 const std::size_t header = (protocol == Protocol::V2) ? 4U : 2U;
390 const std::size_t min_for_len = (protocol == Protocol::V2) ? 7U : 4U;
391 std::size_t pos = 0;
392 while (pos < bytes.size()) {
393 const std::size_t left = bytes.size() - pos;
394 // A partial header at the end of the data may still become one: keep it.
395 if ((left < header) && detail_scan::IsPartialHeader(protocol, bytes, pos, left)) {
396 result.discard = pos;
397 result.need = header - left;
398 return result;
399 }
400 if ((left < header) || !detail_scan::IsCandidate(protocol, bytes, pos, left)) {
401 result.garbage += 1U;
402 ++pos;
403 continue;
404 }
405 if (left < min_for_len) {
406 result.discard = pos;
407 result.need = min_for_len - left;
408 return result;
409 }
410 std::size_t total = 0;
411 if (!detail_scan::IsPlausible(protocol, bytes, pos, left, max_frame, total)) {
412 result.garbage += 1U;
413 ++pos;
414 continue;
415 }
416 if (left < total) {
417 result.discard = pos;
418 result.need = total - left;
419 return result;
420 }
421 if (detail_scan::IsChecked(protocol, bytes.subspan(pos, total))) {
422 result.kind = ScanResult::Kind::Frame;
423 result.discard = pos;
424 result.frame_size = total;
425 return result;
426 }
427 result.crc_drops += 1U;
428 ++pos;
429 }
430 result.discard = pos;
431 return result;
432}
433
436 uint8_t id{0};
437 uint8_t error{0};
438 std::span<uint8_t> params;
439};
440
447[[nodiscard]] constexpr StatusView Split(Protocol protocol, std::span<uint8_t> frame) noexcept {
448 StatusView view{};
449 if (protocol == Protocol::V2) {
450 view.id = frame[4];
451 view.error = frame[8];
452 view.params = frame.subspan(9U, frame.size() - 11U);
453 } else {
454 view.id = frame[2];
455 view.error = frame[4];
456 view.params = frame.subspan(5U, frame.size() - 6U);
457 }
458 return view;
459}
460
466[[nodiscard]] constexpr uint32_t LoadLe(std::span<const uint8_t> data) noexcept {
467 if (data.empty() || (data.size() > 4U)) {
468 return 0U;
469 }
470 uint32_t value = 0U;
471 for (std::size_t i = data.size(); i > 0U; --i) {
472 value = (value << 8U) | static_cast<uint32_t>(data[i - 1U]);
473 }
474 return value;
475}
476
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;
486 switch (kind) {
487 case Kind::Signed:
488 if ((bits < 32U) && ((raw & (1U << (bits - 1U))) != 0U)) {
489 return static_cast<int32_t>(raw) - static_cast<int32_t>(1U << bits);
490 }
491 return static_cast<int32_t>(raw);
492 case Kind::SignMag10: {
493 const auto magnitude = static_cast<int32_t>(raw & 0x3FFU);
494 return ((raw & 0x400U) != 0U) ? -magnitude : magnitude;
495 }
496 case Kind::Offset2048:
497 return static_cast<int32_t>(raw) - 2048;
498 case Kind::Unsigned:
499 return static_cast<int32_t>(raw);
500 default:
501 break;
502 }
503 return static_cast<int32_t>(raw);
504}
505
514[[nodiscard]] constexpr bool EncodeValue(Kind kind, std::size_t size, int32_t value,
515 std::span<uint8_t, 4> out) noexcept {
516 if ((size != 1U) && (size != 2U) && (size != 4U)) {
517 return false;
518 }
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); // 2^bits
522 const int64_t half = range / 2;
523 uint32_t raw = 0U;
524 switch (kind) {
525 case Kind::Unsigned:
526 if ((v < 0) || (v >= range)) {
527 return false;
528 }
529 raw = static_cast<uint32_t>(v);
530 break;
531 case Kind::Signed:
532 if ((v < -half) || (v >= half)) {
533 return false;
534 }
535 raw = static_cast<uint32_t>(static_cast<uint64_t>(v) & 0xFFFFFFFFULL);
536 break;
537 case Kind::SignMag10:
538 if ((v < -1023) || (v > 1023)) {
539 return false;
540 }
541 raw = (v < 0) ? (static_cast<uint32_t>(-v) | 0x400U) : static_cast<uint32_t>(v);
542 break;
543 case Kind::Offset2048:
544 if (((v + 2048) < 0) || ((v + 2048) >= range)) {
545 return false;
546 }
547 raw = static_cast<uint32_t>(v + 2048);
548 break;
549 default:
550 return false;
551 }
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);
554 }
555 return true;
556}
557
558// The CRC of the e-Manual's ping example (protocol 2.0, Ping, ID 1): 19 4E.
559static_assert(Crc16(0_u16, std::array<uint8_t, 8>{0xFF, 0xFF, 0xFD, 0x00, 0x01, 0x03, 0x00,
560 0x01}) == 0x4E19U);
561// The e-Manual's protocol 1.0 checksum example: ID 1, length 5, write, 0C 64 AA gives DC.
562static_assert(Checksum1(std::array<uint8_t, 6>{0x01, 0x05, 0x03, 0x0C, 0x64, 0xAA}) == 0xDCU);
563
564} // namespace dynamixel::protocol
565
566#endif // DYNAMIXEL_PROTOCOL_HPP
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).
@ 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