From 04dc6bc04a4a2e7f4ede155355c35cca9a0afa59 Mon Sep 17 00:00:00 2001 From: TSC21 Date: Thu, 12 Jul 2018 20:12:15 +0100 Subject: [PATCH] simulator: add ODOMETRY Mavlink msg handler --- src/modules/simulator/simulator.h | 6 +- src/modules/simulator/simulator_mavlink.cpp | 99 +++++++++++++++++---- 2 files changed, 89 insertions(+), 16 deletions(-) diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 9e995e9819..9ac696e167 100644 --- a/src/modules/simulator/simulator.h +++ b/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 #include #include +#include #include +#include #include #include #include @@ -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 + int publish_odometry_topic(T *odom_mavlink); int publish_distance_topic(mavlink_distance_sensor_t *dist); #ifndef __PX4_QURT diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 5c6515208f..29757a4d21 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/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 #include #include -#include -#include #include @@ -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; - mavlink_msg_vision_position_estimate_decode(msg, &ev); - publish_ev_topic(&ev); + publish_odometry_topic(msg); 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 +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; - visual_odometry.x = ev_mavlink->x; - visual_odometry.y = ev_mavlink->y; - visual_odometry.z = ev_mavlink->z; + odom.timestamp = timestamp; - matrix::Quatf q(matrix::Eulerf(ev_mavlink->roll, ev_mavlink->pitch, ev_mavlink->yaw)); - q.copyTo(visual_odometry.q); + if (msg->msgid == MAVLINK_MSG_ID_ODOMETRY) { + mavlink_odometry_t odom_msg; + mavlink_msg_odometry_decode(msg, &odom_msg); - int inst = 0; - orb_publish_auto(ORB_ID(vehicle_visual_odometry), &_visual_odometry_pub, &visual_odometry, &inst, ORB_PRIO_HIGH); + /* Dcm rotation matrix from body frame to local NED frame */ + matrix::Dcm 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(matrix::Quatf(odom_msg.q)).I(); + + /* the linear velocities needs to be transformed to the local NED frame */ + matrix::Vector3 linvel_local(Rbl * matrix::Vector3(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; }