From d9a9f7028fd52cab8ebb075e169599312683ddd9 Mon Sep 17 00:00:00 2001 From: Jeff Niu Date: Sat, 28 Oct 2017 14:41:33 -0700 Subject: [PATCH] Added binary encode/decode libs --- lib/.DS_Store | Bin 0 -> 6148 bytes lib/wbinary/wbinary.go | 249 ++++++++++++++++++++++++++++++++++++ lib/wbinary/wbinary_test.go | 235 ++++++++++++++++++++++++++++++++++ lib/wjson/wjson.go | 41 ++++++ lib/wjson/wjson_test.go | 27 ++++ 5 files changed, 552 insertions(+) create mode 100644 lib/.DS_Store create mode 100644 lib/wbinary/wbinary.go create mode 100644 lib/wbinary/wbinary_test.go create mode 100644 lib/wjson/wjson.go create mode 100644 lib/wjson/wjson_test.go diff --git a/lib/.DS_Store b/lib/.DS_Store new file mode 100644 index 0000000000000000000000000000000000000000..5b3d72aa82bbebdacc664ff1c10c3bc6147a61e0 GIT binary patch literal 6148 zcmeHKJ5EDE3>-rck!VsaSXXSCUsPTXoylwW6+x0MsKJD<_OB}Gq6L$EP zKWteN7d(2#9s_n+Hr=Od$twoLfEW-1Vn7W1&VV;q%KR!(Rt$&%G4REJe;*pvu@{bs z@#&z65r8wl q>qw!Ac1(>j=uY=4+n!!Z9(@>5p`xeg>Qui5U2620j6D6&o@D literal 0 HcmV?d00001 diff --git a/lib/wbinary/wbinary.go b/lib/wbinary/wbinary.go new file mode 100644 index 0000000..a003238 --- /dev/null +++ b/lib/wbinary/wbinary.go @@ -0,0 +1,249 @@ +package wbinary + +import ( + "strconv" + "math" +) + +// Packet type +type PacketType uint8 + +// Packet type enums +const ( + Sensor PacketType = iota // 00 + Command // 01 + State // 10 + Log // 11 +) + +func TypeToString(pType PacketType) string { + switch pType { + case Sensor: + return "sensor" + case Command: + return "command" + case State: + return "state" + case Log: + return "log" + default: + return "unknown" + } +} + +func StringToType(pStr string) PacketType { + switch pStr { + case "sensor": + return Sensor + case "command": + return Command + case "state": + return State + case "log": + return Log + default: + return Command + } +} + +// Packet specification: +// [start:1][type:2][name:6][data1:18][data2:18][data3:18][end:1] +var packetStructure = []uint{1, 2, 6, 18, 18, 18, 1} + +// Communication packet type +type CommPacket struct { + integrity bool + PacketType PacketType + PacketName string + Data1 float32 + Data2 float32 + Data3 float32 +} + +// Return the bit at position i in a byte array. +// Returns 0 or a value greater than 0 +func ReadBit(buf []byte, i uint) byte { + return buf[i/8] & (1 << (i % 8)) +} + +func SetBit(buf []byte, i uint, bit byte) { + if bit > 0 { + buf[i/8] |= 1 << (i % 8) + } else { + buf[i/8] &= ^(1 << (i % 8)) + } +} + +// Reads no more than 32 bits at a time from +// a byte buffer between the start position i +// and end position j, inclusive +func ReadBits(buf []byte, i uint, j uint) uint32 { + if i > j { + j, i = i, j + } + var iStart = i / 8 + var jStart = j / 8 + var iMod = i % 8 + var jMod = j % 8 + if iStart >= uint(len(buf)) { + return 0 + } + if jStart >= uint(len(buf)) { + jStart = uint(len(buf) - 1) + jMod = 7 + } + if iStart == jStart { + return uint32((buf[iStart] >> iMod) & (0xff >> (7 - jMod + iMod))) + } + var res uint32 = 0 + var ptr = 8 - iMod + res |= uint32(buf[iStart] >> iMod) + iStart++ + for ; iStart < jStart; iStart++ { + res |= uint32(buf[iStart]) << ptr + ptr += 8 + } + res |= uint32(buf[jStart]&(0xff>>(7-jMod))) << ptr + return res +} + +func SetBits(buf []byte, bits uint32, i uint, length uint) { + bits &= 0xffffffff >> (0x20 - length) + var j = i + length - 1 + var iStart = i / 8 + var jStart = j / 8 + var iMod = i % 8 + var jMod = j % 8 + // Assume that the provided index and length are valid + if iStart == jStart { + val := byte(bits) + buf[iStart] &= ^((0xff >> (7 - jMod)) & (0xff << iMod)) + buf[iStart] |= val << iMod + return + } + var slice = byte(bits) + buf[iStart] &= ^(0xff >> iMod << iMod) + buf[iStart] |= slice << iMod + var bitPtr = 8 - iMod + iStart++ + for ; iStart < jStart; iStart++ { + slice = byte(bits >> bitPtr) + buf[iStart] = slice + bitPtr += 8 + } + buf[jStart] &= 0xff >> jMod << jMod + buf[jStart] |= byte(bits >> bitPtr) +} + +// Read segments of bits, no larger than +// 32 bits as defined by shape +func ReadSegments(buf []byte, shape []uint) []uint32 { + var shapeLen = uint(len(shape)) + var bufLen = uint(len(buf)) + var i uint = 0 + var k uint = 0 + var result = make([]uint32, shapeLen, shapeLen) + for i < bufLen*8 && k < shapeLen { + result[k] = ReadBits(buf, i, i+shape[k]-1) + i += shape[k] + k++ + } + return result +} + +func WriteSegments(buf []byte, shape []uint, values []uint32) { + var shapeLen = uint(len(shape)) + var bufLen = uint(len(buf)) + var k uint = 0 + var i uint = 0 + for i < bufLen*8 && k < shapeLen { + SetBits(buf, values[k], i, shape[k]) + i += shape[k] + k++ + } +} + +// Floating point specifications: +// [decimalPart:7][integerPart:10][sign:1] +func DecodeFloat18(bits uint32) float32 { + var decimalPart = uint8(bits & 0x7f) + var integerPart = uint16(bits & 0x01ff80 >> 7) + if decimalPart > 99 { + decimalPart = 99 // truncate decimal part + } + var result float32 = 0.0 + result += float32(decimalPart) / 100.0 + result += float32(integerPart) + if bits&0x020000 > 0 { + result *= -1 + } + return result +} + +func EncodeFloat18(val float32) uint32 { + var sign uint32 = 0 + if val < 0 { + sign = 1 + val *= -1 + } + var integerPartF = float32(math.Floor(float64(val))) + var decimalPartF = math.Floor(float64((val - integerPartF) * 100) + 0.5) + var integerPart = uint32(integerPartF) + var decimalPart = uint32(decimalPartF) + return (sign << 17) | (integerPart << 7) | decimalPart +} + +// Returns the packet name given a number +// between 0 and 63 +func GetPacketName(bits uint32) string { + // TODO implement the name mapping + return "sensor" + strconv.Itoa(int(bits)) +} + +func EncodePacketName(packetName string) uint32 { + val, err := strconv.Atoi(packetName[6:]) + if err != nil { + return 0 + } + return uint32(val) +} + +// Read the packet data as defined by the packet +// structure. Function will try to read as much +// data as possible. +func ReadPacket(buf []byte) *CommPacket { + var bitSegments = ReadSegments(buf, packetStructure) + var nSegments = len(bitSegments) + var packet CommPacket + packet.integrity = (len(buf) == 8) && (ReadBit(buf, 0) > 0) && (ReadBit(buf, 63) > 0) + if nSegments > 1 { + packet.PacketType = PacketType(bitSegments[1]) + } + if nSegments > 2 { + packet.PacketName = GetPacketName(bitSegments[2]) + } + if nSegments > 3 { + packet.Data1 = DecodeFloat18(bitSegments[3]) + } + if nSegments > 4 { + packet.Data2 = DecodeFloat18(bitSegments[4]) + } + if nSegments > 5 { + packet.Data3 = DecodeFloat18(bitSegments[5]) + } + return &packet +} + +func WritePacket(packet *CommPacket) []byte { + var bitSegments = make([]uint32, 7, 7) + bitSegments[0] = 1 + bitSegments[6] = 1 + bitSegments[1] = uint32(packet.PacketType) + bitSegments[2] = uint32(EncodePacketName(packet.PacketName)) + bitSegments[3] = EncodeFloat18(packet.Data1) + bitSegments[4] = EncodeFloat18(packet.Data2) + bitSegments[5] = EncodeFloat18(packet.Data3) + var buf = make([]byte, 8, 8) + WriteSegments(buf, packetStructure, bitSegments) + return buf +} diff --git a/lib/wbinary/wbinary_test.go b/lib/wbinary/wbinary_test.go new file mode 100644 index 0000000..8439328 --- /dev/null +++ b/lib/wbinary/wbinary_test.go @@ -0,0 +1,235 @@ +package wbinary + +import ( + "math" + "testing" +) + +func TestReadBit(t *testing.T) { + var buf = []byte{3, 12, 128, 99} + if ReadBit(buf, 0) < 1 { + t.Fail() + } + if ReadBit(buf, 1) < 1 { + t.Fail() + } + if ReadBit(buf, 7) != 0 { + t.Fail() + } + if ReadBit(buf, 9) != 0 { + t.Fail() + } + if ReadBit(buf, 10) < 1 { + t.Fail() + } + if ReadBit(buf, 23) < 1 { + t.Fail() + } + if ReadBit(buf, 25) < 1 { + t.Fail() + } + if ReadBit(buf, 31) != 0 { + t.Fail() + } +} + +func TestSetBit(t *testing.T) { + var buf = []byte{255, 255, 255, 255} + SetBits(buf, 629, 10, 10) + if ReadBits(buf, 10, 19) != 629 { + t.Fail() + } + buf = []byte{0, 0, 0, 0} + SetBits(buf, 629, 10, 10) + if ReadBits(buf, 10, 19) != 629 { + t.Fail() + } +} + +func TestReadBits(t *testing.T) { + var buf = []byte{3, 172, 128, 99} + for i := 1; i < 8; i++ { + if ReadBits(buf, 0, uint(i)) != 3 { + t.Fail() + } + } + if ReadBits(buf, 0, 0) != 1 { + t.Fail() + } + if ReadBits(buf, 1, 1) != 1 { + t.Fail() + } + if ReadBits(buf, 1, 7) != 1 { + t.Fail() + } + if ReadBits(buf, 1, 10) != 513 { + t.Fail() + } + if ReadBits(buf, 0, 13) != 11267 { + t.Fail() + } + if ReadBits(buf, 5, 15) != 1376 { + t.Fail() + } + if ReadBits(buf, 0, 31) != 1669377027 { + t.Fail() + } + if ReadBits(buf, 13, 30) != 203781 { + t.Fail() + } + if ReadBits(buf, 30, 13) != 203781 { + t.Fail() + } + if ReadBits(buf, 50, 51) != 0 { + t.Fail() + } + if ReadBits(buf, 28, 50) != 6 { + t.Fail() + } +} + +func TestReadSegments(t *testing.T) { + var buf = []byte{218, 73, 85, 117} + var shape = []uint{1, 5, 6, 6, 12, 2} + var result = ReadSegments(buf, shape) + var expected = []uint32{0, 13, 39, 20, 3413, 1} + if len(expected) != len(result) { + t.Errorf("Expected length %d but got %d", len(expected), len(result)) + t.Fail() + } + for i := 0; i < 6; i++ { + if result[i] != expected[i] { + t.Errorf("Expected %d but got %d", expected[i], result[i]) + t.Fail() + } + } +} + +func TestReadSegmentsUnderFlow(t *testing.T) { + var buf = []byte{218, 73, 85, 117} + var shape = []uint{1, 5, 6, 6, 9} + var result = ReadSegments(buf, shape) + var expected = []uint32{0, 13, 39, 20, 341} + if len(expected) != len(result) { + t.Errorf("Expected length %d but got %d", len(expected), len(result)) + t.Fail() + } + for i := 0; i < 5; i++ { + if result[i] != expected[i] { + t.Errorf("Expected %d but got %d", expected[i], result[i]) + t.Fail() + } + } +} + +func TestReadSegmentsOverFlow(t *testing.T) { + var buf = []byte{218, 73, 85, 117} + var shape = []uint{1, 5, 6, 6, 12, 12, 5} + var result = ReadSegments(buf, shape) + var expected = []uint32{0, 13, 39, 20, 3413, 1, 0} + if len(expected) != len(result) { + t.Errorf("Expected length %d but got %d", len(expected), len(result)) + t.Fail() + } + for i := 0; i < 7; i++ { + if result[i] != expected[i] { + t.Errorf("Expected %d but got %d", expected[i], result[i]) + t.Fail() + } + } +} + +func TestWriteSegments(t *testing.T) { + var buf = []byte{218, 73, 85, 117} + var shape = []uint{1, 5, 6, 6, 12, 2} + var result = ReadSegments(buf, shape) + var nbuf = []byte{0, 0, 0, 0} + WriteSegments(nbuf, shape, result) + for i := 0; i < 4; i++ { + if nbuf[i] != buf[i] { + t.Error("Expected %d but got %d at %d", buf[i], nbuf[i], i) + t.Fail() + } + } +} + +func TestEncodePacketName(t *testing.T) { + var bits uint32 = 252 + name := GetPacketName(bits) + if EncodePacketName(name) != bits { + t.Fail() + } +} + +func TestDecodeFloat18(t *testing.T) { + var result = DecodeFloat18(223862) + var expected float32 = -724.99 + if math.Abs(float64(result-expected)) > 0.00001 { + t.Errorf("Expected %.2f but go %.2f", expected, result) + t.Fail() + } + result = DecodeFloat18(108341) + expected = 846.53 + if math.Abs(float64(result-expected)) > 0.00001 { + t.Errorf("Expected %.2f but go %.2f", expected, result) + t.Fail() + } +} + +func TestEncodeFloat18(t *testing.T) { + if EncodeFloat18(-724.99) != 223843 { + t.Fail() + } + if EncodeFloat18(846.53) != 108341 { + t.Fail() + } +} + +func TestReadPacket(t *testing.T) { + var buf = []byte{181, 237, 212, 174, 57, 109, 167, 155} + var packet = ReadPacket(buf) + var expected = CommPacket{ + integrity: true, + PacketType: State, + PacketName: "sensor54", + Data1: -724.99, + Data2: 846.53, + Data3: 442.59, + } + if packet.integrity != expected.integrity { + t.Errorf("Expected integrity [%b] but got [%b]", expected.integrity, packet.integrity) + t.Fail() + } + if packet.PacketType != expected.PacketType { + t.Errorf("Expected packet type [%d] but got [%d]", expected.PacketType, packet.PacketType) + t.Fail() + } + if packet.PacketName != expected.PacketName { + t.Errorf("Expected packet name [%s] but got [%s]", expected.PacketName, packet.PacketName) + t.Fail() + } + if math.Abs(float64(packet.Data1-expected.Data1)) > 0.00001 { + t.Errorf("Expected Data1 [%.2f] but got [%.2f]", expected.Data1, packet.Data1) + t.Fail() + } + if math.Abs(float64(packet.Data2-expected.Data2)) > 0.00001 { + t.Errorf("Expected Data2 [%.2f] but got [%.2f]", expected.Data2, packet.Data2) + t.Fail() + } + if math.Abs(float64(packet.Data3-expected.Data3)) > 0.00001 { + t.Errorf("Expected Data3 [%.2f] but got [%.2f]", expected.Data3, packet.Data3) + t.Fail() + } +} + +func TestWritePacket(t *testing.T) { + var buf = []byte{181, 199, 212, 174, 57, 109, 167, 155} + var packet = ReadPacket(buf) + var nbuf = WritePacket(packet) + for i := 0; i < 8; i++ { + if nbuf[i] != buf[i] { + t.Errorf("Expected %d but got %d at %d", buf[i], nbuf[i], i) + t.Fail() + } + } +} diff --git a/lib/wjson/wjson.go b/lib/wjson/wjson.go new file mode 100644 index 0000000..f6a81e7 --- /dev/null +++ b/lib/wjson/wjson.go @@ -0,0 +1,41 @@ +package wjson + +import ( + wbin "github.com/mogball/wcomms/wbinary" + "time" + "encoding/json" +) + +type CommPacketJson struct { + Time int64 `json:"time"` + Type string `json:"type"` + Name string `json:"name"` + Data []float32 `json:"data"` +} + +func CurrentTimeMs() int64 { + return time.Now().UnixNano() / int64(time.Millisecond) +} + +func PacketEncodeJson(packet *wbin.CommPacket) ([]byte, error) { + packetJson := &CommPacketJson{ + Time: CurrentTimeMs(), + Type: wbin.TypeToString(packet.PacketType), + Name: packet.PacketName, + Data: []float32{packet.Data1, packet.Data2, packet.Data3}, + } + return json.Marshal(packetJson) +} + +func PacketDecodeJson(encoded []byte) (*wbin.CommPacket, error) { + packetJson := &CommPacketJson{} + err := json.Unmarshal(encoded, packetJson) + packet := &wbin.CommPacket{ + PacketType: wbin.StringToType(packetJson.Type), + PacketName: packetJson.Name, + Data1: packetJson.Data[0], + Data2: packetJson.Data[1], + Data3: packetJson.Data[2], + } + return packet, err +} diff --git a/lib/wjson/wjson_test.go b/lib/wjson/wjson_test.go new file mode 100644 index 0000000..f5281a7 --- /dev/null +++ b/lib/wjson/wjson_test.go @@ -0,0 +1,27 @@ +package wjson + +import ( + "testing" + "github.com/mogball/wcomms/wbinary" +) + +func TestPacketDecodeJson(t *testing.T) { + packet := &wbinary.CommPacket{ + PacketType: wbinary.State, + PacketName: "sensor12", + Data1: 556.67, + Data2: 420.69, + Data3: -123.45, + } + encoded, err := PacketEncodeJson(packet) + if err != nil { + panic(err) + } + npacket, err := PacketDecodeJson(encoded) + if err != nil { + panic(err) + } + if *packet != *npacket { + t.Fail() + } +}