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()