#!/usr/bin/env python
# -*- coding: iso-8859-15 -*-
+import sys
import rospy
import struct
import prctl
from wild_thumper.msg import Sensor
# Board warming offset
-TEMP_ERROR = -2 # -5 # degree celsius
-PRESSURE_ERROR = -4.5
+TEMP_ERROR = 0 # -2.5 # -5 # degree celsius
+PRESSURE_ERROR = -2.5
"""
LDR:
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
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()