diff --git a/network_interface.py b/network_interface.py --- a/network_interface.py +++ b/network_interface.py @@ -1,12 +1,12 @@ -#!/usr/bin/env python - import os import struct import _thread from abc import ABCMeta, abstractmethod +import logging from message import HydroBotMessage import module +import protocol from canard import can from canard.hw import socketcan @@ -14,27 +14,27 @@ from canard.utils import queue class NetworkInterface(metaclass=ABCMeta): - def __init__(self, network, logger): + def __init__(self, network): + self.logger = logging.getLogger('hydrobot') self.network = network - self.logger = logger @abstractmethod def start(self): - print("Network: start") + self.logger.info("Network: start") @abstractmethod def send_message(self, message): pass def process_message(self, message): - print("Network Interface: process message") + self.logger.debug("Network Interface: process message") class CanBusNetworkInterface(NetworkInterface): - def __init__(self, network, interface_name, logger): - super(CanBusNetworkInterface, self).__init__(network, logger) + def __init__(self, network, interface_name): + super(CanBusNetworkInterface, self).__init__(network) # Bring up CAN interface (maybe do this in a systemd service file) # Passing random arguments to sudo is super dangerous @@ -44,12 +44,13 @@ class CanBusNetworkInterface(NetworkInte self.dev = socketcan.SocketCanDev(interface_name) self.queue = queue.CanQueue(self.dev) + #bidirectional address <==> uuid lookup self.address_lookup = {} self.uuid_lookup = {} def start(self): super(CanBusNetworkInterface, self).start() - print("CanBusNetworkInterface: start") + self.logger.info("CanBusNetworkInterface: start") self.queue.start() _thread.start_new_thread(self.process_message, ()) @@ -63,23 +64,30 @@ class CanBusNetworkInterface(NetworkInte while True: frame = self.queue.recv() if frame != None: - print("CanBusNetworkInterface: process message") + + base_id = frame.id & 0x7fe - if not frame.id in self.address_lookup: - new_module = self.network.module_list.new_module(frame.data[0] & 0x7f, frame.id, self) - self.address_lookup[frame.id] = new_module.uuid - self.uuid_lookup[new_module.uuid] = frame.id + if not base_id in self.address_lookup: + self.logger.debug("Uknown module address: " + hex(base_id)) + device_id = frame.data[0] & 0x7f + new_module = self.network.module_list.new_module(device_id, base_id, self) + self.address_lookup[base_id] = new_module.uuid + self.uuid_lookup[new_module.uuid] = base_id self.logger.debug("Received CAN message! ID: " + hex(frame.id)) - message = HydroBotMessage(self.address_lookup[frame.id], frame.data[0], ((frame.data[1] << 8) + frame.data[2]), frame.data[3], ((frame.data[4] << 24) + (frame.data[5] << 16) + (frame.data[6] << 8) + frame.data[7])) - print(message) + + data = [frame.data[7], frame.data[6], frame.data[5], frame.data[4]] + b = struct.pack('4B', *data) + f_data = struct.unpack('>f', b)[0] + message = HydroBotMessage(self.address_lookup[base_id], frame.data[0], ((frame.data[1] << 8) + frame.data[2]), frame.data[3], f_data) + self.logger.debug("Process message: " + str(message)) self.network.process_message(message) class WifiNetworkInterface(NetworkInterface): - def __init__(self, network, logger): - super(WifiNetworkInterface, self).__init__(network, logger) + def __init__(self, network): + super(WifiNetworkInterface, self).__init__(network) def start(self): pass @@ -90,7 +98,3 @@ class WifiNetworkInterface(NetworkInterf def process_message(self, message): pass - - -#module knows how to send and recieve messages -