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