587 lines
15 KiB
Python
Executable File
587 lines
15 KiB
Python
Executable File
# -*- coding: utf-8 -*-
|
|
|
|
########################################
|
|
# HXbot power control module #
|
|
# EoF 2016 EoF@itphx.ru #
|
|
########################################
|
|
|
|
import HX
|
|
import threading
|
|
import time
|
|
import array
|
|
import struct
|
|
|
|
|
|
class HXpowerClass():
|
|
|
|
def __init__(self, i2c_bus, lock):
|
|
super(HXpowerClass, self).__init__()
|
|
|
|
self.i2c_bus = i2c_bus
|
|
self.__e = None
|
|
self.__t = None
|
|
self.lock = lock
|
|
|
|
# MV
|
|
self.__mv_enable_time = 0.0
|
|
self.mv_blocked = True
|
|
|
|
# HXpower data
|
|
self.online = False
|
|
self.is_running = False
|
|
self.error_count = 0
|
|
|
|
self.status = 0x000000
|
|
self.vcc = 0.0
|
|
self.vin = 0.0
|
|
self.vdc = 0.0
|
|
self.vbt = 0.0
|
|
self.vba = 0.0
|
|
self.vpf = 0.0
|
|
self.vzu = 0.0
|
|
self.abt = 0.0
|
|
self.aba = 0.0
|
|
self.ain = 0.0
|
|
self.ahx = 0.0
|
|
self.vcc_ok = False
|
|
self.vin_ok = False
|
|
self.vdc_ok = False
|
|
self.vbt_ok = False
|
|
self.vba_ok = False
|
|
self.vpf_ok = False
|
|
self.vzu_ok = False
|
|
self.abt_ok = False
|
|
self.aba_ok = False
|
|
self.ain_ok = False
|
|
self.ahx_ok = False
|
|
self.bt_enabled = False
|
|
self.ba_enabled = False
|
|
self.bt_blocked = False
|
|
self.ba_blocked = False
|
|
self.vcc_low = False
|
|
self.vcc_high = False
|
|
self.vin_low = False
|
|
self.vin_high = False
|
|
self.vdc_low = False
|
|
self.vdc_high = False
|
|
self.vbt_low = False
|
|
self.vbt_full = False
|
|
self.vba_low = False
|
|
self.vba_full = False
|
|
self.vpf_low = False
|
|
self.vpf_high = False
|
|
self.vzu_low = False
|
|
self.vzu_high = False
|
|
self.abt_low = False
|
|
self.abt_high = False
|
|
self.aba_low = False
|
|
self.aba_high = False
|
|
self.ain_low = False
|
|
self.ain_high = False
|
|
self.ahx_low = False
|
|
self.ahx_high = False
|
|
|
|
self.mv_enabled = False
|
|
self.us_enabled = False
|
|
|
|
self.in_plugged = False
|
|
self.ba_full = False
|
|
self.bt_full = False
|
|
|
|
def start(self):
|
|
self.__e = threading.Event()
|
|
self.__t = threading.Thread(target=self.__process, args=(self.__e, ))
|
|
|
|
self.__t.start()
|
|
self.is_running = True
|
|
|
|
def stop(self):
|
|
self.__e.set()
|
|
self.__t.join()
|
|
self.is_running = False
|
|
|
|
self.__e = None
|
|
self.__t = None
|
|
|
|
def __process(self, stopRequest):
|
|
stat1 = 0x00
|
|
stat2 = 0x00
|
|
stat3 = 0x00
|
|
|
|
# Process loop
|
|
while not stopRequest.is_set():
|
|
try:
|
|
self.status = 0x0000
|
|
|
|
# Get status 1
|
|
tmp = self.__read_byte(HX.COM_GET_STAT1)
|
|
if tmp is not None:
|
|
stat1 = tmp << 8
|
|
else:
|
|
if self.online:
|
|
self.error_count += 1
|
|
|
|
# Get status 2
|
|
tmp = self.__read_byte(HX.COM_GET_STAT2)
|
|
if tmp is not None:
|
|
stat2 = tmp
|
|
else:
|
|
if self.online:
|
|
self.error_count += 1
|
|
|
|
# Get status 3
|
|
tmp = self.__read_byte(HX.COM_GET_STAT3)
|
|
if tmp is not None:
|
|
stat3 = tmp << 16
|
|
else:
|
|
if self.online:
|
|
self.error_count += 1
|
|
|
|
self.status |= stat1
|
|
self.status |= stat2
|
|
self.status |= stat3
|
|
|
|
# Get Voltages
|
|
tmp = self.__read_float(HX.COM_GET_VCC)
|
|
if tmp is not None:
|
|
self.vcc = tmp
|
|
|
|
tmp = self.__read_float(HX.COM_GET_VIN)
|
|
if tmp is not None:
|
|
self.vin = tmp
|
|
|
|
tmp = self.__read_float(HX.COM_GET_VDC)
|
|
if tmp is not None:
|
|
self.vdc = tmp
|
|
|
|
tmp = self.__read_float(HX.COM_GET_VBT)
|
|
if tmp is not None:
|
|
self.vbt = tmp
|
|
|
|
tmp = self.__read_float(HX.COM_GET_VBA)
|
|
if tmp is not None:
|
|
self.vba = tmp
|
|
|
|
tmp = self.__read_float(HX.COM_GET_VPF)
|
|
if tmp is not None:
|
|
self.vpf = tmp
|
|
|
|
tmp = self.__read_float(HX.COM_GET_VZU)
|
|
if tmp is not None:
|
|
self.vzu = tmp
|
|
|
|
tmp = self.__read_float(HX.COM_GET_ABT)
|
|
if tmp is not None:
|
|
self.abt = tmp
|
|
|
|
tmp = self.__read_float(HX.COM_GET_ABA)
|
|
if tmp is not None:
|
|
self.aba = tmp
|
|
|
|
tmp = self.__read_float(HX.COM_GET_AIN)
|
|
if tmp is not None:
|
|
self.ain = tmp
|
|
|
|
tmp = self.__read_float(HX.COM_GET_AHX)
|
|
if tmp is not None:
|
|
self.ahx = tmp
|
|
|
|
self.online = True
|
|
self.error_count = 0
|
|
except IOError:
|
|
if self.online:
|
|
self.error_count += 1
|
|
|
|
# Decode Status
|
|
if self.status & 0x0100:
|
|
self.vcc_ok = True
|
|
else:
|
|
self.vcc_ok = False
|
|
|
|
if self.status & 0x0200:
|
|
self.vin_ok = True
|
|
else:
|
|
self.vin_ok = False
|
|
|
|
if self.status & 0x0400:
|
|
self.vdc_ok = True
|
|
else:
|
|
self.vdc_ok = False
|
|
|
|
if self.status & 0x0800:
|
|
self.vbt_ok = True
|
|
else:
|
|
self.vbt_ok = False
|
|
|
|
if self.status & 0x1000:
|
|
self.vba_ok = True
|
|
else:
|
|
self.vba_ok = False
|
|
|
|
if self.status & 0x0010:
|
|
self.vpf_ok = True
|
|
else:
|
|
self.vpf_ok = False
|
|
|
|
if self.status & 0x0020:
|
|
self.vzu_ok = True
|
|
else:
|
|
self.vzu_ok = False
|
|
|
|
if self.status & 0x2000:
|
|
self.bt_enabled = True
|
|
else:
|
|
self.bt_enabled = False
|
|
|
|
if self.status & 0x4000:
|
|
self.ba_enabled = True
|
|
else:
|
|
self.ba_enabled = False
|
|
|
|
if self.status & 0x0040:
|
|
self.mv_enabled = True
|
|
else:
|
|
self.mv_enabled = False
|
|
|
|
if self.status & 0x0080:
|
|
self.us_enabled = True
|
|
else:
|
|
self.us_enabled = False
|
|
|
|
if self.status & 0x8000:
|
|
self.in_plugged = True
|
|
else:
|
|
self.in_plugged = False
|
|
|
|
if self.status & 0x0001:
|
|
self.bt_blocked = True
|
|
else:
|
|
self.bt_blocked = False
|
|
|
|
if self.status & 0x0002:
|
|
self.ba_blocked = True
|
|
else:
|
|
self.ba_blocked = False
|
|
|
|
if self.status & 0x0004:
|
|
self.bt_full = True
|
|
else:
|
|
self.bt_full = False
|
|
|
|
if self.status & 0x0008:
|
|
self.ba_full = True
|
|
else:
|
|
self.ba_full = False
|
|
|
|
if self.status & 0x010000:
|
|
self.abt_ok = True
|
|
else:
|
|
self.abt_ok = False
|
|
|
|
if self.status & 0x020000:
|
|
self.aba_ok = True
|
|
else:
|
|
self.aba_ok = False
|
|
|
|
if self.status & 0x040000:
|
|
self.ain_ok = True
|
|
else:
|
|
self.ain_ok = False
|
|
|
|
if self.status & 0x080000:
|
|
self.ahx_ok = True
|
|
else:
|
|
self.ahx_ok = False
|
|
|
|
# LOW | HIGH
|
|
if self.vcc < HX.VCC_LOW:
|
|
self.vcc_low = True
|
|
else:
|
|
self.vcc_low = False
|
|
|
|
if self.vcc > HX.VCC_HIGH:
|
|
self.vcc_high = True
|
|
else:
|
|
self.vcc_high = False
|
|
|
|
if self.vin < HX.VIN_LOW:
|
|
self.vin_low = True
|
|
else:
|
|
self.vin_low = False
|
|
|
|
if self.vin > HX.VIN_HIGH:
|
|
self.vin_high = True
|
|
else:
|
|
self.vin_high = False
|
|
|
|
if self.vdc < HX.VDC_LOW:
|
|
self.vdc_low = True
|
|
else:
|
|
self.vdc_low = False
|
|
|
|
if self.vdc > HX.VDC_HIGH:
|
|
self.vdc_high = True
|
|
else:
|
|
self.vdc_high = False
|
|
|
|
if self.vbt < HX.VBT_LOW:
|
|
self.vbt_low = True
|
|
else:
|
|
self.vbt_low = False
|
|
|
|
if self.vbt > HX.VBT_FULL:
|
|
self.vbt_full = True
|
|
else:
|
|
self.vbt_full = False
|
|
|
|
if self.vba < HX.VBA_LOW:
|
|
self.vba_low = True
|
|
else:
|
|
self.vba_low = False
|
|
|
|
if self.vba > HX.VBA_FULL:
|
|
self.vba_full = True
|
|
else:
|
|
self.vba_full = False
|
|
|
|
if self.vpf < HX.VPF_LOW:
|
|
self.vpf_low = True
|
|
else:
|
|
self.vpf_low = False
|
|
|
|
if self.vpf > HX.VPF_HIGH:
|
|
self.vpf_high = True
|
|
else:
|
|
self.vpf_high = False
|
|
|
|
if self.vzu < HX.VZU_LOW:
|
|
self.vzu_low = True
|
|
else:
|
|
self.vzu_low = False
|
|
|
|
if self.vzu > HX.VZU_HIGH:
|
|
self.vzu_high = True
|
|
else:
|
|
self.vzu_high = False
|
|
|
|
if self.abt < HX.ABT_LOW:
|
|
self.abt_low = True
|
|
else:
|
|
self.abt_low = False
|
|
|
|
if self.abt > HX.ABT_HIGH:
|
|
self.abt_high = True
|
|
else:
|
|
self.abt_high = False
|
|
|
|
if self.aba < HX.ABA_LOW:
|
|
self.aba_low = True
|
|
else:
|
|
self.aba_low = False
|
|
|
|
if self.aba > HX.ABA_HIGH:
|
|
self.aba_high = True
|
|
else:
|
|
self.aba_high = False
|
|
|
|
if self.ain < HX.AIN_LOW:
|
|
self.ain_low = True
|
|
else:
|
|
self.ain_low = False
|
|
|
|
if self.ain > HX.AIN_HIGH:
|
|
self.ain_high = True
|
|
else:
|
|
self.ain_high = False
|
|
|
|
if self.ahx < HX.AHX_LOW:
|
|
self.ahx_low = True
|
|
else:
|
|
self.ahx_low = False
|
|
|
|
if self.ahx > HX.AHX_HIGH:
|
|
self.ahx_high = True
|
|
else:
|
|
self.ahx_high = False
|
|
|
|
# Too many errors
|
|
if self.error_count >= HX.POWER_ERROR_LIMIT:
|
|
self.online = False
|
|
|
|
# Unblock PF
|
|
if (self.mv_enabled and time.clock() - self.__mv_enable_time >=
|
|
HX.POWER_MV_TIMEOUT / 100):
|
|
self.mv_blocked = False
|
|
|
|
# Delay
|
|
time.sleep(HX.POWER_DELAY)
|
|
|
|
def __sum_check(self, val, sum_):
|
|
tmp = 0x00
|
|
for b in val:
|
|
tmp ^= b
|
|
tmp ^= HX.XOR_SEQ
|
|
if tmp == sum_:
|
|
return True
|
|
else:
|
|
return False
|
|
|
|
def __read_float(self, cmd):
|
|
# Byte array
|
|
data = array.array('B', [0, 0, 0, 0])
|
|
with self.lock:
|
|
# Send command
|
|
self.__write_byte(cmd)
|
|
time.sleep(HX.POWER_RESPONSE_DELAY)
|
|
|
|
# Get bytes
|
|
data[0] = self.i2c_bus.read_byte(HX.POWER_ADDRESS)
|
|
data[1] = self.i2c_bus.read_byte(HX.POWER_ADDRESS)
|
|
data[2] = self.i2c_bus.read_byte(HX.POWER_ADDRESS)
|
|
data[3] = self.i2c_bus.read_byte(HX.POWER_ADDRESS)
|
|
|
|
# Get check sum
|
|
sum_ = self.i2c_bus.read_byte(HX.POWER_ADDRESS)
|
|
|
|
# Check and return value
|
|
if self.__sum_check(data, sum_):
|
|
return struct.unpack('f', data)[0]
|
|
else:
|
|
return None
|
|
|
|
def __read_byte(self, cmd):
|
|
with self.lock:
|
|
# Send command
|
|
self.__write_byte(cmd)
|
|
time.sleep(HX.POWER_RESPONSE_DELAY)
|
|
|
|
# Get byte
|
|
data = self.i2c_bus.read_byte(HX.POWER_ADDRESS)
|
|
|
|
# Get check sum
|
|
sum_ = self.i2c_bus.read_byte(HX.POWER_ADDRESS)
|
|
|
|
# Check and return value
|
|
if data ^ HX.XOR_SEQ == sum_:
|
|
return data
|
|
else:
|
|
return None
|
|
|
|
def __write_byte(self, cmd):
|
|
cmd = (cmd << 8) | (cmd ^ HX.XOR_SEQ)
|
|
self.i2c_bus.write_word_data(HX.POWER_ADDRESS, HX.EXT_COM, cmd)
|
|
|
|
def send_cmd(self, cmd):
|
|
try:
|
|
with self.lock:
|
|
# Send command
|
|
self.__write_byte(cmd)
|
|
time.sleep(HX.POWER_RESPONSE_DELAY)
|
|
|
|
# Get response
|
|
response = self.i2c_bus.read_byte(HX.POWER_ADDRESS)
|
|
|
|
# Return
|
|
return response
|
|
except IOError:
|
|
return HX.IOE_RSP
|
|
|
|
def enable_bt(self):
|
|
# Enable power from BT
|
|
if not self.bt_enabled:
|
|
# Run command
|
|
result = self.send_cmd(HX.COM_ENABLE_BT)
|
|
|
|
# Return
|
|
return result
|
|
else:
|
|
# Already enabled? OK!
|
|
return HX.OK_RSP
|
|
|
|
def disable_bt(self):
|
|
# Disable power from BT
|
|
if self.bt_enabled:
|
|
# Run command
|
|
result = self.send_cmd(HX.COM_DISABLE_BT)
|
|
|
|
# Return
|
|
return result
|
|
else:
|
|
# Already disabled? OK!
|
|
return HX.OK_RSP
|
|
|
|
def enable_us(self):
|
|
# Enable USB power
|
|
if not self.us_enabled:
|
|
# Run command
|
|
result = self.send_cmd(HX.COM_ENABLE_US)
|
|
|
|
# Return
|
|
return result
|
|
else:
|
|
# Already enabled? OK!
|
|
return HX.OK_RSP
|
|
|
|
def disable_us(self):
|
|
# Disable USB power
|
|
if self.us_enabled:
|
|
# Run command
|
|
result = self.send_cmd(HX.COM_DISABLE_US)
|
|
|
|
# Return
|
|
return result
|
|
else:
|
|
# Already disabled? OK!
|
|
return HX.OK_RSP
|
|
|
|
def enable_mv(self):
|
|
# Enable PowerFunctions and servos
|
|
if not self.mv_enabled:
|
|
# Save MV enable time
|
|
self.__mv_enable_time = time.clock()
|
|
self.mv_blocked = True
|
|
|
|
# Run command
|
|
result = self.send_cmd(HX.COM_ENABLE_MV)
|
|
|
|
# Return
|
|
return result
|
|
else:
|
|
# Already enabled? OK!
|
|
return HX.OK_RSP
|
|
|
|
def disable_mv(self):
|
|
# Disable PowerFunctions and servos
|
|
if self.mv_enabled:
|
|
# Run command
|
|
result = self.send_cmd(HX.COM_DISABLE_MV)
|
|
|
|
# Return
|
|
return result
|
|
else:
|
|
# Already disabled? OK!
|
|
return HX.OK_RSP
|
|
|
|
def enable_ba(self):
|
|
# Enable BA
|
|
if not self.ba_enabled:
|
|
# Run command
|
|
result = self.send_cmd(HX.COM_ENABLE_BA)
|
|
|
|
# Return
|
|
return result
|
|
else:
|
|
# Already enabled? OK!
|
|
return HX.OK_RSP
|
|
|
|
def disable_ba(self):
|
|
# Disable BA
|
|
if self.ba_enabled:
|
|
# Run command
|
|
result = self.send_cmd(HX.COM_DISABLE_BA)
|
|
|
|
# Return
|
|
return result
|
|
else:
|
|
# Already disabled? OK!
|
|
return HX.OK_RSP |