fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-01 03:31:37 +08:00
Code Issues Packages Projects Releases Wiki Activity

Cut case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE content and paste into handle_message_vision_position_estimate() method. Add const specifier to publish_odometry_topic() method.

Browse Source
This commit is contained in:
mcsauder
2019-03-07 08:22:44 +01:00
committed by Beat Küng
parent c3acd3bad3
commit 3918d0ce0a
2 changed files with 9 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/simulator/simulator.h
+2 -1
View File
@@ -276,7 +276,7 @@ private:
int publish_sensor_topics(mavlink_hil_sensor_t *imu); int publish_sensor_topics(mavlink_hil_sensor_t *imu);
int publish_flow_topic(mavlink_hil_optical_flow_t *flow); int publish_flow_topic(mavlink_hil_optical_flow_t *flow);
int publish_odometry_topic(mavlink_message_t *odom_mavlink); int publish_odometry_topic(const mavlink_message_t *odom_mavlink);
int publish_distance_topic(mavlink_distance_sensor_t *dist); int publish_distance_topic(mavlink_distance_sensor_t *dist);
static Simulator *_instance; static Simulator *_instance;
@@ -338,6 +338,7 @@ private:
void handle_message_landing_target(const mavlink_message_t *msg); void handle_message_landing_target(const mavlink_message_t *msg);
void handle_message_optical_flow(const mavlink_message_t *msg); void handle_message_optical_flow(const mavlink_message_t *msg);
void handle_message_rc_channels(const mavlink_message_t *msg); void handle_message_rc_channels(const mavlink_message_t *msg);
void handle_message_vision_position_estimate(const mavlink_message_t *msg);
void parameters_update(bool force); void parameters_update(bool force);
void poll_topics(); void poll_topics();
src/modules/simulator/simulator_mavlink.cpp
+7 -2
View File
@@ -302,7 +302,7 @@ void Simulator::handle_message(mavlink_message_t *msg)
case MAVLINK_MSG_ID_ODOMETRY: case MAVLINK_MSG_ID_ODOMETRY:
case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE: case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE:
publish_odometry_topic(msg); handle_message_vision_position_estimate(msg);
break; break;
case MAVLINK_MSG_ID_DISTANCE_SENSOR: case MAVLINK_MSG_ID_DISTANCE_SENSOR:
@@ -542,6 +542,11 @@ void Simulator::handle_message_rc_channels(const mavlink_message_t *msg)
} }
} }
void Simulator::handle_message_vision_position_estimate(const mavlink_message_t *msg)
{
publish_odometry_topic(msg);
}
void Simulator::send_mavlink_message(const mavlink_message_t &aMsg) void Simulator::send_mavlink_message(const mavlink_message_t &aMsg)
{ {
uint8_t buf[MAVLINK_MAX_PACKET_LEN]; uint8_t buf[MAVLINK_MAX_PACKET_LEN];
@@ -1100,7 +1105,7 @@ int Simulator::publish_flow_topic(mavlink_hil_optical_flow_t *flow_mavlink)
return OK; return OK;
} }
int Simulator::publish_odometry_topic(mavlink_message_t *odom_mavlink) int Simulator::publish_odometry_topic(const mavlink_message_t *odom_mavlink)
{ {
uint64_t timestamp = hrt_absolute_time(); uint64_t timestamp = hrt_absolute_time();