fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 22:03:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink: receiver handle RC_CHANNELS if from MAV_COMP_ID_SYSTEM_CONTROL

Browse Source
This commit is contained in:
Daniel Agar
2021-08-04 17:15:24 -04:00
parent 2b96e330a5
commit fb3b7a4649
3 changed files with 73 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Arming/PreFlightCheck/checks/rcCalibrationCheck.cpp
+1 -1
View File
@@ -50,7 +50,7 @@
/**
* Maximum value
*/
#define RC_INPUT_HIGHEST_MAX_US 2500
#define RC_INPUT_HIGHEST_MAX_US 3500
int PreFlightCheck::rcCalibrationCheck(orb_advert_t *mavlink_log_pub, bool report_fail, bool isVTOL)
{
src/modules/mavlink/mavlink_receiver.cpp
+71
View File
@@ -173,6 +173,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_manual_control(msg);
break;
case MAVLINK_MSG_ID_RC_CHANNELS:
handle_message_rc_channels(msg);
break;
case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE:
handle_message_rc_channels_override(msg);
break;
@@ -1859,6 +1863,73 @@ MavlinkReceiver::decode_switch_pos_n(uint16_t buttons, unsigned sw)
}
}
void
MavlinkReceiver::handle_message_rc_channels(mavlink_message_t *msg)
{
mavlink_rc_channels_t rc_channels;
mavlink_msg_rc_channels_decode(msg, &rc_channels);
if (msg->compid != MAV_COMP_ID_SYSTEM_CONTROL) {
PX4_DEBUG("Mavlink receiver only processes RC_CHANNELS from MAV_COMP_ID_SYSTEM_CONTROL");
return;
}
input_rc_s rc{};
rc.timestamp_last_signal = hrt_absolute_time();
rc.rssi = RC_INPUT_RSSI_MAX;
// TODO: fake RSSI from dropped messages?
// for (auto &component_state : _component_states) {
// if (component_state.component_id == MAV_COMP_ID_SYSTEM_CONTROL) {
// rc.rssi = (float)component_state.missed_messages / (float)component_state.received_messages;
// }
// }
rc.rc_total_frame_count = 1;
rc.input_source = input_rc_s::RC_INPUT_SOURCE_MAVLINK;
// channels
rc.values[0] = rc_channels.chan1_raw;
rc.values[1] = rc_channels.chan2_raw;
rc.values[2] = rc_channels.chan3_raw;
rc.values[3] = rc_channels.chan4_raw;
rc.values[4] = rc_channels.chan5_raw;
rc.values[5] = rc_channels.chan6_raw;
rc.values[6] = rc_channels.chan7_raw;
rc.values[7] = rc_channels.chan8_raw;
rc.values[8] = rc_channels.chan9_raw;
rc.values[9] = rc_channels.chan10_raw;
rc.values[10] = rc_channels.chan11_raw;
rc.values[11] = rc_channels.chan12_raw;
rc.values[12] = rc_channels.chan13_raw;
rc.values[13] = rc_channels.chan14_raw;
rc.values[14] = rc_channels.chan15_raw;
rc.values[15] = rc_channels.chan16_raw;
rc.values[16] = rc_channels.chan17_raw;
rc.values[17] = rc_channels.chan18_raw;
// check how many channels are valid
for (int i = 17; i >= 0; i--) {
const bool ignore_max = rc.values[i] == UINT16_MAX; // ignore any channel with value UINT16_MAX
const bool ignore_zero = (i > 7) && (rc.values[i] == 0); // ignore channel 8-18 if value is 0
if (ignore_max || ignore_zero) {
// set all ignored values to zero
rc.values[i] = 0;
} else {
// first channel to not ignore -> set count considering zero-based index
rc.channel_count = i + 1;
break;
}
}
// publish uORB message
rc.timestamp = hrt_absolute_time();
_rc_pub.publish(rc);
}
void
MavlinkReceiver::handle_message_rc_channels_override(mavlink_message_t *msg)
{
src/modules/mavlink/mavlink_receiver.h
+1
View File
@@ -176,6 +176,7 @@ private:
void handle_message_play_tune(mavlink_message_t *msg);
void handle_message_play_tune_v2(mavlink_message_t *msg);
void handle_message_radio_status(mavlink_message_t *msg);
void handle_message_rc_channels(mavlink_message_t *msg);
void handle_message_rc_channels_override(mavlink_message_t *msg);
void handle_message_serial_control(mavlink_message_t *msg);
void handle_message_set_actuator_control_target(mavlink_message_t *msg);