86 lines
3.2 KiB
Python
86 lines
3.2 KiB
Python
import protocol_kx
|
|
import can
|
|
import numpy as np
|
|
import struct
|
|
import time
|
|
|
|
K = np.zeros(protocol_kx.KX_K_COUNT)
|
|
K_received = np.zeros(protocol_kx.KX_K_COUNT)
|
|
|
|
|
|
class KX:
|
|
def __init__(self, sbl_id, can, dict):
|
|
self.sbl_id = sbl_id
|
|
self.can0 = can
|
|
for element in dict:
|
|
K[element] = dict[element]
|
|
|
|
def reset(self):
|
|
resetMsg = can.Message(arbitration_id=0xA, data=[99, self.sbl_id], is_extended_id=False)
|
|
try:
|
|
self.can0.send(resetMsg)
|
|
except self.can0.CanError:
|
|
print("Message reset NOT sent")
|
|
|
|
def sendK(self):
|
|
i = 0
|
|
for element in K:
|
|
ba = bytearray(struct.pack("f", element))
|
|
messageK = can.Message(arbitration_id=0x2BB, data=[self.sbl_id, i, 0,
|
|
protocol_kx.KX_FLAG_SEND, ba[0], ba[1], ba[2], ba[3]],
|
|
is_extended_id=False)
|
|
try:
|
|
self.can0.send(messageK)
|
|
time.sleep(0.01)
|
|
except self.can0.CanError:
|
|
print("Message sendK NOT sent")
|
|
i = i + 1
|
|
|
|
def writeToFlashK(self):
|
|
messageK = can.Message(arbitration_id=0x2BB, data=[self.sbl_id, 0, 0,
|
|
protocol_kx.KX_FLAG_WRITE_REQ, 0, 0, 0, 0],
|
|
is_extended_id=False)
|
|
try:
|
|
self.can0.send(messageK)
|
|
time.sleep(0.01)
|
|
except self.can0.CanError:
|
|
print("Flash NOT sent")
|
|
|
|
def send_test_source_enable(self, enabaled):
|
|
ba = bytearray(struct.pack("f", enabaled))
|
|
messageK = can.Message(arbitration_id=0x2BB,
|
|
data=[self.sbl_id, protocol_kx.KDescription.K_TEST_SOURCES_ENABLED.value, 0,
|
|
protocol_kx.KX_FLAG_SEND, ba[0], ba[1], ba[2], ba[3]], is_extended_id=False)
|
|
try:
|
|
self.can0.send(messageK)
|
|
time.sleep(0.01)
|
|
except self.can0.CanError:
|
|
print("Test source enable NOT sent")
|
|
|
|
self.sendK()
|
|
self.writeToFlashK()
|
|
|
|
def send_receive_cmd(self):
|
|
messageK = can.Message(arbitration_id=0x2BB,
|
|
data=[self.sbl_id, 0, 0,
|
|
protocol_kx.KX_FLAG_RECEIVE_REQ, 0, 0, 0, 0], is_extended_id=False)
|
|
try:
|
|
self.can0.send(messageK)
|
|
time.sleep(0.01)
|
|
except self.can0.CanError:
|
|
print("Receive command NOT sent")
|
|
|
|
d = 0
|
|
while d < 64:
|
|
message = self.can0.recv(10)
|
|
if message.arbitration_id == 0x2BB:
|
|
frame = bytearray()
|
|
frame.append(message.data[4])
|
|
frame.append(message.data[5])
|
|
frame.append(message.data[6])
|
|
frame.append(message.data[7])
|
|
data= struct.unpack("f", frame)
|
|
K_received[message.data[1]] = data[0]
|
|
if(K_received[message.data[1]] != K[message.data[1]]):
|
|
print("Error in element ", message.data[1], "Need value = ", K[message.data[1]], "Get value = ", K_received[message.data[1]])
|
|
d += 1 |