Files
@ 7b33cc2da5f0
Branch filter:
Location: HydroBot/hydrobot-software/network_interface.py
7b33cc2da5f0
4.0 KiB
text/x-python
Added new gas sensing and other stuff from a long time ago
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 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 | 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
from canard.utils import queue
class NetworkInterface(metaclass=ABCMeta):
def __init__(self, network):
self.logger = logging.getLogger('hydrobot')
self.network = network
@abstractmethod
def start(self):
self.logger.info("Network: start")
@abstractmethod
def send_message(self, message):
pass
def process_message(self, message):
self.logger.debug("Network Interface: process message")
class CanBusNetworkInterface(NetworkInterface):
def __init__(self, network, interface_name):
super(CanBusNetworkInterface, self).__init__(network)
try:
# Bring up CAN interface (maybe do this in a systemd service file)
# Passing random arguments to sudo is super dangerous
os.system("sudo ip link set " + interface_name + " up type can bitrate 500000")
self.interface_name = interface_name
self.queue = None
self.dev = socketcan.SocketCanDev(interface_name)
self.queue = queue.CanQueue(self.dev)
except:
self.logger.error("Can Bus failed to initilize")
#bidirectional address <==> uuid lookup
self.address_lookup = {}
self.uuid_lookup = {}
def start(self):
super(CanBusNetworkInterface, self).start()
self.logger.info("CanBusNetworkInterface: start")
if self.queue != None:
self.queue.start()
_thread.start_new_thread(self.process_message, ())
else:
self.logger.error("Can Bus failed to start")
def send_message(self, message):
address = self.uuid_lookup[message.module_uuid]
can_frame = can.Frame(address, 8, [message.message_type, (message.data_key>>8) & 0xff, (message.data_key>>0) & 0xff, message.sensor_num] + list(struct.pack("f", message.data)))
self.logger.debug("Send CAN message! CAN ID: " + hex(can_frame.id) + " Data: " + str(can_frame.data))
if self.queue != None:
self.queue.send(can_frame)
else:
self.logger.error("Can Bus is not initilized")
def process_message(self):
while True:
if self.queue != None:
frame = self.queue.recv()
if frame != None:
base_id = frame.id & 0x7fe
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))
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)
else:
self.logger.error("Can Bus is not initilized")
class WifiNetworkInterface(NetworkInterface):
def __init__(self, network):
super(WifiNetworkInterface, self).__init__(network)
def start(self):
pass
def send_message(self, message):
pass
def process_message(self, message):
pass
|