Started a Go based Link experiment.

master
Michael Hope 9 years ago
parent f3aa55e688
commit 843a5f1f0b
  1. 235
      experiments/link.go

@ -0,0 +1,235 @@
package main
import (
"github.com/tarm/goserial"
"log"
"io"
"encoding/binary"
"bytes"
"time"
"fmt"
)
const (
Mark = '\n'
Escape = '^'
Xor = 0x20
)
type Link struct {
Received uint
Escaped uint
Overruns uint
CheckErrors uint
ShortFrame uint
Unrecognised uint
DecodeError uint
}
type Heartbeat struct {
Code uint8
Version uint8
DeviceId uint8
Ticks uint8
}
type Input struct {
Code uint8
Channels [6]int8
}
type Pong struct {
Code uint8
}
type Request struct {
Code uint8
Requested uint8
}
type Version struct {
Code uint8
Version [18]byte
}
type State struct {
Code uint8
Flags uint8
}
type Demand struct {
Code uint8
Flags uint8
Channels [6]int8
}
var now time.Time
func init() {
now = time.Now()
}
func checksum(frame []byte) byte {
var sum1, sum2 uint = 0x12, 0x34
for _, ch := range frame {
sum1 += uint(ch)
if sum1 >= 255 {
sum1 -= 255
}
sum2 += sum1
if sum2 >= 255 {
sum2 -= 255
}
}
return byte(sum1 ^ sum2)
}
func make_message(code byte) interface{} {
switch code {
case 'h':
return &Heartbeat{}
case 'i':
return &Input{}
case 'P':
return &Pong{}
case 'v':
return &Version{}
case 's':
return &State{}
default:
return nil
}
}
func (link *Link) dispatch(frame []byte) {
if len(frame) < 2 {
// Need at least the code and checksum.
link.ShortFrame += 1
log.Println("err: shortframe")
} else {
// log.Println("frame:", frame)
length := len(frame) - 1
sum := checksum(frame[:length])
code := frame[0]
msg := make_message(code)
switch {
case sum != frame[length]:
// Bad checksum.
link.CheckErrors += 1
log.Println("err: checksum", sum, frame[length])
case msg == nil:
log.Println("err: unrecognised", code)
link.Unrecognised += 1
default:
src := bytes.NewBuffer(frame[:length])
err := binary.Read(src, binary.LittleEndian, msg)
switch {
case err != nil:
link.DecodeError += 1
log.Println("err: decode error", err, binary.Size(msg), length)
case src.Len() != 0:
link.DecodeError += 1
log.Println("err: unused input while decoding.")
default:
log.Println(fmt.Sprintf("%.3f", time.Now().Sub(now).Seconds()), "ok", string(code), msg)
link.Received += 1
}
}
}
}
func (link *Link) receive(port io.ReadWriteCloser) {
got := make([]byte, 1)
frame := make([]byte, 0)
rx := make([]byte, 0)
var xor byte
for {
n, err := port.Read(got)
if err != nil {
log.Fatal(err)
}
// log.Println("got", got[:n])
for _, ch := range got[:n] {
rx = append(rx, ch)
switch ch {
case Mark:
// log.Println("rx", len(rx), rx)
link.dispatch(frame)
frame = make([]byte, 0)
rx = make([]byte, 0)
case Escape:
xor = Xor
link.Escaped += 1
default:
frame = append(frame, ch ^ xor)
xor = 0
if len(frame) > 100 {
link.Overruns += 1
log.Println("err: overrun")
frame = make([]byte, 0)
}
}
}
}
}
func (link *Link) Send(port io.ReadWriteCloser, msg interface{}) {
encoded := new(bytes.Buffer)
binary.Write(encoded, binary.LittleEndian, msg)
encoded.WriteByte(checksum(encoded.Bytes()))
log.Println("send", encoded.Bytes())
escaped := make([]byte, 0)
for _, ch := range encoded.Bytes() {
switch ch {
case Mark, Escape:
escaped = append(escaped, Escape)
escaped = append(escaped, ch ^ Xor)
default:
escaped = append(escaped, ch)
}
}
escaped = append(escaped, Mark)
port.Write(escaped)
}
func main() {
c := &serial.Config{Name: "/dev/ttyO4", Baud: 38400}
s, err := serial.OpenPort(c)
if err != nil {
log.Fatal(err)
}
link := &Link{}
link.receive(s)
// var v int8 = -50
// for {
// time.Sleep(50 * time.Millisecond)
// msg := new(Demand)
// msg.Code = 'd'
// msg.Flags = 1
// msg.Channels[2] = 0
// msg.Channels[0] = v
// v += 1
// if v == 50 {
// v = -50
// }
// link.Send(s, msg)
// // link.Send(s, &Request{'R', 'p'})
// }
}
Loading…
Cancel
Save