X-Git-Url: https://defiant.homedns.org/gitweb/?p=ros_wild_thumper.git;a=blobdiff_plain;f=scripts%2Ftinkerforge_imu2.py;h=59161c7e966f9b536d5f418e48c235e9d6106acf;hp=fa00c26c782c08b1aa3921c7fcc81f89860d1e64;hb=3d10c26a464468169ace654c14a29d26cbda13e8;hpb=f69047e2f52a6e05b95f13c2d0aa55d53de98e4d

diff --git a/scripts/tinkerforge_imu2.py b/scripts/tinkerforge_imu2.py
index fa00c26..59161c7 100755
--- a/scripts/tinkerforge_imu2.py
+++ b/scripts/tinkerforge_imu2.py
@@ -1,12 +1,9 @@
 #!/usr/bin/env python
 # -*- coding: iso-8859-15 -*-
 
-HOST = "localhost"
-PORT = 4223
-UID = "6DdNSn"
-
 import rospy
 import prctl
+from math import *
 from sensor_msgs.msg import Imu
 from tinkerforge.ip_connection import IPConnection
 from tinkerforge.brick_imu_v2 import BrickIMUV2
@@ -17,9 +14,13 @@ class ImuBrickv2:
 		prctl.set_name("imu_brick_v2")
 		self.pub_imu = rospy.Publisher("imu", Imu, queue_size=16)
 
+		host = rospy.get_param('~host', 'localhost')
+		port = rospy.get_param('~port', 4223)
+		uid = rospy.get_param('~uid')
+
 		ipcon = IPConnection() # Create IP connection
-		imu = BrickIMUV2(UID, ipcon) # Create device object
-		ipcon.connect(HOST, PORT) # Connect to brickd
+		imu = BrickIMUV2(uid, ipcon) # Create device object
+		ipcon.connect(host, port) # Connect to brickd
 
 		imu.leds_off()
 		#imu.disable_status_led()
@@ -49,21 +50,21 @@ class ImuBrickv2:
 			0    , 0    , 0.008
 		]
 
-		msg.angular_velocity.x = angular_velocity[0]/16.0
-		msg.angular_velocity.y = angular_velocity[1]/16.0
-		msg.angular_velocity.z = angular_velocity[2]/16.0
-		# Observed angular velocity variance: 0.006223 (10k samples), => round up to 0.01
+		msg.angular_velocity.x = angular_velocity[0]/16.0*pi/180
+		msg.angular_velocity.y = angular_velocity[1]/16.0*pi/180
+		msg.angular_velocity.z = angular_velocity[2]/16.0*pi/180
+		# Observed angular velocity variance: 0.006223 (10k samples), => round up to 0.02
 		msg.angular_velocity_covariance = [
-			0.01, 0   , 0,
-			0   , 0.01, 0,
-			0   , 0   , 0.01
+			0.02, 0   , 0,
+			0   , 0.02, 0,
+			0   , 0   , 0.02
 		]
 
-		msg.linear_acceleration.x = linear_acceleration[0]/100.0
-		msg.linear_acceleration.y = linear_acceleration[1]/100.0
-		msg.linear_acceleration.z = linear_acceleration[2]/100.0
+		msg.linear_acceleration.x = acceleration[0]/100.0
+		msg.linear_acceleration.y = acceleration[1]/100.0
+		msg.linear_acceleration.z = acceleration[2]/100.0
 		# Observed linear acceleration variance: 0.001532 (10k samples)
-		# Calculation for variance raken from razor imu:
+		# Calculation for variance taken from razor imu:
 		# nonliniarity spec: 1% of full scale (+-2G) => 0.2m/s^2
 		# Choosing 0.2 as std dev, variance = 0.2^2 = 0.04
 		msg.linear_acceleration_covariance = [