simulator: add ODOMETRY Mavlink msg handler

src/modules/simulator/simulator.h

@@ -1,6 +1,7 @@
 /****************************************************************************
  *
  *   Copyright (c) 2015 Mark Charlebois. All rights reserved.
+ *   Copyright (c) 2018 PX4 Pro Dev Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -43,7 +44,9 @@
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_odometry.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/irlock_report.h>
@@ -341,7 +344,8 @@ private:
 	// class methods
 	int publish_sensor_topics(mavlink_hil_sensor_t *imu);
 	int publish_flow_topic(mavlink_hil_optical_flow_t *flow);
-	int publish_ev_topic(mavlink_vision_position_estimate_t *ev_mavlink);
+	template<typename T>
+	int publish_odometry_topic(T *odom_mavlink);
 	int publish_distance_topic(mavlink_distance_sensor_t *dist);
 
 #ifndef __PX4_QURT

src/modules/simulator/simulator_mavlink.cpp

@@ -2,6 +2,7 @@
  *
  *   Copyright (c) 2015 Mark Charlebois. All rights reserved.
  *   Copyright (c) 2016 Anton Matosov. All rights reserved.
+ *   Copyright (c) 2018 PX4 Pro Dev Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -45,8 +46,6 @@
 #include <pthread.h>
 #include <conversion/rotation.h>
 #include <mathlib/mathlib.h>
-#include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/vehicle_odometry.h>
 
 #include <limits>
 
@@ -393,10 +392,9 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish)
 		publish_flow_topic(&flow);
 		break;
 
+	case MAVLINK_MSG_ID_ODOMETRY:
 	case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE:
-		mavlink_vision_position_estimate_t ev;
+		publish_odometry_topic(msg);
-		mavlink_msg_vision_position_estimate_decode(msg, &ev);
-		publish_ev_topic(&ev);
 		break;
 
 	case MAVLINK_MSG_ID_DISTANCE_SENSOR:
@@ -1125,22 +1123,93 @@ int Simulator::publish_flow_topic(mavlink_hil_optical_flow_t *flow_mavlink)
 	return OK;
 }
 
-int Simulator::publish_ev_topic(mavlink_vision_position_estimate_t *ev_mavlink)
+template<typename T>
+int Simulator::publish_odometry_topic(T *msg)
 {
 	uint64_t timestamp = hrt_absolute_time();
 
-	struct vehicle_odometry_s visual_odometry = {};
+	struct vehicle_odometry_s odom = {};
 
-	visual_odometry.timestamp = timestamp;
+	odom.timestamp = timestamp;
-	visual_odometry.x = ev_mavlink->x;
-	visual_odometry.y = ev_mavlink->y;
-	visual_odometry.z = ev_mavlink->z;
 
-	matrix::Quatf q(matrix::Eulerf(ev_mavlink->roll, ev_mavlink->pitch, ev_mavlink->yaw));
+	if (msg->msgid == MAVLINK_MSG_ID_ODOMETRY) {
-	q.copyTo(visual_odometry.q);
+		mavlink_odometry_t odom_msg;
+		mavlink_msg_odometry_decode(msg, &odom_msg);
 
-	int inst = 0;
+		/* Dcm rotation matrix from body frame to local NED frame */
-	orb_publish_auto(ORB_ID(vehicle_visual_odometry), &_visual_odometry_pub, &visual_odometry, &inst, ORB_PRIO_HIGH);
+		matrix::Dcm<float> Rbl;
+
+		/* since odom.child_frame_id == MAV_FRAME_BODY_FRD, WRT to estimated vehicle body-fixed frame */
+		/* get quaternion from the msg quaternion itself and build DCM matrix from it */
+		/* No need to transform the covariance matrices since the non-diagonal values are all zero */
+		Rbl = matrix::Dcm<float>(matrix::Quatf(odom_msg.q)).I();
+
+		/* the linear velocities needs to be transformed to the local NED frame */
+		matrix::Vector3<float> linvel_local(Rbl * matrix::Vector3<float>(odom_msg.vx, odom_msg.vy, odom_msg.vz));
+
+		/* The position in the local NED frame */
+		odom.x = odom_msg.x;
+		odom.y = odom_msg.y;
+		odom.z = odom_msg.z;
+		/* The quaternion of the ODOMETRY msg represents a rotation from
+		 * NED earth/local frame to XYZ body frame */
+		matrix::Quatf q(odom_msg.q[0], odom_msg.q[1], odom_msg.q[2], odom_msg.q[3]);
+		q.copyTo(odom.q);
+
+		/* The pose covariance URT */
+		for (size_t i = 0; i < 21; i++) {
+			odom.pose_covariance[i] = odom_msg.pose_covariance[i];
+		}
+
+		/* The velocity in the local NED frame */
+		odom.vx = linvel_local(0);
+		odom.vy = linvel_local(1);
+		odom.vz = linvel_local(2);
+		/* The angular velocity in the body-fixed frame */
+		odom.rollspeed = odom_msg.rollspeed;
+		odom.pitchspeed = odom_msg.pitchspeed;
+		odom.yawspeed = odom_msg.yawspeed;
+
+		/* The velocity covariance URT */
+		for (size_t i = 0; i < 21; i++) {
+			odom.velocity_covariance[i] = odom_msg.twist_covariance[i];
+		}
+
+	} else if (msg->msgid == MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE) {
+		mavlink_vision_position_estimate_t ev;
+		mavlink_msg_vision_position_estimate_decode(msg, &ev);
+		/* The position in the local NED frame */
+		odom.x = ev.x;
+		odom.y = ev.y;
+		odom.z = ev.z;
+		/* The euler angles of the VISUAL_POSITION_ESTIMATE msg represent a
+		 * rotation from NED earth/local frame to XYZ body frame */
+		matrix::Quatf q(matrix::Eulerf(ev.roll, ev.pitch, ev.yaw));
+		q.copyTo(odom.q);
+
+		/* The pose covariance URT */
+		for (size_t i = 0; i < 21; i++) {
+			odom.pose_covariance[i] = ev.covariance[i];
+		}
+
+		/* The velocity in the local NED frame - unknown */
+		odom.vx = NAN;
+		odom.vy = NAN;
+		odom.vz = NAN;
+		/* The angular velocity in body-fixed frame - unknown */
+		odom.rollspeed = NAN;
+		odom.pitchspeed = NAN;
+		odom.yawspeed = NAN;
+
+		/* The velocity covariance URT - unknown */
+		odom.velocity_covariance[0] = NAN;
+
+	}
+
+	int instance_id = 0;
+
+	/** @note: frame_id == MAV_FRAME_VISION_NED) */
+	orb_publish_auto(ORB_ID(vehicle_visual_odometry), &_visual_odometry_pub, &odom, &instance_id, ORB_PRIO_HIGH);
 
 	return OK;
 }