def _i2c_read(address, reg, data_p, length): ret_val = 0 msg_w = i2c_msg.write(address, [reg >> 8, reg & 0xff]) msg_r = i2c_msg.read(address, length) self._i2c.i2c_rdwr(msg_w, msg_r) if ret_val == 0: for index in range(length): data_p[index] = ord(msg_r.buf[index]) return ret_val
def _read_data_step(self, chip_addr, cmd, read_addr, addr_step, nr_bytes): write = None read = None try: write = i2c_msg.write(chip_addr, [cmd, read_addr, addr_step, nr_bytes]) read = i2c_msg.read(chip_addr, (addr_step + 1) * nr_bytes) self.smbus.i2c_rdwr(write, read) except IOError: logging.exception("Can not read from I2C Bus") self._init() return self._read_data_step(chip_addr, cmd, read_addr, addr_step, nr_bytes) return bytearray(ctypes.string_at(read.buf, read.len))
def read_humidity(self): """Reads the humidity from the sensor. Not that this call blocks for 250ms to allow the sensor to return the data""" data = [] self.bus.write_byte(self._I2C_ADDRESS, self._TRIGGER_HUMIDITY_NO_HOLD) time.sleep(0.250) read = i2c_msg.read(self._I2C_ADDRESS, 3) self.bus.i2c_rdwr(read) data=list(read) if (self._calculate_checksum(data,3)!=0): return(float('nan')) else: return self._get_humidity_from_buffer(data)
def read_scale_and_unit(i2c_bus, crc_check=False): ''' Reads the user register of the SF04 Sensirion sensor and then reads the calibration field of the sensor each setting corresponds to an EEPROM address that must then be read to get the scale factor adjustment and the flow rate units code. ---------------------------------------------------------------------------- input: i2c_bus, crc_check - type: SMBus2 bus, bool[=False] output: scale_factor, units_string, scale_crc_result, unit_crc_result - type: int, string, bool, bool ''' units_dict = { 2115: 'nl/min', 2116: 'ul/min', 2117: 'ml/min', 2100: 'ul/sec', 2133: 'ml/hr' } scale_factor_addresses = (0x2B6, 0x5B6, 0x8B6, 0xBB6, 0xEB6) user_reg, crc_byte, user_crc_result = read_user_reg(i2c_bus, crc_check) if user_crc_result is None or user_crc_result: scale_factor_address = scale_factor_addresses[(user_reg & 0x70) >> 4] write_comms(i2c_bus, _read_eeprom, [ scale_factor_address >> 4, c_uint16(scale_factor_address << 12).value >> 8 ]) read = i2c_msg.read(_sensor_address, 6) i2c_bus.i2c_rdwr(read) scale_and_unit = list(read) scale_factor = scale_and_unit[0] << 8 | scale_and_unit[1] scale_crc = scale_and_unit[2] unit_code = scale_and_unit[3] << 8 | scale_and_unit[4] unit_crc = scale_and_unit[5] scale_crc_result = None unit_crc_result = None if crc_check: scale_crc_result = check_CRC(scale_factor, scale_crc) unit_crc_result = check_CRC(unit_code, unit_crc) return (scale_factor, units_dict[unit_code], scale_crc_result, unit_crc_result) else: print( 'CRC check failed on retrieving the user register, error codes have been transmitted' ) return (0, "Error", False, False)
def luminance(self) -> float: """Get 12-bit luminance value. 8-bit value with 4-bit exponent. :meth:`SMBus.read_byte_data` is not able to read two bytes with a re-start sequence in between which is needed by MAX44009 for correct results. There will be two options: 1. Just re-read MSB byte and detected changes of MSB which is an indication for a new measurement. 2. use :method:`i2c_rdwr` which is supported by `smbus2` but unluckily does not work on Raspi (OSError: [Errno 95] Operation not supported) """ use_option = 1 if use_option == 1: # 1st option msb = 0 msb_2nd = 1 while msb != msb_2nd: msb = self.read_byte_data(Reg.luminance_msb) lsb = self.read_byte_data(Reg.luminance_lsb) msb_2nd = self.read_byte_data(Reg.luminance_msb) elif use_option == 2: # 2nd option, which does not work on rpi OSError: [Errno 95] Operation not supported wr_msb = i2c_msg.write(self.device_addr, [Reg.luminance_msb]) rd_msb = i2c_msg.read(self.device_addr, 1) wr_lsb = i2c_msg.write(self.device_addr, [Reg.luminance_lsb]) rd_lsb = i2c_msg.read(self.device_addr, 1) self.i2c_rdwr(wr_msb, rd_msb, wr_lsb, rd_lsb) msb = ord(rd_msb.data) lsb = ord(rd_lsb.data) # Convert the data to lux exponent = (msb & 0xF0) >> 4 mantissa = ((msb & 0x0F) << 4) | (lsb & 0x0F) return 2.0**exponent * mantissa * 0.045
def __init__(self, address=0x62, bus=None): '''The bus address of this device is 0x60 plus three address bits. The low-order bit (A0) depends on the value of the address input pin, while address bits A1 and A2 are set by the manufacturer. Therefore, when A0 is zero (ground), the actual address may be 0x60, 0x62, 0x64, or 0x66. When A0 is one, the actual accress may be 0x61, 0x63, 0x65, or 0x67. The device used during code development had A1 programmed by the manufacturer to be 1, which is why the default <address> value is set to 0x62. ''' i2c_device.__init__(self, address=address, bus=bus) self.m_read = i2c_msg.read(self.address, 3) self.power = 0
def i2cRdwr(self, send, recieve): """Sends a hexadecimal-command list to the desired component. The function then waits for and retrieves a response from the device with a length based on the parameters. Args: send (list): a list of bytes (sends this list of hexadecimal commands). recieve (int): the amount of bytes in a list to retrieve from the device. Returns: list: a list of bytes with length of int recieve. """ write = i2c_msg.write(self.getDeviceAddress(), send) recieve = i2c_msg.read(self.getDeviceAddress(), recieve) return self.geti2cBus().i2c_rdwr(write, recieve)
def getSerialNumber(self): """Reads the serial number from the sensor.""" serialNumber = [] err=0 write = i2c_msg.write(self._I2C_ADDRESS, [0xFA, 0x0F]) read = i2c_msg.read(self._I2C_ADDRESS, 8) self.bus.i2c_rdwr(write, read) readlistb=list(read) err+=self._calculate_checksum(readlistb[0:2],2) err+=self._calculate_checksum(readlistb[2:4],2) err+=self._calculate_checksum(readlistb[4:6],2) err+=self._calculate_checksum(readlistb[6:8],2) write = i2c_msg.write(self._I2C_ADDRESS, [0xFC, 0xC9]) read = i2c_msg.read(self._I2C_ADDRESS, 7) self.bus.i2c_rdwr(write, read) readlistac=list(read) err+=self._calculate_checksum(readlistac[0:3],3) err+=self._calculate_checksum(readlistac[3:6],3) serialNumber[0:2]=readlistac[3:5] serialNumber[2:5]=readlistb[0:8:2] serialNumber[6:8]=readlistac[0:2] if err!=0: serialNumber=[-1] return serialNumber
def i2c_read(self, i2c_addr, addr, count=1, unpack_format='H'): addr_msb = addr >> 8 & 0x00FF addr_lsb = addr & 0x00FF write = i2c_msg.write(i2c_addr, [addr_msb, addr_lsb]) read = i2c_msg.read(i2c_addr, count * 2) self.i2c.i2c_rdwr(write, read) if unpack_format is None: return bytes(list(read)) results = struct.unpack(">{}{}".format(count, unpack_format), bytes(list(read))) if count == 1: return results[0] return results
def read_adc(self): if self.connected(): # Read all 16 channels (2 bytes each) msg = i2c_msg.read(self.address, self.adc_count * 2) self.bus.i2c_rdwr(msg) temp = [x for x in msg] for i in range(self.adc_count): an = int(temp[2 * i] & 0x03) << 8 | int(temp[2 * i + 1]) self.values[i] = an else: self.values = np.zeros(self.adc_count) return self.values
def i2c_read(addr, count): wr = i2c_msg.write(cam_addr, [(addr >> 8) & 0xff, addr & 0xff]) rd = i2c_msg.read(cam_addr, count * 2) bus.i2c_rdwr(wr, rd) ret = list() hilo = True for val in rd: if hilo: hi = val hilo = False else: ret.append((hi << 8) | val) hilo = True return ret
def hw_to_memory(): """Read pin data from hardware and write to memory.""" global _BUS global _PORT_VALUE global _I2C_ADDR global _READMODE if (not _READMODE): raise ValueError( "You cannot read from the hardware as it is in write mode!") msg = i2c_msg.read(_I2C_ADDR, len(_PORT_VALUE)) if _BUS: _BUS.i2c_rdwr(msg) for k in range(msg.len): _PORT_VALUE[k] = int(msg.buf[k].hex(), 16) else: raise ReferenceError("I2C bus was not created, did you run setup?")
def read_data_ready_flag(self): result = [] write = i2c_msg.write(self.SPS_ADDR, self.R_DATA_RDY) self.bus.i2c_rdwr(write) read = i2c_msg.read(self.SPS_ADDR, 3) self.bus.i2c_rdwr(read) for i in range(read.len): result.append(bytes_to_int(read.buf[i])) if checkCRC(result): return result[1] else: return self.DATA_READY_FLAG_ERROR
def read_i2c(self, address, byte_num): """Read using the i2c protocol. Args: address: The address of the spi slave byte_num: How many bytes to read from the device. Max 32 Returns: A list with byte_num elements. """ read = i2c_msg.read(address, byte_num) self._smbus.i2c_rdwr(read) res = [ord(read.buf[i]) for i in range(byte_num)] return res
def get_sid(self): sid = 99999 try: info = [0xffff] * 3 test = 0 bus = SMBus(I2CBUS) bus.write_byte_data(self.I2Caddr, SGP30_SID_MSB, SGP30_SID_LSB) time.sleep(0.01) #resp = bus.read_i2c_block_data( self.I2Caddr, 0, 9 ) read = i2c_msg.read(self.I2Caddr, 9) bus.i2c_rdwr(read) resp = list(read) bus.close() sid_1 = [resp[0], resp[1], resp[2]] sid_2 = [resp[3], resp[4], resp[5]] sid_3 = [resp[6], resp[7], resp[8]] if (self.crc8(sid_1) == 0): info[0] = (sid_1[0] << 8) | sid_1[1] test += 1 if (self.crc8(sid_2) == 0): info[1] = (sid_2[0] << 8) | sid_2[1] test += 1 if (self.crc8(sid_3) == 0): info[2] = (sid_3[0] << 8) | sid_3[1] test += 1 if test == 3: sid = (info[0] << 32) | (info[1] << 16) | info[2] print "sgp30.get_sid(): 0x%012x" % (sid) #log Serial ID f = open(self.FptrSID, "w") f.write("%d" % sid) f.close() else: print "sgp30.get_sid(): CRC error" except: bus.close() print "sgp30.get_sid() failed" return sid
def task(self): co2_val = 99999 try: bus = SMBus(I2CBUS) write = i2c_msg.write(self.I2Caddr, K30_MSG) bus.i2c_rdwr(write) time.sleep(0.02) read = i2c_msg.read(self.I2Caddr, 4) bus.i2c_rdwr(read) resp = list(read) bus.close() #print resp cs = resp[0] + resp[1] + resp[2] cs &= 0xff # check checksum if cs == resp[3]: co2_val = (resp[1] << 8) | resp[2] # sensor provides signed int if co2_val < 32767 and co2_val > 100: # read of i2c co2 value passes muster # include it in the rolling average self.co2_buff[self.co2_ptr] = co2_val self.co2_ptr += 1 if self.co2_ptr >= CO2_MAX: self.co2_ptr = 0 co2_avg = 0 for i in range(CO2_MAX): co2_avg += self.co2_buff[i] co2_avg /= CO2_MAX #print "CO2: %d" % co2_avg f = open(self.FptrCO2, "w") f.write("%d" % co2_avg) f.close() except: bus.close() print "k30.task() failed" return co2_val
def read_data(self): # rospy.loginfo('Modulo ligado mensagem no topico /Modulo/CO2 tem a estru$ # rospy.loginfo('[Leitura CO2(ppm),Qualidade dos dados,Resistencia intern$ # rospy.loginfo('Qualidade e definida em 0 = OK ; 1 = Aquecimento ; 2 = B$ while not rospy.is_shutdown(): #self.Read_co2 = std_msgs.msg.Float32MultiArray() self.read = i2c_msg.read(self.ADDRESS,9) self.h = self.bus.i2c_rdwr(self.read) self.data_array = list(self.read) self.read_co2.data = [self.co2_plus_status(self.data_array), self.information(self.data_array), self.resistance(self.data_array), self.Tvoc(self.data_array)] self.pub.publish(self.read_co2) self.rate.sleep()
def readTrill( self ): # read the Trill device and process it according to the current mode self.prepareForDataRead() bytesToRead = kCentroidLengthDefault self.num_touches_ = 0 if ("CENTROID" == modeTypes[self.modeNumber]): if self.tt == 'square' or self.tt == 'hex': bytesToRead = kCentroidLength2D if self.tt == 'ring': bytesToRead = kCentroidLengthRing else: bytesToRead = kRawLength msg = i2c_msg.read(self.addr, bytesToRead) self.bus.i2c_rdwr(msg) self.dataBuffer = list(msg) if len(self.dataBuffer) != bytesToRead: print("only got", len(self.dataBuffer), "when asked for", bytesToRead) return 1 if "CENTROID" != modeTypes[self.modeNumber]: for i in range(0, self.getNumChannels()): self.rawData[i] = (( (self.dataBuffer[2 * i] << 8) + self.dataBuffer[2 * i + 1]) & 0x0FFF) * self.rawRescale else: locations = 0 #Look for 1st instance of 0xFFFF (no touch) in the buffer for locations in range(0, self.max_touch_1D_or_2D + 1): if self.dataBuffer[2 * locations] == 0xFF and self.dataBuffer[ 2 * locations + 1] == 0xFF: break self.num_touches_ = locations if self.tt == "square" or self.tt == 'hex': # Look for the number of horizontal touches in 2D sliders # which might be different from number of vertical touches self.topHalf = bytesToRead // 2 for locations in range(0, self.max_touch_1D_or_2D): if self.dataBuffer[ 2 * locations + self.topHalf] == 0xFF and self.dataBuffer[ 2 * locations + self.topHalf + 1] == 0xFF: break self.num_touches_ |= locations << 4 # pack into the top four bits return 0
def read_raw_data(i2c_bus, crc_check=False): ''' Reads the SF04 sensor and returns a reading and a crc8 checksum byte and checksum result. --------------------------------------------------------------------------- input: i2c_bus, crc_check - type: SMBus2 bus, bool[=False] output: raw_flow_reading, crc_byte, crc_result - type: int, int, bool ''' read = i2c_msg.read(_sensor_address, 3) i2c_bus.i2c_rdwr(read) reading_data = list(read) raw_data_val = c_int16(reading_data[0] << 8 | reading_data[1]) crc_byte = reading_data[2] crc_result = None if crc_check: crc_result = check_CRC(c_uint16(raw_data_val.value).value, crc_byte) return (raw_data_val.value, crc_byte, crc_result)
def read_int_register(register): try: if not init_okay: logging.warning("Error in initialisation of YRL039") return 0 utils.i2c_lock() i2c_bus.write_byte(s.YRL039_ADDRESS, register) msg = i2c_msg.read(s.YRL039_ADDRESS, 2) i2c_bus.i2c_rdwr(msg) utils.i2c_unlock() vals = list(msg) return (vals[0] << 8) + vals[1] except OSError: logging.error( "Error reading register from YRL039 (address %X bus %d)" % (s.YRL039_ADDRESS, s.YRL039_BUS)) return 0
def smbus_i2c_read(self, address, reg, data_p, length, addrsize=8): ret_val = 0 if addrsize == 8: msg_w = i2c_msg.write( address, [reg]) # warning this is set up for 16-bit addresses elif addrsize == 16: msg_w = i2c_msg.write( address, [reg >> 8, reg & 0xff ]) # warning this is set up for 16-bit addresses else: raise Exception("address must be 8 or 16 bits long only") msg_r = i2c_msg.read(address, length) self.i2c.i2c_rdwr(msg_w, msg_r) if ret_val == 0: for index in range(length): data_p[index] = ord(msg_r.buf[index]) return ret_val
def _i2c_read(address, reg, data_p, length): ret_val = 0 msg_w = i2c_msg.write(address, [reg >> 8, reg & 0xff]) msg_r = i2c_msg.read(address, length) # print("R: a: %x\tr:%d" % (address,reg)) try: self._i2c.i2c_rdwr(msg_w, msg_r) except: print("Cannot read on 0x%x I2C bus, reg: %d" % (address, reg)) if ret_val == 0: for index in range(length): data_p[index] = ord(msg_r.buf[index]) return ret_val
def read_measured_values(self): result = [] write = i2c_msg.write(self.SPS_ADDR, self.R_VALUES) self.bus.i2c_rdwr(write) read = i2c_msg.read(self.SPS_ADDR, 60) self.bus.i2c_rdwr(read) for i in range(read.len): result.append(bytes_to_int(read.buf[i])) if checkCRC(result): self.parse_sensor_values(result) return self.NO_ERROR else: return self.MEASURED_VALUES_ERROR
def read_auto_cleaning_interval(self): result = [] write = i2c_msg.write(self.SPS_ADDR, self.RW_AUTO_CLN) self.bus.i2c_rdwr(write) read = i2c_msg.read(self.SPS_ADDR, 6) self.bus.i2c_rdwr(read) for i in range(read.len): result.append(bytes_to_int(read.buf[i])) if checkCRC(result): result = result[0] * pow(2, 24) + result[1] * pow(2, 16) + result[3] * pow(2, 8) + result[4] return result else: return self.AUTO_CLN_INTERVAL_ERROR
def send(self, operation: list, data=None) -> list: """ Interface for communicating with the Tic stepper driver. Parameters ---------- operation : list Specifies command offset and write/read operation type. data : list Data to pass to the registers at offset specified in operation. Returns ------- read : list Data read from Tic registers, broken into a list of bytes. """ offset = operation[0] protocol = operation[1] if protocol == 'quick': # Quick write command = [offset] read = None elif protocol == 'read': # Block read command = [offset, data[0]] read = i2c_msg.read(self.address, data[1]) elif protocol == 7: # 7-bit write command = [offset, int(data)] read = None elif protocol == 32: # 32-bit write data = int(data) command = [ offset, data >> 0 & 0xFF, data >> 8 & 0xFF, data >> 16 & 0xFF, data >> 24 & 0xFF ] read = None else: warnings.warn('Protocol `{}` not recognized.'.format(protocol)) write = i2c_msg.write(self.address, command) self.bus.i2c_rdwr(write) ret = [] if read is not None: self.bus.i2c_rdwr(read) ret = list(read) return ret
def main(): bus = SMBus(1) address = [0x10, 0x11] while True: for i in range(len(address)): message = 'j,' + str(round(random.uniform(-1, 1), 4)) + ',' + str( round(random.uniform(-1, 1), 4)) print("[+]", message) message = list(message.encode('utf-8')) print("[+]", message) write = i2c_msg.write(address[i], message) read = i2c_msg.read(address[i], 16) print("[*] Issuing read/write") test = bus.i2c_rdwr(write, read) time.sleep(0.125)
def read_temp(self): temp_list = [] self.start_temp_measurement() readTemp = i2c_msg.read(self.SHT20_ADDR, 3) self.bus.i2c_rdwr(readTemp) for i in range(readTemp.len): temp_list.append(bytes_to_int(readTemp.buf[i])) if checkCRC(temp_list): temp_list[1] &= 0xFC temp = (((temp_list[0] * pow(2, 8) + temp_list[1]) * 175.72) / pow(2, 16)) - 46.85 return temp else: return self.ERROR
def read_humid(self): humid_list = [] self.start_humid_measurement() readHumid = i2c_msg.read(self.SHT20_ADDR, 3) self.bus.i2c_rdwr(readHumid) for i in range(readHumid.len): humid_list.append(bytes_to_int(readHumid.buf[i])) if checkCRC(humid_list): humid_list[1] &= 0xFC humid = (((humid_list[0] * pow(2, 8) + humid_list[1]) * 125) / pow(2, 16)) - 6 return humid else: return self.ERROR
def _get_response(self, length, retries=20): self.wakeup() for _ in range(retries): try: r_msg = i2c_msg.read(self._address, length + 3) self._i2c.i2c_rdwr(r_msg) response = bytes(r_msg) break except OSError: pass else: raise RuntimeError("Failed to read data from chip") if self._debug: print("\tReceived: ", [hex(i) for i in response]) crc = response[-2] | (response[-1] << 8) crc2 = self._at_crc(response[0:-2]) if crc != crc2: raise RuntimeError("CRC Mismatch") return response[1:-2]
def read_from_custom_memory(index, number_of_bytes): """ Read data from specified custom memory address in the CO2 sensor :param index: index of the data to be read (see sensor doc) :param number_of_bytes: number of bytes to read (see sensor doc) :return: list[bytes] from right to left """ logger.info("Reading " + str(number_of_bytes) + " bytes from customer memory at index " + str(hex(index)) + "...") write = i2c_msg.write( CO2_address, [0x71, 0x54, index ]) # usual bytes to send/write to initiate the reading attempts = 1 read = [] # avoid return issue while attempts < 4: try: with SMBus(1) as bus: bus.i2c_rdwr(write) read = i2c_msg.read(CO2_address, number_of_bytes) with SMBus(1) as bus: bus.i2c_rdwr(read) break # break the trial loop if the above has not failed except: # if i2c communication fails if attempts >= 3: logger.warning( "i2c communication failed 3 times while writing to customer memory, skipping reading" ) return False # indicate that the writing process failed, exit this function else: logger.error( "i2c communication failed to read from customer memory (" + str(attempts) + "/3)") attempts += 1 print("Waiting 3 seconds...", end='\r') time.sleep( 3 ) # avoid too close i2c communication, let time to the sensor, may be busy reading = list(read) logger.info("Reading from custom memory returned " + str(reading)) return reading
def read(num): #with SMBusWrapper(1) as bus: #msg = [bus.read_byte(address) for x in range(0,num)] msg = i2c_msg.read(address, num) bus.i2c_rdwr(msg) return list(msg)
def client(): ################################################################################################ # MAKE CONNECTION TO SERVER ################################################################################################ poll = select.poll() go_go_go = False pack_format = "=ffffb?" pack_size = struct.calcsize(pack_format) unpack_format = "=?" unpack_size = struct.calcsize(unpack_format) client = socket.socket() addr = "192.168.42.1" port = 31415 ''' client_fd = client.fileno() poll.register(client_fd, select.POLLIN | select.POLLPRI) ''' ################################################################################################ # SET UP THE JOYSTICKS FSM ################################################################################################ RC_PASSIVE = 0 RC_TAKEOFF = 1 RC_FLYING = 2 RC_LANDING = 3 RC_DONE = 4 RC_ABORT = 5 state_name = ["PASSIVE", "TAKEOFF", "FLYING", "LANDING", "DONE", "ABORT"] status_quo = [0.0, 0.0, 0.0, 0.0] #----------------------------------------------------------------------------------------------- # Set up GPIO for button and buzzer #----------------------------------------------------------------------------------------------- GPIO.setmode(GPIO.BCM) GPIO_BUTTON = 10 GPIO.setup(GPIO_BUTTON, GPIO.IN, GPIO.PUD_DOWN) # GPIO button pushed pulls GPIO high GPIO_BUZZER = 24 GPIO.setup(GPIO_BUZZER, GPIO.OUT) # GPIO buzzer set high sounds the buzzer GPIO.output(GPIO_BUZZER, GPIO.LOW) #----------------------------------------------------------------------------------------------- # Acquire contact with the piDrone; only continue below once acquired. #----------------------------------------------------------------------------------------------- with SMBusWrapper(1) as bus: try: while True: #----------------------------------------------------------------------------------- # Connect to the server #----------------------------------------------------------------------------------- print "CONNECTING..." while True: try: client.connect((addr, port)) except: time.sleep(0.1) else: break print "CONNECTED." #----------------------------------------------------------------------------------- # We are connected to the piDrone. #----------------------------------------------------------------------------------- client_fd = client.fileno() poll.register(client_fd, select.POLLIN | select.POLLPRI) connected = True #----------------------------------------------------------------------------------- # Wait for a piDrone status update for go / stop #----------------------------------------------------------------------------------- go_go_go = False while connected: ################################################################################ # HANDLE MESSAGES FROM THE SERVER ################################################################################ #------------------------------------------------------------------------------- # Sleep pending an input for the piDrone #------------------------------------------------------------------------------- results = poll.poll(200) # milliseconds #------------------------------------------------------------------------------- # Check whether there's I/O from RC #------------------------------------------------------------------------------- for fd, event in results: assert fd == client_fd, "WTF HAPPENED HERE" #--------------------------------------------------------------------------- # Has piDrone told piRC to start? #--------------------------------------------------------------------------- raw = client.recv(unpack_size) assert (len(raw) == unpack_size), "Invalid data" #--------------------------------------------------------------------------- # React on the action from the message from the piDrone #--------------------------------------------------------------------------- formatted = struct.unpack(unpack_format, raw) if formatted[0]: # is True GPIO.add_event_detect(GPIO_BUTTON, GPIO.FALLING) #----------------------------------------------------------------------- # piDrone is connected, start the flight #----------------------------------------------------------------------- go_go_go = True state = RC_TAKEOFF takeoff_time = time.time() print "GO GO GO" else: GPIO.remove_event_detect(GPIO_BUTTON) connected = False #----------------------------------------------------------------------- # piDrone is disconnected #----------------------------------------------------------------------- go_go_go = False poll.unregister(client_fd) client.close() print "STOP STOP STOP" #--------------------------------------------------------------------------- # If we're now not connected, report back. #--------------------------------------------------------------------------- if not connected: continue #--------------------------------------------------------------------------- # Are we ready to go? #--------------------------------------------------------------------------- if not go_go_go: continue #------------------------------------------------------------------------------- # The piDrone is live and kicking; set the next flight increment. # # UD = Up / Down - upwards is positive # YR = Yaw Rate - anticlockwise is positive # LR = Left / Right - leftwards is positive # FB = Forwards / Backwards - forwards is positive #------------------------------------------------------------------------------- msg = i2c_msg.read(0x40, 2) bus.i2c_rdwr(msg) data = list(msg) assert (len(data) == 2), "Joystick 0 data len: %d" % len(data) if (data[0] > 127): UD = data[0] - 256 else: UD = data[0] if (data[1] > 127): YR = data[1] - 256 else: YR = data[1] msg = i2c_msg.read(0x41, 2) bus.i2c_rdwr(msg) data = list(msg) assert (len(data) == 2), "Joystick 1 data len: %d" % len(data) if (data[0] > 127): LR = data[0] - 256 else: LR = data[0] if (data[1] > 127): FB = -(data[1] - 256) else: FB = -data[1] #=============================================================================== # FSM INPUT, STATES, OUTPUT #=============================================================================== beep = False #------------------------------------------------------------------------------- # Have we have a falling edge from the button? #------------------------------------------------------------------------------- if GPIO.event_detected(GPIO_BUTTON): #--------------------------------------------------------------------------- # If we're flying, start landing otherise abort #--------------------------------------------------------------------------- if state == RC_FLYING: print "landing-landing-landing" state = RC_LANDING landing_time = time.time() else: print "abort-abort-abort" state = RC_ABORT #=============================================================================== # FSM INPUTS #=============================================================================== if state == RC_TAKEOFF: if time.time() - takeoff_time < 3.0: # seconds UD = 0.33 YR = 0.0 LR = 0.0 FB = 0.0 else: UD, YR, FB, LR = status_quo state = RC_FLYING beep = True #----------------------------------------------------------------------- # Take the timestamp of flight, and initiate initial height #----------------------------------------------------------------------- before = time.time() height = 1.0 elif state == RC_FLYING: #----------------------------------------------------------------------- # Joysticks are +/- 80, convert these to +/- 1m/s. The exception is the # yaw rate where +/-80 maps to +/- 90 degrees (pi/2) per second #----------------------------------------------------------------------- UD /= 80 YR /= (80 * 2 / math.pi) FB /= 80 LR /= 80 #----------------------------------------------------------------------- # Integrate the height increment and mark the landing timestamo #----------------------------------------------------------------------- now = time.time() dt = now - before before = now height += UD * dt #RC! Flawed: future velocity x historic dt landing_period = height / 0.33 + 1 #RC! OK? elif state == RC_LANDING: if time.time() - landing_time < landing_period: # seconds UD = -0.33 YR = 0.0 FB = 0.0 LR = 0.0 beep = True else: UD, YR, FB, LR = status_quo state = RC_DONE beep = False else: assert state == RC_ABORT, "Should be on abort here!" UD, YR, FB, LR = status_quo output = struct.pack(pack_format, UD, YR, FB, LR, state, beep) client.send(output) print "SENT: UD = %f | YR = %f | FB = %f | LR = %f | status = %s | beep = %d" % (UD, YR, FB, LR, state_name[state], beep) else: print "DISCONNECTED DISCONNECTED DISCONNECTED" except KeyboardInterrupt: pass else: pass finally: pass