dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

simulator: add publishing of HIL_OPTICAL_FLOW as uORB topic

This commit is contained in:
v01d 2015-09-20 18:17:19 -03:00 committed by Lorenz Meier
parent 5191731f22
commit 068dd76763
2 changed files with 47 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/modules/simulator/simulator.h
View File

@ -51,6 +51,7 @@
#include <drivers/drv_hrt.h>
#include <drivers/drv_rc_input.h>
#include <uORB/uORB.h>
#include <uORB/topics/optical_flow.h>
#include <v1.0/mavlink_types.h>
#include <v1.0/common/mavlink.h>
namespace simulator {
@ -255,11 +256,13 @@ private:
orb_advert_t _baro_pub;
orb_advert_t _gyro_pub;
orb_advert_t _mag_pub;
orb_advert_t _flow_pub;
bool _initialized;
// class methods
int publish_sensor_topics(mavlink_hil_sensor_t *imu);
int publish_flow_topic(mavlink_hil_optical_flow_t *flow);
#ifndef __PX4_QURT
// uORB publisher handlers

43
src/modules/simulator/simulator_mavlink.cpp
View File

@ -229,6 +229,14 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish) {
update_sensors(&imu);
break;
case MAVLINK_MSG_ID_HIL_OPTICAL_FLOW:
mavlink_hil_optical_flow_t flow;
mavlink_msg_hil_optical_flow_decode(msg, &flow);
if (publish) {
publish_flow_topic(&flow);
}
break;
case MAVLINK_MSG_ID_HIL_GPS:
mavlink_hil_gps_t gps_sim;
mavlink_msg_hil_gps_decode(msg, &gps_sim);
@ -710,3 +718,38 @@ int Simulator::publish_sensor_topics(mavlink_hil_sensor_t *imu) {
return OK;
}
int Simulator::publish_flow_topic(mavlink_hil_optical_flow_t* flow_mavlink)
{
uint64_t timestamp = hrt_absolute_time();
/* gyro */
{
struct optical_flow_s flow;
memset(&flow, 0, sizeof(flow));
flow.sensor_id = flow_mavlink->sensor_id;
flow.timestamp = timestamp;
flow.time_since_last_sonar_update = 0;
flow.frame_count_since_last_readout = 0; // ?
flow.integration_timespan = flow_mavlink->integration_time_us;
flow.ground_distance_m = flow_mavlink->distance;
flow.gyro_temperature = flow_mavlink->temperature;
flow.gyro_x_rate_integral = flow_mavlink->integrated_xgyro;
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.quality = flow_mavlink->quality;
if (_flow_pub == nullptr) {
_flow_pub = orb_advertise(ORB_ID(optical_flow), &flow);
} else {
orb_publish(ORB_ID(optical_flow), _flow_pub, &flow);
}
}
return OK;
}