Merge branch 'master' of ssh://vontaene/home/erik_alt/git/ros_wild_thumper
[ros_wild_thumper.git] / scripts / i2c.py
1 #!/usr/bin/env python
2 # -*- coding: iso-8859-15 -*-
3
4 import threading
5 import inspect
6 import os
7 import logging
8 import struct
9 import fcntl
10 from ctypes import *
11 from time import sleep
12
13 DEBUG=0
14 I2C_FILENAME = "/dev/i2c-2"
15 logger = logging.getLogger(__name__)
16
17 class i2c:
18         libc = CDLL("libc.so.6")
19         I2C_SLAVE = 0x0703  # Use this slave address
20         __single = None
21         __lock = threading.Lock()
22         __parent_owner = None
23
24         def __init__(self, addr):
25                 with i2c.__lock:
26                         count = 0
27                         while(i2c.__single):
28                                 parent = inspect.stack()[1][3]
29                                 count += 1
30                                 sleep(0.001)
31                         if DEBUG:
32                                 if count > 10:
33                                         parent_owner = "%s (%d), %s()" % (self.__parent_owner[1], self.__parent_owner[2], self.__parent_owner[3])
34                                         logger.warning("Error: (%s) I2C blocked %fs by %s!", parent, count*0.001, parent_owner)
35                                 i2c.__parent_owner = inspect.stack()[1]
36                         i2c.__single = True
37                 self.dev = i2c.libc.open(I2C_FILENAME, os.O_RDWR)
38                 if self.dev < 0:
39                         raise IOError("open")
40                 fcntl.flock(self.dev, fcntl.LOCK_EX)
41                 err = i2c.libc.ioctl(self.dev, i2c.I2C_SLAVE, addr>>1)
42                 if err < 0:
43                         raise IOError("ioctl")
44
45         def write(self, s):
46                 num_write = i2c.libc.write(self.dev, s, len(s))
47                 if num_write != len(s):
48                         self.close()
49                         raise IOError("write: %d" % (num_write))
50         
51         def read(self, num):
52                 buf = create_string_buffer(num)
53                 num_read = i2c.libc.read(self.dev, buf, num)
54                 if num_read != num:
55                         self.close()
56                         raise IOError("read: %d" % (num_read))
57                 return buf.raw
58
59         def close(self):
60                 if self.dev:
61                         i2c.libc.close(self.dev)
62                         self.dev = None
63                         #i2c.__parent_owner = None
64                         i2c.__single = None
65
66         def __del__(self):
67                 self.close()
68
69
70 def i2c_write_reg(addr, reg, buf):
71         dev = i2c(addr)
72         s = struct.pack(">B", reg) + buf
73         dev.write(s)
74         dev.close()
75
76
77 def i2c_read_reg(addr, reg, num=1):
78         dev = i2c(addr)
79         s = struct.pack(">B", reg)
80         dev.write(s)
81         s = dev.read(num)
82         dev.close()
83         return s
84
85
86 if __name__ == "__main__":
87         import struct
88         import sys
89
90         dev = i2c(0x50)
91         s = struct.pack(">Bh", int(sys.argv[1]), int(sys.argv[2]))
92         dev.write(s)
93         dev.close()