fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-15 05:40:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

[WIP] manual_control selector hacks

Browse Source
This commit is contained in:
Daniel Agar
2021-04-07 13:51:52 -04:00
committed by Matthias Grob
parent e18cf3da3e
commit 2d816e0b3e
54 changed files with 574 additions and 115 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+10 -52
View File
@@ -2069,16 +2069,14 @@ MavlinkReceiver::handle_message_rc_channels_override(mavlink_message_t *msg)
// fill uORB message
input_rc_s rc{};
// metadata
rc.timestamp = hrt_absolute_time();
rc.timestamp_last_signal = rc.timestamp;
rc.timestamp_last_signal = hrt_absolute_time();
rc.rssi = input_rc_s::RSSI_MAX;
rc.rc_failsafe = false;
rc.rc_lost = false;
rc.rc_lost_frame_count = 0;
rc.rc_total_frame_count = 1;
rc.rc_ppm_frame_length = 0;
rc.input_source = input_rc_s::RC_INPUT_SOURCE_MAVLINK;
// channels
@@ -2132,55 +2130,15 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
return;
}
if (_mavlink->should_generate_virtual_rc_input()) {
input_rc_s rc{};
rc.timestamp = hrt_absolute_time();
rc.timestamp_last_signal = rc.timestamp;
rc.channel_count = 8;
rc.rc_failsafe = false;
rc.rc_lost = false;
rc.rc_lost_frame_count = 0;
rc.rc_total_frame_count = 1;
rc.rc_ppm_frame_length = 0;
rc.input_source = input_rc_s::RC_INPUT_SOURCE_MAVLINK;
rc.rssi = input_rc_s::RSSI_MAX;
rc.values[0] = man.x / 2 + 1500; // roll
rc.values[1] = man.y / 2 + 1500; // pitch
rc.values[2] = man.r / 2 + 1500; // yaw
rc.values[3] = math::constrain(man.z / 0.9f + 800.0f, 1000.0f, 2000.0f); // throttle
/* decode all switches which fit into the channel mask */
unsigned max_switch = (sizeof(man.buttons) * 8);
unsigned max_channels = (sizeof(rc.values) / sizeof(rc.values[0]));
if (max_switch > (max_channels - 4)) {
max_switch = (max_channels - 4);
}
/* fill all channels */
for (unsigned i = 0; i < max_switch; i++) {
rc.values[i + 4] = decode_switch_pos_n(man.buttons, i);
}
_mom_switch_state = man.buttons;
_rc_pub.publish(rc);
} else {
manual_control_setpoint_s manual{};
manual.timestamp = hrt_absolute_time();
manual.x = man.x / 1000.0f;
manual.y = man.y / 1000.0f;
manual.r = man.r / 1000.0f;
manual.z = man.z / 1000.0f;
manual.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0 + _mavlink->get_instance_id();
_manual_control_setpoint_pub.publish(manual);
}
manual_control_input_s manual{};
manual.timestamp_sample = hrt_absolute_time();
manual.x = man.x / 1000.0f;
manual.y = man.y / 1000.0f;
manual.r = man.r / 1000.0f;
manual.z = man.z / 1000.0f;
manual.data_source = manual_control_input_s::SOURCE_MAVLINK_0 + _mavlink->get_instance_id();
manual.timestamp = hrt_absolute_time();
_manual_control_input_pub.publish(manual);
}
void