fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 04:03:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

general fixes on VIO data access

Browse Source
This commit is contained in:
TSC21
2018-09-18 09:52:33 +02:00
committed by Lorenz Meier
parent 7d7ee8e4bf
commit 8325724079
12 changed files with 56 additions and 141 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/simulator/simulator.h
+1 -2
View File
@@ -344,8 +344,7 @@ private:
// class methods
int publish_sensor_topics(mavlink_hil_sensor_t *imu);
int publish_flow_topic(mavlink_hil_optical_flow_t *flow);
template<typename T>
int publish_odometry_topic(T *odom_mavlink);
int publish_odometry_topic(mavlink_message_t *odom_mavlink);
int publish_distance_topic(mavlink_distance_sensor_t *dist);
#ifndef __PX4_QURT
src/modules/simulator/simulator_mavlink.cpp
+6 -7
View File
@@ -1123,18 +1123,17 @@ int Simulator::publish_flow_topic(mavlink_hil_optical_flow_t *flow_mavlink)
return OK;
}
template<typename T>
int Simulator::publish_odometry_topic(T *msg)
int Simulator::publish_odometry_topic(mavlink_message_t *odom_mavlink)
{
uint64_t timestamp = hrt_absolute_time();
struct vehicle_odometry_s odom = {};
struct vehicle_odometry_s odom;
odom.timestamp = timestamp;
if (msg->msgid == MAVLINK_MSG_ID_ODOMETRY) {
if (odom_mavlink->msgid == MAVLINK_MSG_ID_ODOMETRY) {
mavlink_odometry_t odom_msg;
mavlink_msg_odometry_decode(msg, &odom_msg);
mavlink_msg_odometry_decode(odom_mavlink, &odom_msg);
/* Dcm rotation matrix from body frame to local NED frame */
matrix::Dcm<float> Rbl;
@@ -1175,9 +1174,9 @@ int Simulator::publish_odometry_topic(T *msg)
odom.velocity_covariance[i] = odom_msg.twist_covariance[i];
}
} else if (msg->msgid == MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE) {
} else if (odom_mavlink->msgid == MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE) {
mavlink_vision_position_estimate_t ev;
mavlink_msg_vision_position_estimate_decode(msg, &ev);
mavlink_msg_vision_position_estimate_decode(odom_mavlink, &ev);
/* The position in the local NED frame */
odom.x = ev.x;
odom.y = ev.y;