func handlePacket(addr *net.UDPAddr, msg []byte, ch chan HamiltonMessage) {
serial := (uint16(addr.IP[14]) << 8) + uint16(addr.IP[15])
sstr := fmt.Sprintf("%04x", serial)
//First four bytes are echo ID
eid := uint32(msg[0]) + (uint32(msg[1]) << 8) + (uint32(msg[2]) << 16) + (uint32(msg[3]) << 24)
tablock.Lock()
lastid, ok := eidTable[addr.String()]
if !ok || lastid < eid {
eidTable[addr.String()] = eid
tablock.Unlock()
} else {
tablock.Unlock()
return
}
//Then
vx := int8(msg[4])
vy := int8(msg[5])
vz := int8(msg[6])
vax := int8(msg[7])
vay := int8(msg[8])
vaz := int8(msg[9])
temp := int32(uint32(msg[10]) + (uint32(msg[11] << 8)) + (uint32(msg[12] << 16)) + (uint32(msg[13] << 24)))
tF := (float64(temp)/10000.0)*1.8 + 32
tmSince2016 := uint32(uint32(msg[14]) + (uint32(msg[15] << 8)) + (uint32(msg[16] << 16)) + (uint32(msg[17] << 24)))
tm := uint64((uint64(tmSince2016*100) + EPOCH) * 1000000)
obj := map[string]interface{}{
"#": serial,
"A": []int8{vx, vy, vz},
"MA": []int8{vax, vay, vaz},
"T": temp,
"F": tF,
"W": tm,
}
po, err := bw.CreateMsgPackPayloadObject(bw.PONumHamiltonTelemetry, obj)
if err != nil {
panic(err)
}
ch <- HamiltonMessage{
sstr,
[]bw.PayloadObject{po},
}
}
func genDemoPackets(ch chan HamiltonMessage) {
for {
time.Sleep(2 * time.Second)
vx := int8(10)
vy := int8(20)
vz := int8(30)
sstr := "FFFF"
obj := map[string]interface{}{
"#": 0xFFFF,
"A": []int8{vx, vy, vz},
}
po, err := bw.CreateMsgPackPayloadObject(bw.PONumHamiltonTelemetry, obj)
if err != nil {
panic(err)
}
ch <- HamiltonMessage{
sstr,
[]bw.PayloadObject{po},
}
}
}