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
