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
  20 """
  21 LDR:
  22 val = [1023, 396.8, 110.75, 28.1]
  23 lux = [0, 13.8, 164.9, 1341]
  24 """
  25
  26 logger = logging.getLogger(__name__)
  27
  28 def get_i2c(addr):
  29         dev = i2c(addr)
  30         dev.write(struct.pack(">BB", 0x00, 0x0f))
  31         dev.close()
  32
  33         dev = i2c(addr)
  34         dev.write(chr(0x00))
  35         msg = dev.read(11) # for unknown reason need to read one more byte
  36         dev.close()
  37         return msg[:10]
  38
  39
  40 def get(addr=0x58):
  41         msg = get_i2c(addr)
  42         ldr, temp_mess, humidity_mess, pressure, co = struct.unpack(">hhhhh", msg)
  43
  44         temp_mess/=10.0
  45         humidity_mess/=10.0
  46         lux = exp(-8.1065e-08*ldr**3 +8.8678e-05*ldr**2 -0.03636*ldr +8.1547)
  47         pressure_v = pressure*2.56/1024
  48         pressure_kpa = P = (pressure_v/5 + 0.04) / 0.004 + PRESSURE_ERROR # datasheet
  49
  50         # fix temperature/humidity
  51         if TEMP_ERROR:
  52                 temp_real = temp_mess + TEMP_ERROR
  53                 humidity_abs = calc_humidity_abs(temp_mess, humidity_mess)
  54                 humidity_real = calc_humidity_rel(temp_real, humidity_abs)
  55         else:
  56                 temp_real = temp_mess
  57                 humidity_real = humidity_mess
  58
  59         return lux, temp_real, humidity_real, pressure_kpa, co
  60
  61 class SensorBoard:
  62         def __init__(self):
  63                 rospy.init_node('sensor_board')
  64                 prctl.set_name("sensor_board")
  65                 try:
  66                         get()
  67                 except:
  68                         print >>sys.stderr, "No sensor board, shutting down"
  69                         exit(1)
  70                 self.pub = rospy.Publisher("sensors", Sensor, queue_size=16)
  71                 self.run()
  72
  73         def run(self):
  74                 rate = rospy.Rate(1.0)
  75                 t_last_check = datetime.min
  76                 ventilate = 0
  77                 while not rospy.is_shutdown():
  78                         if self.pub.get_num_connections() > 0:
  79                                 ldr, temp, humidity, pressure, co = get()
  80                                 ventilate = None
  81                                 try:
  82                                         if (datetime.now() - t_last_check).seconds > 900:
  83                                                 ventilate = check_ventilate(temp, humidity)
  84                                                 t_last_check = datetime.now()
  85                                 except Exception as e:
  86                                         logger.error(e)
  87
  88                                 msg = Sensor()
  89                                 msg.header.stamp = rospy.Time.now()
  90                                 msg.light = ldr
  91                                 msg.temp = temp
  92                                 msg.humidity = humidity
  93                                 msg.pressure = pressure
  94                                 msg.co = co
  95                                 msg.ventilate = True if ventilate > 1.10 else False
  96
  97                                 self.pub.publish(msg)
  98
  99                         rate.sleep()
 100
 101 if __name__ == "__main__":
 102         SensorBoard()