linak-desk-control.py

# Application to let your desk dance.
# Copyright (C) 2018 Lukas Schreiner <dev@lschreiner.de>
# 
# This program is free software: you can redistribute it and/or modify it under 
# the terms of the GNU General Public License as published by the Free Software 
# Foundation, either version 3 of the License, or (at your option) any later version.
#
# This program is distributed in the hope that it will be useful, but WITHOUT ANY 
# WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 
# PARTICULAR PURPOSE. See the GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License along with this 
# program. If not, see <https://www.gnu.org/licenses/>. 

from ctypes import sizeof
import sys
import time
import usb1

REQ_INIT = 0x0303
REQ_GET_STATUS = 0x0304
REQ_MOVE = 0x0305
REQ_GET_EXT = 0x0309

TYPE_SET_CI = 0x21
TYPE_GET_CI = 0xA1

HID_REPORT_GET = 0x01
HID_REPORT_SET = 0x09

CMD_STATUS_REPORT = 4
LEN_STATUS_REPORT = 64
NRB_STATUS_REPORT = 56

CMD_MODE_OF_OPERATION = 3
CMD_GET_LIN_DATA = 4
CMD_CONTROL_CBC = 5
CMD_CONTROL_TD = 6
CMD_CONTROL_CBD_TD = 8
CMD_GET_LIN_DATA_EXT = 9

DEF_MODE_OF_OPERATION = 4

LINAK_TIMEOUT = 1000

HEIGHT_MOVE_DOWNWARDS = 32767
HEIGHT_MOVE_UPWARDS = 32768
HEIGHT_MOVE_END = 32769

class Status(object):
	positionLost = True
	antiColision = True
	overloadDown = True
	overloadUp = True
	unknown = 4

	@classmethod
	def fromBuf(sr, buf):
		self = sr()
		attr = ['positionLost', 'antiColision', 'overloadDown', 'overloadUp']
		bitlist = '{:0>8s}'.format(bin(int(buf, base=16)).lstrip('0b'))
		for i in range(0, 4):
			setattr(self, attr[i], True if bitlist[i] == '1' else False)
		# set unknown
		self.unkown = int(buf[1:], 16)

		return self

class StatusPositionSpeed(object):
	pos = None
	status = None
	speed = 0

	@classmethod
	def fromBuf(sr, buf):
		self = sr()
		self.pos = int(buf[2:4] + buf[:2], 16)
		self.status = Status.fromBuf(buf[4:6])
		self.speed = int(buf[6:8], 16)

		return self

class ValidFlags(object):
	ID00_Ref1_pos_stat_speed = True
	ID01_Ref2_pos_stat_speed = True
	ID02_Ref3_pos_stat_speed = True
	ID03_Ref4_pos_stat_speed = True
	ID10_Ref1_controlInput = True
	ID11_Ref2_controlInput = True
	ID12_Ref3_controlInput = True
	ID13_Ref4_controlInput = True
	ID04_Ref5_pos_stat_speed = True
	ID28_Diagnostic = True
	ID05_Ref6_pos_stat_speed = True
	ID37_Handset1command = True
	ID38_Handset2command = True
	ID06_Ref7_pos_stat_speed = True
	ID07_Ref8_pos_stat_speed = True
	unknown = True

	@classmethod
	def fromBuf(sr, buf):
		self = sr()
		attr = ['ID00_Ref1_pos_stat_speed',	'ID01_Ref2_pos_stat_speed',	'ID02_Ref3_pos_stat_speed',	'ID03_Ref4_pos_stat_speed',	'ID10_Ref1_controlInput',	'ID11_Ref2_controlInput',	'ID12_Ref3_controlInput',	'ID13_Ref4_controlInput',	'ID04_Ref5_pos_stat_speed',	'ID28_Diagnostic',	'ID05_Ref6_pos_stat_speed',	'ID37_Handset1command',	'ID38_Handset2command',	'ID06_Ref7_pos_stat_speed',	'ID07_Ref8_pos_stat_speed',	'unknown']
		bitlist = '{:0>16s}'.format(bin(int(buf, base=16)).lstrip('0b'))
		for i in range(0, len(bitlist)):
			setattr(self, attr[i], True if bitlist[i] == '1' else False)

		return self

class StatusReport(object):
	featureRaportID = 0
	numberOfBytes = 0
	validFlag = None
	ref1 = None
	ref2 = None
	ref3 = None
	ref4 = None
	ref1cnt = 0
	ref2cnt = 0
	ref3cnt = 0
	ref4cnt = 0
	ref5 = None
	diagnostic = None
	undefined1 = None
	handset1 = 0
	handset2 = 0
	ref6 = None
	ref7 = None
	ref8 = None
	undefined2 = None

	@classmethod
	def fromBuf(sr, buf):
		self = sr()
		raw = buf.hex()
		self.featureRaportID = buf[0]
		self.numberOfBytes = buf[1]
		self.validFlag = ValidFlags.fromBuf(raw[4:8])
		self.ref1 = StatusPositionSpeed.fromBuf(raw[8:8+8])
		self.ref2 = StatusPositionSpeed.fromBuf(raw[16:16+8])
		self.ref3 = StatusPositionSpeed.fromBuf(raw[24:24+8])
		self.ref4 = StatusPositionSpeed.fromBuf(raw[32:32+8])
		self.ref1cnt = int(raw[42:44] + raw[40:42], 16)
		self.ref2cnt = int(raw[46:48] + raw[44:46], 16)
		self.ref3cnt = int(raw[50:52] + raw[48:50], 16)
		self.ref4cnt = int(raw[54:56] + raw[52:54], 16)
		self.ref5 = StatusPositionSpeed.fromBuf(raw[56:56+8])
		self.diagnostic = raw[64:64+16]
		self.undefined1 = raw[80:84]
		self.handset1 = int(raw[86:88] + raw[84:86], 16)
		self.handset2 = int(raw[88:90] + raw[86:88], 16)
		self.ref6 = StatusPositionSpeed.fromBuf(raw[90:90+8])
		self.ref7 = StatusPositionSpeed.fromBuf(raw[98:98+8])
		self.ref8 = StatusPositionSpeed.fromBuf(raw[106:106+8])
		self.undefined2 = raw[114:]

		return self

class LinakController(object):
	_handle = None
	_ctx = None

	def __init__(self, vendor_id=0x12d3, product_id=0x0002):
		self._ctx =usb1.USBContext() 
		#self._ctx.setDebug(4)
		self._handle = self._ctx.openByVendorIDAndProductID(
			vendor_id,
			product_id,
			skip_on_error=True,
		)
		if not self._handle:
			raise Exception('Could not connect to usb device')

		self._handle.claimInterface(0)
		self._initDevice()

	def close(self):
		if self._handle:
			self._handle.releaseInterface(0)

		del(self._handle)
		del(self._ctx)

	def _controlWriteRead(self, request_type, request, value, index, data, timeout=0):
		data, data_buffer = usb1.create_initialised_buffer(data)
		transferred = self._handle._controlTransfer(request_type, request, value, index, data,
									 sizeof(data), timeout)
		return transferred, data_buffer[:transferred]

	def _getStatusReport(self):
		buf = bytearray(b'\x00'*LEN_STATUS_REPORT)
		buf[0] = CMD_STATUS_REPORT
		#print('> {:s}'.format(buf.hex()))
		x, buf = self._controlWriteRead(
			TYPE_GET_CI, 
			HID_REPORT_GET, 
			REQ_GET_STATUS, 
			0, 
			buf, 
			LINAK_TIMEOUT
		)

		# check if the response match to request!
		if buf[0] != CMD_STATUS_REPORT:
			raise Exception('Invalid status report received!')

		return buf

	def _setStatusReport(self):
		buf = bytearray(b'\x00'*LEN_STATUS_REPORT)
		buf[0] = CMD_MODE_OF_OPERATION
		buf[1] = DEF_MODE_OF_OPERATION
		buf[2] = 0
		buf[3] = 251

		x, buf = self._controlWriteRead(
			TYPE_SET_CI, 
			HID_REPORT_SET, 
			REQ_INIT, 
			0, 
			buf, 
			LINAK_TIMEOUT
		)

		if x != LEN_STATUS_REPORT:
			raise Exception('Device is not ready yet. Initialization failed in step 1.')

	def _move(self, height):
		buf = bytearray(b'\x00' * LEN_STATUS_REPORT)
		buf[0] = CMD_CONTROL_CBC

		hHex = '{:04x}'.format(height)
		hHigh = int(hHex[2:], 16)
		hLow = int(hHex[:2], 16)

		buf[1] = hHigh
		buf[2] = hLow
		buf[3] = hHigh
		buf[4] = hLow
		buf[5] = hHigh
		buf[6] = hLow
		buf[7] = hHigh
		buf[8] = hLow

		x, buf = self._controlWriteRead(
			TYPE_SET_CI, 
			HID_REPORT_SET, 
			REQ_MOVE, 
			0, 
			buf, 
			LINAK_TIMEOUT
		)
		return x == LEN_STATUS_REPORT

	def _moveDown(self):
		return self._move(HEIGHT_MOVE_DOWNWARDS)

	def _moveUp(self):
		return self._move(HEIGHT_MOVE_UPWARDS)

	def _moveEnd(self):
		return self._move(HEIGHT_MOVE_END)

	def _isStatusReportNotReady(self, buf):
		if buf[0] != CMD_STATUS_REPORT or buf[1] != NRB_STATUS_REPORT:
			return False

		for i in range(2, LEN_STATUS_REPORT - 5):
			if buf[i] != 0:
				return False

		return True

	def _initDevice(self):
		buf = self._getStatusReport()
		if not self._isStatusReportNotReady(buf):
			return
		else:
			print('Device not ready!')

		self._setStatusReport()
		time.sleep(1000/1000000.0)
		if not _moveEnd():
			raise Exception('Device not ready - initialization failed on step 2 (moveEnd)')

		time.sleep(100000/1000000.0)

	def move(self, target):
		a = max_a = 3
		epsilon = 13
		oldH = 0

		while True:
			self._move(target)
			time.sleep(200000/1000000.0)

			buf = self._getStatusReport()
			r = StatusReport.fromBuf(buf)
			distance = r.ref1cnt - r.ref1.pos
			delta = oldH-r.ref1.pos
			if abs(distance) <= epsilon or abs(delta) <= epsilon or oldH == r.ref1.pos:
				a -= 1
			else:
				a = max_a

			print(
				'Current height: {:d}; target height: {:d}; distance: {:d}'.format(
					r.ref1.pos,
					target,
					distance
				)
			)

			if a == 0:
				break
			oldH = r.ref1.pos

		return abs(r.ref1.pos - target) <= epsilon

	def getHeight(self):
		buf = self._getStatusReport()
		r = StatusReport.fromBuf(buf)

		return r.ref1.pos, r.ref1.pos/98.0

if __name__ == '__main__':
	import argparse
	parser = argparse.ArgumentParser(description='Get the control on your desk!')
	parser.add_argument('command', choices=['move', 'height'], help='Command to execute.')
	parser.add_argument('height', type=int, nargs='?', help='For command "move", give the destination height.')

	args = parser.parse_args()
	if args.command == 'move' and not args.height:
		sys.stderr.write('Height missing in case of move!\n')
		parser.print_help()
		sys.exit(1)

	co = LinakController()
	try:
		if args.command == 'move':
			r = co.move(args.height)
			if r:
				print('Command executed successfuly')
			else:
				print('Command failed')
		elif args.command == 'height':
			h, hcm = co.getHeight()
			print('Current height is: {:d} / {:.2f} cm'.format(h, hcm))
	except Exception as e:
		co.close()
		raise e
	finally:
		co.close()