Big Gotool Refactoring
- Added Event Bus - Reworked Package Parsing - Rewokred Frame Parsing
This commit is contained in:
@@ -0,0 +1,101 @@
|
||||
package uart
|
||||
|
||||
import (
|
||||
"context"
|
||||
"log"
|
||||
"time"
|
||||
|
||||
"alox.tool/api"
|
||||
"alox.tool/eventbus"
|
||||
"go.bug.st/serial"
|
||||
)
|
||||
|
||||
type Com struct {
|
||||
bus eventbus.EventBus
|
||||
port serial.Port
|
||||
cancel context.CancelFunc
|
||||
}
|
||||
|
||||
func Connect(bus eventbus.EventBus, portName string, baudrate int) (*Com, error) {
|
||||
mode := &serial.Mode{BaudRate: baudrate}
|
||||
port, err := serial.Open(portName, mode)
|
||||
if err != nil {
|
||||
return nil, err
|
||||
}
|
||||
|
||||
ctx, cancel := context.WithCancel(context.Background())
|
||||
drv := New(bus)
|
||||
|
||||
go func() {
|
||||
buff := make([]byte, 1024)
|
||||
for {
|
||||
select {
|
||||
case <-ctx.Done():
|
||||
return
|
||||
default:
|
||||
n, err := port.Read(buff)
|
||||
if err != nil {
|
||||
log.Print("Read Error:", err)
|
||||
return // Loop beenden bei Hardware-Fehler
|
||||
}
|
||||
if n > 0 {
|
||||
for _, b := range buff[:n] {
|
||||
log.Printf("[RAW][RX] % X", b)
|
||||
drv.ParseByte(b)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}()
|
||||
|
||||
return &Com{
|
||||
bus: bus,
|
||||
port: port,
|
||||
cancel: cancel,
|
||||
}, nil
|
||||
}
|
||||
|
||||
func (c *Com) Close() {
|
||||
c.cancel()
|
||||
c.port.Close()
|
||||
}
|
||||
|
||||
func packFrame(id byte, payload []byte) []byte {
|
||||
out := make([]byte, 0, len(payload)+5) // Guessing extra Puffer size
|
||||
checksum := id
|
||||
|
||||
out = append(out, StartByte)
|
||||
|
||||
// Helper für Escaping
|
||||
writeEscaped := func(b byte) {
|
||||
if b == StartByte || b == EscapeByte || b == EndByte {
|
||||
out = append(out, EscapeByte)
|
||||
}
|
||||
out = append(out, b)
|
||||
}
|
||||
|
||||
writeEscaped(id)
|
||||
for _, b := range payload {
|
||||
writeEscaped(b)
|
||||
checksum ^= b
|
||||
}
|
||||
|
||||
writeEscaped(checksum)
|
||||
out = append(out, EndByte)
|
||||
return out
|
||||
}
|
||||
|
||||
func (c *Com) Send(id byte, payload []byte) error {
|
||||
raw := packFrame(id, payload)
|
||||
|
||||
log.Printf("RAW: % X", raw)
|
||||
_, err := c.port.Write(raw)
|
||||
|
||||
c.bus.Publish(api.TopicUARTTx, api.Frame{
|
||||
Time: uint64(time.Now().UnixNano()),
|
||||
ID: id,
|
||||
Data: payload,
|
||||
})
|
||||
|
||||
return err
|
||||
}
|
||||
@@ -0,0 +1,109 @@
|
||||
package uart
|
||||
|
||||
import (
|
||||
"encoding/binary"
|
||||
"fmt"
|
||||
"log"
|
||||
"math"
|
||||
|
||||
"alox.tool/api"
|
||||
)
|
||||
|
||||
func ParseFrameVersion(frame api.Frame) (api.PayloadVersion, error) {
|
||||
if len(frame.Data) != 10 {
|
||||
return api.PayloadVersion{}, fmt.Errorf("payload wrong size: got %d bytes, want 10", len(frame.Data))
|
||||
}
|
||||
|
||||
v := api.PayloadVersion{
|
||||
Version: binary.LittleEndian.Uint16(frame.Data[0:2]),
|
||||
Buildhash: [7]uint8(frame.Data[2:10])}
|
||||
return v, nil
|
||||
}
|
||||
|
||||
func parseFrameClientInfoPart(data []byte) (api.PayloadClientInfo, error) {
|
||||
if len(data) != 19 {
|
||||
return api.PayloadClientInfo{}, fmt.Errorf("payload wrong size: got %d bytes, want 19", len(data))
|
||||
}
|
||||
|
||||
v := api.PayloadClientInfo{
|
||||
ClientID: data[0],
|
||||
IsAvailable: data[1],
|
||||
SlotIsUsed: data[2],
|
||||
MACAddr: [6]uint8(data[3:9]),
|
||||
LastPing: binary.LittleEndian.Uint32(data[9:13]),
|
||||
LastSuccessfulPing: binary.LittleEndian.Uint32(data[13:17]),
|
||||
Version: binary.LittleEndian.Uint16(data[17:19]),
|
||||
}
|
||||
|
||||
return v, nil
|
||||
}
|
||||
|
||||
func ParseFrameClientInfo(frame api.Frame) ([]api.PayloadClientInfo, error) {
|
||||
if len(frame.Data) == 0 {
|
||||
return nil, fmt.Errorf("empty frame data")
|
||||
}
|
||||
|
||||
clientCount := int(frame.Data[0])
|
||||
|
||||
log.Printf("Clients %d", clientCount)
|
||||
|
||||
expectedLen := 1 + (clientCount * 19)
|
||||
|
||||
if len(frame.Data) < expectedLen {
|
||||
return nil, fmt.Errorf("frame data too short: got %d, want %d", len(frame.Data), expectedLen)
|
||||
}
|
||||
|
||||
clientList := make([]api.PayloadClientInfo, 0, clientCount)
|
||||
|
||||
for i := 0; i < clientCount; i++ {
|
||||
start := 1 + (i * 19)
|
||||
end := start + 19
|
||||
|
||||
client, err := parseFrameClientInfoPart(frame.Data[start:end])
|
||||
if err != nil {
|
||||
log.Printf("Could not parse client part %d: %v", i, err)
|
||||
continue
|
||||
}
|
||||
clientList = append(clientList, client)
|
||||
}
|
||||
|
||||
return clientList, nil
|
||||
}
|
||||
|
||||
func parse_uart_client_input(payloadBuffer []byte, payload_len int) {
|
||||
|
||||
clientCount := payloadBuffer[1]
|
||||
fmt.Printf("Client Count %d\n", clientCount)
|
||||
clientInputLen := 13
|
||||
|
||||
for i := 0; i < int(clientCount); i++ {
|
||||
offset := 2 + (i * clientInputLen)
|
||||
|
||||
// --- Client ID (uint8) ---
|
||||
clientID := payloadBuffer[offset]
|
||||
offset += 1
|
||||
|
||||
fmt.Printf("Client: %d\n", clientID)
|
||||
|
||||
// --- Lage X (float32) ---
|
||||
xBits := binary.LittleEndian.Uint32(payloadBuffer[offset : offset+4])
|
||||
lageX := math.Float32frombits(xBits)
|
||||
offset += 4
|
||||
|
||||
fmt.Printf("\tLAGE_X: %f\n", lageX)
|
||||
|
||||
// --- Lage Y (float32) ---
|
||||
yBits := binary.LittleEndian.Uint32(payloadBuffer[offset : offset+4])
|
||||
lageY := math.Float32frombits(yBits)
|
||||
offset += 4
|
||||
|
||||
fmt.Printf("\tLAGE_Y: %f\n", lageY)
|
||||
|
||||
// --- Bitmask (int32) ---
|
||||
maskBits := binary.LittleEndian.Uint32(payloadBuffer[offset : offset+4])
|
||||
bitmask := uint32(maskBits)
|
||||
offset += 4
|
||||
|
||||
fmt.Printf("\tBITMASK: %032b\n", bitmask)
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,128 @@
|
||||
package uart
|
||||
|
||||
import (
|
||||
"fmt"
|
||||
"time"
|
||||
|
||||
"alox.tool/api"
|
||||
"alox.tool/eventbus"
|
||||
)
|
||||
|
||||
const (
|
||||
StartByte = 0xAA
|
||||
EscapeByte = 0xBB
|
||||
EndByte = 0xCC
|
||||
)
|
||||
|
||||
type parserState int
|
||||
|
||||
const (
|
||||
stateWaitingForStart parserState = iota
|
||||
stateGetID
|
||||
stateEscapedID
|
||||
stateInPayload
|
||||
stateEscapedPayload
|
||||
)
|
||||
|
||||
type Parser struct {
|
||||
bus eventbus.EventBus
|
||||
state parserState
|
||||
parsedData []byte
|
||||
rawCapture []byte
|
||||
checksum byte
|
||||
}
|
||||
|
||||
func New(bus eventbus.EventBus) *Parser {
|
||||
return &Parser{
|
||||
bus: bus,
|
||||
state: stateWaitingForStart,
|
||||
parsedData: make([]byte, 0, 1024*4),
|
||||
rawCapture: make([]byte, 0, 1024*4),
|
||||
}
|
||||
}
|
||||
|
||||
func (p *Parser) reset() {
|
||||
p.state = stateWaitingForStart
|
||||
p.parsedData = p.parsedData[:0]
|
||||
p.rawCapture = p.rawCapture[:0]
|
||||
p.checksum = 0
|
||||
}
|
||||
|
||||
func (p *Parser) pushError(reason string) {
|
||||
// Throw Error on the Bus befor resetting
|
||||
p.bus.Publish(api.TopicUARTError, fmt.Errorf("%s: %02X", reason, p.rawCapture))
|
||||
p.reset()
|
||||
}
|
||||
|
||||
func (p *Parser) emitFrame() {
|
||||
if len(p.parsedData) == 0 {
|
||||
p.reset()
|
||||
return
|
||||
}
|
||||
|
||||
// Copy Data for Message Frame
|
||||
dataCopy := make([]byte, len(p.parsedData)-1) // Exclude ID
|
||||
copy(dataCopy, p.parsedData[1:])
|
||||
|
||||
f := api.Frame{
|
||||
Time: uint64(time.Now().UnixNano()),
|
||||
ID: p.parsedData[0],
|
||||
Data: dataCopy,
|
||||
}
|
||||
|
||||
p.bus.Publish(api.TopicUARTRx, f)
|
||||
p.reset()
|
||||
}
|
||||
|
||||
func (p *Parser) addByte(b byte) {
|
||||
p.parsedData = append(p.parsedData, b)
|
||||
p.checksum ^= b
|
||||
}
|
||||
|
||||
func (p *Parser) ParseByte(b byte) {
|
||||
p.rawCapture = append(p.rawCapture, b)
|
||||
|
||||
switch p.state {
|
||||
case stateWaitingForStart:
|
||||
if b == StartByte {
|
||||
p.reset()
|
||||
p.rawCapture = append(p.rawCapture, b) // Start Byte behalten
|
||||
p.state = stateGetID
|
||||
}
|
||||
|
||||
case stateGetID:
|
||||
if b == EscapeByte {
|
||||
p.state = stateEscapedID
|
||||
} else {
|
||||
p.addByte(b)
|
||||
p.state = stateInPayload
|
||||
}
|
||||
|
||||
case stateEscapedID:
|
||||
p.addByte(b)
|
||||
p.state = stateInPayload
|
||||
|
||||
case stateInPayload:
|
||||
if b == EscapeByte {
|
||||
p.state = stateEscapedPayload
|
||||
return
|
||||
}
|
||||
if b == StartByte {
|
||||
p.pushError("unexpected start byte")
|
||||
return
|
||||
}
|
||||
if b == EndByte {
|
||||
if p.checksum != 0 {
|
||||
p.pushError("checksum mismatch")
|
||||
return
|
||||
}
|
||||
p.emitFrame()
|
||||
return
|
||||
}
|
||||
p.addByte(b)
|
||||
|
||||
case stateEscapedPayload:
|
||||
p.addByte(b)
|
||||
p.state = stateInPayload
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user