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 = -2 # -5 # degree celsius
  15 PRESSURE_ERROR = -4.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         temp_real = temp_mess + TEMP_ERROR
  47         humidity_abs = calc_humidity_abs(temp_mess, humidity_mess)
  48         humidity_real = calc_humidity_rel(temp_real, humidity_abs)
  49
  50         return lux, temp_real, humidity_real, pressure_kpa, co
  51
  52 class SensorBoard:
  53         def __init__(self):
  54                 rospy.init_node('sensor_board')
  55                 prctl.set_name("sensor_board")
  56                 self.pub = rospy.Publisher("sensors", Sensor, queue_size=16)
  57                 self.run()
  58
  59         def run(self):
  60                 rate = rospy.Rate(1.0)
  61                 t_last_check = datetime.min
  62                 ventilate = 0
  63                 while not rospy.is_shutdown():
  64                         if self.pub.get_num_connections() > 0:
  65                                 ldr, temp, humidity, pressure, co = get()
  66                                 if (datetime.now() - t_last_check).seconds > 900:
  67                                         ventilate = check_ventilate(temp, humidity)
  68                                         t_last_check = datetime.now()
  69                                 if (ventilate > 1.10):
  70                                         print "Lüften!"
  71
  72                                 msg = Sensor()
  73                                 msg.header.stamp = rospy.Time.now()
  74                                 msg.light = ldr
  75                                 msg.temp = temp
  76                                 msg.humidity = humidity
  77                                 msg.pressure = pressure
  78                                 msg.co = co
  79                                 msg.ventilate = ventilate
  80
  81                                 self.pub.publish(msg)
  82
  83                         rate.sleep()
  84
  85 if __name__ == "__main__":
  86         SensorBoard()