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 import logging
   9 from datetime import datetime
  10 from time import sleep
  11 from math import *
  12 from pyshared.i2c import i2c
  13 from pyshared.humidity import *
  14 from wild_thumper.msg import Sensor
  15
  16 # Board warming offset
  17 TEMP_ERROR = -3.0 # -5 # degree celsius
  18 PRESSURE_ERROR = -2.5
  19 A_dew_point = 17.271
  20 B_dew_point = 237.7
  21
  22 """
  23 LDR:
  24 val = [1023, 396.8, 110.75, 28.1]
  25 lux = [0, 13.8, 164.9, 1341]
  26 """
  27
  28 logger = logging.getLogger(__name__)
  29
  30 def get_i2c(addr):
  31         dev = i2c(addr)
  32         dev.write(struct.pack(">BB", 0x00, 0x0f))
  33         dev.close()
  34
  35         dev = i2c(addr)
  36         dev.write(chr(0x00))
  37         msg = dev.read(11) # for unknown reason need to read one more byte
  38         dev.close()
  39         return msg[:10]
  40
  41
  42 def get(addr=0x58):
  43         msg = get_i2c(addr)
  44         ldr, temp_mess, humidity_mess, pressure, co = struct.unpack(">hhhhh", msg)
  45
  46         temp_mess/=10.0
  47         humidity_mess/=10.0
  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
  51
  52         # fix temperature/humidity
  53         if TEMP_ERROR:
  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)
  57         else:
  58                 temp_real = temp_mess
  59                 humidity_real = humidity_mess
  60
  61         return lux, temp_real, humidity_real, pressure_kpa, co
  62
  63 class SensorBoard:
  64         def __init__(self):
  65                 rospy.init_node('sensor_board')
  66                 prctl.set_name("sensor_board")
  67                 try:
  68                         get()
  69                 except:
  70                         print >>sys.stderr, "No sensor board, shutting down"
  71                         exit(1)
  72                 self.pub = rospy.Publisher("sensors", Sensor, queue_size=16)
  73                 self.run()
  74
  75         def run(self):
  76                 rate = rospy.Rate(1.0)
  77                 t_last_check = datetime.min
  78                 ventilate = 0
  79                 while not rospy.is_shutdown():
  80                         if self.pub.get_num_connections() > 0:
  81                                 ldr, temp, humidity, pressure, co = get()
  82                                 ventilate = None
  83                                 try:
  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:
  88                                         logger.error(e)
  89
  90                                 msg = Sensor()
  91                                 msg.header.stamp = rospy.Time.now()
  92                                 msg.light = ldr
  93                                 msg.temp = temp
  94                                 msg.humidity = humidity
  95                                 msg.pressure = pressure
  96                                 msg.co = co
  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)
 100
 101                                 self.pub.publish(msg)
 102
 103                         rate.sleep()
 104
 105 if __name__ == "__main__":
 106         SensorBoard()