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

Support MAVLink extension MANUAL_CONTROL.aux

Browse Source 
Note that in uORB we don't currently know if the aux fields are
specifically valid or not so we can also not set the corresponding
bits in the field.
This commit is contained in:
Matthias Grob 2023-09-20 15:14:36 +02:00
parent 58a699e3cb
commit 6cf0bf5e19
2 changed files with 47 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -2110,6 +2110,19 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
manual_control_setpoint.yaw = mavlink_manual_control.r / 1000.f;
// Pass along the button states
manual_control_setpoint.buttons = mavlink_manual_control.buttons;
if (mavlink_manual_control.enabled_extensions & (1u << 2)) { manual_control_setpoint.aux1 = mavlink_manual_control.aux1 / 1000.0f; }
if (mavlink_manual_control.enabled_extensions & (1u << 3)) { manual_control_setpoint.aux2 = mavlink_manual_control.aux2 / 1000.0f; }
if (mavlink_manual_control.enabled_extensions & (1u << 4)) { manual_control_setpoint.aux3 = mavlink_manual_control.aux3 / 1000.0f; }
if (mavlink_manual_control.enabled_extensions & (1u << 5)) { manual_control_setpoint.aux4 = mavlink_manual_control.aux4 / 1000.0f; }
if (mavlink_manual_control.enabled_extensions & (1u << 6)) { manual_control_setpoint.aux5 = mavlink_manual_control.aux5 / 1000.0f; }
if (mavlink_manual_control.enabled_extensions & (1u << 7)) { manual_control_setpoint.aux6 = mavlink_manual_control.aux6 / 1000.0f; }
manual_control_setpoint.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0 + _mavlink.get_instance_id();
manual_control_setpoint.timestamp = manual_control_setpoint.timestamp_sample = hrt_absolute_time();
manual_control_setpoint.valid = true;

38
src/modules/mavlink/streams/MANUAL_CONTROL.hpp
View File

@ -68,10 +68,10 @@ private:
mavlink_manual_control_t msg{};
msg.target = mavlink_system.sysid;
msg.x = manual_control_setpoint.pitch * 1000;
msg.y = manual_control_setpoint.roll * 1000;
msg.z = manual_control_setpoint.throttle * 1000;
msg.r = manual_control_setpoint.yaw * 1000;
msg.x = manual_control_setpoint.pitch * 1000.f;
msg.y = manual_control_setpoint.roll * 1000.f;
msg.z = manual_control_setpoint.throttle * 1000.f;
msg.r = manual_control_setpoint.yaw * 1000.f;
manual_control_switches_s manual_control_switches{};
@ -84,6 +84,36 @@ private:
msg.buttons |= (manual_control_switches.kill_switch << (shift * 6));
}
if (PX4_ISFINITE(manual_control_setpoint.aux1)) {
msg.enabled_extensions |= (1u << 2);
msg.aux1 = manual_control_setpoint.aux1 * 1000.f;
}
if (PX4_ISFINITE(manual_control_setpoint.aux2)) {
msg.enabled_extensions |= (1u << 3);
msg.aux2 = manual_control_setpoint.aux2 * 1000.f;
}
if (PX4_ISFINITE(manual_control_setpoint.aux3)) {
msg.enabled_extensions |= (1u << 4);
msg.aux3 = manual_control_setpoint.aux3 * 1000.f;
}
if (PX4_ISFINITE(manual_control_setpoint.aux4)) {
msg.enabled_extensions |= (1u << 5);
msg.aux4 = manual_control_setpoint.aux4 * 1000.f;
}
if (PX4_ISFINITE(manual_control_setpoint.aux5)) {
msg.enabled_extensions |= (1u << 6);
msg.aux5 = manual_control_setpoint.aux5 * 1000.f;
}
if (PX4_ISFINITE(manual_control_setpoint.aux6)) {
msg.enabled_extensions |= (1u << 7);
msg.aux6 = manual_control_setpoint.aux6 * 1000.f;
}
mavlink_msg_manual_control_send_struct(_mavlink->get_channel(), &msg);
return true;