diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index e744a9897a..38e6a7b3fb 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -413,18 +413,27 @@ static void NOINLINE send_radio_in(mavlink_channel_t chan) static void NOINLINE send_radio_out(mavlink_channel_t chan) { + uint8_t i; + uint16_t rcout[8]; + hal.rcout->read(rcout,8); + // clear out unreasonable values + for (i=0; i<8; i++) { + if (rcout[i] > 10000) { + rcout[i] = 0; + } + } mavlink_msg_servo_output_raw_send( chan, micros(), 0, // port - motors.motor_out[AP_MOTORS_MOT_1], - motors.motor_out[AP_MOTORS_MOT_2], - motors.motor_out[AP_MOTORS_MOT_3], - motors.motor_out[AP_MOTORS_MOT_4], - motors.motor_out[AP_MOTORS_MOT_5], - motors.motor_out[AP_MOTORS_MOT_6], - motors.motor_out[AP_MOTORS_MOT_7], - motors.motor_out[AP_MOTORS_MOT_8]); + rcout[0], + rcout[1], + rcout[2], + rcout[3], + rcout[4], + rcout[5], + rcout[6], + rcout[7]); } static void NOINLINE send_vfr_hud(mavlink_channel_t chan)