2 # -*- coding: iso-8859-15 -*-
8 from datetime import datetime
10 from pyshared.i2c import i2c
11 from pyshared.humidity import *
12 from wild_thumper.msg import Sensor
14 # Board warming offset
15 TEMP_ERROR = 0 # -2.5 # -5 # degree celsius
20 val = [100 285 321 515 636 758 940 1023]
21 lux = [95 34 31 12 11 8 5 0]
26 dev.write(struct.pack(">BB", 0x00, 0x0f))
31 msg = dev.read(11) # for unknown reason need to read one more byte
38 ldr, temp_mess, humidity_mess, pressure, co = struct.unpack(">hhhhh", msg)
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
46 # fix temperature/humidity
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)
53 humidity_real = humidity_mess
55 return lux, temp_real, humidity_real, pressure_kpa, co
59 rospy.init_node('sensor_board')
60 prctl.set_name("sensor_board")
64 print >>sys.stderr, "No sensor board, shutting down"
66 self.pub = rospy.Publisher("sensors", Sensor, queue_size=16)
70 rate = rospy.Rate(1.0)
71 t_last_check = datetime.min
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()
81 msg.header.stamp = rospy.Time.now()
84 msg.humidity = humidity
85 msg.pressure = pressure
87 msg.ventilate = True if ventilate > 1.10 else False
93 if __name__ == "__main__":