Skip to content

Commit

Permalink
tello: switch to using external crc8 and cr16 packages
Browse files Browse the repository at this point in the history 
Signed-off-by: Ron Evans <[email protected]>
  • Loading branch information
@deadprogram
deadprogram committed Apr 23, 2018
1 parent c0e08e2 commit d113da1
Show file tree
Hide file tree
Showing 3 changed files with 68 additions and 81 deletions.
29 changes: 14 additions & 15 deletions Gopkg.lock
View file

Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.

68 changes: 11 additions & 57 deletions platforms/dji/tello/crc.go
View file
Original file line number Diff line number Diff line change
@@ -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
}
52 changes: 43 additions & 9 deletions platforms/dji/tello/driver.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -55,15 +55,15 @@ const (
wifiMessage = 26
videoRateQuery = 40
lightMessage = 53
timeMessage = 70
flightMessage = 86

logMessage = 0x50

videoEncoderRateCommand = 0x20
videoStartCommand = 0x25
exposureCommand = 0x34
stickCommand = 0x50
timeCommand = 70
stickCommand = 80
takeoffCommand = 0x54
landCommand = 0x55
flipCommand = 0x5c
Expand Down Expand Up @@ -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 {
Expand Down Expand Up @@ -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])
Expand All @@ -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()
}

Expand Down Expand Up @@ -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
Expand All @@ -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
Expand All @@ -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
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down

0 comments on commit d113da1

Please sign in to comment.