SPE-1784: New compressed (binary) gcode format integration

Added MeatPack encoding to GCode Block
This commit is contained in:
enricoturri1966 2023-07-24 12:37:03 +02:00
parent 8bb6224ba8
commit 6331077645
3 changed files with 479 additions and 41 deletions

View File

@ -12,7 +12,429 @@
namespace BinaryGCode {
static size_t g_checksum_max_cache_size = 65536;
static constexpr size_t MAX_GCODE_CACHE_SIZE = 65536;
static constexpr const size_t MAX_GCODE_CACHE_SIZE = 65536;
namespace MeatPack {
static constexpr const uint8_t Command_None{ 0 };
//#Command_TogglePacking = 253 -- Currently unused, byte 253 can be reused later.
static constexpr const uint8_t Command_EnablePacking{ 251 };
static constexpr const uint8_t Command_DisablePacking{ 250 };
static constexpr const uint8_t Command_ResetAll{ 249 };
static constexpr const uint8_t Command_QueryConfig{ 248 };
static constexpr const uint8_t Command_EnableNoSpaces{ 247 };
static constexpr const uint8_t Command_DisableNoSpaces{ 246 };
static constexpr const uint8_t Command_SignalByte{ 0xFF };
static constexpr const uint8_t Flag_OmitWhitespaces{ 0x01 };
static constexpr const uint8_t Flag_RemoveComments{ 0x02 };
static constexpr const uint8_t BothUnpackable{ 0b11111111 };
static constexpr const char SpaceReplacedCharacter{ 'E' };
static const std::unordered_map<char, uint8_t> ReverseLookupTbl = {
{ '0', 0b00000000 },
{ '1', 0b00000001 },
{ '2', 0b00000010 },
{ '3', 0b00000011 },
{ '4', 0b00000100 },
{ '5', 0b00000101 },
{ '6', 0b00000110 },
{ '7', 0b00000111 },
{ '8', 0b00001000 },
{ '9', 0b00001001 },
{ '.', 0b00001010 },
{ ' ', 0b00001011 },
{ '\n', 0b00001100 },
{ 'G', 0b00001101 },
{ 'X', 0b00001110 },
{ '\0', 0b00001111 } // never used, 0b1111 is used to indicate the next 8-bits is a full character
};
class MPBinarizer
{
public:
explicit MPBinarizer(uint8_t flags = 0) : m_flags(flags) {}
void initialize(std::vector<uint8_t>& dst) {
initialize_lookup_tables();
append_command(Command_EnablePacking, dst);
m_binarizing = true;
}
void finalize(std::vector<uint8_t>& dst) {
if ((m_flags & Flag_RemoveComments) != 0) {
assert(m_binarizing);
append_command(Command_ResetAll, dst);
m_binarizing = false;
}
}
void binarize_line(const std::string& line, std::vector<uint8_t>& dst) {
auto unified_method = [this](const std::string& line) {
const std::string::size_type g_idx = line.find('G');
if (g_idx != std::string::npos) {
if (g_idx + 1 < line.size() && line[g_idx + 1] >= '0' && line[g_idx + 1] <= '9') {
if ((m_flags & Flag_OmitWhitespaces) != 0) {
std::string result = line;
std::replace(result.begin(), result.end(), 'e', 'E');
std::replace(result.begin(), result.end(), 'x', 'X');
std::replace(result.begin(), result.end(), 'g', 'G');
result.erase(std::remove(result.begin(), result.end(), ' '), result.end());
if (result.find('*') != std::string::npos) {
size_t checksum = 0;
result.erase(std::remove(result.begin(), result.end(), '*'), result.end());
for (size_t i = 0; i < result.size(); ++i) {
checksum ^= static_cast<uint8_t>(result[i]);
}
result += "*" + std::to_string(checksum);
}
result += '\n';
return result;
}
else {
std::string result = line;
std::replace(result.begin(), result.end(), 'x', 'X');
std::replace(result.begin(), result.end(), 'g', 'G');
result.erase(std::remove(result.begin(), result.end(), ' '), result.end());
if (result.find('*') != std::string::npos) {
size_t checksum = 0;
result.erase(std::remove(result.begin(), result.end(), '*'), result.end());
for (size_t i = 0; i < result.size(); ++i) {
checksum ^= static_cast<uint8_t>(result[i]);
}
result += "*" + std::to_string(checksum);
}
result += '\n';
return result;
}
}
}
return line;
};
auto is_packable = [this](char c) {
return (s_lookup_tables.packable[static_cast<uint8_t>(c)] != 0);
};
auto pack_chars = [this](char low, char high) {
return (((s_lookup_tables.value[static_cast<uint8_t>(high)] & 0xF) << 4) |
(s_lookup_tables.value[static_cast<uint8_t>(low)] & 0xF));
};
if (!line.empty()) {
if ((m_flags & Flag_RemoveComments) == 0) {
if (line[0] == ';') {
if (m_binarizing) {
append_command(Command_DisablePacking, dst);
m_binarizing = false;
}
dst.insert(dst.end(), line.begin(), line.end());
return;
}
}
if (line[0] == ';' ||
line[0] == '\n' ||
line[0] == '\r' ||
line.size() < 2)
return;
std::string modifiedLine = line.substr(0, line.find(';'));
if (modifiedLine.empty())
return;
auto trim_right = [](const std::string& str) {
if (str.back() != ' ')
return str;
auto bit = str.rbegin();
while (bit != str.rend() && *bit == ' ') {
++bit;
}
return str.substr(0, std::distance(str.begin(), bit.base()));
};
modifiedLine = trim_right(modifiedLine);
modifiedLine = unified_method(modifiedLine);
if (modifiedLine.back() != '\n')
modifiedLine.push_back('\n');
const size_t line_len = modifiedLine.size();
std::vector<uint8_t> temp_buffer;
temp_buffer.reserve(line_len);
for (size_t line_idx = 0; line_idx < line_len; line_idx += 2) {
const bool skip_last = line_idx == (line_len - 1);
const char char_1 = modifiedLine[line_idx];
const char char_2 = (skip_last ? '\n' : modifiedLine[line_idx + 1]);
const bool c1_p = is_packable(char_1);
const bool c2_p = is_packable(char_2);
if (c1_p) {
if (c2_p)
temp_buffer.emplace_back(static_cast<uint8_t>(pack_chars(char_1, char_2)));
else {
temp_buffer.emplace_back(static_cast<uint8_t>(pack_chars(char_1, '\0')));
temp_buffer.emplace_back(static_cast<uint8_t>(char_2));
}
}
else {
if (c2_p) {
temp_buffer.emplace_back(static_cast<uint8_t>(pack_chars('\0', char_2)));
temp_buffer.emplace_back(static_cast<uint8_t>(char_1));
}
else {
temp_buffer.emplace_back(static_cast<uint8_t>(BothUnpackable));
temp_buffer.emplace_back(static_cast<uint8_t>(char_1));
temp_buffer.emplace_back(static_cast<uint8_t>(char_2));
}
}
}
if (!m_binarizing && !temp_buffer.empty()) {
append_command(Command_EnablePacking, dst);
m_binarizing = true;
}
dst.insert(dst.end(), temp_buffer.begin(), temp_buffer.end());
}
}
private:
unsigned char m_flags{ 0 };
bool m_binarizing{ false };
struct LookupTables
{
std::array<uint8_t, 256> packable;
std::array<uint8_t, 256> value;
bool initialized;
unsigned char flags;
};
static LookupTables s_lookup_tables;
void append_command(unsigned char cmd, std::vector<uint8_t>& dst) {
dst.emplace_back(Command_SignalByte);
dst.emplace_back(Command_SignalByte);
dst.emplace_back(cmd);
}
void initialize_lookup_tables() {
if (s_lookup_tables.initialized && m_flags == s_lookup_tables.flags)
return;
for (const auto& [c, value] : ReverseLookupTbl) {
const int index = static_cast<int>(c);
s_lookup_tables.packable[index] = 1;
s_lookup_tables.value[index] = value;
}
if ((m_flags & Flag_OmitWhitespaces) != 0) {
s_lookup_tables.value[static_cast<uint8_t>(SpaceReplacedCharacter)] = ReverseLookupTbl.at(' ');
s_lookup_tables.packable[static_cast<uint8_t>(SpaceReplacedCharacter)] = 1;
s_lookup_tables.packable[static_cast<uint8_t>(' ')] = 0;
}
else {
s_lookup_tables.packable[static_cast<uint8_t>(SpaceReplacedCharacter)] = 0;
s_lookup_tables.packable[static_cast<uint8_t>(' ')] = 1;
}
s_lookup_tables.initialized = true;
s_lookup_tables.flags = m_flags;
}
};
MPBinarizer::LookupTables MPBinarizer::s_lookup_tables = { { 0 }, { 0 }, false, 0 };
static constexpr const unsigned char SecondNotPacked{ 0b11110000 };
static constexpr const unsigned char FirstNotPacked{ 0b00001111 };
static constexpr const unsigned char NextPackedFirst{ 0b00000001 };
static constexpr const unsigned char NextPackedSecond{ 0b00000010 };
// See for reference: https://github.com/scottmudge/Prusa-Firmware-MeatPack/blob/MK3_sm_MeatPack/Firmware/meatpack.cpp
static void unbinarize(const std::vector<uint8_t>& src, std::string& dst) {
bool unbinarizing = false;
bool cmd_active = false; // Is a command pending
uint8_t char_buf = 0; // Buffers a character if dealing with out-of-sequence pairs
size_t cmd_count = 0; // Counts how many command bytes are received (need 2)
size_t full_char_queue = 0; // Counts how many full-width characters are to be received
std::array<uint8_t, 2> char_out_buf; // Output buffer for caching up to 2 characters
size_t char_out_count = 0; // Stores number of characters to be read out
auto handle_command = [&](uint8_t c) {
switch (c)
{
case Command_EnablePacking: { unbinarizing = true; break; }
case Command_DisablePacking: { unbinarizing = false; break; }
case Command_ResetAll: { unbinarizing = false; break; }
default:
case Command_QueryConfig: { break; }
}
};
auto handle_output_char = [&](uint8_t c) {
char_out_buf[char_out_count++] = c;
};
auto get_char = [](uint8_t c) {
switch (c)
{
case 0b0000: { return '0'; }
case 0b0001: { return '1'; }
case 0b0010: { return '2'; }
case 0b0011: { return '3'; }
case 0b0100: { return '4'; }
case 0b0101: { return '5'; }
case 0b0110: { return '6'; }
case 0b0111: { return '7'; }
case 0b1000: { return '8'; }
case 0b1001: { return '9'; }
case 0b1010: { return '.'; }
case 0b1011: { return 'E'; }
case 0b1100: { return '\n'; }
case 0b1101: { return 'G'; }
case 0b1110: { return 'X'; }
}
return '\0';
};
auto unpack_chars = [&](uint8_t pk, std::array<uint8_t, 2>& chars_out) {
uint8_t out = 0;
// If lower 4 bytes is 0b1111, the higher 4 are unused, and next char is full.
if ((pk & FirstNotPacked) == FirstNotPacked)
out |= NextPackedFirst;
else
chars_out[0] = get_char(pk & 0xF); // Assign lower char
// Check if upper 4 bytes is 0b1111... if so, we don't need the second char.
if ((pk & SecondNotPacked) == SecondNotPacked)
out |= NextPackedSecond;
else
chars_out[1] = get_char((pk >> 4) & 0xF); // Assign upper char
return out;
};
auto handle_rx_char = [&](uint8_t c) {
if (unbinarizing) {
if (full_char_queue > 0) {
handle_output_char(c);
if (char_buf > 0) {
handle_output_char(char_buf);
char_buf = 0;
}
--full_char_queue;
}
else {
std::array<uint8_t, 2> buf = { 0, 0 };
const uint8_t res = unpack_chars(c, buf);
if ((res & NextPackedFirst) != 0) {
++full_char_queue;
if ((res & NextPackedSecond) != 0)
++full_char_queue;
else
char_buf = buf[1];
}
else {
handle_output_char(buf[0]);
if (buf[0] != '\n') {
if ((res & NextPackedSecond) != 0)
++full_char_queue;
else
handle_output_char(buf[1]);
}
}
}
}
else // Packing not enabled, just copy character to output
handle_output_char(c);
};
auto get_result_char = [&](std::array<char, 2>& chars_out) {
if (char_out_count > 0) {
const size_t res = char_out_count;
for (uint8_t i = 0; i < char_out_count; ++i) {
chars_out[i] = (char)char_out_buf[i];
}
char_out_count = 0;
return res;
}
return (size_t)0;
};
std::vector<uint8_t> unbin_buffer(2 * src.size(), 0);
auto it_unbin_end = unbin_buffer.begin();
#if ENABLE_BINARIZED_GCODE_DEBUG
size_t line_start = 0;
#endif // ENABLE_BINARIZED_GCODE_DEBUG
bool add_space = false;
auto begin = src.begin();
auto end = src.end();
auto it_bin = begin;
while (it_bin != end) {
uint8_t c_bin = *it_bin;
if (c_bin == Command_SignalByte) {
if (cmd_count > 0) {
cmd_active = true;
cmd_count = 0;
}
else
++cmd_count;
}
else {
if (cmd_active) {
handle_command(c_bin);
cmd_active = false;
}
else {
if (cmd_count > 0) {
handle_rx_char(Command_SignalByte);
cmd_count = 0;
}
handle_rx_char(c_bin);
}
}
std::array<char, 2> c_unbin{ 0, 0 };
const size_t char_count = get_result_char(c_unbin);
for (size_t i = 0; i < char_count; ++i) {
// GCodeReader::parse_line_internal() is unable to parse a G line where the data are not separated by spaces
// so we add them where needed
if (c_unbin[i] == 'G' && std::distance(unbin_buffer.begin(), it_unbin_end) > 0 && *std::prev(it_unbin_end, 1) == '\n')
add_space = true;
else if (c_unbin[i] == '\n')
add_space = false;
if (add_space && *std::prev(it_unbin_end, 1) != ' ' &&
(c_unbin[i] == 'X' || c_unbin[i] == 'Y' || c_unbin[i] == 'Z' || c_unbin[i] == 'E' || c_unbin[i] == 'F')) {
*it_unbin_end = ' ';
++it_unbin_end;
}
if (c_unbin[i] != '\n' || std::distance(unbin_buffer.begin(), it_unbin_end) == 0 || *std::prev(it_unbin_end, 1) != '\n') {
*it_unbin_end = c_unbin[i];
++it_unbin_end;
}
#if ENABLE_BINARIZED_GCODE_DEBUG
if (c_unbin[i] == '\n') {
const std::string out(unbin_buffer.begin() + line_start, it_unbin_end);
if (!out.empty()) {
OutputDebugStringA(out.c_str());
line_start = std::distance(unbin_buffer.begin(), it_unbin_end);
}
}
#endif // ENABLE_BINARIZED_GCODE_DEBUG
}
++it_bin;
}
dst.insert(dst.end(), unbin_buffer.begin(), it_unbin_end);
}
} // namespace MeatPack
std::string translate_result(BinaryGCode::EResult result)
{
@ -52,7 +474,7 @@ static uint16_t block_types_count() { return 1 + (uint16_t)EBlockTyp
static uint16_t compression_types_count() { return 1 + (uint16_t)ECompressionType::None; }
static uint16_t thumbnail_formats_count() { return 1 + (uint16_t)EThumbnailFormat::QOI; }
static uint16_t metadata_encoding_types_count() { return 1 + (uint16_t)EMetadataEncodingType::INI; }
static uint16_t gcode_encoding_types_count() { return 1 + (uint16_t)EGCodeEncodingType::MeatPack; }
static uint16_t gcode_encoding_types_count() { return 1 + (uint16_t)EGCodeEncodingType::MeatPackComments; }
static bool write_to_file(FILE& file, const void* data, size_t data_size)
{
@ -85,24 +507,6 @@ static bool encode_metadata(const std::vector<std::pair<std::string, std::string
return true;
}
static bool encode_gcode(const std::string& src, std::vector<uint8_t>& dst, EGCodeEncodingType encoding_type)
{
switch (encoding_type)
{
case EGCodeEncodingType::None:
{
dst.insert(dst.end(), src.begin(), src.end());
break;
}
case EGCodeEncodingType::MeatPack:
{
// TODO
break;
}
}
return true;
}
static bool decode_metadata(const std::vector<uint8_t>& src, std::vector<std::pair<std::string, std::string>>& dst,
EMetadataEncodingType encoding_type)
{
@ -130,6 +534,39 @@ static bool decode_metadata(const std::vector<uint8_t>& src, std::vector<std::pa
return true;
}
static bool encode_gcode(const std::string& src, std::vector<uint8_t>& dst, EGCodeEncodingType encoding_type)
{
switch (encoding_type)
{
case EGCodeEncodingType::None:
{
dst.insert(dst.end(), src.begin(), src.end());
break;
}
case EGCodeEncodingType::MeatPack:
case EGCodeEncodingType::MeatPackComments:
{
uint8_t binarizer_flags = (encoding_type == EGCodeEncodingType::MeatPack) ? MeatPack::Flag_RemoveComments : 0;
binarizer_flags |= MeatPack::Flag_OmitWhitespaces;
MeatPack::MPBinarizer binarizer(binarizer_flags);
binarizer.initialize(dst);
auto begin_it = src.begin();
auto end_it = src.begin();
while (end_it != src.end()) {
while (end_it != src.end() && *end_it != '\n') {
++end_it;
}
const std::string line(begin_it, ++end_it);
binarizer.binarize_line(line, dst);
begin_it = end_it;
}
binarizer.finalize(dst);
break;
}
}
return true;
}
static bool decode_gcode(const std::vector<uint8_t>& src, std::string& dst, EGCodeEncodingType encoding_type)
{
switch (encoding_type)
@ -140,8 +577,9 @@ static bool decode_gcode(const std::vector<uint8_t>& src, std::string& dst, EGCo
break;
}
case EGCodeEncodingType::MeatPack:
case EGCodeEncodingType::MeatPackComments:
{
// TODO
MeatPack::unbinarize(src, dst);
break;
}
}
@ -837,13 +1275,12 @@ EResult Binarizer::initialize(FILE& file, const BinarizerConfig& config)
return EResult::Success;
}
static EResult write_gcode_block(FILE& file, const std::string& raw_data, EGCodeEncodingType encoding_type, ECompressionType compression_type,
EChecksumType checksum_type)
static EResult write_gcode_block(FILE& file, const std::string& raw_data, const BinarizerConfig& config)
{
GCodeBlock block;
block.encoding_type = (uint16_t)encoding_type;
block.encoding_type = (uint16_t)config.gcode_encoding;
block.raw_data = raw_data;
return block.write(file, compression_type, checksum_type);
return block.write(file, config.compression, config.checksum);
}
EResult Binarizer::append_gcode(const std::string& gcode)
@ -865,7 +1302,7 @@ EResult Binarizer::append_gcode(const std::string& gcode)
const size_t line_size = 1 + end_line_pos - begin_pos;
if (line_size + m_gcode_cache.length() > MAX_GCODE_CACHE_SIZE) {
if (!m_gcode_cache.empty()) {
const EResult res = write_gcode_block(*m_file, m_gcode_cache, m_config.gcode_encoding, m_config.compression, m_config.checksum);
const EResult res = write_gcode_block(*m_file, m_gcode_cache, m_config);
if (res != EResult::Success)
// propagate error
return res;
@ -891,7 +1328,7 @@ EResult Binarizer::finalize()
// save gcode cache, if not empty
if (!m_gcode_cache.empty()) {
const EResult res = write_gcode_block(*m_file, m_gcode_cache, m_config.gcode_encoding, m_config.compression, m_config.checksum);
const EResult res = write_gcode_block(*m_file, m_gcode_cache, m_config);
if (res != EResult::Success)
// propagate error
return res;

View File

@ -202,6 +202,7 @@ enum class EGCodeEncodingType : uint16_t
{
None,
MeatPack,
MeatPackComments
};
struct GCodeBlock
@ -257,7 +258,7 @@ struct BinarizerConfig
ECompressionType compression{ ECompressionType::None };
EGCodeEncodingType gcode_encoding{ EGCodeEncodingType::None };
EMetadataEncodingType metadata_encoding{ EMetadataEncodingType::INI };
EChecksumType checksum{ EChecksumType::None };
EChecksumType checksum{ EChecksumType::CRC32 };
};
class Binarizer

View File

@ -7885,36 +7885,36 @@ void GLCanvas3D::show_binary_gcode_debug_window()
imgui.text_colored(ImGuiWrapper::COL_ORANGE_LIGHT, "Compression");
ImGui::TableSetColumnIndex(1);
const std::vector<std::string> gcode_compressions = { "None" };
int compression = (int)binarizer_config.compression;
if (imgui.combo(std::string("##1"), gcode_compressions, compression, ImGuiComboFlags_HeightLargest, 0.0f, 100.0f))
binarizer_config.compression = (BinaryGCode::ECompressionType)compression;
int compressions_id = (int)binarizer_config.compression;
if (imgui.combo(std::string("##1"), gcode_compressions, compressions_id, ImGuiComboFlags_HeightLargest, 0.0f, 200.0f))
binarizer_config.compression = (BinaryGCode::ECompressionType)compressions_id;
ImGui::TableNextRow();
ImGui::TableSetColumnIndex(0);
imgui.text_colored(ImGuiWrapper::COL_ORANGE_LIGHT, "GGcode encoding");
ImGui::TableSetColumnIndex(1);
const std::vector<std::string> gcode_encodings = { "None", "MeatPack" };
int gcode_encoding = (int)binarizer_config.gcode_encoding;
if (imgui.combo(std::string("##2"), gcode_encodings, gcode_encoding, ImGuiComboFlags_HeightLargest, 0.0f, 100.0f))
binarizer_config.gcode_encoding = (BinaryGCode::EGCodeEncodingType)gcode_encoding;
const std::vector<std::string> gcode_encodings = { "None", "MeatPack", "MeatPack Comments"};
int gcode_encodings_id = (int)binarizer_config.gcode_encoding;
if (imgui.combo(std::string("##2"), gcode_encodings, gcode_encodings_id, ImGuiComboFlags_HeightLargest, 0.0f, 200.0f))
binarizer_config.gcode_encoding = (BinaryGCode::EGCodeEncodingType)gcode_encodings_id;
ImGui::TableNextRow();
ImGui::TableSetColumnIndex(0);
imgui.text_colored(ImGuiWrapper::COL_ORANGE_LIGHT, "Metadata encoding");
ImGui::TableSetColumnIndex(1);
const std::vector<std::string> metadata_encodings = { "INI" };
int metadata_encoding = (int)binarizer_config.metadata_encoding;
if (imgui.combo(std::string("##3"), metadata_encodings, metadata_encoding, ImGuiComboFlags_HeightLargest, 0.0f, 100.0f))
binarizer_config.metadata_encoding = (BinaryGCode::EMetadataEncodingType)metadata_encoding;
int metadata_encodings_id = (int)binarizer_config.metadata_encoding;
if (imgui.combo(std::string("##3"), metadata_encodings, metadata_encodings_id, ImGuiComboFlags_HeightLargest, 0.0f, 200.0f))
binarizer_config.metadata_encoding = (BinaryGCode::EMetadataEncodingType)metadata_encodings_id;
ImGui::TableNextRow();
ImGui::TableSetColumnIndex(0);
imgui.text_colored(ImGuiWrapper::COL_ORANGE_LIGHT, "Checksum type");
ImGui::TableSetColumnIndex(1);
const std::vector<std::string> checksums = { "None", "CRC32" };
int checksum = (int)binarizer_config.checksum;
if (imgui.combo(std::string("##4"), checksums, checksum, ImGuiComboFlags_HeightLargest, 0.0f, 100.0f))
binarizer_config.checksum = (BinaryGCode::EChecksumType)checksum;
int checksums_id = (int)binarizer_config.checksum;
if (imgui.combo(std::string("##4"), checksums, checksums_id, ImGuiComboFlags_HeightLargest, 0.0f, 200.0f))
binarizer_config.checksum = (BinaryGCode::EChecksumType)checksums_id;
ImGui::EndTable();