2 # -*- coding: iso-8859-15 -*-
9 from datetime import datetime
10 from time import sleep
12 from pyshared.i2c import i2c
13 from pyshared.humidity import *
14 from wild_thumper.msg import Sensor
16 # Board warming offset
17 TEMP_ERROR = -3.0 # -5 # degree celsius
24 val = [1023, 396.8, 110.75, 28.1]
25 lux = [0, 13.8, 164.9, 1341]
28 logger = logging.getLogger(__name__)
32 dev.write(struct.pack(">BB", 0x00, 0x0f))
37 msg = dev.read(11) # for unknown reason need to read one more byte
44 ldr, temp_mess, humidity_mess, pressure, co = struct.unpack(">hhhhh", msg)
48 lux = exp(-8.1065e-08*ldr**3 +8.8678e-05*ldr**2 -0.03636*ldr +8.1547)
49 pressure_v = pressure*2.56/1024
50 pressure_kpa = P = (pressure_v/5 + 0.04) / 0.004 + PRESSURE_ERROR # datasheet
52 # fix temperature/humidity
54 temp_real = temp_mess + TEMP_ERROR
55 humidity_abs = calc_humidity_abs(temp_mess, humidity_mess)
56 humidity_real = calc_humidity_rel(temp_real, humidity_abs)
59 humidity_real = humidity_mess
61 return lux, temp_real, humidity_real, pressure_kpa, co
65 rospy.init_node('sensor_board')
66 prctl.set_name("sensor_board")
70 print >>sys.stderr, "No sensor board, shutting down"
72 self.pub = rospy.Publisher("sensors", Sensor, queue_size=16)
76 rate = rospy.Rate(1.0)
77 t_last_check = datetime.min
79 while not rospy.is_shutdown():
80 if self.pub.get_num_connections() > 0:
81 ldr, temp, humidity, pressure, co = get()
84 if (datetime.now() - t_last_check).seconds > 900:
85 ventilate = check_ventilate(temp, humidity)
86 t_last_check = datetime.now()
87 except Exception as e:
91 msg.header.stamp = rospy.Time.now()
94 msg.humidity = humidity
95 msg.pressure = pressure
97 msg.ventilate = True if ventilate > 1.10 else False
98 tmp = ((A_dew_point * temp) / (B_dew_point + temp)) + log(humidity/100.0)
99 msg.dew_point = (B_dew_point * tmp) / (A_dew_point - tmp)
101 self.pub.publish(msg)
105 if __name__ == "__main__":