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