fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-29 16:40: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.h
+4
View File
@@ -55,6 +55,7 @@
#include <systemlib/battery.h>
#include <uORB/uORB.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/distance_sensor.h>
#include <v1.0/mavlink_types.h>
#include <v1.0/common/mavlink.h>
namespace simulator
@@ -238,6 +239,7 @@ private:
_gyro_pub(nullptr),
_mag_pub(nullptr),
_flow_pub(nullptr),
_dist_pub(nullptr),
_battery_pub(nullptr),
_initialized(false)
#ifndef __PX4_QURT
@@ -288,6 +290,7 @@ private:
orb_advert_t _gyro_pub;
orb_advert_t _mag_pub;
orb_advert_t _flow_pub;
orb_advert_t _dist_pub;
orb_advert_t _battery_pub;
bool _initialized;
@@ -298,6 +301,7 @@ private:
// class methods
int publish_sensor_topics(mavlink_hil_sensor_t *imu);
int publish_flow_topic(mavlink_hil_optical_flow_t *flow);
int publish_distance_topic(mavlink_distance_sensor_t *dist);
#ifndef __PX4_QURT
// uORB publisher handlers
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;
}