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

Optical flow simulator and install cleanup. (#5132)

Browse Source 
sitl CI is having some issue cloning, but I have verified it locally for various configs
This commit is contained in:
James Goppert
2016-07-24 13:07:14 -04:00
committed by GitHub
parent 5cee3fa0e1
commit 05315abc89
11 changed files with 172 additions and 75 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/simulator/simulator_mavlink.cpp
+29 -1
View File
@@ -283,6 +283,12 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish)
publish_flow_topic(&flow);
break;
case MAVLINK_MSG_ID_DISTANCE_SENSOR:
mavlink_distance_sensor_t dist;
mavlink_msg_distance_sensor_decode(msg, &dist);
publish_distance_topic(&dist);
break;
case MAVLINK_MSG_ID_HIL_GPS:
mavlink_hil_gps_t gps_sim;
mavlink_msg_hil_gps_decode(msg, &gps_sim);
@@ -863,7 +869,7 @@ int Simulator::publish_flow_topic(mavlink_hil_optical_flow_t *flow_mavlink)
flow.gyro_y_rate_integral = flow_mavlink->integrated_ygyro;
flow.gyro_z_rate_integral = flow_mavlink->integrated_zgyro;
flow.pixel_flow_x_integral = flow_mavlink->integrated_x;
flow.pixel_flow_x_integral = flow_mavlink->integrated_y;
flow.pixel_flow_y_integral = flow_mavlink->integrated_y;
flow.quality = flow_mavlink->quality;
int flow_multi;
@@ -871,3 +877,25 @@ int Simulator::publish_flow_topic(mavlink_hil_optical_flow_t *flow_mavlink)
return OK;
}
int Simulator::publish_distance_topic(mavlink_distance_sensor_t *dist_mavlink)
{
uint64_t timestamp = hrt_absolute_time();
struct distance_sensor_s dist;
memset(&dist, 0, sizeof(dist));
dist.timestamp = timestamp;
dist.min_distance = dist_mavlink->min_distance / 100.0f;
dist.max_distance = dist_mavlink->max_distance / 100.0f;
dist.current_distance = dist_mavlink->current_distance / 100.0f;
dist.type = dist_mavlink->type;
dist.id = dist_mavlink->id;
dist.orientation = dist_mavlink->orientation;
dist.covariance = dist_mavlink->covariance / 100.0f;
int dist_multi;
orb_publish_auto(ORB_ID(distance_sensor), &_dist_pub, &dist, &dist_multi, ORB_PRIO_HIGH);
return OK;
}