diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 0f10b0568e..e66f2c2466 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -1102,14 +1102,14 @@ void GCS_MAVLINK::send_radio_in(uint8_t receiver_rssi) { uint32_t now = AP_HAL::millis(); - uint16_t values[8]; + uint16_t values[18]; memset(values, 0, sizeof(values)); - hal.rcin->read(values, 8); + hal.rcin->read(values, 18); - mavlink_msg_rc_channels_raw_send( + mavlink_msg_rc_channels_send( chan, now, - 0, // port + hal.rcin->num_channels(), values[0], values[1], values[2], @@ -1118,33 +1118,17 @@ void GCS_MAVLINK::send_radio_in(uint8_t receiver_rssi) values[5], values[6], values[7], - receiver_rssi); - - if (hal.rcin->num_channels() > 8 && HAVE_PAYLOAD_SPACE(chan, RC_CHANNELS)) { - mavlink_msg_rc_channels_send( - chan, - now, - hal.rcin->num_channels(), - hal.rcin->read(CH_1), - hal.rcin->read(CH_2), - hal.rcin->read(CH_3), - hal.rcin->read(CH_4), - hal.rcin->read(CH_5), - hal.rcin->read(CH_6), - hal.rcin->read(CH_7), - hal.rcin->read(CH_8), - hal.rcin->read(CH_9), - hal.rcin->read(CH_10), - hal.rcin->read(CH_11), - hal.rcin->read(CH_12), - hal.rcin->read(CH_13), - hal.rcin->read(CH_14), - hal.rcin->read(CH_15), - hal.rcin->read(CH_16), - hal.rcin->read(CH_17), - hal.rcin->read(CH_18), - receiver_rssi); - } + values[8], + values[9], + values[10], + values[11], + values[12], + values[13], + values[14], + values[15], + values[16], + values[17], + receiver_rssi); } void GCS_MAVLINK::send_raw_imu(const AP_InertialSensor &ins, const Compass &compass)