class I2cBus(BaseI2cDriver.I2cBus):
def __init__(self, bus_id):
super().__init__(bus_id)
try:
self.bus = SMBus(bus_id)
except Exception as e:
raise InvalidDriverError(
'Unable to initialize i2c connection with bus {}. Check if i2c bus is avalable.'.format(bus_id), e)
def close(self):
if self.bus is not None:
self.bus.close()
def read_byte(self, addr: int, register: int) -> int:
return self.bus.read_byte_data(addr, register)
def read_word(self, addr: int, register: int) -> int:
return self.bus.read_word_data(addr, register)
def read_block(self, addr: int, register, length: int) -> List[int]:
return self.bus.read_i2c_block_data(addr, register, length)
def write_byte_data(self, addr, register, value):
self.bus.write_byte_data(addr, register, value)
def write_word_data(self, addr, register, value):
self.bus.write_word_data(addr, register, value)
def write_block_data(self, addr, register, data):
self.bus.write_i2c_block_data(addr, register, data)
class INA233:
CLEAR_FAULTS = 0x03
RESTORE_DEFAULT_ALL = 0x12
CAPABILITY = 0x19
IOUT_OC_WARN_LIMIT = 0x4A
VIN_OV_WARN_LIMIT = 0x57
VIN_UV_WARN_LIMIT = 0x58
PIN_OP_WARN_LIMIT = 0x6B
STATUS_BYTE = 0x78
STATUS_WORD = 0x79
STATUS_IOUT = 0x79
STATUS_IOUT = 0x7B
STATUS_INPUT = 0x7C
STATUS_CML = 0x7E
STATUS_MFR_SPECIFIC = 0x80
READ_EIN = 0x86
READ_VIN = 0x88
READ_IIN = 0x89
READ_VOUT = 0x8B
READ_IOUT = 0x8C
READ_POUT = 0x96
READ_PIN = 0x97
MFR_ID = 0x99
MFR_MODEL = 0x9A
MFR_REVISION = 0x9B
MFR_ADC_CONFIG = 0xD0
MFR_READ_VSHUNT = 0xD1
MFR_ALERT_MASK = 0xD2
MFR_CALIBRATION = 0xD4
MFR_DEVICE_CONFIG = 0xD5
CLEAR_EIN = 0xD6
TI_MFR_ID = 0xE0
TI_MFR_MODEL = 0xE1
TI_MFR_REVISION = 0xE2
# from table 1 p 17 of INA233 documentation pdf
#SHUNT VOLTAGE TELEMETRY & WARNING COEFFICIENTS
_m_vs = 4
_R_vs = 5
_b_vs = 0
_m_c =0
_R_c = 0
_m_p = 0
_R_p = 0
#BUS VOLTAGE TELEMETRY & WARNING COEFFICIENTS
_R_vb = 2
_b_vb = 0
_m_vb = 8
# CURRENT & POWER CONSTANT TELEMETRY & WARNING COEFFICIENTS
_b_c = 0
_b_p = 0
_BUS_MILLIVOLTS_LSB = 0.00125
_SHUNT_MILLIVOLTS_LSB = 0.0000025
_accumulator_24 = 0
_sample_count = 0
def __init__(self, bus, address):
self._bus = SMBus(bus)
self._address = address
def calibrate(self, R_shunt, I_max):
""" Calibration and scaling values per section 7.5.2
of TI INA233 datasheet
"""
self._R_shunt = R_shunt
self._I_max = I_max
self._Current_LSB = 0
self._Power_LSB = 0
self._CAL = 0
tmp = 0
round_done = False
ERROR = 0
self._Current_LSB=self._I_max/(pow(2,15))
self._Power_LSB=25*self._Current_LSB
self._CAL=0.00512/(self._R_shunt*self._Current_LSB)
#check if CAL is in the uint16 range
if self._CAL>0xFFFF:
ERROR=1
else:
self._bus.write_word_data(self._address,self.MFR_CALIBRATION,int(self._CAL))
self._m_c=1/self._Current_LSB
self._m_p=1/self._Power_LSB
#Calculate m and R for maximum accuracy in current measurement
tmp=int(self._m_c)
while ((tmp > 32768) or (tmp < -32768)):
self._m_c=self._m_c/10
self._R_c = self._R_c + 1
tmp=int(self._m_c)
while round_done==False:
tmp=int(self._m_c)
if tmp==self._m_c:
round_done=True
else:
tmp=int(self._m_c*10) #shift decimal to the right
if ((tmp>32768) or (tmp<-32768)): #m_c is out of int16 (-32768 to 32768)
round_done=True
else:
self._m_c=self._m_c*10
self._R_c = self._R_c - 1
round_done=False
#Calculate m and R for maximum accuracy in power measurement
tmp = int(self._m_p)
while tmp>32768 or tmp<-32768:
self._m_p=self._m_p/10
self._R_p = self._R_p + 1
tmp = int(self._m_p)
while round_done == False:
tmp = int(self._m_p)
if tmp==self._m_p:
round_done=True
else:
tmp = int(self._m_p*10) #shift decimal to the right
if tmp>32768 or tmp<-32768: #m_p is out of int16 (-32768 to 32768)
round_done=True
else:
self._m_p = self._m_p*10
self._R_p = self._R_p - 1
self._m_c = int(self._m_c)
self._m_p = int(self._m_p)
def _getBusVoltageIn_raw(self):
raw_read = self._bus.read_word_data(self._address, self.READ_VIN)
return int(raw_read)
def _getBusVoltageOut_raw(self):
raw_read = self._bus.read_word_data(self._address, self.READ_VOUT)
return int(raw_read)
def _getShuntVoltage_raw(self):
raw_read = self._bus.read_word_data(self._address, self.MFR_READ_VSHUNT)
return int(raw_read)
def _getCurrentOut_raw(self):
raw_read = self._bus.read_i2c_block_data(self._address, self.READ_IOUT, 2)
return raw_read[0] * 256 + raw_read[1]
def _getCurrentIn_raw(self):
raw_read = self._bus.read_i2c_block_data(self._address, self.READ_IIN, 2)
return raw_read[0] * 256 + raw_read[1]
def getShuntVoltage_mV(self):
raw_read = self._getShuntVoltage_raw()
#return ((raw_read*pow(10,-self._R_vs)-self._b_vs)/self._m_vs)
return raw_read * self._SHUNT_MILLIVOLTS_LSB * 1000
def getBusVoltageIn_V(self):
raw_read = self._getBusVoltageIn_raw()
#return ((raw_read*pow(10,-self._R_vb)-self._b_vb)/self._m_vb)
return raw_read * self._BUS_MILLIVOLTS_LSB
def getBusVoltageOut_V(self):
raw_read = self._getBusVoltageOut_raw()
#return ((raw_read*pow(10,-self._R_vb)-self._b_vb)/self._m_vb)
return raw_read * self._BUS_MILLIVOLTS_LSB
def getCurrentIn_mA(self):
word_rdata=self._getCurrentIn_raw()
current_twos_compliment = word_rdata
current_sign_bit = current_twos_compliment >> 15
if(current_sign_bit == 1):
current = float(self._twos_compliment_to_int(current_twos_compliment, 16)) * self._Current_LSB
else:
#current =(value*(pow(10,-self._R_c))-self._b_c)/self._m_c
current = float(current_twos_compliment) * self._Current_LSB
return current
def getCurrentOut_mA(self):
word_rdata=self._getCurrentOut_raw()
current_twos_compliment = word_rdata
current_sign_bit = current_twos_compliment >> 15
if(current_sign_bit == 1):
current = float(self._twos_compliment_to_int(current_twos_compliment, 16)) * self._Current_LSB
else:
#current =(value*(pow(10,-self._R_c))-self._b_c)/self._m_c
current = float(current_twos_compliment) * self._Current_LSB
return current
def _getPower_raw(self):
raw_read = self._bus.read_word_data(self._address, self.READ_PIN)
return int(raw_read)
def _getEnergy_raw(self):
raw_read = self._bus.read_i2c_block_data(self._address,self.READ_EIN,6)
self._accumulator=(raw_read[0] << 8) | raw_read[1]
self._roll_over=raw_read[2]
self._sample_count=raw_read[5]<< 16
self._sample_count=(raw_read[4]<< 8) | self._sample_count
self._sample_count=(raw_read[3] | self._sample_count)
def getAv_Power_mW(self):
raw_av_power=0
av_power=0
# prev_accumulator_24 = self._accumulator_24
# prev_sample_count = self._sample_count
self._getEnergy_raw()
#Total Accumulated Unscaled Power (Accumulator_24) = (rollover_count × 2^16) + Accumulator
self._accumulator_24=int(self._roll_over)*65536+int(self._accumulator)
# raw_av_power=(self._accumulator_24-prev_accumulator_24)/(self._sample_count-prev_sample_count)
# doing it this way may be less accurate, but it avoids the divide by zero in the first reading
raw_av_power=(self._accumulator_24)/(self._sample_count)
#av_power=(raw_av_power*pow(10,-self._R_p)-self._b_p)/self._m_p
av_power = raw_av_power * self._Power_LSB
return av_power * 1000
def getPower_mW(self):
raw_read=self._getPower_raw()
#power =(raw_read*pow(10,-self._R_p)-self._b_p)/self._m_p
power = raw_read * self._Power_LSB
return power
def _twos_compliment_to_int(self, val, bits):
if (val & (1 << (bits - 1))) != 0:
val = val - (1 << bits)
return val
class AntennaDeployer(Device):
BUS_NUMBER = 1
PRIMARY_ADDRESS = 0x31
SECONDARY_ADDRESS = 0x32
EXPECTED_BYTES = {
AntennaDeployerCommand.SYSTEM_RESET: 0,
AntennaDeployerCommand.WATCHDOG_RESET: 0,
AntennaDeployerCommand.ARM_ANTS: 0,
AntennaDeployerCommand.DISARM_ANTS: 0,
AntennaDeployerCommand.DEPLOY_1: 0,
AntennaDeployerCommand.DEPLOY_2: 0,
AntennaDeployerCommand.DEPLOY_3: 0,
AntennaDeployerCommand.DEPLOY_4: 0,
AntennaDeployerCommand.AUTO_DEPLOY: 0,
AntennaDeployerCommand.CANCEL_DEPLOY: 0,
AntennaDeployerCommand.DEPLOY_1_OVERRIDE: 0,
AntennaDeployerCommand.DEPLOY_2_OVERRIDE: 0,
AntennaDeployerCommand.DEPLOY_3_OVERRIDE: 0,
AntennaDeployerCommand.DEPLOY_4_OVERRIDE: 0,
AntennaDeployerCommand.GET_TEMP: 2,
AntennaDeployerCommand.GET_STATUS: 2,
AntennaDeployerCommand.GET_COUNT_1: 1,
AntennaDeployerCommand.GET_COUNT_2: 1,
AntennaDeployerCommand.GET_COUNT_3: 1,
AntennaDeployerCommand.GET_COUNT_4: 1,
AntennaDeployerCommand.GET_UPTIME_1: 2,
AntennaDeployerCommand.GET_UPTIME_2: 2,
AntennaDeployerCommand.GET_UPTIME_3: 2,
AntennaDeployerCommand.GET_UPTIME_4: 2,
}
def __init__(self):
super().__init__("AntennaDeployer")
self.bus = SMBus()
def write(self, command: AntennaDeployerCommand, parameter: int) -> bool or None:
"""
Wrapper for SMBus write word data
:param command: (AntennaDeployerCommand) The antenna deployer command to run
:param parameter: (int) The parameter to pass in to the command (usually 0x00)
:return: (bool or None) success
"""
if type(command) != AntennaDeployerCommand:
return
try:
self.bus.open(self.BUS_NUMBER)
self.bus.write_word_data(self.PRIMARY_ADDRESS, command.value, parameter)
self.bus.close()
except:
return False
return True
def read(self, command: AntennaDeployerCommand) -> bytes or None:
"""
Wrapper for SMBus to read from AntennaDeployer
:param command: (AntennaDeployerCommand) The antenna deployer command to run
:return: (ctypes.LP_c_char, bool) buffer, success
"""
if type(command) != AntennaDeployerCommand:
return
success = self.write(command, 0x00)
if not success:
return None, False
time.sleep(0.5)
try:
i2c_obj = i2c_msg.read(self.PRIMARY_ADDRESS, self.EXPECTED_BYTES[command])
self.bus.open(self.BUS_NUMBER)
self.bus.i2c_rdwr(i2c_obj)
self.bus.close()
return i2c_obj.buf, True
except:
return None, False
def functional(self):
"""
:return: (bool) i2c file opened by SMBus
"""
return self.bus.fd is not None
def reset(self):
"""
Resets the Microcontroller on the ISIS Antenna Deployer
:return: (bool) no error
"""
try:
self.bus.open(self.BUS_NUMBER)
self.write(AntennaDeployerCommand.SYSTEM_RESET, 0x00)
self.bus.close()
return True
except:
return False
def disable(self):
"""
Disarms the ISIS Antenna Deployer
"""
try:
self.bus.open(self.BUS_NUMBER)
self.write(AntennaDeployerCommand.DISARM_ANTS, 0x00)
self.bus.close()
return True
except:
return False
def enable(self):
"""
Arms the ISIS Antenna Deployer
"""
try:
self.bus.open(self.BUS_NUMBER)
self.write(AntennaDeployerCommand.ARM_ANTS, 0x00)
self.bus.close()
return True
except:
return False
class PanTilt:
"""PanTilt HAT Driver
Communicates with PanTilt HAT over i2c
to control pan, tilt and light functions
"""
REG_CONFIG = 0x00
REG_SERVO1 = 0x01
REG_SERVO2 = 0x03
REG_WS2812 = 0x05
REG_UPDATE = 0x4E
UPDATE_WAIT = 0.03
NUM_LEDS = 24
def __init__(self,
enable_lights=True,
idle_timeout=2, # Idle timeout in seconds
light_mode=WS2812,
light_type=RGB,
servo1_min=575,
servo1_max=2325,
servo2_min=575,
servo2_max=2325,
address=0x15,
i2c_bus=None):
self._is_setup = False
self._idle_timeout = idle_timeout
self._servo1_timeout = None
self._servo2_timeout = None
self._i2c_retries = 10
self._i2c_retry_time = 0.01
self._enable_servo1 = False
self._enable_servo2 = False
self._enable_lights = enable_lights
self._light_on = 0
self._servo_min = [servo1_min, servo2_min]
self._servo_max = [servo1_max, servo2_max]
self._light_mode = light_mode
self._light_type = light_type
self._i2c_address = address
self._i2c = i2c_bus
def setup(self):
if self._is_setup:
return True
if self._i2c is None:
try:
from smbus2 import SMBus
self._i2c = SMBus(1)
except ImportError:
if version_info[0] < 3:
raise ImportError("This library requires python-smbus\nInstall with: sudo apt-get install python-smbus")
elif version_info[0] == 3:
raise ImportError("This library requires python3-smbus\nInstall with: sudo apt-get install python3-smbus")
self.clear()
self._set_config()
atexit.register(self._atexit)
self._is_setup = True
def _atexit(self):
if self._servo1_timeout is not None:
self._servo1_timeout.cancel()
if self._servo2_timeout is not None:
self._servo2_timeout.cancel()
self._enable_servo1 = False
self._enable_servo2 = False
self._set_config()
def idle_timeout(self, value):
"""Set the idle timeout for the servos
Configure the time, in seconds, after which the servos will be automatically disabled.
:param value: Timeout in seconds
"""
self._idle_timeout = value
def _set_config(self):
"""Generate config value for PanTilt HAT and write to device."""
config = 0
config |= self._enable_servo1
config |= self._enable_servo2 << 1
config |= self._enable_lights << 2
config |= self._light_mode << 3
config |= self._light_on << 4
self._i2c_write_byte(self.REG_CONFIG, config)
def _check_int_range(self, value, value_min, value_max):
"""Check the type and bounds check an expected int value."""
if type(value) is not int:
raise TypeError("Value should be an integer")
if value < value_min or value > value_max:
raise ValueError("Value {value} should be between {min} and {max}".format(
value=value,
min=value_min,
max=value_max))
def _check_range(self, value, value_min, value_max):
"""Check the type and bounds check an expected int value."""
if value < value_min or value > value_max:
raise ValueError("Value {value} should be between {min} and {max}".format(
value=value,
min=value_min,
max=value_max))
def _servo_us_to_degrees(self, us, us_min, us_max):
"""Converts pulse time in microseconds to degrees
:param us: Pulse time in microseconds
:param us_min: Minimum possible pulse time in microseconds
:param us_max: Maximum possible pulse time in microseconds
"""
self._check_range(us, us_min, us_max)
servo_range = us_max - us_min
angle = (float(us - us_min) / float(servo_range)) * 180.0
return int(round(angle, 0)) - 90
def _servo_degrees_to_us(self, angle, us_min, us_max):
"""Converts degrees into a servo pulse time in microseconds
:param angle: Angle in degrees from -90 to 90
"""
self._check_range(angle, -90, 90)
angle += 90
servo_range = us_max - us_min
us = (servo_range / 180.0) * angle
return us_min + int(us)
def _servo_range(self, servo_index):
"""Get the min and max range values for a servo"""
return (self._servo_min[servo_index], self._servo_max[servo_index])
def _i2c_write_block(self, reg, data):
if type(data) is list:
for x in range(self._i2c_retries):
try:
self._i2c.write_i2c_block_data(self._i2c_address, reg, data)
return
except IOError:
time.sleep(self._i2c_retry_time)
continue
raise IOError("Failed to write block")
else:
raise ValueError("Value must be a list")
def _i2c_write_word(self, reg, data):
if type(data) is int:
for x in range(self._i2c_retries):
try:
self._i2c.write_word_data(self._i2c_address, reg, data)
return
except IOError:
time.sleep(self._i2c_retry_time)
continue
raise IOError("Failed to write word")
def _i2c_write_byte(self, reg, data):
if type(data) is int:
for x in range(self._i2c_retries):
try:
self._i2c.write_byte_data(self._i2c_address, reg, data)
return
except IOError:
time.sleep(self._i2c_retry_time)
continue
raise IOError("Failed to write byte")
def _i2c_read_byte(self, reg):
for x in range(self._i2c_retries):
try:
return self._i2c.read_byte_data(self._i2c_address, reg)
except IOError:
time.sleep(self._i2c_retry_time)
continue
raise IOError("Failed to read byte")
def _i2c_read_word(self, reg):
for x in range(self._i2c_retries):
try:
return self._i2c.read_word_data(self._i2c_address, reg)
except IOError:
time.sleep(self._i2c_retry_time)
continue
raise IOError("Failed to read byte")
def clear(self):
"""Clear the buffer."""
self._pixels = [0] * self.NUM_LEDS * 3
self._pixels += [1]
def light_mode(self, mode):
"""Set the light mode for attached lights.
PanTiltHAT can drive either WS2812 or SK6812 pixels,
or provide a PWM dimming signal for regular LEDs.
* PWM - PWM-dimmable LEDs
* WS2812 - 24 WS2812 or 18 SK6812 pixels
"""
self.setup()
self._light_mode = mode
self._set_config()
def light_type(self, set_type):
"""Set the light type for attached lights.
Set the type of lighting strip connected:
* RGB - WS2812 pixels with RGB pixel order
* RGB - WS2812 pixels with GRB pixel order
* RGBW - SK6812 pixels with RGBW pixel order
* GRBW - SK6812 pixels with GRBW pixel order
"""
self._light_type = set_type
def num_pixels(self):
"""Returns the supported number of pixels depending on light mode.
RGBW or GRBW support 18 pixels
RGB supports 24 pixels
"""
if self._light_type in [RGBW, GRBW]:
return 18
return 24
def brightness(self, brightness):
"""Set the brightness of the connected LED ring.
This only applies if light_mode has been set to PWM.
It will be ignored otherwise.
:param brightness: Brightness from 0 to 255
"""
self.setup()
self._check_int_range(brightness, 0, 255)
if self._light_mode == PWM:
# The brightness value is taken from the first register of the WS2812 chain
self._i2c_write_byte(self.REG_WS2812, brightness)
def set_all(self, red, green, blue, white=None):
"""Set all pixels in the buffer.
:param red: Amount of red, from 0 to 255
:param green: Amount of green, from 0 to 255
:param blue: Amount of blue, from 0 to 255
:param white: Optional amount of white for RGBW and GRBW strips
"""
for index in range(self.num_pixels()):
self.set_pixel(index, red, green, blue, white)
def set_pixel_rgbw(self, index, red, green, blue, white):
"""Set a single pixel in the buffer for GRBW lighting stick
:param index: Index of pixel from 0 to 17
:param red: Amount of red, from 0 to 255
:param green: Amount of green, from 0 to 255
:param blue: Amount of blue, from 0 to 255
:param white: Amount of white, from 0 to 255
"""
self.set_pixel(index, red, green, blue, white)
def set_pixel(self, index, red, green, blue, white=None):
"""Set a single pixel in the buffer.
:param index: Index of pixel from 0 to 23
:param red: Amount of red, from 0 to 255
:param green: Amount of green, from 0 to 255
:param blue: Amount of blue, from 0 to 255
:param white: Optional amount of white for RGBW and GRBW strips
"""
self._check_int_range(index, 0, self.num_pixels() - 1)
for color in [red, green, blue]:
self._check_int_range(color, 0, 255)
if white is not None:
self._check_int_range(white, 0, 255)
if self._light_type in [RGBW, GRBW]:
index *= 4
if self._light_type == RGBW:
self._pixels[index] = red
self._pixels[index+1] = green
self._pixels[index+2] = blue
if self._light_type == GRBW:
self._pixels[index] = green
self._pixels[index+1] = red
self._pixels[index+2] = blue
if white is not None:
self._pixels[index+3] = white
else:
index *= 3
if self._light_type == RGB:
self._pixels[index] = red
self._pixels[index+1] = green
self._pixels[index+2] = blue
if self._light_type == GRB:
self._pixels[index] = green
self._pixels[index+1] = red
self._pixels[index+2] = blue
def show(self):
"""Display the buffer on the connected WS2812 strip."""
self.setup()
self._i2c_write_block(self.REG_WS2812, self._pixels[:32])
self._i2c_write_block(self.REG_WS2812 + 32, self._pixels[32:64])
self._i2c_write_block(self.REG_WS2812 + 64, self._pixels[64:])
self._i2c_write_byte(self.REG_UPDATE, 1)
def servo_enable(self, index, state):
"""Enable or disable a servo.
Disabling a servo turns off the drive signal.
It's good practise to do this if you don't want
the Pan/Tilt to point in a certain direction and
instead want to save power.
:param index: Servo index: either 1 or 2
:param state: Servo state: True = on, False = off
"""
self.setup()
if index not in [1, 2]:
raise ValueError("Servo index must be 1 or 2")
if state not in [True, False]:
raise ValueError("State must be True/False")
if index == 1:
self._enable_servo1 = state
else:
self._enable_servo2 = state
self._set_config()
def servo_pulse_min(self, index, value):
"""Set the minimum high pulse for a servo in microseconds.
:param value: Value in microseconds
"""
if index not in [1, 2]:
raise ValueError("Servo index must be 1 or 2")
self._servo_min[index-1] = value
def servo_pulse_max(self, index, value):
"""Set the maximum high pulse for a servo in microseconds.
:param value: Value in microseconds
"""
if index not in [1, 2]:
raise ValueError("Servo index must be 1 or 2")
self._servo_max[index-1] = value
def get_servo_one(self):
"""Get position of servo 1 in degrees."""
self.setup()
us_min, us_max = self._servo_range(0)
us = self._i2c_read_word(self.REG_SERVO1)
try:
return self._servo_us_to_degrees(us, us_min, us_max)
except ValueError:
return 0
def get_servo_two(self):
"""Get position of servo 2 in degrees."""
self.setup()
us_min, us_max = self._servo_range(1)
us = self._i2c_read_word(self.REG_SERVO2)
try:
return self._servo_us_to_degrees(us, us_min, us_max)
except ValueError:
return 0
def servo_one(self, angle):
"""Set position of servo 1 in degrees.
:param angle: Angle in degrees from -90 to 90
"""
self.setup()
if not self._enable_servo1:
self._enable_servo1 = True
self._set_config()
us_min, us_max = self._servo_range(0)
us = self._servo_degrees_to_us(angle, us_min, us_max)
self._i2c_write_word(self.REG_SERVO1, us)
if self._idle_timeout > 0:
if self._servo1_timeout is not None:
self._servo1_timeout.cancel()
self._servo1_timeout = Timer(self._idle_timeout, self._servo1_stop)
self._servo1_timeout.daemon = True
self._servo1_timeout.start()
def _servo1_stop(self):
self._servo1_timeout = None
self._enable_servo1 = False
self._set_config()
def servo_two(self, angle):
"""Set position of servo 2 in degrees.
:param angle: Angle in degrees from -90 to 90
"""
self.setup()
if not self._enable_servo2:
self._enable_servo2 = True
self._set_config()
us_min, us_max = self._servo_range(1)
us = self._servo_degrees_to_us(angle, us_min, us_max)
self._i2c_write_word(self.REG_SERVO2, us)
if self._idle_timeout > 0:
if self._servo2_timeout is not None:
self._servo2_timeout.cancel()
self._servo2_timeout = Timer(self._idle_timeout, self._servo2_stop)
self._servo2_timeout.daemon = True
self._servo2_timeout.start()
def _servo2_stop(self):
self._servo2_timeout = None
self._enable_servo2 = False
self._set_config()
pan = servo_one
tilt = servo_two
get_pan = get_servo_one
get_tilt = get_servo_two
class i2c:
# Get the Raspberry pi bus number
@staticmethod
def getPiI2CBusNumber():
# Gets the I2C bus number /dev/i2c#
return 1 if i2c.getPiRevision() > 1 else 0
# Get the Raspberry pi board version from /proc/cpuinfo
@staticmethod
def getPiRevision():
revision = 1
with open("/proc/cpuinfo") as f:
cpuinfo = f.read()
rev_hex = re.search(r"(?<=\nRevision)[ |:|\t]*(\w+)", cpuinfo).group(1)
rev_int = int(rev_hex, 16)
if rev_int > 2:
revision = 2
return revision
def __init__(self, address, busnum=1, debug=False):
self.address = address
self.busnum = busnum
self.bus = SMBus(self.busnum)
# Force I2C1 (512MB Pi's)
self.debug = debug
def reverseByteOrder(self, data):
"Reverses the byte order of an int (16-bit) or long (32-bit) value"
# Courtesy Vishal Sapre
byteCount = len(hex(data)[2:].replace('L', '')[::2])
val = 0
for i in range(byteCount):
val = (val << 8) | (data & 0xff)
data >>= 8
return val
def errMsg(self):
print("Error accessing 0x%02X: Check your I2C address" % self.address)
return -1
def write8(self, reg, value):
"Writes an 8-bit value to the specified register/address"
try:
self.bus.write_byte_data(self.address, reg, value)
if self.debug:
print("I2C: Wrote 0x%02X to register 0x%02X" % (value, reg))
except IOError as err:
return self.errMsg()
def write16(self, reg, value):
"Writes a 16-bit value to the specified register/address pair"
try:
self.bus.write_word_data(self.address, reg, value)
if self.debug:
print(("I2C: Wrote 0x%02X to register pair 0x%02X,0x%02X" %
(value, reg, reg + 1)))
except IOError as err:
return self.errMsg()
def writeList(self, reg, list):
"Writes an array of bytes using I2C format"
try:
if self.debug:
print("I2C: Writing list to register 0x%02X:" % reg)
print(list)
self.bus.write_i2c_block_data(self.address, reg, list)
except IOError as err:
return self.errMsg()
def readList(self, reg, length):
"Read a list of bytes from the I2C device"
try:
results = self.bus.read_i2c_block_data(self.address, reg, length)
if self.debug:
print((
"I2C: Device 0x%02X returned the following from reg 0x%02X"
% (self.address, reg)))
print(results)
return results
except IOError as err:
return self.errMsg()
def readU8(self, reg):
"Read an unsigned byte from the I2C device"
try:
result = self.bus.read_byte_data(self.address, reg)
if self.debug:
print(("I2C: Device 0x%02X returned 0x%02X from reg 0x%02X" %
(self.address, result & 0xFF, reg)))
return result
except IOError as err:
return self.errMsg()
def readS8(self, reg):
"Reads a signed byte from the I2C device"
try:
result = self.bus.read_byte_data(self.address, reg)
if result > 127: result -= 256
if self.debug:
print(("I2C: Device 0x%02X returned 0x%02X from reg 0x%02X" %
(self.address, result & 0xFF, reg)))
return result
except IOError as err:
return self.errMsg()
def readU16(self, reg):
"Reads an unsigned 16-bit value from the I2C device"
try:
hibyte = self.readU8(reg)
lobyte = self.readU8(reg + 1)
result = (hibyte << 8) + lobyte
if (self.debug):
print("I2C: Device 0x%02X returned 0x%04X from reg 0x%02X" %
(self.address, result & 0xFFFF, reg))
return result
except IOError as err:
return self.errMsg()
def readS16(self, reg):
"Reads a signed 16-bit value from the I2C device"
try:
hibyte = self.readS8(reg)
lobyte = self.readU8(reg + 1)
result = (hibyte << 8) + lobyte
if (self.debug):
print("I2C: Device 0x%02X returned 0x%04X from reg 0x%02X" %
(self.address, result & 0xFFFF, reg))
return result
except IOError as err:
return self.errMsg()
def readU16Rev(self, reg):
"Reads an unsigned 16-bit value from the I2C device with rev byte order"
try:
lobyte = self.readU8(reg)
hibyte = self.readU8(reg + 1)
result = (hibyte << 8) + lobyte
if (self.debug):
print("I2C: Device 0x%02X returned 0x%04X from reg 0x%02X" %
(self.address, result & 0xFFFF, reg))
return result
except IOError as err:
return self.errMsg()
def readS16Rev(self, reg):
"Reads a signed 16-bit value from the I2C device with rev byte order"
try:
lobyte = self.readS8(reg)
hibyte = self.readU8(reg + 1)
result = (hibyte << 8) + lobyte
if (self.debug):
print("I2C: Device 0x%02X returned 0x%04X from reg 0x%02X" %
(self.address, result & 0xFFFF, reg))
return result
except IOError as err:
return self.errMsg()
class i2c_device:
# Bit masks.
bit0 = 0x01
bit1 = 0x02
bit2 = 0x04
bit3 = 0x08
bit4 = 0x10
bit5 = 0x20
bit6 = 0x40
bit7 = 0x80
def __init__(self, address=None, bus=None):
self.address = address # "address" is the device address on the i2c bus.
self.bus = bus
self.DEBUG = 0
if self.bus == None:
self.bus = DEFAULT_I2C_BUS
# "bus" is the Raspberry Pi external i2c bus, where the MCP23017 is connected.
# This may vary accross different Raspberry Pi models. 0 is used by the
# original Raspbery Pi model B (256 MB memory, 26-pin GPIO connector).
# 1 is used by recent Raspberry Pi devices.
self.device = SMBus(self.bus)
return
def bus_reset(self):
'''Caution: bus reset is a general call operation. It
is not device-specific. All devices on the bus are affected.
In systems with multiple i2c busses, only one bus (the bus associated
with this device) is reset.'''
self.device.write_byte(0, 6)
return
def wbyte_only(self, value):
# Write a single byte to device, without any register address.
self.device.write_byte(self.address, value)
def rbyte_only(self):
# Read a single byte from device, without any register address.
return self.device.read_byte(self.address)
def wbyte(self, reg, value):
# Write a single byte <value> to register <reg>.
if self.DEBUG >= 32:
sys.stdout.write(
' write x{:02X} to chip x{:02X} register {}\n'.format(
value, self.address, reg))
sys.flush()
self.device.write_byte_data(self.address, reg, value)
return
def rbyte(self, reg):
# Read one byte from register <reg>.
return self.device.read_byte_data(self.address, reg)
def wword(self, reg, value):
'''Write the integer <value> (two bytes) to register <reg>.'''
if byte_swap:
value = ((value & 0xff) << 8) | ((value >> 8) & 0xff)
self.device.write_word_data(self.address, reg, value)
return
def rword(self, reg):
value = self.device.read_word_data(self.address, reg)
if byte_swap:
value = ((value & 0xff) << 8) | ((value >> 8) & 0xff)
return value
bus.write_byte_data(ADDRESS, 4, 0x44)
b = bus.read_byte_data(ADDRESS, 1)
assert b == 0x44
print("Write byte data@4 OK")
bus.write_byte_data(ADDRESS, 5, 0x55)
b = bus.read_byte_data(ADDRESS, 2)
assert b == 0x55
print("Write byte data@5 OK")
bus.write_byte_data(ADDRESS, 6, 0x66)
b = bus.read_byte_data(ADDRESS, 3)
assert b == 0x66
print("Write byte data@6 OK")
bus.write_word_data(ADDRESS, 9, 0xabcd)
w = bus.read_word_data(ADDRESS, 7)
assert w == 0xabcd
print("Write word data@9, read word data@7 OK")
bus.write_word_data(ADDRESS, 10, 0xef12)
w = bus.read_word_data(ADDRESS, 8)
assert w == 0xef12
print("Write word data@10, read word data@8 OK")
bus.write_block_data(
0x19, 13, [1, 2, 3, 4, 5, 6, 7, 8, 9, 8, 7, 6, 5, 4, 3, 2, 1, 0, 1, 2])
block = bus.read_i2c_block_data(0x19, 11, 9)
assert block == [8, 1, 2, 3, 4, 5, 6, 7, 8]
print("Write/Read block OK")
class INA233:
def __init__(self, bus, addr, max_i=15.0, r_shunt=0.002, debug=False):
self.addr = addr
self.max_i = max_i
self.r_shunt = r_shunt
self.debug = debug
self.energy_acc = 0
self.last = datetime.utcnow()
self.bus = SMBus(bus)
self.init()
# Set the INA233 to default settings
def set_defaults(self):
self.bus.write_byte(self.addr, 0x12)
self.energy_acc = 0
self.last = datetime.utcnow()
# Initialise the INA233 for I and P readings
def init(self):
# Reset device to defaults
self.set_defaults()
# Debug
if self.debug:
print(
"INA233 0x%02X Init with max_i: %.1f Amps, r_shunt: %.5f ohms"
% (self.addr, self.max_i, self.r_shunt))
# Current LSB is stored as float so we do not need to compute m later
self.current_lsb = float(self.max_i) / 2**15
cal = 0.00512 / (self.current_lsb * self.r_shunt)
# Round to nearest int for programming
cal_r = int(round(cal))
# Debug
if self.debug:
print(
"INA233 0x%02X MFR_CALIBRATION Rounded from %.4f to %d (0x%04X)"
% (self.addr, cal, cal_r, cal_r))
print("INA233 0x%02X current_lsb: %.15f" %
(self.addr, self.current_lsb))
# Set calibration registers in device
self.set_mfr_calibration(cal_r)
self.set_mfr_device_config(read_ein=1)
self.set_mfr_adc_config(avg=5, vbusct=5, vshct=4, mode=7)
# Set the MFR_CALIBRATION register
def set_mfr_calibration(self, cal):
# Build config value
cali = (cal & 0x7FFF)
#Write to register
self.bus.write_word_data(self.addr, 0xD4, cali)
mfr_calibration = self.bus.read_word_data(self.addr, 0xD4)
# Debug
if self.debug:
if (mfr_calibration != cali):
print(
"INA233 0x%02X MFR_CALIBRATION readback error, write: 0x%04X, read: 0x%04X"
% (self.addr, cali, mfr_calibration))
else:
print(
"INA233 0x%02X MFR_CALIBRATION readback OK, read: 0x%04X" %
(self.addr, mfr_calibration))
# Set MFR_ADC_CONFIG register
def set_mfr_adc_config(self, avg=0, vbusct=4, vshct=4, mode=7):
# Build config value
adc_conf = ((0x4 & 0xF) << 12)
adc_conf += ((avg & 0x7) << 9)
adc_conf += ((vbusct & 0x7) << 6)
adc_conf += ((vshct & 0x7) << 3)
adc_conf += ((mode & 0x7) << 0)
# Write to register
self.bus.write_word_data(self.addr, 0xD0, adc_conf)
adc_config = self.bus.read_word_data(self.addr, 0xD0)
# Debug
if self.debug:
if (adc_config != adc_conf):
print(
"INA233 0x%02X MFR_ADC_CONFIG readback error, write: 0x%04X, read: 0x%04X"
% (self.addr, adc_conf, adc_config))
else:
print(
"INA233 0x%02X MFR_ADC_CONFIG readback OK, read: 0x%04X" %
(self.addr, adc_config))
#Set MFR_DEVICE_CONFIG register
def set_mfr_device_config(self,
ein_status=0,
ein_accum=0,
i2c_filt=0,
read_ein=0,
alert=1,
apol=0):
# Build config value
dev_conf = ((ein_status & 0x01) << 7)
dev_conf += ((ein_accum & 0x03) << 4)
dev_conf += ((i2c_filt & 0x01) << 3)
dev_conf += ((read_ein & 0x01) << 2)
dev_conf += ((alert & 0x01) << 1)
dev_conf += ((apol & 0x01) << 0)
# Write to register
self.bus.write_byte_data(self.addr, 0xD5, dev_conf)
device_config = self.bus.read_byte_data(self.addr, 0xD5)
# Debug
if self.debug:
if (device_config != dev_conf):
print(
"INA233 0x%02X MFR_DEVICE_CONFIG readback error, write: 0x%02X, read: 0x%02X"
% (self.addr, dev_conf, device_config))
else:
print(
"INA233 0x%02X MFR_DEVICE_CONFIG readback OK, read: 0x%02X"
% (self.addr, device_config))
# Obtain voltage reading
def v_read(self):
vin_read = float(self.bus.read_word_data(self.addr, 0x88))
vin = (1.0 / 8) * (vin_read * 10**-2)
# Debug
if self.debug:
print("INA233 0x%02X Voltage : %.2fV" % (self.addr, vin))
return vin
# Obtain current reading
def i_read(self):
iin_read = self.bus.read_word_data(self.addr, 0x89)
# Check sign
if (iin_read & 0x8000):
iin_read = iin_read - 0x10000
iin = self.current_lsb * iin_read
# Debug
if self.debug:
print("INA233 0x%02X Current : %.2fA" % (self.addr, iin))
return iin
# Obtain power reading
def p_read(self):
pin_read = float(self.bus.read_word_data(self.addr, 0x97))
pin = self.current_lsb * 25 * pin_read
# Debug
if self.debug:
print("INA233 0x%02X Power : %.2fW" % (self.addr, pin))
return pin
# Read total energy (kWh) since last reset
def e_read(self):
# Read energy register
ein_read = self.bus.read_i2c_block_data(self.addr, 0x86, 7)
# Get time of this read
t = datetime.utcnow()
# Extract sub-values
pow_acc = (ein_read[1] & 0xFF) + ((ein_read[2] & 0xFF) << 8)
pow_acc_roll = ein_read[3] & 0xFF
samples = (ein_read[4] & 0xFF) + ((ein_read[5] & 0xFF) << 8) + (
(ein_read[6] & 0xFF) << 16)
# Total energy for this sample period (since self.last)
# Assumes autoclear enabled in MFR_DEVICE_CONFIG
# Assumes this command is run frequently enough to avoid roll overs (via reset on read)
total_e = self.current_lsb * 25 * ((pow_acc_roll * 0xFFFF) + pow_acc)
# Energy is total power accumulated * time since measurement start
if (samples != 0):
dur = (t - self.last).total_seconds()
self.last = t
self.energy_acc += (
(float(total_e) / samples) * dur) / (3600 * 1000)
#Debug
if self.debug:
print("INA233 0x%02X Energy : %.8fkWh" %
(self.addr, self.energy_acc))
return self.energy_acc
else:
return 0.0
# Print debug information for this INA233 device
def print_debug(self):
print("\nDebug information for INA233 addr: 0x%02X" % self.addr)
# Get MFR_MODEL
mfr_model = self.bus.read_i2c_block_data(self.addr, 0x9A, 7)
print("MFR_MODEL : " + bytearray(mfr_model).decode('utf-8'))
# Get MFR_ID
mfr_id = self.bus.read_i2c_block_data(self.addr, 0x99, 3)
print("MFR_ID : " + bytearray(mfr_id).decode('utf-8'))
# Get MFR_MODEL
mfr_revision = self.bus.read_word_data(self.addr, 0x9B)
print("MFR_REVISION: 0x%04X" % mfr_revision)
# Get MFR_CALIBRATION
mfr_calibration = self.bus.read_word_data(self.addr, 0xD4)
print("MFR_CALIC : 0x%04X" % mfr_calibration)
# Get MFR_DEVICE_CONDIG
mfr_dev_config = self.bus.read_byte_data(self.addr, 0xD5)
print("MFR_DEV_CONF: 0x%02X" % mfr_dev_config)
# Capability
capability = self.bus.read_byte_data(self.addr, 0x19)
print("Capability : 0x%02X" % capability)
# Status byte
status_byte = self.bus.read_byte_data(self.addr, 0x78)
print("Status byte : 0x%02X" % status_byte)
# Status word
status_word = self.bus.read_word_data(self.addr, 0x79)
print("Status word : 0x%04X" % status_word)
# Status iout
status_iout = self.bus.read_byte_data(self.addr, 0x7B)
print("Status IOUT : 0x%02X" % status_iout)
# Status input
status_input = self.bus.read_byte_data(self.addr, 0x7C)
print("Status INPUT: 0x%02X" % status_input)
# Status communications
status_cml = self.bus.read_byte_data(self.addr, 0x7E)
print("Status CML : 0x%02X" % status_cml)
# Status MFR Specific
status_mfr = self.bus.read_byte_data(self.addr, 0x80)
print("Status MFR : 0x%02X" % status_mfr)
# ADC_CONFIG
adc_config = self.bus.read_word_data(self.addr, 0xD0)
print("ADC_CONFIG : 0x%04X" % adc_config)
print("\n")
class Device(object):
"""Class for communicating with an I2C device using the smbus library.
Allows reading and writing 8-bit, 16-bit, and byte array values to registers
on the device."""
def __init__(self, address, busnum):
"""Create an instance of the I2C device at the specified address on the
specified I2C bus number."""
self._address = address
self._bus = SMBus(busnum)
def writeRaw8(self, value):
"""Write an 8-bit value on the bus (without register)."""
value = value & 0xFF
self._bus.write_byte(self._address, value)
def write8(self, register, value):
"""Write an 8-bit value to the specified register."""
value = value & 0xFF
self._bus.write_byte_data(self._address, register, value)
def write16(self, register, value):
"""Write a 16-bit value to the specified register."""
value = value & 0xFFFF
self._bus.write_word_data(self._address, register, value)
def writeList(self, register, data):
"""Write bytes to the specified register."""
self._bus.write_i2c_block_data(self._address, register, data)
def readList(self, register, length):
"""Read a length number of bytes from the specified register. Results
will be returned as a bytearray."""
results = self._bus.read_i2c_block_data(self._address, register,
length)
return results
def readRaw8(self):
"""Read an 8-bit value on the bus (without register)."""
result = self._bus.read_byte(self._address) & 0xFF
return result
def readU8(self, register):
"""Read an unsigned byte from the specified register."""
result = self._bus.read_byte_data(self._address, register) & 0xFF
return result
def readS8(self, register):
"""Read a signed byte from the specified register."""
result = self.readU8(register)
if result > 127:
result -= 256
return result
def readU16(self, register, little_endian=True):
"""Read an unsigned 16-bit value from the specified register, with the
specified endianness (default little endian, or least significant byte
first)."""
result = self._bus.read_word_data(self._address, register) & 0xFFFF
# Swap bytes if using big endian because read_word_data assumes little
# endian on ARM (little endian) systems.
if not little_endian:
result = ((result << 8) & 0xFF00) + (result >> 8)
return result
def readS16(self, register, little_endian=True):
"""Read a signed 16-bit value from the specified register, with the
specified endianness (default little endian, or least significant byte
first)."""
result = self.readU16(register, little_endian)
if result > 32767:
result -= 65536
return result
def readU16LE(self, register):
"""Read an unsigned 16-bit value from the specified register, in little
endian byte order."""
return self.readU16(register, little_endian=True)
def readU16BE(self, register):
"""Read an unsigned 16-bit value from the specified register, in big
endian byte order."""
return self.readU16(register, little_endian=False)
def readS16LE(self, register):
"""Read a signed 16-bit value from the specified register, in little
endian byte order."""
return self.readS16(register, little_endian=True)
def readS16BE(self, register):
"""Read a signed 16-bit value from the specified register, in big
endian byte order."""
return self.readS16(register, little_endian=False)
def write_word_data(self, *args, **kwargs):
"""Overridden :method:`write_word_data` from :class:`SMBus` without I2C address parameter."""
return SMBus.write_word_data(self, self.device_addr, *args, **kwargs)
relay_en = 162 #PIN16
relay_s0 = 234 #PIN28
relay_s1 = 171 #PIN22
relay_s2 = 163 #PIN18
#SPI_SDO PIN 19
#SPI_SDI PIN 21
#SPI_CLK PIN 23
pga_enable = 1 #1 to enable PGA2311 volume 0 to dam1941 volume
pga_volume = 0
dam1941_volume = -90
#I2C setting
_I2C = SMBus(1)
_I2C.write_word_data(0x49, 0x01, 0x8b85)
_I2C.close
#Serial Setting
_LCD_rate = 115200
_LCD_port = '/dev/ttyS1'
_LCD = serial.Serial(_LCD_port, _LCD_rate)
_DAM_rate = 115200
_DAM_port = '/dev/ttyS2'
_DAM = serial.Serial(_DAM_port, _DAM_rate)
def GpioSetup():
GPIO.setwarnings(False) #disable warning
GPIO.setmode(GPIO.ASUS) #USE ASUS mode
