dwm1000: Use kalman filter for beacon position, not dwm distance
[ros_wild_thumper.git] / scripts / sensor_board.py
1 #!/usr/bin/env python
2 # -*- coding: iso-8859-15 -*-
3
4 import sys
5 import rospy
6 import struct
7 import prctl
8 from datetime import datetime
9 from time import sleep
10 from pyshared.i2c import i2c
11 from pyshared.humidity import *
12 from wild_thumper.msg import Sensor
13
14 # Board warming offset
15 TEMP_ERROR = 0 # -2.5 # -5 # degree celsius
16 PRESSURE_ERROR = -2.5
17
18 """
19 LDR:
20 val = [100 285 321 515 636 758 940 1023]
21 lux = [95  34  31  12  11   8    5    0]
22 """
23
24 def get_i2c(addr):
25         dev = i2c(addr)
26         dev.write(struct.pack(">BB", 0x00, 0x0f))
27         dev.close()
28
29         dev = i2c(addr)
30         dev.write(chr(0x00))
31         msg = dev.read(11) # for unknown reason need to read one more byte
32         dev.close()
33         return msg[:10]
34
35
36 def get(addr=0x58):
37         msg = get_i2c(addr)
38         ldr, temp_mess, humidity_mess, pressure, co = struct.unpack(">hhhhh", msg)
39
40         temp_mess/=10.0
41         humidity_mess/=10.0
42         lux = -3.9338e-07*ldr**3 +0.00083596*ldr**2 -0.58608*ldr +144.96
43         pressure_v = pressure*2.56/1024
44         pressure_kpa = P = (pressure_v/5 + 0.04) / 0.004 + PRESSURE_ERROR # datasheet
45
46         # fix temperature/humidity
47         if TEMP_ERROR:
48                 temp_real = temp_mess + TEMP_ERROR
49                 humidity_abs = calc_humidity_abs(temp_mess, humidity_mess)
50                 humidity_real = calc_humidity_rel(temp_real, humidity_abs)
51         else:
52                 temp_real = temp_mess
53                 humidity_real = humidity_mess
54
55         return lux, temp_real, humidity_real, pressure_kpa, co
56
57 class SensorBoard:
58         def __init__(self):
59                 rospy.init_node('sensor_board')
60                 prctl.set_name("sensor_board")
61                 try:
62                         get()
63                 except:
64                         print >>sys.stderr, "No sensor board, shutting down"
65                         exit(1)
66                 self.pub = rospy.Publisher("sensors", Sensor, queue_size=16)
67                 self.run()
68
69         def run(self):
70                 rate = rospy.Rate(1.0)
71                 t_last_check = datetime.min
72                 ventilate = 0
73                 while not rospy.is_shutdown():
74                         if self.pub.get_num_connections() > 0:
75                                 ldr, temp, humidity, pressure, co = get()
76                                 if (datetime.now() - t_last_check).seconds > 900:
77                                         ventilate = check_ventilate(temp, humidity)
78                                         t_last_check = datetime.now()
79                                 
80                                 msg = Sensor()
81                                 msg.header.stamp = rospy.Time.now()
82                                 msg.light = ldr
83                                 msg.temp = temp
84                                 msg.humidity = humidity
85                                 msg.pressure = pressure
86                                 msg.co = co
87                                 msg.ventilate = True if ventilate > 1.10 else False
88
89                                 self.pub.publish(msg)
90
91                         rate.sleep()
92
93 if __name__ == "__main__":
94         SensorBoard()