110 lines
2.5 KiB
Go
110 lines
2.5 KiB
Go
package main
|
|
|
|
import (
|
|
"context"
|
|
"flag"
|
|
"fmt"
|
|
"io"
|
|
"log"
|
|
"os"
|
|
"os/exec"
|
|
"os/signal"
|
|
"syscall"
|
|
|
|
"git.piksar.eu/piotr/groundstation/internal/protocol"
|
|
"git.piksar.eu/piotr/groundstation/internal/serialport"
|
|
)
|
|
|
|
func main() {
|
|
serialPath := flag.String("serial", "/dev/ttyS0", "GX12 AUX1 UART device")
|
|
stdin := flag.Bool("stdin", false, "read framed data from standard input")
|
|
startVideo := flag.Bool("start-video", false, "start fpv-video with Ground Station")
|
|
flag.Parse()
|
|
|
|
if *stdin && *startVideo {
|
|
log.Fatal("-stdin is intended for protocol tests; omit -start-video")
|
|
}
|
|
|
|
ctx, stop := signal.NotifyContext(context.Background(), os.Interrupt, syscall.SIGTERM)
|
|
defer stop()
|
|
|
|
if *startVideo {
|
|
cmd := exec.CommandContext(ctx, "fpv-video")
|
|
cmd.Stdout, cmd.Stderr = os.Stdout, os.Stderr
|
|
if err := cmd.Start(); err != nil {
|
|
log.Fatalf("start fpv-video: %v", err)
|
|
}
|
|
defer func() { _ = cmd.Wait() }()
|
|
}
|
|
|
|
var input io.Reader = os.Stdin
|
|
var port *os.File
|
|
var err error
|
|
if !*stdin {
|
|
port, err = serialport.Open115200(*serialPath)
|
|
if err != nil {
|
|
log.Fatal(err)
|
|
}
|
|
defer port.Close()
|
|
input = port
|
|
}
|
|
|
|
if err := run(ctx, input, os.Stdout); err != nil && err != context.Canceled {
|
|
log.Fatal(err)
|
|
}
|
|
}
|
|
|
|
func run(ctx context.Context, input io.Reader, output io.Writer) error {
|
|
buf := make([]byte, protocol.RCSize)
|
|
for {
|
|
if err := readRCFrame(ctx, input, buf); err != nil {
|
|
return err
|
|
}
|
|
frame, err := protocol.DecodeRC(buf)
|
|
if err != nil {
|
|
log.Printf("discard frame: %v", err)
|
|
continue
|
|
}
|
|
fmt.Fprintf(output, "seq=%d t=%d ch1=%d ch2=%d ch3=%d ch4=%d aux1=%d aux2=%d\n",
|
|
frame.Sequence, frame.Timestamp,
|
|
frame.Channels[0], frame.Channels[1], frame.Channels[2], frame.Channels[3],
|
|
frame.Channels[4], frame.Channels[5])
|
|
}
|
|
}
|
|
|
|
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) {
|
|
select {
|
|
case <-ctx.Done():
|
|
return ctx.Err()
|
|
default:
|
|
}
|
|
n, err := input.Read(dst[read:])
|
|
read += n
|
|
if err != nil {
|
|
return err
|
|
}
|
|
}
|
|
return nil
|
|
}
|