X-Git-Url: https://defiant.homedns.org/gitweb/?p=ros_wild_thumper.git;a=blobdiff_plain;f=scripts%2Fsensor_board.py;h=c8a0c0ce018c256f9f623c6b949958edf6b98b13;hp=bddfb580ead0291735485386e601d5ff99e76909;hb=2f194b3fe82009a77b79021f0c57e6ca55c1706f;hpb=dad024ae410398fbf60b75619545e9ee4ab9dad3

diff --git a/scripts/sensor_board.py b/scripts/sensor_board.py
index bddfb58..c8a0c0c 100755
--- a/scripts/sensor_board.py
+++ b/scripts/sensor_board.py
@@ -1,25 +1,32 @@
 #!/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 = -2 # -5 # degree celsius
-PRESSURE_ERROR = -4.5
+TEMP_ERROR = -3.0 # -5 # degree celsius
+PRESSURE_ERROR = -2.5
+A_dew_point = 17.271
+B_dew_point = 237.7
 
 """
 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))
@@ -38,14 +45,18 @@ def get(addr=0x58):
 
 	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
 
 	# fix temperature/humidity
-	temp_real = temp_mess + TEMP_ERROR 
-	humidity_abs = calc_humidity_abs(temp_mess, humidity_mess)
-	humidity_real = calc_humidity_rel(temp_real, humidity_abs)
+	if TEMP_ERROR:
+		temp_real = temp_mess + TEMP_ERROR
+		humidity_abs = calc_humidity_abs(temp_mess, humidity_mess)
+		humidity_real = calc_humidity_rel(temp_real, humidity_abs)
+	else:
+		temp_real = temp_mess
+		humidity_real = humidity_mess
 
 	return lux, temp_real, humidity_real, pressure_kpa, co
 
@@ -53,6 +64,11 @@ class SensorBoard:
 	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()
 
@@ -63,9 +79,13 @@ class SensorBoard:
 		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()
@@ -75,6 +95,8 @@ class SensorBoard:
 				msg.pressure = pressure
 				msg.co = co
 				msg.ventilate = True if ventilate > 1.10 else False
+				tmp = ((A_dew_point * temp) / (B_dew_point + temp)) + log(humidity/100.0)
+				msg.dew_point = (B_dew_point * tmp) / (A_dew_point - tmp)
 
 				self.pub.publish(msg)