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

Merge remote-tracking branch 'upstream/master' into offboard2_externalsetpointmessages

Browse Source 
Conflicts:
	src/modules/navigator/navigator.h
	src/modules/navigator/navigator_main.cpp
This commit is contained in:
Thomas Gubler
2014-10-05 13:17:32 +02:00
parent 9bda573151 80ed01a5b4
commit 7f9a231b40
116 changed files with 6051 additions and 1308 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+57 -3
View File
@@ -55,6 +55,7 @@
#include <drivers/drv_gyro.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_baro.h>
#include <drivers/drv_range_finder.h>
#include <time.h>
#include <float.h>
#include <unistd.h>
@@ -103,6 +104,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_battery_pub(-1),
_cmd_pub(-1),
_flow_pub(-1),
_range_pub(-1),
_offboard_control_sp_pub(-1),
_global_vel_sp_pub(-1),
_att_sp_pub(-1),
@@ -123,7 +125,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
{
// make sure the FTP server is started
(void)MavlinkFTP::getServer();
(void)MavlinkFTP::get_server();
}
MavlinkReceiver::~MavlinkReceiver()
@@ -182,8 +184,8 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_request_data_stream(msg);
break;
case MAVLINK_MSG_ID_ENCAPSULATED_DATA:
MavlinkFTP::getServer()->handle_message(_mavlink, msg);
case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL:
MavlinkFTP::get_server()->handle_message(_mavlink, msg);
break;
default:
@@ -210,6 +212,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_hil_state_quaternion(msg);
break;
case MAVLINK_MSG_ID_HIL_OPTICAL_FLOW:
handle_message_hil_optical_flow(msg);
break;
default:
break;
}
@@ -373,6 +379,52 @@ MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg)
}
}
void
MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg)
{
/* optical flow */
mavlink_hil_optical_flow_t flow;
mavlink_msg_hil_optical_flow_decode(msg, &flow);
struct optical_flow_s f;
memset(&f, 0, sizeof(f));
f.timestamp = hrt_absolute_time();
f.flow_timestamp = flow.time_usec;
f.flow_raw_x = flow.flow_x;
f.flow_raw_y = flow.flow_y;
f.flow_comp_x_m = flow.flow_comp_m_x;
f.flow_comp_y_m = flow.flow_comp_m_y;
f.ground_distance_m = flow.ground_distance;
f.quality = flow.quality;
f.sensor_id = flow.sensor_id;
if (_flow_pub < 0) {
_flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
} else {
orb_publish(ORB_ID(optical_flow), _flow_pub, &f);
}
/* Use distance value for range finder report */
struct range_finder_report r;
memset(&r, 0, sizeof(f));
r.timestamp = hrt_absolute_time();
r.error_count = 0;
r.type = RANGE_FINDER_TYPE_LASER;
r.distance = flow.ground_distance;
r.minimum_distance = 0.0f;
r.maximum_distance = 40.0f; // this is set to match the typical range of real sensors, could be made configurable
r.valid = (r.distance > r.minimum_distance) && (r.distance < r.maximum_distance);
if (_range_pub < 0) {
_range_pub = orb_advertise(ORB_ID(sensor_range_finder), &r);
} else {
orb_publish(ORB_ID(sensor_range_finder), _range_pub, &r);
}
}
void
MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg)
{
@@ -772,6 +824,8 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
tstatus.remote_noise = rstatus.remnoise;
tstatus.rxerrors = rstatus.rxerrors;
tstatus.fixed = rstatus.fixed;
tstatus.system_id = msg->sysid;
tstatus.component_id = msg->compid;
if (_telemetry_status_pub < 0) {
_telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus);