plugin: local_position: Fix orientation source

Part of #33.
mavlink · Jul 12, 2014 · 9f9dc25 · 9f9dc25
1 parent 1f3fb63
commit 9f9dc25
Show file tree

Hide file tree

Showing 3 changed files with 48 additions and 8 deletions.
diff --git a/include/mavros/mavros_uas.h b/include/mavros/mavros_uas.h
@@ -113,6 +113,38 @@ class UAS {
 		angular_velocity = vec;
 	}
 
+	/**
+	 * @brief Get Attitude linear acceleration vector ENU
+	 */
+	inline tf::Vector3 get_attitude_linear_acceleration() {
+		boost::recursive_mutex::scoped_lock lock(mutex);
+		return linear_acceleration;
+	}
+
+	/**
+	 * @brief Store Attitude linear acceleration vector ENU
+	 */
+	inline void set_attitude_linear_acceleration(tf::Vector3 &vec) {
+		boost::recursive_mutex::scoped_lock lock(mutex);
+		linear_acceleration = vec;
+	}
+
+	/**
+	 * @brief Get Attitude orientation quaternion ENU
+	 */
+	inline tf::Quaternion get_attitude_orientation() {
+		boost::recursive_mutex::scoped_lock lock(mutex);
+		return orientation;
+	}
+
+	/**
+	 * @brief Store Attitude orientation quaternion ENU
+	 */
+	inline void set_attitude_orientation(tf::Quaternion &quat) {
+		boost::recursive_mutex::scoped_lock lock(mutex);
+		orientation = quat;
+	}
+
 	/**
 	 * For APM quirks
 	 */
@@ -154,6 +186,8 @@ class UAS {
 	uint8_t target_component;
 	bool connected;
 	tf::Vector3 angular_velocity;
+	tf::Vector3 linear_acceleration;
+	tf::Quaternion orientation;
 	std::unique_ptr<boost::asio::io_service::work> timer_work;
 	boost::thread timer_thread;
 };

diff --git a/src/plugins/imu_pub.cpp b/src/plugins/imu_pub.cpp
@@ -154,18 +154,15 @@ class IMUPubPlugin : public MavRosPlugin {
 		sensor_msgs::ImuPtr imu_msg = boost::make_shared<sensor_msgs::Imu>();
 
 		// TODO: check/verify that these are body-fixed
-		imu_msg->orientation = tf::createQuaternionMsgFromRollPitchYaw(
+		tf::Quaternion orientation = tf::createQuaternionFromRPY(
 				att.roll, -att.pitch, -att.yaw);
+		tf::quaternionTFToMsg(orientation, imu_msg->orientation);
 
 		imu_msg->angular_velocity.x = att.rollspeed;
 		imu_msg->angular_velocity.y = -att.pitchspeed;
 		imu_msg->angular_velocity.z = -att.yawspeed;
 
 		// store gyro data to UAS
-		tf::Vector3 angular_velocity;
-		tf::vector3MsgToTF(imu_msg->angular_velocity, angular_velocity);
-		uas->set_attitude_angular_velocity(angular_velocity);
-
 		// vector from HIGHRES_IMU or RAW_IMU
 		imu_msg->linear_acceleration = linear_accel_vec;
 
@@ -176,6 +173,16 @@ class IMUPubPlugin : public MavRosPlugin {
 		imu_msg->header.frame_id = frame_id;
 		imu_msg->header.stamp = ros::Time::now();
 		imu_pub.publish(imu_msg);
+
+		// store data in UAS
+		tf::Vector3 angular_velocity;
+		tf::Vector3 linear_acceleration;
+		tf::vector3MsgToTF(imu_msg->angular_velocity, angular_velocity);
+		tf::vector3MsgToTF(imu_msg->linear_acceleration, linear_acceleration);
+
+		uas->set_attitude_angular_velocity(angular_velocity);
+		uas->set_attitude_linear_acceleration(linear_acceleration);
+		uas->set_attitude_orientation(orientation);
 	}
 
 	void fill_imu_msg_vec(sensor_msgs::ImuPtr &imu_msg,

diff --git a/src/plugins/local_position.cpp b/src/plugins/local_position.cpp
@@ -94,12 +94,11 @@ class LocalPositionPlugin : public MavRosPlugin {
 				pos_ned.vx, pos_ned.vy, pos_ned.vz);
 
 		/* TODO: check convertion to ENU
-		 * velocity stored in ENU
+		 * orientation in ENU
 		 */
-		tf::Vector3 avel = uas->get_attitude_angular_velocity();
 		tf::Transform transform;
 		transform.setOrigin(tf::Vector3(pos_ned.x, -pos_ned.y, -pos_ned.z));
-		transform.setRotation(tf::createQuaternionFromRPY(avel.x(), avel.y(), avel.z()));
+		transform.setRotation(uas->get_attitude_orientation());
 
 		if (send_tf)
 			tf_broadcaster.sendTransform(