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