87 lines
2.1 KiB
Go
87 lines
2.1 KiB
Go
package protocol
|
|
|
|
import (
|
|
"encoding/binary"
|
|
"errors"
|
|
"fmt"
|
|
)
|
|
|
|
const (
|
|
Magic0 byte = 'G'
|
|
Magic1 byte = 'S'
|
|
Version byte = 1
|
|
|
|
TypeRCChannels byte = 1
|
|
|
|
Channels = 16
|
|
Header = 10
|
|
CRCSize = 2
|
|
RCSize = Header + Channels*2 + CRCSize
|
|
)
|
|
|
|
var (
|
|
ErrFrameLength = errors.New("groundstation: invalid frame length")
|
|
ErrMagic = errors.New("groundstation: invalid frame magic")
|
|
ErrVersion = errors.New("groundstation: unsupported frame version")
|
|
ErrCRC = errors.New("groundstation: invalid frame crc")
|
|
ErrType = errors.New("groundstation: unexpected frame type")
|
|
)
|
|
|
|
type RCFrame struct {
|
|
Sequence uint16
|
|
Timestamp uint32
|
|
Channels [Channels]int16
|
|
}
|
|
|
|
func EncodeRC(frame RCFrame) []byte {
|
|
buf := make([]byte, RCSize)
|
|
buf[0], buf[1], buf[2], buf[3] = Magic0, Magic1, Version, TypeRCChannels
|
|
binary.LittleEndian.PutUint16(buf[4:6], frame.Sequence)
|
|
binary.LittleEndian.PutUint32(buf[6:10], frame.Timestamp)
|
|
for i, value := range frame.Channels {
|
|
binary.LittleEndian.PutUint16(buf[Header+i*2:Header+i*2+2], uint16(value))
|
|
}
|
|
binary.LittleEndian.PutUint16(buf[RCSize-CRCSize:], CRC16CCITT(buf[:RCSize-CRCSize]))
|
|
return buf
|
|
}
|
|
|
|
func DecodeRC(buf []byte) (RCFrame, error) {
|
|
var frame RCFrame
|
|
if len(buf) != RCSize {
|
|
return frame, ErrFrameLength
|
|
}
|
|
if buf[0] != Magic0 || buf[1] != Magic1 {
|
|
return frame, ErrMagic
|
|
}
|
|
if buf[2] != Version {
|
|
return frame, ErrVersion
|
|
}
|
|
if buf[3] != TypeRCChannels {
|
|
return frame, ErrType
|
|
}
|
|
if got, want := binary.LittleEndian.Uint16(buf[RCSize-CRCSize:]), CRC16CCITT(buf[:RCSize-CRCSize]); got != want {
|
|
return frame, fmt.Errorf("%w: got %04x want %04x", ErrCRC, got, want)
|
|
}
|
|
frame.Sequence = binary.LittleEndian.Uint16(buf[4:6])
|
|
frame.Timestamp = binary.LittleEndian.Uint32(buf[6:10])
|
|
for i := range frame.Channels {
|
|
frame.Channels[i] = int16(binary.LittleEndian.Uint16(buf[Header+i*2 : Header+i*2+2]))
|
|
}
|
|
return frame, nil
|
|
}
|
|
|
|
func CRC16CCITT(data []byte) uint16 {
|
|
crc := uint16(0xffff)
|
|
for _, b := range data {
|
|
crc ^= uint16(b) << 8
|
|
for bit := 0; bit < 8; bit++ {
|
|
if crc&0x8000 != 0 {
|
|
crc = (crc << 1) ^ 0x1021
|
|
} else {
|
|
crc <<= 1
|
|
}
|
|
}
|
|
}
|
|
return crc
|
|
}
|