class RegisterIO_SPI: def __init__(self, port, bitrate=None): if bitrate is None: bitrate = DEFAULT_SPI_BITRATE_HZ simPort = None if hal.HALIsSimulation(): from ._impl.sim_io import NavXSPISim simPort = NavXSPISim() self.port = SPI(port, simPort=simPort) self.bitrate = bitrate self.mutex = threading.Lock() def init(self): logger.info("Initializing NavX SPI") self.port.setClockRate(self.bitrate) self.port.setMSBFirst() self.port.setSampleDataOnFalling() self.port.setClockActiveLow() self.port.setChipSelectActiveLow() logger.info("Initialized at bitrate %s", self.bitrate) return True def write(self, address, value): data = [address | 0x80, value] data.append(crc7(data)) with self.mutex: if self.port.write(data) != len(data): return False return True def read(self, first_address, count): data = [first_address, count] data.append(crc7(data)) with self.mutex: retcount = self.port.write(data) if retcount != len(data): raise IOError("Write error (%s != %s)" % (retcount, len(data))) # FIXME if not hal.isSimulation(): Timer.delay(0.001) data = self.port.read(True, count + 1) if len(data) != count + 1: raise IOError("Read error (%s != %s)" % (len(data), count + 1)) crc = data[-1] data = data[:-1] if crc7(data) != crc: raise IOError("CRC error") return data def shutdown(self): # TODO: should free? logger.info("NavX i2c shutdown") self.port.close() return True