forked from Archive/PX4-Autopilot
mavlink: use 'buttons' field of MANUAL_CONTROL message as mode switches
This commit is contained in:
parent
bed746c213
commit
af546def91
|
@ -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) {
|
||||||
|
|
Loading…
Reference in New Issue