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

Support MAVLink extension MANUAL_CONTROL.aux 

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 7ff00db9c5
commit 2668ecaafb
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

@ -2063,6 +2063,19 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
// For backwards compatibility at the moment interpret throttle in range [0,1000]
manual_control_setpoint.throttle = ((mavlink_manual_control.z / 1000.f) * 2.f) - 1.f;
manual_control_setpoint.yaw = mavlink_manual_control.r / 1000.f;
if (mavlink_manual_control.enabled_extensions & (1u << 2)) { manual_control_setpoint.aux1 = mavlink_manual_control.aux1; }
if (mavlink_manual_control.enabled_extensions & (1u << 3)) { manual_control_setpoint.aux2 = mavlink_manual_control.aux2; }
if (mavlink_manual_control.enabled_extensions & (1u << 4)) { manual_control_setpoint.aux3 = mavlink_manual_control.aux3; }
if (mavlink_manual_control.enabled_extensions & (1u << 5)) { manual_control_setpoint.aux4 = mavlink_manual_control.aux4; }
if (mavlink_manual_control.enabled_extensions & (1u << 6)) { manual_control_setpoint.aux5 = mavlink_manual_control.aux5; }
if (mavlink_manual_control.enabled_extensions & (1u << 7)) { manual_control_setpoint.aux6 = mavlink_manual_control.aux6; }
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;