add self_test script

[ros_wild_thumper.git] / scripts / sensor_board.py

diff --git a/scripts/sensor_board.py b/scripts/sensor_board.py
index c688ff9616c019d5f7b5c5c3b30cb1d8a54a6d3f..c5e84c45bca6a1acbddd784137217bf720443ee1 100755 (executable)
--- a/scripts/sensor_board.py
+++ b/scripts/sensor_board.py
@@ -5,22 +5,26 @@ 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 = 0 # -2.5 # -5 # degree celsius
+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))
@@ -39,7 +43,7 @@ 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
 
@@ -73,9 +77,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()