diff --git a/cmd/groundstation/main.go b/cmd/groundstation/main.go index 93ce94b..8372767 100644 --- a/cmd/groundstation/main.go +++ b/cmd/groundstation/main.go @@ -57,7 +57,7 @@ func main() { func run(ctx context.Context, input io.Reader, output io.Writer) error { buf := make([]byte, protocol.RCSize) for { - if err := readFull(ctx, input, buf); err != nil { + if err := readRCFrame(ctx, input, buf); err != nil { return err } frame, err := protocol.DecodeRC(buf) @@ -72,6 +72,25 @@ func run(ctx context.Context, input io.Reader, output io.Writer) error { } } +func readRCFrame(ctx context.Context, input io.Reader, dst []byte) error { + if len(dst) != protocol.RCSize { + return fmt.Errorf("invalid RC frame buffer size %d", len(dst)) + } + + var previous byte + for { + var current [1]byte + if err := readFull(ctx, input, current[:]); err != nil { + return err + } + if previous == protocol.Magic0 && current[0] == protocol.Magic1 { + dst[0], dst[1] = protocol.Magic0, protocol.Magic1 + return readFull(ctx, input, dst[2:]) + } + previous = current[0] + } +} + func readFull(ctx context.Context, input io.Reader, dst []byte) error { read := 0 for read < len(dst) { diff --git a/cmd/groundstation/main_test.go b/cmd/groundstation/main_test.go new file mode 100644 index 0000000..bf67f6d --- /dev/null +++ b/cmd/groundstation/main_test.go @@ -0,0 +1,25 @@ +package main + +import ( + "bytes" + "context" + "testing" + + "git.piksar.eu/piotr/groundstation/internal/protocol" +) + +func TestReadRCFrameResynchronizesAfterPartialFrame(t *testing.T) { + packet := protocol.EncodeRC(protocol.RCFrame{Sequence: 7}) + input := bytes.NewReader(append([]byte{0xaa, 0xbb, 'G'}, packet...)) + got := make([]byte, protocol.RCSize) + if err := readRCFrame(context.Background(), input, got); err != nil { + t.Fatal(err) + } + frame, err := protocol.DecodeRC(got) + if err != nil { + t.Fatal(err) + } + if frame.Sequence != 7 { + t.Fatalf("sequence=%d want 7", frame.Sequence) + } +}