diff --git a/Gopkg.lock b/Gopkg.lock index 3d9a1945..08c68c00 100644 --- a/Gopkg.lock +++ b/Gopkg.lock @@ -51,6 +51,12 @@ ] revision = "788214691384e85e345bff9fd5eeb046f5983594" +[[projects]] + branch = "master" + name = "github.com/go-daq/crc8" + packages = ["."] + revision = "380c22547098d3312cdeca99eba43cbbee549f77" + [[projects]] branch = "master" name = "github.com/hashicorp/errwrap" @@ -63,6 +69,12 @@ packages = ["."] revision = "83588e72410abfbe4df460eeb6f30841ae47d4c4" +[[projects]] + branch = "master" + name = "github.com/howeyc/crc16" + packages = ["."] + revision = "2b2a61e366a66d3efb279e46176e7291001e0354" + [[projects]] branch = "master" name = "github.com/hybridgroup/go-ardrone" @@ -195,33 +207,20 @@ "conn", "conn/gpio", "conn/gpio/gpioreg", - "conn/gpio/gpiostream", "conn/i2c", "conn/i2c/i2creg", "conn/pin", - "conn/pin/pinreg", "conn/spi", "conn/spi/spireg", "devices", - "host", - "host/allwinner", - "host/bcm283x", - "host/chip", - "host/cpu", - "host/distro", "host/fs", - "host/odroidc1", - "host/pine64", - "host/pmem", - "host/rpi", - "host/sysfs", - "host/videocore" + "host/sysfs" ] revision = "a8b45e63d61cc00328dfe54149cfe7abd7115a7f" [solve-meta] analyzer-name = "dep" analyzer-version = 1 - inputs-digest = "3bcf2897d7ee15e9e4b2923907a9b6ebd412da4b3c00c7feea10267bb35c86f2" + inputs-digest = "b44d7a852c8f09ede08c21c5e72ca4822f2e04af363da033646c5b36b1763d68" solver-name = "gps-cdcl" solver-version = 1 diff --git a/platforms/dji/tello/crc.go b/platforms/dji/tello/crc.go index 2e4c31fc..bdb03253 100644 --- a/platforms/dji/tello/crc.go +++ b/platforms/dji/tello/crc.go @@ -1,65 +1,19 @@ package tello -const ( - poly int = 13970 +import ( + "github.com/go-daq/crc8" + "github.com/howeyc/crc16" ) -func fcstab() []int { - return []int{0, 4489, 8978, 12955, 17956, 22445, 25910, 29887, 35912, 40385, 44890, 48851, 51820, 56293, 59774, 63735, 4225, 264, 13203, 8730, 22181, 18220, 30135, 25662, 40137, 36160, 49115, 44626, 56045, 52068, 63999, 59510, 8450, 12427, 528, 5017, 26406, 30383, 17460, 21949, 44362, 48323, 36440, 40913, 60270, 64231, 51324, 55797, 12675, 8202, 4753, 792, 30631, 26158, 21685, 17724, 48587, 44098, 40665, 36688, 64495, 60006, 55549, 51572, 16900, 21389, 24854, 28831, 1056, 5545, 10034, 14011, 52812, 57285, 60766, 64727, 34920, 39393, 43898, 47859, 21125, 17164, 29079, 24606, 5281, 1320, 14259, 9786, 57037, 53060, 64991, 60502, 39145, 35168, 48123, 43634, 25350, 29327, 16404, 20893, 9506, 13483, 1584, 6073, 61262, 65223, 52316, 56789, 43370, 47331, 35448, 39921, 29575, 25102, 20629, 16668, 13731, 9258, 5809, 1848, 65487, 60998, 56541, 52564, 47595, 43106, 39673, 35696, 33800, 38273, 42778, 46739, 49708, 54181, 57662, 61623, 2112, 6601, 11090, 15067, 20068, 24557, 28022, 31999, 38025, 34048, 47003, 42514, 53933, 49956, 61887, 57398, 6337, 2376, 15315, 10842, 24293, 20332, 32247, 27774, 42250, 46211, 34328, 38801, 58158, 62119, 49212, 53685, 10562, 14539, 2640, 7129, 28518, 32495, 19572, 24061, 46475, 41986, 38553, 34576, 62383, 57894, 53437, 49460, 14787, 10314, 6865, 2904, 32743, 28270, 23797, 19836, 50700, 55173, 58654, 62615, 32808, 37281, 41786, 45747, 19012, 23501, 26966, 30943, 3168, 7657, 12146, 16123, 54925, 50948, 62879, 58390, 37033, 33056, 46011, 41522, 23237, 19276, 31191, 26718, 7393, 3432, 16371, 11898, 59150, 63111, 50204, 54677, 41258, 45219, 33336, 37809, 27462, 31439, 18516, 23005, 11618, 15595, 3696, 8185, 63375, 58886, 54429, 50452, 45483, 40994, 37561, 33584, 31687, 27214, 22741, 18780, 15843, 11370, 7921, 3960} +// CalculateCRC8 calculates the starting CRC8 byte for packet. +func CalculateCRC8(bytes []byte) byte { + return crc8.Checksum(bytes, crc8.MakeTable(0x77)) } -func fsc16(bytes []byte, length int, poly int) int { - fs := fcstab() - if len(bytes) == 0 { - return 65535 - } - i := 0 - j := poly - poly = length - length = j - for { - j = length - if poly == 0 { - break - } - j = int(bytes[i]) - length = fs[((length^j)&0xFF)] ^ length>>8 - i++ - poly-- - } - return j -} - -// CalculateCRC calculates the ending CRC bytes for packet. -func CalculateCRC(pkt []byte) (bLow, bHigh byte) { - l := len(pkt) - i := fsc16(pkt, l-2, poly) - bLow = ((byte)(i & 0xFF)) - bHigh = ((byte)(i >> 8 & 0xFF)) +// CalculateCRC16 calculates the ending CRC16 bytes for packet. +func CalculateCRC16(pkt []byte) (low, high byte) { + i := crc16.Checksum(pkt, crc16.MakeTable(0x3692)) + low = ((byte)(i & 0xFF)) + high = ((byte)(i >> 8 & 0xFF)) return } - -// uCRC -func uCRCTable() []int { - return []int{0, 94, 188, 226, 97, 63, 221, 131, 194, 156, 126, 32, 163, 253, 31, 65, 157, 195, 33, 127, 252, 162, 64, 30, 95, 1, 227, 189, 62, 96, 130, 220, 35, 125, 159, 193, 66, 28, 254, 160, 225, 191, 93, 3, 128, 222, 60, 98, 190, 224, 2, 92, 223, 129, 99, 61, 124, 34, 192, 158, 29, 67, 161, 255, 70, 24, 250, 164, 39, 121, 155, 197, 132, 218, 56, 102, 229, 187, 89, 7, 219, 133, 103, 57, 186, 228, 6, 88, 25, 71, 165, 251, 120, 38, 196, 154, 101, 59, 217, 135, 4, 90, 184, 230, 167, 249, 27, 69, 198, 152, 122, 36, 248, 166, 68, 26, 153, 199, 37, 123, 58, 100, 134, 216, 91, 5, 231, 185, 140, 210, 48, 110, 237, 179, 81, 15, 78, 16, 242, 172, 47, 113, 147, 205, 17, 79, 173, 243, 112, 46, 204, 146, 211, 141, 111, 49, 178, 236, 14, 80, 175, 241, 19, 77, 206, 144, 114, 44, 109, 51, 209, 143, 12, 82, 176, 238, 50, 108, 142, 208, 83, 13, 239, 177, 240, 174, 76, 18, 145, 207, 45, 115, 202, 148, 118, 40, 171, 245, 23, 73, 8, 86, 180, 234, 105, 55, 213, 139, 87, 9, 235, 181, 54, 104, 138, 212, 149, 203, 41, 119, 244, 170, 72, 22, 233, 183, 85, 11, 136, 214, 52, 106, 43, 117, 151, 201, 74, 20, 246, 168, 116, 42, 200, 150, 21, 75, 169, 247, 182, 232, 10, 84, 215, 137, 107, 53} -} - -// CalculateUCRC calculates the starting uCRC byte for packet. -func CalculateUCRC(bytes []byte, length int, poly int) int { - fs := uCRCTable() - - j := 0 - i := poly - poly = j - for length != 0 { - j = int(bytes[poly]) ^ i - i = j - if j < 0 { - i = j + 256 - } - i = fs[i] - poly++ - length-- - } - return i -} diff --git a/platforms/dji/tello/driver.go b/platforms/dji/tello/driver.go index 66200c4d..659b4648 100644 --- a/platforms/dji/tello/driver.go +++ b/platforms/dji/tello/driver.go @@ -55,7 +55,6 @@ const ( wifiMessage = 26 videoRateQuery = 40 lightMessage = 53 - timeMessage = 70 flightMessage = 86 logMessage = 0x50 @@ -63,7 +62,8 @@ const ( videoEncoderRateCommand = 0x20 videoStartCommand = 0x25 exposureCommand = 0x34 - stickCommand = 0x50 + timeCommand = 70 + stickCommand = 80 takeoffCommand = 0x54 landCommand = 0x55 flipCommand = 0x5c @@ -248,7 +248,6 @@ func (d *Driver) Start() error { // send stick commands go func() { - time.Sleep(50 * time.Millisecond) for { err := d.SendStickCommand() if err != nil { @@ -294,7 +293,7 @@ func (d *Driver) handleResponse() error { err = binary.Read(buf, binary.LittleEndian, &ld) d.Publish(d.Event(LightStrengthEvent), ld) - case timeMessage: + case timeCommand: d.Publish(d.Event(TimeEvent), buf[7:8]) case takeoffCommand: d.Publish(d.Event(TakeoffEvent), buf[7:8]) @@ -318,6 +317,7 @@ func (d *Driver) handleResponse() error { // parse text packet if buf[0] == 0x63 && buf[1] == 0x6f && buf[2] == 0x6e { d.Publish(d.Event(ConnectedEvent), nil) + d.SendDateTime() d.processVideo() } @@ -386,7 +386,7 @@ func (d *Driver) SetExposure(level int) (err error) { // sets ending crc bytes for packet l := len(pkt) - pkt[(l - 2)], pkt[(l - 1)] = CalculateCRC(pkt) + pkt[(l - 2)], pkt[(l - 1)] = CalculateCRC16(pkt) _, err = d.reqConn.Write(pkt) return @@ -398,7 +398,7 @@ func (d *Driver) SetVideoEncoderRate(rate VideoBitRate) (err error) { // sets ending crc bytes for packet l := len(pkt) - pkt[(l - 2)], pkt[(l - 1)] = CalculateCRC(pkt) + pkt[(l - 2)], pkt[(l - 1)] = CalculateCRC16(pkt) _, err = d.reqConn.Write(pkt) return @@ -410,7 +410,7 @@ func (d *Driver) Rate() (err error) { // sets ending crc bytes for packet l := len(pkt) - pkt[(l - 2)], pkt[(l - 1)] = CalculateCRC(pkt) + pkt[(l - 2)], pkt[(l - 1)] = CalculateCRC16(pkt) _, err = d.reqConn.Write(pkt) return @@ -495,7 +495,7 @@ func (d *Driver) Flip(direction FlipType) (err error) { // sets ending crc bytes for packet l := len(pkt) - pkt[(l - 2)], pkt[(l - 1)] = CalculateCRC(pkt) + pkt[(l - 2)], pkt[(l - 1)] = CalculateCRC16(pkt) _, err = d.reqConn.Write(pkt) return @@ -620,7 +620,41 @@ func (d *Driver) SendStickCommand() (err error) { // sets ending crc for packet l := len(pkt) - pkt[(l - 2)], pkt[(l - 1)] = CalculateCRC(pkt) + pkt[(l - 2)], pkt[(l - 1)] = CalculateCRC16(pkt) + + _, err = d.reqConn.Write(pkt) + return +} + +// SendDateTime sends the current date/time to the drone. +func (d *Driver) SendDateTime() (err error) { + d.cmdMutex.Lock() + defer d.cmdMutex.Unlock() + + l := 22 + pkt := make([]byte, l) + buf := bytes.NewBuffer(pkt) + + binary.Write(buf, binary.LittleEndian, byte(messageStart)) + binary.Write(buf, binary.LittleEndian, byte(l<<3)) + binary.Write(buf, binary.LittleEndian, byte(0x00)) + binary.Write(buf, binary.LittleEndian, byte(CalculateCRC8(pkt[0:2]))) + binary.Write(buf, binary.LittleEndian, byte(0x50)) // packet type 0x50 + binary.Write(buf, binary.LittleEndian, timeCommand) + binary.Write(buf, binary.LittleEndian, byte(0x00)) + binary.Write(buf, binary.LittleEndian, byte(0x12)) // seq + binary.Write(buf, binary.LittleEndian, byte(0x00)) + + now := time.Now() + binary.Write(buf, binary.LittleEndian, byte(0x00)) + binary.Write(buf, binary.LittleEndian, now.Hour()) + binary.Write(buf, binary.LittleEndian, now.Minute()) + binary.Write(buf, binary.LittleEndian, now.Second()) + binary.Write(buf, binary.LittleEndian, int16(now.UnixNano()/int64(time.Millisecond)&0xff)) + binary.Write(buf, binary.LittleEndian, int16(now.UnixNano()/int64(time.Millisecond)>>8)) + + // sets ending crc for packet + pkt[(l - 2)], pkt[(l - 1)] = CalculateCRC16(pkt) _, err = d.reqConn.Write(pkt) return