diff --git a/main/protocol.c b/main/protocol.c index 8e53997..81d431c 100644 --- a/main/protocol.c +++ b/main/protocol.c @@ -35,6 +35,29 @@ static void encode_u16(uint16_t val, uint8_t *out) out[2] = p[1] & 0x7F; } +void encode_u32(uint32_t val, uint8_t *out) +{ + uint8_t *p = (uint8_t *)&val; + out[0] = ((p[0] >> 7) & 1) | ((p[1] >> 6) & 2) | + ((p[2] >> 5) & 4) | ((p[3] >> 4) & 8); + out[1] = p[0] & 0x7F; + out[2] = p[1] & 0x7F; + out[3] = p[2] & 0x7F; + out[4] = p[3] & 0x7F; +} + +uint32_t decode_u32(const uint8_t *d) +{ + uint8_t b[4]; + b[0] = d[1] | ((d[0] & 1) << 7); + b[1] = d[2] | ((d[0] & 2) << 6); + b[2] = d[3] | ((d[0] & 4) << 5); + b[3] = d[4] | ((d[0] & 8) << 4); + uint32_t v; + memcpy(&v, b, 4); + return v; +} + float decode_float(const uint8_t *d) { uint8_t b[4]; diff --git a/main/protocol.h b/main/protocol.h index 77dece0..c040c3d 100644 --- a/main/protocol.h +++ b/main/protocol.h @@ -106,7 +106,9 @@ int protocol_init(void); int protocol_recv_command(Command *cmd, uint32_t timeout_ms); void protocol_push_command(const Command *cmd); -/* 7-bit decode helpers */ +/* 7-bit encode/decode helpers */ +void encode_u32(uint32_t val, uint8_t *out); +uint32_t decode_u32(const uint8_t *d); float decode_float(const uint8_t *d); uint16_t decode_u16(const uint8_t *d);