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

update sitl_gazebo to use the vision position estimate

Browse Source
This commit is contained in:
ChristophTobler
2017-07-27 11:47:46 +02:00
committed by ChristophTobler
parent dc8caeaedf
commit 44cd65798b
3 changed files with 40 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/simulator/simulator_mavlink.cpp
+34
View File
@@ -345,6 +345,12 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish)
publish_flow_topic(&flow);
break;
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);
break;
case MAVLINK_MSG_ID_DISTANCE_SENSOR:
mavlink_distance_sensor_t dist;
mavlink_msg_distance_sensor_decode(msg, &dist);
@@ -1059,6 +1065,34 @@ 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)
{
uint64_t timestamp = hrt_absolute_time();
struct vehicle_local_position_s vision_position = {};
// Use the estimator type to identify the simple vision estimate
vision_position.estimator_type = MAV_ESTIMATOR_TYPE_VISION;
vision_position.timestamp = timestamp;
vision_position.x = ev_mavlink->x;
vision_position.y = ev_mavlink->y;
vision_position.z = ev_mavlink->z;
struct vehicle_attitude_s vision_attitude = {};
vision_attitude.timestamp = timestamp;
matrix::Quatf q(matrix::Eulerf(ev_mavlink->roll, ev_mavlink->pitch, ev_mavlink->yaw));
q.copyTo(vision_attitude.q);
int inst = 0;
orb_publish_auto(ORB_ID(vehicle_vision_position), &_vision_position_pub, &vision_position, &inst, ORB_PRIO_HIGH);
orb_publish_auto(ORB_ID(vehicle_vision_attitude), &_vision_attitude_pub, &vision_attitude, &inst, ORB_PRIO_HIGH);
return OK;
}
int Simulator::publish_distance_topic(mavlink_distance_sensor_t *dist_mavlink)
{
uint64_t timestamp = hrt_absolute_time();