X-Git-Url: https://defiant.homedns.org/gitweb/?p=ros_wild_thumper.git;a=blobdiff_plain;f=src%2Ftinkerforge_imu2.cpp;fp=src%2Ftinkerforge_imu2.cpp;h=d07b8e700a05d98c925e1d2de7519f4e5f3c636f;hp=0000000000000000000000000000000000000000;hb=10405f7578977ead223bda080bb10e73839d1219;hpb=255212530a5073e1ebd97f7b8c168c4b1535fb8e

diff --git a/src/tinkerforge_imu2.cpp b/src/tinkerforge_imu2.cpp
new file mode 100644
index 0000000..d07b8e7
--- /dev/null
+++ b/src/tinkerforge_imu2.cpp
@@ -0,0 +1,88 @@
+#include <stdio.h>
+#include <math.h>
+#include "ros/ros.h"
+#include "sensor_msgs/Imu.h"
+#include "ip_connection.h"
+#include "brick_imu_v2.h"
+
+ros::Publisher pub_imu;
+
+void cb_all_data(int16_t acceleration[3], int16_t magnetic_field[3], int16_t angular_velocity[3], int16_t euler_angle[3], int16_t quaternion[4], int16_t linear_acceleration[3], int16_t gravity_vector[3], int8_t temperature, uint8_t calibration_status, void *user_data) {
+	sensor_msgs::Imu msg;
+	msg.header.stamp = ros::Time::now();
+	msg.header.frame_id = "base_imu_link";
+
+	msg.orientation.x = quaternion[1]/16383.0;
+	msg.orientation.y = quaternion[2]/16383.0;
+	msg.orientation.z = quaternion[3]/16383.0;
+	msg.orientation.w = quaternion[0]/16383.0;
+	// Observed orientation variance: 0.0 (10k samples)
+	// Magnometer heading accuracy is +-2.5 deg => 0.088 rad
+	// With heading accuracy as std dev, variance = 0.088^2 = 0.008 
+	msg.orientation_covariance = {
+		0.008, 0     , 0,
+		0    , 0.008, 0,
+		0    , 0    , 0.008
+	};
+
+	msg.angular_velocity.x = angular_velocity[0]/16.0*M_PI/180;
+	msg.angular_velocity.y = angular_velocity[1]/16.0*M_PI/180;
+	msg.angular_velocity.z = angular_velocity[2]/16.0*M_PI/180;
+	// Observed angular velocity variance: 0.006223 (10k samples), => round up to 0.02
+	msg.angular_velocity_covariance = {
+		0.02, 0   , 0,
+		0   , 0.02, 0,
+		0   , 0   , 0.02
+	};
+
+	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 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 = {
+		0.04, 0    , 0,
+		0    , 0.04, 0,
+		0    , 0    , 0.04
+	};
+
+	pub_imu.publish(msg);
+}
+
+int main(int argc, char **argv) {
+	std::string host, uid;
+	int port;
+	IPConnection ipcon;
+	IMUV2 imu;
+
+	ros::init(argc, argv, "imu_brick_v2");
+	ros::NodeHandle nh;
+	ros::NodeHandle pnh("~");
+
+	pub_imu = nh.advertise<sensor_msgs::Imu>("imu", 10);
+
+	pnh.param("host", host, std::string("localhost"));
+	pnh.param("port", port, 4223);
+	pnh.getParam("uid", uid);
+
+	// Create IP connection
+	ipcon_create(&ipcon);
+	// Create device object
+	imu_v2_create(&imu, uid.c_str(), &ipcon);
+	// Connect to brickd
+	if(ipcon_connect(&ipcon, host.c_str(), port) < 0) {
+		fprintf(stderr, "Could not connect\n");
+		return 1;
+	}
+	imu_v2_leds_off(&imu);
+	imu_v2_disable_status_led(&imu);
+	imu_v2_register_callback(&imu, IMU_V2_CALLBACK_ALL_DATA, (void (*)(void))cb_all_data, NULL);
+	imu_v2_set_all_data_period(&imu, 20); // ms
+
+	ros::spin();
+
+	imu_v2_destroy(&imu);
+	ipcon_destroy(&ipcon); // Calls ipcon_disconnect internally
+}