From 14db607284e1f386fc7ded5cbee2c50da9928a47 Mon Sep 17 00:00:00 2001
From: Erik Andresen <erik@vontaene.de>
Date: Wed, 28 Dec 2016 18:38:17 +0100
Subject: [PATCH 1/1] added sensor msg and script

---
 CMakeLists.txt          |  1 +
 msg/Sensor.msg          |  7 ++++
 scripts/pyshared        |  2 +-
 scripts/sensor_board.py | 86 +++++++++++++++++++++++++++++++++++++++++
 4 files changed, 95 insertions(+), 1 deletion(-)
 create mode 100644 msg/Sensor.msg
 create mode 100755 scripts/sensor_board.py

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 2b9c873..eb763b5 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -44,6 +44,7 @@ add_message_files(
   FILES
   LedStripe.msg
   Led.msg
+  Sensor.msg
 )
 
 ## Generate services in the 'srv' folder
diff --git a/msg/Sensor.msg b/msg/Sensor.msg
new file mode 100644
index 0000000..72aedff
--- /dev/null
+++ b/msg/Sensor.msg
@@ -0,0 +1,7 @@
+Header header
+uint16 light
+float32 temp
+uint8 humidity
+int16 pressure
+uint16 co
+bool ventilate
diff --git a/scripts/pyshared b/scripts/pyshared
index 44c7f36..f239712 160000
--- a/scripts/pyshared
+++ b/scripts/pyshared
@@ -1 +1 @@
-Subproject commit 44c7f3645d8839dbc49a54bf1481dd2f75199a7a
+Subproject commit f23971240165455313729886360541bee726b42c
diff --git a/scripts/sensor_board.py b/scripts/sensor_board.py
new file mode 100755
index 0000000..337830d
--- /dev/null
+++ b/scripts/sensor_board.py
@@ -0,0 +1,86 @@
+#!/usr/bin/env python
+# -*- coding: iso-8859-15 -*-
+
+import rospy
+import struct
+import prctl
+from datetime import datetime
+from time import sleep
+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
+
+"""
+LDR:
+val = [100 285 321 515 636 758 940 1023]
+lux = [95  34  31  12  11   8    5    0]
+"""
+
+def get_i2c(addr):
+	dev = i2c(addr)
+	dev.write(struct.pack(">BB", 0x00, 0x0f))
+	dev.close()
+
+	dev = i2c(addr)
+	dev.write(chr(0x00))
+	msg = dev.read(11) # for unknown reason need to read one more byte
+	dev.close()
+	return msg[:10]
+
+
+def get(addr=0x58):
+	msg = get_i2c(addr)
+	ldr, temp_mess, humidity_mess, pressure, co = struct.unpack(">hhhhh", msg)
+
+	temp_mess/=10.0
+	humidity_mess/=10.0
+	lux = -3.9338e-07*ldr**3 +0.00083596*ldr**2 -0.58608*ldr +144.96
+	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)
+
+	return lux, temp_real, humidity_real, pressure_kpa, co
+
+class SensorBoard:
+	def __init__(self):
+		rospy.init_node('sensor_board')
+		prctl.set_name("sensor_board")
+		self.pub = rospy.Publisher("sensors", Sensor, queue_size=16)
+		self.run()
+
+	def run(self):
+		rate = rospy.Rate(1.0)
+		t_last_check = datetime.min
+		ventilate = 0
+		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()
+				if (ventilate > 1.10):
+					print "Lüften!"
+				
+				msg = Sensor()
+				msg.header.stamp = rospy.Time.now()
+				msg.light = ldr
+				msg.temp = temp
+				msg.humidity = humidity
+				msg.pressure = pressure
+				msg.co = co
+				msg.ventilate = ventilate
+
+				self.pub.publish(msg)
+
+			rate.sleep()
+
+if __name__ == "__main__":
+	SensorBoard()
-- 
2.39.2