diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/rcCalibrationCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/rcCalibrationCheck.cpp index 26bee53d76..9c144b47cf 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/rcCalibrationCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/rcCalibrationCheck.cpp @@ -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) { diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 160702dbbb..d6596f5e92 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -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) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 4f5a972788..3d8edce595 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -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);