Example #1
0
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},
	}
}
Example #2
0
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},
		}
	}
}