#!/usr/bin/env python
# -*- coding: iso-8859-15 -*-
+import sys
import rospy
import struct
import prctl
+import logging
from datetime import datetime
from time import sleep
+from math import *
from pyshared.i2c import i2c
from pyshared.humidity import *
from wild_thumper.msg import Sensor
# Board warming offset
-TEMP_ERROR = -5 # degree celsius
-PRESSURE_ERROR = -4.5
+TEMP_ERROR = -3.0 # -5 # degree celsius
+PRESSURE_ERROR = -2.5
"""
LDR:
-val = [100 285 321 515 636 758 940 1023]
-lux = [95 34 31 12 11 8 5 0]
+val = [1023, 396.8, 110.75, 28.1]
+lux = [0, 13.8, 164.9, 1341]
"""
+logger = logging.getLogger(__name__)
+
def get_i2c(addr):
dev = i2c(addr)
dev.write(struct.pack(">BB", 0x00, 0x0f))
temp_mess/=10.0
humidity_mess/=10.0
- lux = -3.9338e-07*ldr**3 +0.00083596*ldr**2 -0.58608*ldr +144.96
+ lux = exp(-8.1065e-08*ldr**3 +8.8678e-05*ldr**2 -0.03636*ldr +8.1547)
pressure_v = pressure*2.56/1024
pressure_kpa = P = (pressure_v/5 + 0.04) / 0.004 + PRESSURE_ERROR # datasheet
def __init__(self):
rospy.init_node('sensor_board')
prctl.set_name("sensor_board")
+ try:
+ get()
+ except:
+ print >>sys.stderr, "No sensor board, shutting down"
+ exit(1)
self.pub = rospy.Publisher("sensors", Sensor, queue_size=16)
self.run()
while not rospy.is_shutdown():
if self.pub.get_num_connections() > 0:
ldr, temp, humidity, pressure, co = get()
- if (datetime.now() - t_last_check).seconds > 900:
- ventilate = check_ventilate(temp, humidity)
- t_last_check = datetime.now()
+ ventilate = None
+ try:
+ if (datetime.now() - t_last_check).seconds > 900:
+ ventilate = check_ventilate(temp, humidity)
+ t_last_check = datetime.now()
+ except Exception as e:
+ logger.error(e)
msg = Sensor()
msg.header.stamp = rospy.Time.now()