C++を使ったシリアル通信で、他のデバイスとバイナリデータを高速・安全・簡単にやり取りしたい【COBS/CRC8】

概要

今回紹介するコードは、ArduinoなどのC++対応マイコン同士、またはArduinoとPCとの間での高速なシリアル通信を可能にします。具体的には、データの送受信時にエラーを検出・訂正するためのCRC8チェック、そして、0の値を持つバイトを安全に送受信できるようにするためのCOBS（Consistent Overhead Byte Stuffing）エンコード/デコードの機能を提供します。これらの機能を通じて、安全かつ効率的なデータの送受信が可能となります。

なぜこれが必要か？

ArduinoやPCなどのC++対応マイコン同士の間でデータを送受信する場合、多くのプロジェクトで直面する問題はデータの整合性とエラーチェックです。ノイズによるデータの損失や、転送中のデータがゼロの値を持つバイトである場合、データの終わりと間違って解釈される可能性があります。また、データが大量であるほど、エラーチェックのオーバーヘッドが増大し、通信速度が低下します。これらの問題は、ロボティクスやIoTデバイスなど、リアルタイム処理と高い信頼性が求められるアプリケーションでは特に深刻です。

実装方法 - CRC8とCOBSの活用

ヘッダオンリーライブラリは、これらの問題に対する解決策としてCRC8とCOBSを活用します。

CRC8

CRC8は、送受信データのエラーチェックを行うためのアルゴリズムです。具体的には、データの各バイトに対して特定の計算を行い、その結果をチェックサムとしてデータに追加します。受信側では、同じ計算を行い、結果が送信側で計算されたチェックサムと一致するかを確認します。これにより、データ転送中のエラーを検出できます。

COBS

COBSは、データの区切りとして0を利用しつつも、0の値を持つバイトを安全に送受信できるようにするためのエンコーディング方法です。これにより、データの任意のバイト列を送受信できます。

#pragma once

#include <vector>
#include <cstddef>
#include <cstdint>
#include <algorithm>
#include <iterator>
#include <cstring>

template <typename T>
std::vector<uint8_t> Serialize(const T& object, bool little_endian = true)
{
	std::vector<uint8_t> bytes(sizeof(object));
	std::memcpy(bytes.data(), &object, sizeof(object));

	if (!little_endian)
	{
		std::reverse(bytes.begin(), bytes.end());
	}

	return bytes;
}

template <typename T>
std::vector<uint8_t> Serialize(const std::vector<T>& objects, bool little_endian = true)
{
	std::vector<uint8_t> bytes;
	for (const auto& object : objects)
	{
		std::vector<uint8_t> object_bytes = Serialize(object, little_endian);
		bytes.insert(bytes.end(), object_bytes.begin(), object_bytes.end());
	}
	return bytes;
}

template <typename T>
T DeserializeFromBytes(const std::vector<uint8_t>& bytes, bool little_endian = true)
{
	if (bytes.size() < sizeof(T))
	{
		throw std::runtime_error("Not enough bytes to convert to the target type.");
	}

	T result;
	std::vector<uint8_t> tmp(bytes.begin(), bytes.begin() + sizeof(T));

	if (!little_endian)
	{
		std::reverse(tmp.begin(), tmp.end());
	}

	std::memcpy(&result, tmp.data(), sizeof(T));

	return result;
}

template <typename T>
std::vector<T> DeserializeVectorFromBytes(const std::vector<uint8_t>& bytes, bool little_endian = true)
{
	if (bytes.size() % sizeof(T) != 0)
	{
		return {};

		//throw std::runtime_error("Byte array size is not a multiple of the target type size.");
	}

	std::vector<T> result;
	for (size_t i = 0; i < bytes.size(); i += sizeof(T))
	{
		std::vector<uint8_t> object_bytes(bytes.begin() + i, bytes.begin() + i + sizeof(T));
		result.push_back(DeserializeFromBytes<T>(object_bytes, little_endian));
	}

	return result;
}

uint8_t CalculateCRC8(const std::vector<uint8_t>& data)
{
	uint8_t crc = 0x00;
	for (auto byte : data)
	{
		crc ^= static_cast<uint8_t>(byte);
		for (int i = 0; i < 8; i++)
		{
			if ((crc & 0x80) != 0)
				crc = (uint8_t)((crc << 1) ^ 0x07);
			else
				crc <<= 1;
		}
	}
	return crc;
}

std::vector<uint8_t> COBSEncode(const std::vector<uint8_t>& data)
{
	if (data.empty())
	{
		throw std::invalid_argument("data cannot be empty.");
	}

	std::vector<uint8_t> buffer;
	buffer.reserve(data.size() + data.size() / 254 + 2);  // Extra space for overhead bytes

	auto dst = buffer.begin();
	auto code_ptr = dst;
	buffer.push_back(0);  // Initial placeholder
	++dst;
	uint8_t code = 0x01;

	for (auto ptr = data.cbegin(); ptr != data.cend(); ++ptr)
	{
		if (*ptr == 0)
		{
			*code_ptr = code;
			code_ptr = dst;
			buffer.push_back(0);  // Start a new block
			++dst;
			code = 0x01;
		}
		else
		{
			buffer.push_back(*ptr);
			++dst;
			++code;
			if (code == 0xFF)
			{
				*code_ptr = code;
				code_ptr = dst;
				buffer.push_back(0);  // Start a new block
				++dst;
				code = 0x01;
			}
		}
	}

	*code_ptr = code;

	return buffer;
}


std::optional<std::vector<uint8_t>> COBSDecode(const std::vector<uint8_t>& buffer)
{
	if (buffer.empty())
	{
		return std::nullopt;
	}

	std::vector<uint8_t> data;
	data.reserve(buffer.size());

	for (auto it = buffer.cbegin(); it != buffer.cend(); )
	{
		int code = *it++;
		for (int i = 1; i < code; i++)
		{
			if (it != buffer.cend())
			{
				data.push_back(*it++);
			}
			else
			{
				// "Invalid COBS encoding : Not enough bytes according to the code.";
				return std::nullopt;
			}
		}
		if (code < 0xFF)
		{
			if (it != buffer.cend())
			{
				data.push_back(0);
			}
			else
			{
				// We are at the end of the buffer and there's no zero byte,
				// but we'll accept it anyway, just not insert a zero into our decoded data.
				break;
			}
		}
	}

	return std::make_optional(data);
}


std::vector<uint8_t> EncodePacket(const std::vector<uint8_t>& data)
{
	std::vector<uint8_t> packet(data.begin(), data.end());

	// Calculate and add the CRC
	uint8_t crc = static_cast<uint8_t>(CalculateCRC8(packet));
	packet.push_back(crc);

	// COBS encode the packet
	packet = COBSEncode(packet);

	// Add a delimiter
	packet.push_back(0);

	return packet;
}

std::optional<std::vector<uint8_t>> DecodePacket(const std::vector<uint8_t>& packet)
{
	if (packet.empty())
	{
		return std::nullopt;
	}

	auto tDecodedPacket = COBSDecode(packet);

	if (!tDecodedPacket.has_value())
	{
		return std::nullopt;
	}

	auto decodedPacket = tDecodedPacket.value();

	// CRC check
	std::vector<uint8_t>
		data(decodedPacket.begin(), decodedPacket.end() - 1);

	uint8_t crc = static_cast<uint8_t>(CalculateCRC8(data));

	if (crc != decodedPacket.back())
	{
		// CRC failed
		return std::nullopt;
	}

	return std::make_optional(data);
}

利用例

ここでは、OpenSiv3D とESP32とのデータのやり取りをする例です。
上記のコードをSimplePacket.hppとしてどちらにも入れています。

PC側

# include <Siv3D.hpp>
#include "SimplePacket.hpp"

void Main()
{
	// シリアルポートの一覧を取得
	const Array<SerialPortInfo> infos = System::EnumerateSerialPorts();
	const Array<String> options = infos.map([](const SerialPortInfo& info)
	{
		return U"{} ({})"_fmt(info.port, info.description);
	}) << U"none";

	Serial serial;
	size_t index = (options.size() - 1);


	std::vector<uint16_t> servoData = { 1894, 512, 256, 128, 64, 32, 16, 8, 4, 2, 1 };
	uint32_t timestamp = 10000;

	std::vector<uint8_t> packet;

	{
		std::vector<uint8_t> data;
		// Add the timestamp
		for (size_t i = 0; i < sizeof(timestamp); i++)
		{
			data.push_back(static_cast<uint8_t>((timestamp >> (i * 8)) & 0xFF));
		}

		// Add the servo data
		for (auto servo : servoData)
		{
			for (size_t i = 0; i < sizeof(uint16_t); i++)
			{
				data.push_back(static_cast<uint8_t>((servo >> (i * 8)) & 0xFF));
			}
		}

		packet = EncodePacket(data);
	}

	while (System::Update())
	{
		const bool isOpen = serial.isOpen(); // OpenSiv3D v0.4.2 以前は serial.isOpened()

		serial.write(packet.data(), packet.size());


		if (SimpleGUI::RadioButtons(index, options, Vec2{ 200, 60 }))
		{
			ClearPrint();

			if (index == (options.size() - 1))
			{
				serial = Serial{};
			}
			else
			{
				Print << U"Open {}"_fmt(infos[index].port);

				// シリアルポートをオープン
				if (serial.open(infos[index].port, 115200))
				{
					Print << U"Succeeded";
				}
				else
				{
					Print << U"Failed";
				}
			}
		}

		if (const size_t available = serial.available())
		{
			// シリアル通信で受信したデータを読み込んで表示
			auto data = serial.readBytes();
			Print << U"READ: " << Unicode::Widen(std::string(data.begin(), data.end()));
		}

	}
}

ESP32（M5Stack-atom）側

#include <Arduino.h>

#include "SimplePacket.hpp"

void setup()
{
  Serial.begin(115200);
  delay(2000);
}

void loop()
{

  if (Serial.available())
  {
    char buffer[256];
    size_t bufferSize = Serial.readBytesUntil(0x00, buffer, sizeof(buffer));
    std::vector<uint8_t> packet;

    for (size_t i = 0; i < bufferSize; ++i)
    {
      packet.push_back(static_cast<uint8_t>(buffer[i]));
    }

    for (const auto &byte : packet)
    {
      Serial.print(static_cast<uint8_t>(byte), HEX);
      Serial.print(' ');
    }
    Serial.println("");

    auto result = DecodePacket(packet);

    if (!result.has_value())
    {
      // Error occurred, handle it
      Serial.println("Error decoding packet");
    }
    else
    {
      // Use result
      auto data = result.value();

      // Extract the timestamp
      uint32_t timestamp = 0;
      for (size_t i = 0; i < sizeof(timestamp); i++)
      {
        timestamp |= (static_cast<uint32_t>(data[i]) << (i * 8));
      }

      // Extract the servo data
      std::vector<uint16_t> servoData;
      for (size_t i = sizeof(timestamp); i < data.size(); i += sizeof(uint16_t))
      {
        uint16_t servo = 0;
        for (size_t j = 0; j < sizeof(uint16_t); j++)
        {
          servo |= (static_cast<uint16_t>(data[i + j]) << (j * 8));
        }
        servoData.push_back(servo);
      }

      // Now you can use servoData and timestamp...
      Serial.println("Packet decoded:");
      Serial.print("Timestamp: ");
      Serial.println(timestamp);
      Serial.println("Servo data:");

      for (const auto &servo : servoData)
      {
        Serial.println(servo);
      }
    }
  }
}

注意事項

COBSは、連続するゼロ間の距離をカウントする方式を採用しています。したがって、COBSの許容する最大ブロックサイズは254バイトまでとなります。それを超えると、適切なエンコーディングやデコーディングが行えないため、それ以上のサイズのパケットを生成しないよう注意してください。
論文の詳細を確認した結果、先述の制限は存在しないことが分かりました。正確には、0xFFを適切に利用することで、254バイトという最大ブロックサイズを超えても、データを正確にエンコード・デコードすることが可能です。したがって、元の実装に基づき、254バイト以上の配列も適切に処理できるように修正を行いました。

参考資料