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}, } } }