-
Notifications
You must be signed in to change notification settings - Fork 0
/
recsewip.py
48 lines (43 loc) · 1.69 KB
/
recsewip.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
import can
import time
import struct
import datetime
def milliseconds_since_midnight():
now = datetime.datetime.now()
midnight = datetime.datetime(now.year, now.month, now.day)
return int((now - midnight).total_seconds() * 1000) # Convert to milliseconds
def send_response(bus, arbitration_id, received_time):
response_time = milliseconds_since_midnight()
one_way_latency = response_time - received_time # Calculate one-way latency (a1)
data = struct.pack('II', one_way_latency, response_time) # Pack a1 and response_time
message = can.Message(arbitration_id=arbitration_id, data=data, is_extended_id=False)
try:
bus.send(message)
print(f"Sent response with latency {one_way_latency} ms on {bus.channel_info}")
print(f"Response data: {data}")
except can.CanError:
print("Response NOT sent")
def listen_and_respond(bus, arbitration_id):
while True:
message = bus.recv(60.0) # Timeout in seconds
if message is None:
print("Timeout occurred, no message.")
continue
if message.arbitration_id == arbitration_id:
received_time = milliseconds_since_midnight()
print(f"Message received: {message}")
print(f"Received data: {message.data}")
send_response(bus, arbitration_id, received_time)
def main():
global bus
bus = can.interface.Bus(channel='can0', bustype='socketcan', bitrate=500000)
print("Listening for messages...")
listen_and_respond(bus, 749)
if __name__ == "__main__":
try:
main()
except KeyboardInterrupt:
print("\nInterrupted by user.")
finally:
print("Closing CAN bus.")
bus.shutdown()