def start_radar(self):
# Create start command, enabling both raw and tracked targets by default:
msg = can.Message(arbitration_id=RADAR_SYSTEM_CMD,
data=[
RADAR_START, RADAR_RESERVED, RADAR_SEND_BOTH,
RADAR_RESERVED, RADAR_RESERVED, RADAR_RESERVED,
RADAR_RESERVED, RADAR_RESERVED
],
is_extended_id=False)
# Send the start command:
try:
self.bus.send(msg)
except can.CanError:
self.update_text_display_newline(
">> Failed to send start command.")
self.update_text_display_newline(">> Radar started.")
self.update_text_display_newline(
"Monitor data with pcanview or candump. Be sure to STOP the radar again before reconfiguring."
)
def find_devices(self):
devices = []
self._bus.send(
can.Message(arbitration_id=0,
data=[MessageTypes.DEVICE_DETECT_REQUEST.value],
extended_id=False), 5)
last_message = time.time()
while True:
resp = self.recv()
if resp is None:
break
if len(resp.data) >= 1:
if resp.data[0] == MessageTypes.DEVICE_DETECT_RESPONSE.value:
device = CanDevice(resp.arbitration_id, self._bus, self)
devices.append(device)
last_message = time.time()
continue
self.return_message(resp)
if time.time() - last_message > 5:
break
self.end_return()
return devices
def CAN_data_send(self, send_id, send_data):
'''
功能:
- 发送CAN数据
- 通过条件 : 输入数据格式无误
参数:
- send_id : 发送的ID
- send_data : 发送的数据
(注意没有默认值的参数是必填的,有默认值的参数不填写,就会按照默认的参数执行)
例子:
| CAN_send_data | 720 | 02 10 03 |
'''
send_id = self._convert_can_id(send_id)
send_data = self._convert_can_send_data(send_data)
msg = can.Message(
arbitration_id = send_id,
data = send_data,
extended_id = False)
BUS_CAN.send(msg)
time.sleep(0.15)
def test_cycle_time(self):
msg = can.Message(is_extended_id=False,
arbitration_id=0x123,
data=[0, 1, 2, 3, 4, 5, 6, 7])
with can.interface.Bus(bustype="virtual") as bus1:
with can.interface.Bus(bustype="virtual") as bus2:
# disabling the garbage collector makes the time readings more reliable
gc.disable()
task = bus1.send_periodic(msg, 0.01, 1)
self.assertIsInstance(task,
can.broadcastmanager.CyclicSendTaskABC)
sleep(2)
size = bus2.queue.qsize()
# About 100 messages should have been transmitted
self.assertTrue(
80 <= size <= 120,
"100 +/- 20 messages should have been transmitted. But queue contained {}"
.format(size),
)
last_msg = bus2.recv()
next_last_msg = bus2.recv()
# we need to reenable the garbage collector again
gc.enable()
# Check consecutive messages are spaced properly in time and have
# the same id/data
self.assertMessageEqual(last_msg, next_last_msg)
# Check the message id/data sent is the same as message received
# Set timestamp and channel to match recv'd because we don't care
# and they are not initialized by the can.Message constructor.
msg.timestamp = last_msg.timestamp
msg.channel = last_msg.channel
self.assertMessageEqual(msg, last_msg)
def main():
"""Test different cyclic sending tasks."""
reset_msg = can.Message(arbitration_id=0x00,
data=[0, 0, 0, 0, 0, 0],
is_extended_id=False)
with can.Bus(interface="virtual") as bus:
bus.send(reset_msg)
simple_periodic_send(bus)
bus.send(reset_msg)
limited_periodic_send(bus)
test_periodic_send_with_modifying_data(bus)
# print("Carrying out multirate cyclic test for {} interface".format(interface))
# can.rc['interface'] = interface
# test_dual_rate_periodic_send()
time.sleep(2)
def implement():
recvd = "" #declare string
data = [] #declare list
for i in range(
len(thelist)
): #for-loop gets the groups of 8 bytes till it has pulled all data from the list
inputt = (thelist[i]
) #the 8 bytes are stored in the variable ready to be sent
asciii = list(
map(ord, str(inputt))
) #the string is converted to ascii values to be sent on the bus
send = str(
asciii
) #formats the data to be able to then convert to byte data type
msg = bytes(
send, 'utf-8'
) #message is converted to byte data type to be sent on the bus
msg1 = can.Message(
arbitration_id=0x12345, data=msg,
is_extended_id=False) #generates the CAN-bus message.
try: #this tries to send the message and returns if it is successful
bus1.send(msg1)
#print ("message sent on {}".format(bus1.channel_info))
continue
except can.CanError:
#print("Error, message not sent")
continue
msg2 = bus2.recv() #the message is received on the second bus
recvd = (msg2.data) #the data portion of the message is extracted
fixed = json.loads(
recvd) #the message's formatting is fixed using json
letters = "".join([
chr(c) for c in fixed
]) #the ascii values are converted back to ascii characters
data.append(
str(letters)
) #each 8 byte message that comes through the for loop is added to a list
implement.string = "".join(
data) #the list of 8 bytes are reconnected to one whole string
def send_one():
# this uses the default configuration (for example from the config file)
# see https://python-can.readthedocs.io/en/stable/configuration.html
bus = can.interface.Bus()
# Using specific buses works similar:
# bus = can.interface.Bus(bustype='socketcan', channel='vcan0', bitrate=250000)
# bus = can.interface.Bus(bustype='pcan', channel='PCAN_USBBUS1', bitrate=250000)
# bus = can.interface.Bus(bustype='ixxat', channel=0, bitrate=250000)
# bus = can.interface.Bus(bustype='vector', app_name='CANalyzer', channel=0, bitrate=250000)
# ...
msg = can.Message(arbitration_id=0xc0ffee,
data=[0, 25, 0, 1, 3, 1, 4, 1],
is_extended_id=True)
try:
bus.send(msg)
print("Message sent on {}".format(bus.channel_info))
except can.CanError:
print("Message NOT sent")
def send_message(self, message_name, signal_name, value):
""" Send a CAN signal from Database
Keyword arguments:
signal_name -- Name of the signal to send
value -- Value of the signal to send
"""
signal_dict = {signal_name: float(value)}
message_to_send = self.db.get_message_by_name(message_name)
if message_to_send is not None:
for signal in message_to_send.signals:
if signal.name != signal_name and signal is not None:
signal_dict[signal.name] = 0.0
message_to_send = self.db.get_message_by_name(message_name)
data = message_to_send.encode(signal_dict)
message = can.Message(arbitration_id=message_to_send.frame_id,
data=data)
print(message)
self.bus.send(message)
else:
raise AssertionError('Signal : %s is not in Database' %
(signal_name))
def __writePage(self, ch, address):
sframe = can.Message(arbitration_id=0x7E0 + ch,
is_extended_id=False,
data=[
0x03, address & 0xFF, (address & 0xFF00) >> 8,
(address & 0xFF0000) >> 16,
(address & 0xFF000000) >> 24
])
self.can.send(sframe)
endtime = time.time() + 0.5
while True: # Channel wait loop
try:
rframe = self.can.recv(1)
except connection.Timeout:
pass
else:
if rframe.arbitration_id == sframe.arbitration_id+1 and \
rframe.data == sframe.data:
break
now = time.time()
if now > endtime:
raise connection.Timeout
def _recieveAll(self):
# iterate over received messages
for msg in self.bus:
if (msg.arbitration_id == vcan_active.test_message.frame_id):
data = vcan_active.test_message.encode({'Signal1': 1})
rsp_msg = can.Message(
arbitration_id=vcan_active.test_message.frame_id,
data=data)
self.bus.send(rsp_msg)
#print(msg)
try:
print(
msg.arbitration_id,
vcan_active.db.decode_message(msg.arbitration_id,
msg.data))
logging.info(
(msg.arbitration_id,
vcan_active.db.decode_message(msg.arbitration_id,
msg.data)))
break
except:
print("Message NOT sent")
def recv(self, timeout=None):
while 1:
# read canbus
dat = self.readline_fd()
t = datetime.datetime.now()
ts = t.timestamp()
if len(dat) <= 0:
continue
# check frame type
if dat[0] == 'T':
ext_id = True
remote = False
elif dat[0] == 't':
ext_id = False
remote = False
elif dat[0] == 'R':
ext_id = True
remote = True
elif dat[0] == 'r':
ext_id = False
remote = True
else:
#print("error", dat)
continue
# parse USBCAN recive data
# 't7E8804610C0C41000000' => 7E8 8 04610C0C41000000
try:
msg_id = int(dat[1:4], 16)
msg_size = int(dat[4], 16)
#msg_dat = int(dat[5:], 16)
#msg_dat = [(msg_dat>>i*8)&0xff for i in range(msg_size-1,-1,-1)]
msg_dat = []
for i in range(0, msg_size):
msg_dat.append(int(dat[5+i*2:7+i*2], 16))
dev_name_dummy = "can0"
except:
continue
msg = can.Message(timestamp = ts, arbitration_id = msg_id, extended_id = ext_id, is_remote_frame = remote, is_error_frame = False, dlc = len(msg_dat), data = msg_dat, channel = dev_name_dummy)
yield msg
def on_message(client, userdata, msg):
# print("Msg:" + msg.topic + " " + str(msg.payload))
if arg_results.serverMode:
arbID = int(msg.topic.split('/')[2])
else:
arbID = int(msg.topic.split('/')[1])
flags = int.from_bytes(msg.payload[8:9], byteorder='little')
isExt = False
if ((flags & 1) == 1): isExt = True
msg = can.Message(arbitration_id=arbID,
data=msg.payload[9:],
is_extended_id=isExt)
try:
bus.send(msg)
# print("Message sent on {}".format(bus.channel_info))
except can.CanError:
print("Message NOT sent - Something broke")
def send_periodic(self,
data,
arbitration_id,
extended_id,
period,
duration=None):
'''
:param float period:
Period in seconds between each message
:param float duration:
The duration to keep sending this message at given rate. If
no duration is provided, the task will continue indefinitely.
:return: A started task instance
:rtype: can.CyclicSendTaskABC
'''
message = can.Message(arbitration_id=arbitration_id,
data=data,
extended_id=extended_id)
task = can.send_periodic(message, period, duration)
assert isinstance(task, can.CyclicSendTaskABC)
return task
def sendToICSim(self, cmd):
'''
Send CAN packet to ICSim when receiving the data of the message.
@param cmd (string): message received on virtual serial port.
'''
logging.info("Olrait!!" + cmd)
if self.tmp_arbitration_id != False:
arb_id = self.tmp_arbitration_id
self.tmp_arbitration_id = False
else:
logging.error("NO ARB ID PREVIOUSLY STORED...")
return
bus = can.interface.Bus(channel='vcan0', bustype='socketcan')
data = bytearray.fromhex(cmd)
logging.info("This is the data I want to send: " + str(arb_id) + "#" +
str(data))
msg = can.Message(arbitration_id=int(arb_id, 16),
data=data,
is_extended_id=False)
bus.send(msg)
bus.shutdown()
return
def test_removing_bus_tasks(self):
bus = can.interface.Bus(bustype="virtual")
tasks = []
for task_i in range(10):
msg = can.Message(
is_extended_id=False,
arbitration_id=0x123,
data=[0, 1, 2, 3, 4, 5, 6, 7],
)
msg.arbitration_id = task_i
task = bus.send_periodic(msg, 0.1, 1)
tasks.append(task)
self.assertIsInstance(task, can.broadcastmanager.CyclicSendTaskABC)
assert len(bus._periodic_tasks) == 10
for task in tasks:
# Note calling task.stop will remove the task from the Bus's internal task management list
task.stop()
assert len(bus._periodic_tasks) == 0
bus.shutdown()
def SendCommandToServomotor(
bus: can.interface.Bus,
nodeAddress: int,
command: Command,
data: List[int],
responseTimeout: Optional[float] = None) -> Optional[int]:
assert isinstance(data, list)
# Przygotuj komunikat dla serwosilnika
payload = [0] * 5
payload[0] = command & 0xFF
data = data or []
for i in range(0, len(data)):
payload[i + 1] = data[i] & 0xFF
#print(f"Sendin {command}...")
try:
msg = can.Message(arbitration_id=0x300 + nodeAddress,
data=payload,
extended_id=False)
bus.send(msg)
except can.CanError as ce:
print("CanError: ", ce)
return None
if responseTimeout is None:
return None # nie czekamy na odpowiedź sterownika
try:
msg = bus.recv(responseTimeout)
if msg is None:
return None # timeout
except can.CanError as ce:
print("CanError: ", ce)
return None
return msg.data
def _canwrite_process(intfs, calibration, end_process):
print("canwrite_process: START")
for k, v in intfs.items():
try:
v['ctx'] = context.create_ctx(channel=v['channel'],
bustype=BusType.SOCKETCAN,
bitrate=v['bitrate'],
canid_recv=0,
canid_send=0,
timeout=None,
trace=0)
except:
print("Error creating context on interface {}".format(
v['channel']))
while not end_process.value:
try:
e = forward_queue.get(timeout=2)
except queue.Empty:
pass
except EOFError:
return
else:
i = e['intf']
if i in intfs:
# Memorize message that will be written
ecache = {'msg': e, 'time': time.time()}
write_cache.append(ecache)
msg = can.Message(data=e['data'])
out = "W: {} [0x{:03x} - {}]".format(
i, e['canid'], codecs.encode(msg.data, 'hex'))
print(colored(out, 'green'))
vcan.write(intfs[i]['ctx'], msg, can_id=e['canid'])
else:
print("Warning interface {} doesn't exist".format(i))
def multiFrameResponse_target():
global bus
working = True
startTime = time()
canMsg = can.Message(arbitration_id=0x650)
clearReceiveBuffer()
index = 0
response = False
while working:
currTime = time()
if (currTime - startTime) > 50:
working = False
recvMsg = getNextReceivedMessage()
if recvMsg is not None:
response = True
if response:
if index == 0:
sleep(0.002)
canMsg.data = [0x10, 0x13, 0x62, 0xF1, 0x8C, 0x30, 0x30, 0x30]
index = 1
elif index == 1:
canMsg.data = [0x21, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30]
index = 2
elif index == 2:
canMsg.data = [0x22, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x00]
working = False
bus.send(canMsg)
sleep(0.020)
def CAN_send_and_rec_neg(self, send_id, send_data, recv_id, timeout=20):
'''
功能:
- 发送并接收CAN数据,判断否定响应
- 通过条件 : 只要收到否定响应就通过
参数:
- send_id : 发送的ID
- send_data : 发送的数据
- recv_id : 接收的ID
- timeout : 超时时间(ms)
(注意没有默认值的参数是必填的,有默认值的参数不填写,就会按照默认的参数执行)
例子:
| CAN_send_and_rec_neg | 720 | 02 10 03 | 728 | timeout=20 |
'''
#global SHOULD_RECV_CAN_OBJECT
send_id = self._convert_can_id(send_id)
recv_id = self._convert_can_id(recv_id)
send_data = self._convert_can_send_data(send_data)
#recv_data = self._convert_can_recv_data(recv_data)
msg = can.Message(
arbitration_id = send_id,
data = send_data,
extended_id = False)
self._can_send_and_wait_rsp(recv_id, msg)
if SHOULD_RECV_CAN_OBJECT:
for bus_recv_msg in SHOULD_RECV_CAN_OBJECT:
print('expect negtive response')
print('infact recv',hex(bus_recv_msg.arbitration_id),self._bytes_to_hex(bus_recv_msg.data))
if bus_recv_msg.arbitration_id == recv_id:
if bus_recv_msg.data[1] == 127:
logger.info('收到否定响应,测试通过')
else:
asserts.fail('不是否定响应')
break
else:
asserts.fail('未接收到CAN数据')
def motor_sim():
# Simulation parameters
setpoint = 100
tmax = 20
step = 100
# Vehicule parameters
m = 1000 # (kg) vehicle mass
b = 50 # (N.s/m) damping coefficient
# PI controller
Kp = 300
Ki = 15
Cpi = control.tf([Kp, Ki], [1, 0])
# Transfer function
mot = control.tf([1], [m, b])
gol = control.series(Cpi, mot)
gcl = control.feedback(gol, 1)
# Time
t = np.linspace(0, tmax, step)
# Input
r = np.empty(len(t))
r.fill(setpoint)
# Output
t, y, _ = control.forced_response(gcl, t, r)
# Plot
plt.figure()
plt.title('Cruise Control Simulation')
plt.axis([0, tmax + 1, 0, setpoint + 1])
plt.grid()
plt.xlabel('Time (s)')
plt.ylabel('Speed (km/h)')
# Animation + Sending message
for i in range(step):
y_int = int(round(y[i]))
msg = can.Message(arbitration_id=100, data=[y_int, 0, 0, 0, 0, 0, 0])
node0.send(msg)
plt.scatter(t[i], y[i])
plt.pause(OPTIONS['update_time'])
plt.show()
def send_periodic_message(self, message_to_send, period, data=None):
"""Send a message with the given periodicity
Keyword arguments:
message_to_send -- name of the message
(relative to databse) to send
period -- periodicity in second
data -- data to send
"""
if data is None or data == 'None':
data = '0'
is_in_db = False
for message in self.db.messages:
if message.name == message_to_send:
messagets = message
is_in_db = True
if is_in_db:
msg = can.Message(arbitration_id=messagets.frame_id,
data=bytearray([int(data)]))
print(msg)
self.bus.send_periodic(msg, float(period))
else:
raise AssertionError('Message : %s is not in Database' %
(message_to_send))
def createTargetMessage(angle):
steering = angle
pedal = 1000
brake = 1000
mission_finished = 0
ebs = 0
# Format message values
#steering += STEERING_OFFSET
steering = steering
if steering > 90:
steering = 90
if steering < -90:
steering = -90
steering += 128
steering = int(steering)
steer_hex = steering.to_bytes(1, "little")
msg_data = [0, 0, 0, 0, 0, 0, 0, steer_hex[0]]
return can.Message(arbitration_id=CAN_DRIVERLESS_ID,
data=msg_data,
extended_id=False)
def BSP_CAN_StartHB_BroadCast(self):
_temp = []
retval = 1
_txHeader = CAN_TxHeaderTypeDef()
#/*1. find header file */
_txHeader.StdId = 0x2f0
_txHeader.ExtId = 0x00
_txHeader.IDE = False
_txHeader.RTR = False #Data frame
_txHeader.DLC = 2 #/*ask for 2 byte data to send from DCB nodes*/
_txHeader.TransmitGlobalTime = 0.0
_temp.append(devInfoMgt.hbPeriod)
_temp.append(devInfoMgt.hbPeriod >> 8)
msg = can.Message(arbitration_id=_txHeader.StdId,
data=_temp,
dlc=_txHeader.DLC,
extended_id=False)
try:
bus.send(msg)
except can.CanError:
print("Can not send Message HeartBeat!")
retval = 0
return retval
def ReadNextLineSendCan(can_channel, can_file):
can_line = can_file.readline()
if(can_line == ''):
print('Restarting CANBUS Data Send.' + can_channel) # Loop data
print('Restart Time:',time)
Time = time.strftime('%X %x %Z')
print('Start Time:',Time)
can_file.seek(0,0)
can_line = file.readline()
print('sending ' + can_channel + ' '+ can_line)
ID = int(can_line[8:11],16)
DataLength = int(can_line[15:16])
Data = GetData(DataLength,can_line)
bus = can.interface.Bus(channel=can_channel, bustype='socketcan_native')
msg = can.Message(arbitration_id=ID, data=Data, extended_id=False)
bus.send(msg)
def readmemory(self):
self.interface.send(self.msg_req)
answer = self.interface.recv(1)
with open(now.strftime('%Y%m%d-%H%M%S.bin'), 'wb') as fn:
for n in range(128):
while answer is not None and answer.arbitration_id != 0x3EB and answer.data[
7] != 0x53:
answer = self.interface.recv(1)
msg = can.Message(extended_id=False,
arbitration_id=0x3F0,
data=bytearray([
0x31, 0x0C, n, 0x00, 0x00, 0x00, 0x00,
0x00
]))
self.interface.send(msg)
answer = self.interface.recv(1)
while answer is not None:
if answer.arbitration_id == 0x3E8:
print('%02X: %02X' % (n, answer.data[0]))
sys.stdout.flush()
fn.write(bytes([answer.data[0]]))
break
answer = self.interface.recv(1)
def send_CAN(self, address, data):
"""Translates command to CAN frame and sends it
:param message: command to be translated and send over CAN
"""
# Try to write the message
try:
msg = can.Message(arbitration_id=address,
data=data,
is_extended_id=True)
# print(msg)
except AttributeError as error:
print("error:Create message")
print(error)
return
# Try to send the message
try:
self.can_bus.send(msg)
# print("Message sent on {}".format(self.can_bus.channel_info))
except can.CanError:
print("Message could NOT be sent!")
def _bytes_to_message(self, b):
"""convert raw TCP bytes to can.Message object"""
ts = int.from_bytes(b[:4],
"little") + int.from_bytes(b[4:8], "little") / 1e6
can_id = int.from_bytes(b[8:12], "little")
dlc = b[
12] #TODO: sanity check on these values in case of corrupted messages.
#decompose ID
is_extended = bool(can_id & self.CAN_EFF_FLAG) #CAN_EFF_FLAG
if is_extended:
arb_id = can_id & 0x1FFFFFFF
else:
arb_id = can_id & 0x000007FF
return can.Message(
timestamp=ts,
arbitration_id=arb_id,
is_extended_id=is_extended,
is_error_frame=bool(can_id & self.CAN_ERR_FLAG), #CAN_ERR_FLAG
is_remote_frame=bool(can_id & self.CAN_RTR_FLAG), #CAN_RTR_FLAG
dlc=dlc,
data=b[13:13 + dlc])
def send_checkback_msg(i):
msg = can.Message(arbitration_id = 0x05, # Configuración del msg, estamos
data = [4,0,0,6,6,6,0,0], # enviando un mensaje cualquiera
is_extended_id = True)
try:
bus.send(msg) # Envío del mensaje msg al bus
print("Mensaje enviado a Sensor: {}".format(i))
try:
msg_received = list(receive_msg(i)) # Uso de función receiveMsg
pos = len(msg_received)-1 # Posición del dato en el mensaje
if msg_received[pos] is not None: # Se asume que si el dato es cero: error/sin sensor
msg_rc = msg_received[pos]*(mV/div)
print("Mensaje recibido: {}".format(msg_rc))
save_data(i,msg_rc)
else:
print("Error: el sensor #{} no está conectado o no existe".format(i))
save_data(i, 0)
except Exception as e:
print(e)
except can.CanError:
aux = False
save_data(i, 0)
print("Mensage NO enviado/Ningún sensor conectado")
def send_motor_command(self, command: float, mode=InverterMode.Torque, duration=None):
""" Sends a motor command using existing info plus new speed/torque command and mode
Uses all existing message configuration except for speed/torque command, and mode.
Can be used to issue subsequent commands after the motor has been configured intially
Args:
command (float): New speed/torque command, depending on the mode
mode (InverterMode)
duration (float): duration for which the message should be sent
"""
speed_command = int(
command * 10 if mode == InverterMode.Speed else 0).to_bytes(2, 'little')
torque_commmand = (
command * 10 if mode == InverterMode.Torque else 0).to_bytes(2, 'little')
mode = ((int(mode.value) << 2) +
(int(self.inverter_discharge) << 1) + (int(self.inverter_enable) << 0)).to_bytes(1, 'little')
command_list = torque_commmand + speed_command + \
self.direction_command + mode + self.commanded_torque_limit
message = can.Message(arbitration_id=192, data=command_list)
if self.current_command_message is not None:
self.current_command_message.stop()
self.current_command_message = self.bus.send_message_periodic(message, duration)
def test_dual_rate_periodic_send():
"""Send a message 10 times at 1ms intervals, then continue to send every 500ms"""
msg = can.Message(arbitration_id=0x0c112200, data=[0, 1, 2, 3, 4, 5])
print(
"Creating cyclic task to send message 10 times at 1ms, then every 500ms"
)
task = can.interface.MultiRateCyclicSendTask('vcan0', msg, 10, 0.001, 0.50)
time.sleep(2)
print("Changing data[0] = 0x42")
msg.data[0] = 0x42
task.modify_data(msg)
time.sleep(2)
task.stop()
print("stopped cyclic send")
time.sleep(2)
task.start()
print("starting again")
time.sleep(2)
print("done")
