fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 11:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'upstream/mpc_rc' into geo

Browse Source
This commit is contained in:
Thomas Gubler
2014-04-25 13:31:31 +02:00
parent 5285eb2f99 22aaae197b
commit a467bd61c1
16 changed files with 354 additions and 473 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+11 -42
View File
@@ -88,8 +88,6 @@ static const float mg2ms2 = CONSTANTS_ONE_G / 1000.0f;
MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_mavlink(parent),
_manual_sub(-1),
_global_pos_pub(-1),
_local_pos_pub(-1),
_attitude_pub(-1),
@@ -418,47 +416,20 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
mavlink_manual_control_t man;
mavlink_msg_manual_control_decode(msg, &man);
/* rc channels */
{
struct rc_channels_s rc;
memset(&rc, 0, sizeof(rc));
struct manual_control_setpoint_s manual;
memset(&manual, 0, sizeof(manual));
rc.timestamp = hrt_absolute_time();
rc.chan_count = 4;
manual.timestamp = hrt_absolute_time();
manual.roll = man.x / 1000.0f;
manual.pitch = man.y / 1000.0f;
manual.yaw = man.r / 1000.0f;
manual.throttle = man.z / 1000.0f;
rc.chan[0].scaled = man.x / 1000.0f;
rc.chan[1].scaled = man.y / 1000.0f;
rc.chan[2].scaled = man.r / 1000.0f;
rc.chan[3].scaled = man.z / 1000.0f;
if (_manual_pub < 0) {
_manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual);
if (_rc_pub < 0) {
_rc_pub = orb_advertise(ORB_ID(rc_channels), &rc);
} else {
orb_publish(ORB_ID(rc_channels), _rc_pub, &rc);
}
}
/* manual control */
{
struct manual_control_setpoint_s manual;
memset(&manual, 0, sizeof(manual));
/* get a copy first, to prevent altering values that are not sent by the mavlink command */
orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &manual);
manual.timestamp = hrt_absolute_time();
manual.roll = man.x / 1000.0f;
manual.pitch = man.y / 1000.0f;
manual.yaw = man.r / 1000.0f;
manual.throttle = man.z / 1000.0f;
if (_manual_pub < 0) {
_manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual);
} else {
orb_publish(ORB_ID(manual_control_setpoint), _manual_pub, &manual);
}
} else {
orb_publish(ORB_ID(manual_control_setpoint), _manual_pub, &manual);
}
}
@@ -891,8 +862,6 @@ MavlinkReceiver::receive_thread(void *arg)
sprintf(thread_name, "mavlink_rcv_if%d", _mavlink->get_instance_id());
prctl(PR_SET_NAME, thread_name, getpid());
_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
struct pollfd fds[1];
fds[0].fd = uart_fd;
fds[0].events = POLLIN;
src/modules/mavlink/mavlink_receiver.h
-1
View File
@@ -138,7 +138,6 @@ private:
orb_advert_t _telemetry_status_pub;
orb_advert_t _rc_pub;
orb_advert_t _manual_pub;
int _manual_sub;
int _hil_frames;
uint64_t _old_timestamp;
bool _hil_local_proj_inited;