Im having trouble reproducing the solution in processing.org , as well trying to access the magnetometer data(to figure out direction/angle) but im keep getting None as a result does anyone has an idea?
from machine import UART, Pin, I2C
import time
import sys
TOF_length = 16
TOF_header=(87,0,255)
TOF_system_time = 0
TOF_distance = 0
TOF_status = 0
TOF_signal = 0
TOF_check = 0
uart0 = UART(0, baudrate=921600 , tx=Pin(16), rx=Pin(17)) # 921600 speed if errors appear
## SET I2C bus - using pico pins 1-2 | 1, scl=machine.Pin(3), sda=machine.Pin(2)
sda=machine.Pin(2)
scl=machine.Pin(3)
i2c=machine.I2C(1,sda=sda, scl=scl, freq=100000)
## Enable LSM303D accelerator at I2C addr 0x1D
config=bytearray(1)
config[0]=39+8
## Reg Control1: (0x20) 50Hz+enable (0x57) + block update
i2c.writeto_mem(29, 32, config)
config=bytearray(1)
config[0]=7+8
# LSM303DLHC Mag address, 0x1E(30)
# Select MR register, 0x02(02)
# 0x00(00) Continous conversion mode
i2c.writeto_mem(29, 34, config)
def get_axis(reg): ## 40 - X , 42 - Y , 44 - Z
high=bytearray(1)
low=bytearray(1)
i2c.readfrom_mem_into(29, reg, low)
i2c.readfrom_mem_into(29, reg+1, high)
res = high[0] * 256 + low[0]
if (res<16384):
result = res/16384.0
elif (res>=16384 & res<49152):
result = (32768-res)/16384.0
else:
result = (res-65536)/16384.0
return result
def verifyCheckSum(data, len):
#print(data)
TOF_check = 0
for k in range(0,len-1):
TOF_check += data[k]
TOF_check=TOF_check%256
if(TOF_check == data[len-1]):
##print("TOF data is ok!")
return 1
else:
##print("TOF data is error!")
return 0
def getMag(reg):
high=bytearray(1)
low=bytearray(1)
# LSM303DLHC Mag address, 0x1E(30)
# Read data back from 0x03(03), 2 bytes
# X-Axis Mag MSB, X-Axis Mag LSB
##data0 = bus.read_byte_data(0x1E, 0x03)
##data1 = bus.read_byte_data(0x1E, 0x04)
i2c.readfrom_mem_into(29, reg, low)
i2c.readfrom_mem_into(29, reg+1, high)
# Convert the data
xMag = high[0] * 256 + low[0]
#xMag = data0 * 256 + data1
if xMag > 32767 :
xMag -= 65536
# LSM303DLHC Mag address, 0x1E(30)
# Read data back from 0x05(05), 2 bytes
# Y-Axis Mag MSB, Y-Axis Mag LSB
##data0 = bus.read_byte_data(0x1E, 0x07)
##data1 = bus.read_byte_data(0x1E, 0x08)
i2c.readfrom_mem_into(29, reg, low)
i2c.readfrom_mem_into(29, reg+1, high)
# Convert the data
yMag = high[0] * 256 + low[0]
if yMag > 32767 :
yMag -= 65536
# LSM303DLHC Mag address, 0x1E(30)
# Read data back from 0x07(07), 2 bytes
# Z-Axis Mag MSB, Z-Axis Mag LSB
##data0 = bus.read_byte_data(0x1E, 0x05)
##data1 = bus.read_byte_data(0x1E, 0x06)
i2c.readfrom_mem_into(29, reg, low)
i2c.readfrom_mem_into(29, reg+1, high)
# Convert the data
#zMag = data0 * 256 + data1
zMag = high[0] * 256 + low[0]
if zMag > 32767 :
zMag -= 65536
while True:
time.sleep(0.1)
#magx = getMag(10)
#print(magx)
x = ( get_axis(40) + 1) #xx is displayscreen * xx / 2.0
y = (-get_axis(42) + 1) #yy is displayscreen * yy / 2.0
z = (-get_axis(44) + 1)
##print("accel:",f'{x:.3f}',",",f'{y:.3f}',",",f'{z:.3f}')
TOF_data=()
if(uart0.any()>=32):
try:
for i in range(0,16):
TOF_data=TOF_data+(ord(uart0.read(1)),ord(uart0.read(1)))
#print(TOF_data)
for j in range(0,16):
if((TOF_data[j]==TOF_header[0] and TOF_data[j+1]==TOF_header[1] and TOF_data[j+2]==TOF_header[2]) and (verifyCheckSum(TOF_data[j:TOF_length],TOF_length))):
if(((TOF_data[j+12]) | (TOF_data[j+13]<<8) )==0):
print("Out of range!")
else:
#print("TOF id is: "+ str(TOF_data[j+3]))
TOF_system_time = TOF_data[j+4] | TOF_data[j+5]<<8 | TOF_data[j+6]<<16 | TOF_data[j+7]<<24;
#print("TOF system time is: "+str(TOF_system_time)+'ms')
TOF_distance = (TOF_data[j+8]) | (TOF_data[j+9]<<8) | (TOF_data[j+10]<<16);
##print("TOF distance is: "+str(TOF_distance)+'mm')
TOF_status = TOF_data[j+11];
#print("TOF status is: "+str(TOF_status))
TOF_signal = TOF_data[j+12] | TOF_data[j+13]<<8;
#print("TOF signal is: "+str(TOF_signal))
break
print("(",str(TOF_distance),",",f'{x:.3f}',",",f'{y:.3f}',",",f'{z:.3f}',")")
except:
print("TOF fail")
##output is (distanceinMM,x,y,z) xyz accel