mirror
/
GuyCarver-MicroPython
kopia lustrzana https://github.com/GuyCarver/MicroPython
1
0
Forkuj 0
Kod Zgłoszenia Projekty Wydania Wiki Aktywność
GuyCarver-MicroPython/ttgo/lidar.py

339 wiersze
11 KiB
Python
Czysty Bezpośredni odnośnik Wina Historia

from machine import UART, Pin, PWM
from utime import sleep_ms
import _thread
import math
#angle data.
# distance = 14 bits - Distance or error code (invalid flag set)
# strength = 1 bit - Flag indicating signal strength was lower than expected.
# invalid = 1 bit - Set when distance couldn't be calculated
# uint16_t signal strength
#frame data
# uint8_t start = 0xFA
# uint8_t index = index - 0xA0 * 4 is the angle for the readings array
# uint16_t speed = divide by 64 to get speed in rpm
# 16 bytes - angledata[4] = angle data for 4 consecutive angles
# uint16_t checksum
# Total size = 22 bytes
#invalid data codes.
# XV11LIDAR_CRC_FAILURE = 0x66 the frame had incorrect CRC, don't use the data
# XV11LIDAR_ERROR1 = 0x02
# XV11LIDAR_ERROR2 = 0x03
# XV11LIDAR_ERROR3 = 0x21
# XV11LIDAR_ERROR4 = 0x25
# XV11LIDAR_ERROR5 = 0x35
# XV11LIDAR_ERROR6 = 0x50
#--------------------------------------------------------
#Constants for lidar driver
_FRAMESIZE = const(22) # See frame data below
_FRAMESTART = const(0xFA) # Each frame starts with this tag
_FRAMEINDEX = const(0xA0) # Indexes are 0xA0 + 0-90
_ANGLESPERFRAME = const(4)
_FRAMESPERROT = const(90)
_ANGLESPERROT = const(_FRAMESPERROT * _ANGLESPERFRAME) # We get 90 frames with 4 angles per frame
_FRAMESPERREAD = const(90) # Up this to read more data per attempt
_INVALID = const(1 << 15) # Bit set in destance word of angle data indicating invalid data
_BUFFERSIZE = const(_FRAMESIZE * _FRAMESPERREAD)
_MOTORSPEED = const(32)
_MOTORSPEEDMIN = const(20)
_MOTORSPEEDMAX = const(40)
_MAXDISTANCE = const(16383)
#--------------------------------------------------------
# def printit( aBuffer ):
# ''' Print bytearray data as hex. '''
# for v in aBuffer:
# print(hex(v), end=',')
#
# print('')
#--------------------------------------------------------
class lidar(object):
''' xv11 lidar driver. Reads data into an angles buffer as well as keep track of rpm.
Attempts to maintain ~245 rpm. '''
def __init__( self ):
super(lidar, self).__init__()
#rx pin is connected to the orange wire. tx isn't necessary but we have to
# supply a pin.
self._uart = UART(1, tx = 13, rx = 15, baudrate = 115200, buffer_size = 8192)
self._motor = PWM(Pin(17), freq = 100)
self._speed = _MOTORSPEED
self._speedcheck = 0
self._motor.duty(self._speed)
self._rpm = 0
self._buffer = bytearray(_BUFFERSIZE)
self._inbuffer = bytearray(_BUFFERSIZE)
self._angles = [0] * _ANGLESPERROT # Storage for angles
self._insync = False
self._good = 0
#--------------------------------------------------------
def _sync( self ):
''' sync up the serial data read with the start of frame. '''
# print('syncing')
#Function to return true if the start tag is found at the desired location
def isstart( aOffset ):
return self._buffer[aOffset] == _FRAMESTART
while not self._insync:
self._fillbuffer()
offset = -1
#Loop for up to 4 frames worth of data. If we didn't find a tag by then we probably won't
for x in range(_FRAMESIZE * 4):
#If start tag is found in 3 consecutive frames then this is probably a real frame start
if isstart(x) and isstart(x + _FRAMESIZE) and isstart(x + (_FRAMESIZE * 2)):
offset = x
break
#If we found an start tag then let's try and use it
if offset != -1:
#If the tag is at the beginning of the buffer, don't need to move data
if offset > 0:
tomove = len(self._buffer) - offset
#Move data so frame start is at the beginning
for x in range(tomove):
self._buffer[x] = self._buffer[offset + x]
# printit(self._buffer[:_FRAMESIZE])
toread = offset
#Now read in data to fill in the rest of the buffer
while toread:
cnt = self._uart.readinto(self._inbuffer, toread)
for y in range(cnt):
self._buffer[offset + y] = self._inbuffer[0]
offset += cnt
toread -= cnt
#Process the buffer data
self._process()
#If we got enough good values then we are synced
# This is half the number of angles read
if self._good > _FRAMESPERREAD * 2:
print('sync good')
self._insync = True
#--------------------------------------------------------
@property
def speed( self ):
return self._speed
#--------------------------------------------------------
@speed.setter
def speed( self, aValue ):
self._speed = aValue
self._motor.duty(aValue)
#--------------------------------------------------------
def _getrpm( self, aIndex ):
''' Get the rpm value. '''
return (self._buffer[aIndex + 2] | (self._buffer[aIndex + 3] << 8)) >> 6
#--------------------------------------------------------
def _adjustrpm( self ):
''' '''
if self._speedcheck >= 0:
if self._rpm < 240:
self.speed = min(self.speed + 1, _MOTORSPEEDMAX)
elif self._rpm > 248:
self.speed = max(self.speed - 1, _MOTORSPEEDMIN)
self._speedcheck = -20
else:
self._speedcheck += 1
#--------------------------------------------------------
def _checksum( self, aIndex = 0 ):
''' Calculate the checksum and return true if matched. '''
chk = 0
crc = self._buffer[aIndex + 20] | (self._buffer[aIndex + 21] << 8)
for i in range(aIndex, aIndex + 20, 2):
w = self._buffer[i] + (self._buffer[i + 1] << 8)
chk = (chk << 1) + w
chk = (chk & 0x7FFF) + (chk >> 15)
return (chk & 0x7FFF) == crc
#--------------------------------------------------------
def _fillbuffer( self ):
''' Fill the buffer with serial data '''
read = self._uart.readinto(self._buffer, _BUFFERSIZE)
#Read until the buffer is full
while read < _BUFFERSIZE:
#Read into temp buffer then append to _buffer
cnt = self._uart.readinto(self._inbuffer, _BUFFERSIZE - read)
if cnt:
for x in range(cnt):
self._buffer[read + x] = self._inbuffer[x]
read += cnt
#--------------------------------------------------------
def _process( self ):
''' Process buffer data and return number of good values read. '''
self._good = 0
#Loop for each frame in the buffer
for x in range(0, _BUFFERSIZE, _FRAMESIZE):
# if x == 0:
# printit(self._buffer[x:x + _FRAMESIZE])
#Make sure buffer starts with start tag
if self._buffer[x] == _FRAMESTART:
#Make sure checksum is good
if self._checksum(x):
#Average rpm value.
self._rpm = self._getrpm(x) #(self._rpm + self._getrpm(x)) >> 1
angle = (self._buffer[x + 1] - 0xA0) << 2
a = x + 4 #Point to angle data array
#Process 4 angle values
for y in range(a, a + 16, 4): #4 angle entries of 4 bytes each
dist = self._buffer[y] | (self._buffer[y + 1] << 8)
#If invalid flag set then clear distance
if dist & _INVALID:
dist = _MAXDISTANCE
else:
self._good += 1
self._angles[angle] = dist
angle += 1
#todo: May want to keep track of bad checksums and frames and report them
# else:
# print('checksum bad')
# else:
# self._insync = False
# else:
# print('bad frame')
#--------------------------------------------------------
def update( self ):
''' Update lidar data '''
if self._insync:
self._fillbuffer() # Read new data
self._process() # Process the data
self._adjustrpm() # Try and maintain 240-250 rpm
else:
insync = self._sync()
#--------------------------------------------------------
def output( self ):
''' '''
a = self._angles[0]
b = self._angles[90]
c = self._angles[180]
d = self._angles[270]
print('rpm:', self._rpm, a, b, c, d, self._good, ' ', end='\r')
#--------------------------------------------------------
# Constants for lidardisplay
_DISPLAYPERFRAME = const(90)
_MAXDISPLAYDISTANCE = const(68)
_DADJ = const(2048 // _MAXDISPLAYDISTANCE)
_CX = const(120)
_CY = const(67)
#--------------------------------------------------------
class lidardisplay(object):
''' Display lidar data on TFT '''
#--------------------------------------------------------
def __init__( self, aLidar, aDisplay ):
''' Setup display for give aLidar using tft aDisplay. '''
super(lidardisplay, self).__init__()
self._lidar = aLidar
self._tft = aDisplay
self._clear = 0xFFFFFF - self._tft.BLACK
self._draw = 0xFFFFFF - self._tft.YELLOW
self._tft.set_bg(self._clear)
self._tft.clearwin()
self._tft.pixel(_CX, _CY, 0xFFFFFF - self._tft.DARKGREY)
self._angles = [-1] * len(aLidar._angles)
self._angle = 0
self._rpm = 0
#--------------------------------------------------------
def update( self ):
''' Update give number of angles per call. Clears old displayed line
and draws a new line if distance has changed. '''
a0 = self._angle
plotted = 0
#Loop for number of angles we wish to update this frame
for i in range(360):
d = min(self._lidar._angles[a0] // _DADJ, _MAXDISPLAYDISTANCE)
od = self._angles[a0]
a1 = (a0 + 1) % 360
#if line has changed since last update
if d != od:
plotted += 1
a0r = math.radians(a0)
a1r = math.radians(a1)
x0 = math.sin(a0r)
y0 = math.cos(a0r)
x1 = math.sin(a1r)
y1 = math.cos(a1r)
#Only clear if we drew something before.
oldx0 = int(x0 * od) + _CX
oldx1 = int(x1 * od) + _CX
oldy0 = int(y0 * od) + _CY
oldy1 = int(y1 * od) + _CY
# print('erase:', oldx0, oldy0, oldx1, oldy1)
if (abs(oldx0 - oldx1) + abs(oldy0 - oldy1)) < 2:
self._tft.pixel(oldx0, oldy0, self._clear)
else:
self._tft.line(oldx0, oldy0, oldx1, oldy1, self._clear)
self._angles[a0] = d
#Only draw new line if distance is not negative.
x0 = int(x0 * d) + _CX
x1 = int(x1 * d) + _CX
y0 = int(y0 * d) + _CY
y1 = int(y1 * d) + _CY
# print('plot:', x0, y0, x1, y1)
if (abs(x0 - x1) + abs(y0 - y1)) < 2:
self._tft.pixel(x0, y0, self._draw)
else:
self._tft.line(x0, y0, x1, y1, self._draw)
if plotted >= _DISPLAYPERFRAME:
break;
a0 = a1
self._angle = a0
if self._rpm != self._lidar._rpm:
self._rpm = self._lidar._rpm
self._tft.rect(0, 0, 35, 15, self._clear, self._clear)
self._tft.text(0, 0, str(self._rpm), self._draw)
# self._tft.text(0, 15, str(self.speed), self._draw)
#--------------------------------------------------------
def displayloop( aLidar, aTFT ):
''' Run display loop in background thread. '''
d = lidardisplay(aLidar, aTFT)
while True:
if aLidar._insync:
d.update()
sleep_ms(33)
n = _thread.getnotification()
if n == _thread.EXIT:
break
#--------------------------------------------------------
def run( aTFT ):
l = lidar()
displaythread = _thread.start_new_thread('display', displayloop, (l, aTFT))
while True:
l.update()
#todo: Sleep?
Reference in New Issue View Git Blame Kopiuj bezpośredni odnośnik