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

mavlink: use 'buttons' field of MANUAL_CONTROL message as mode switches

This commit is contained in:
Anton Babushkin 2015-04-29 15:54:21 +02:00 committed by Lorenz Meier
parent bed746c213
commit af546def91
1 changed files with 16 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -899,12 +899,21 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
} }
} }
static switch_pos_t decode_switch_pos(uint16_t buttons, int sw) {
return (buttons >> (sw * 2)) & 3;
}
void void
MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
{ {
mavlink_manual_control_t man; mavlink_manual_control_t man;
mavlink_msg_manual_control_decode(msg, &man); mavlink_msg_manual_control_decode(msg, &man);
// Check target
if (man.target != 0 && man.target != _mavlink->get_system_id()) {
return;
}
struct manual_control_setpoint_s manual; struct manual_control_setpoint_s manual;
memset(&manual, 0, sizeof(manual)); memset(&manual, 0, sizeof(manual));
@ -914,6 +923,13 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
manual.r = man.r / 1000.0f; manual.r = man.r / 1000.0f;
manual.z = man.z / 1000.0f; manual.z = man.z / 1000.0f;
manual.mode_switch = decode_switch_pos(man.buttons, 0);
manual.return_switch = decode_switch_pos(man.buttons, 1);
manual.posctl_switch = decode_switch_pos(man.buttons, 2);
manual.loiter_switch = decode_switch_pos(man.buttons, 3);
manual.acro_switch = decode_switch_pos(man.buttons, 4);
manual.offboard_switch = decode_switch_pos(man.buttons, 5);
// warnx("pitch: %.2f, roll: %.2f, yaw: %.2f, throttle: %.2f", (double)manual.x, (double)manual.y, (double)manual.r, (double)manual.z); // warnx("pitch: %.2f, roll: %.2f, yaw: %.2f, throttle: %.2f", (double)manual.x, (double)manual.y, (double)manual.r, (double)manual.z);
if (_manual_pub < 0) { if (_manual_pub < 0) {