Files
a7-can-sdk/tests/test_manual_protocol.py

173 lines
9.7 KiB
Python

import queue
import struct
import unittest
from unittest.mock import Mock,patch
from a7_can_sdk.protocol import Frame,decode_state,decode_parameter
from a7_can_sdk.manual_protocol import CommunicationRanges,P80Codec,PA43Codec,P80Mode,PA43Mode
from a7_can_sdk.manual_client import ManualClient
from a7_can_sdk.transport import SocketCan
RANGES=CommunicationRanges((-1,1),(-1,1),(-1,1),(0,500),(0,5),(0,100))
class ManualTests(unittest.TestCase):
def test_family_specific_modes_and_read_vectors(self):
self.assertEqual(int(P80Mode.MIT),1);self.assertEqual(int(PA43Mode.MIT),2)
a=P80Codec(63).read_parameter('control_mode')
self.assertEqual((a.can_id,a.data.hex(),a.extended),(0x3f,'5500000000000000',False))
b=PA43Codec(67).read_parameter('control_mode')
self.assertEqual((b.can_id,b.data.hex(),b.extended),(0x643,'670b000000000476',False))
self.assertIsNone(decode_state(a,0));self.assertIsNone(decode_parameter(b))
def test_p80_ten_bit_matches_manual_receiver(self):
c=P80Codec(63,ranges=RANGES)
self.assertEqual(c.mit(0,1,0,0,0).data.hex(),'0b7fffffc00001ff')
for i in range(101):
fraction=i/100
f=c.mit(2*fraction-1,2*fraction-1,500*fraction,5*fraction,2*fraction-1)
d=f.data[1:]
raw=[(d[0]<<8)|d[1],(d[2]<<2)|(d[3]>>6),((d[3]&63)<<4)|(d[4]>>4),
((d[4]&15)<<6)|(d[5]>>2),((d[5]&3)<<8)|d[6]]
for actual,maximum in zip(raw,[65535,1023,1023,1023,1023]):
self.assertLessEqual(abs(actual-fraction*maximum),1.000001)
def test_pa43_mit_servo_velocity_vectors(self):
c=PA43Codec(67,ranges=RANGES)
self.assertEqual(c.mit(0,0,0,0,0).data.hex(),'7fff7ff0000007ff')
self.assertEqual(c.velocity(0,0,0,0).data.hex(),'7fff0000000000ac')
self.assertEqual(c.servo_position(0,0,0,0,0,0,0).data.hex(),'7fff7f0000000000')
self.assertEqual(c.mit(1,1,500,5,1).data,b'\xff'*8)
self.assertEqual(c.state_command('query').data.hex(),'fffffffffffffffa')
self.assertEqual(c.state_command('query').can_id,0x143)
def test_feedback_flags_and_physical_endpoints(self):
p=P80Codec(61,ranges=RANGES)
row=p.decode_feedback(Frame(61,bytes.fromhex('0bffffffffffffff'),extended=False))
self.assertEqual((row['position_rad'],row['velocity_rad_s'],row['torque_nm'],row['error_code']),(1,1,1,65535))
c=PA43Codec(67,ranges=RANGES,feedback_id=253)
row=c.decode_feedback(Frame(253,bytes.fromhex('ffffffffffff2001'),extended=False))
self.assertEqual((row['position_rad'],row['velocity_rad_s'],row['torque_nm']),(1,1,1))
self.assertEqual(row['temperature_c'],215);self.assertTrue(row['enabled'])
self.assertEqual(row['fault_bits'],0x2000)
self.assertIsNone(c.decode_feedback(Frame(67,bytes(8),extended=False)))
raw=PA43Codec(67).decode_feedback(Frame(67,bytes(8),extended=False))
self.assertIsNone(raw['position_rad']);self.assertFalse(raw['enabled'])
def test_pa43_parameter_endianness_and_reply(self):
c=PA43Codec(6)
# Exact IEEE754 parameter vector from local manual p45.
self.assertEqual(c.write_parameter('id_kp',.3).data.hex(),'670c9a99993e1576')
frame=Frame(0x606,bytes.fromhex('060c9a99993e00ff'),extended=False)
self.assertAlmostEqual(c.decode_parameter(frame,'id_kp'),.3,places=7)
self.assertIsNone(c.decode_parameter(frame,'iq_kp'))
self.assertEqual(c.write_parameter('control_mode',3).data.hex(),'670b030000001576')
self.assertEqual(c.write_parameter('control_mode',PA43Mode.MIT).data.hex(),'670b020000001576')
with self.assertRaises(ValueError):c.write_parameter('control_mode',P80Mode.MIT)
for name in ('firmware_version','error_code','nonius_flag'):
with self.assertRaises(ValueError):c.write_parameter(name,0)
def test_p80_unspecified_scalar_byteorder_never_guessed(self):
c=P80Codec(61)
with self.assertRaises(ValueError):c.write_parameter('position',.5)
with self.assertRaises(ValueError):c.decode_parameter(Frame(61,b'\x06'+bytes(7),extended=False),'position')
self.assertEqual(P80Codec(61,scalar_byteorder='<').write_parameter('position',.5).data.hex(),'0a0000003f000000')
self.assertEqual(c.write_parameter('control_mode',1).data.hex(),'0701000000000000')
self.assertEqual(c.write_parameter('control_mode',P80Mode.MIT).data.hex(),'0701000000000000')
with self.assertRaises(ValueError):c.write_parameter('control_mode',PA43Mode.MIT)
def test_invalid_inputs_reject_before_transmission(self):
for mid in (0,129,255,True):
with self.assertRaises(ValueError):P80Codec(mid)
for mid in (0,256,True):
with self.assertRaises(ValueError):PA43Codec(mid)
c=PA43Codec(67,ranges=RANGES)
for value in (float('nan'),float('inf'),2,True):
with self.assertRaises(ValueError):c.mit(value,0,0,0,0)
with self.assertRaises(ValueError):c.servo_position(0,-.1,0,0,0,0,0)
with self.assertRaises(ValueError):PA43Codec(67).mit(0,0,0,0,0)
with self.assertRaises(ValueError):Frame(0x800,bytes(8),extended=False)
with self.assertRaises(ValueError):Frame(1,bytes(64),extended=False)
def test_fd_broadcast_requires_all_eight_valid_slots(self):
frames=[PA43Codec(i,ranges=RANGES).mit(0,0,0,0,0) for i in range(1,9)]
f=PA43Codec(1).broadcast(frames,bitrate_switch=True)
self.assertEqual((f.can_id,len(f.data),f.is_fd,f.extended,f.fd_flags),(0x20,64,True,False,1))
with self.assertRaises(ValueError):PA43Codec(1).broadcast(frames[:7])
with self.assertRaises(ValueError):PA43Codec(1).broadcast(frames[::-1])
with self.assertRaises(ValueError):PA43Codec(1).broadcast([PA43Codec(67,ranges=RANGES).mit(0,0,0,0,0)]*8)
def test_p80_fd_broadcast_uses_twelve_not_ten_bit_payload(self):
pairs=[(P80Codec(i,ranges=RANGES),(0,0,0,0,0)) for i in range(1,9)]
frame=P80Codec.broadcast_mit(pairs)
self.assertEqual((frame.can_id,frame.is_fd,frame.extended),(0,True,False))
self.assertEqual(frame.data,bytes.fromhex('7fff7ff0000007ff')*8)
commands=[P80Codec(i).read_parameter('enabled') for i in range(1,9)]
self.assertEqual(P80Codec.broadcast_commands(commands).can_id,255)
with self.assertRaises(ValueError):P80Codec.broadcast_mit(pairs[::-1])
def test_manual_client_explicit_fd_envelope(self):
class Peer:
def __init__(self):self.sent=[];self.pending=None
def send(self,f):
self.sent.append(f)
self.pending=Frame(0x643,bytes.fromhex('430b0100000000ff'),extended=False,is_fd=True)
def receive(self,timeout):
f=self.pending;self.pending=None;return f
def close(self):pass
peer=Peer();client=ManualClient(peer,PA43Codec(67),can_fd=True,bitrate_switch=True)
self.assertEqual(client.read_parameter('control_mode'),1)
self.assertTrue(peer.sent[0].is_fd);self.assertEqual(peer.sent[0].fd_flags,1)
def test_transport_keeps_standard_fd_and_extended_distinct(self):
t=SocketCan.__new__(SocketCan);t.sock=Mock();t.lock=None;t.fd=True;t.frame_format='mixed'
t.sock.send.side_effect=lambda raw:len(raw)
a=P80Codec(61).read_parameter('enabled');t.send(a)
raw=t.sock.send.call_args.args[0]
self.assertEqual(len(raw),16);self.assertEqual(struct.unpack('=I',raw[:4])[0],61)
ext=Frame(0x1100fd3d,bytes(8));t.send(ext)
self.assertEqual(struct.unpack('=I',t.sock.send.call_args.args[0][:4])[0],0x9100fd3d)
fd=Frame(0x20,bytes(range(64)),extended=False,is_fd=True,fd_flags=1);t.send(fd)
raw=t.sock.send.call_args.args[0];self.assertEqual(len(raw),72)
t.sock.recvmsg.return_value=(raw,[],0,None)
with patch('a7_can_sdk.transport.select.select',return_value=([t.sock],[],[])):
self.assertEqual(t.receive(),fd)
t.fd=False
with self.assertRaises(ValueError):t.send(fd)
t.frame_format='extended'
with self.assertRaises(ValueError):t.send(a)
def test_manual_client_accepts_valid_zero_value_reply_and_is_readonly(self):
class Peer:
def __init__(self):self.sent=[];self.queue=queue.Queue()
def send(self,f):self.sent.append(f);self.queue.put(f)
def receive(self,timeout):
try:return self.queue.get(timeout=timeout)
except queue.Empty:return None
def close(self):pass
peer=Peer();client=ManualClient(peer,P80Codec(61),timeout=.02)
# Mode0 response equals request bytes; equality is not evidence of echo.
self.assertEqual(client.read_parameter('control_mode'),0)
count=len(peer.sent)
with self.assertRaises(PermissionError):client.write_parameter('control_mode',1)
self.assertEqual(len(peer.sent),count)
client.close()
with self.assertRaises(RuntimeError):client.read_parameter('control_mode')
def test_manual_client_mode_write_rejects_enabled_motor(self):
class Peer:
def __init__(self):self.sent=[];self.queue=queue.Queue()
def send(self,f):
self.sent.append(f)
if f.can_id==0x143:self.queue.put(Frame(67,bytes.fromhex('7fff7ff7ff410001'),extended=False))
def receive(self,timeout):
try:return self.queue.get(timeout=timeout)
except queue.Empty:return None
def close(self):pass
peer=Peer();client=ManualClient(peer,PA43Codec(67),allow_control=True,protocol_verified=True,timeout=.02)
with self.assertRaises(ValueError):client.write_parameter('control_mode',2)
self.assertEqual(len(peer.sent),1);self.assertEqual(peer.sent[0].data[-1],0xfa)
if __name__=='__main__':unittest.main()