diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 334eadfd05..60a532f1ea 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -1144,8 +1144,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) init_barometer(false); result = MAV_RESULT_ACCEPTED; } else if (packet.param4 == 1) { - trim_radio(); - result = MAV_RESULT_ACCEPTED; + result = MAV_RESULT_UNSUPPORTED; } else if (packet.param5 == 1) { // 3d accel calibration float trim_roll, trim_pitch; diff --git a/ArduCopter/radio.pde b/ArduCopter/radio.pde index 549be8f5ac..b1862923fc 100644 --- a/ArduCopter/radio.pde +++ b/ArduCopter/radio.pde @@ -185,20 +185,4 @@ static void set_throttle_zero_flag(int16_t throttle_control) } else if (tnow_ms - last_nonzero_throttle_ms > THROTTLE_ZERO_DEBOUNCE_TIME_MS) { ap.throttle_zero = true; } -} - -static void trim_radio() -{ - for (uint8_t i = 0; i < 30; i++) { - read_radio(); - } - - g.rc_1.trim(); // roll - g.rc_2.trim(); // pitch - g.rc_4.trim(); // yaw - - g.rc_1.save_eeprom(); - g.rc_2.save_eeprom(); - g.rc_4.save_eeprom(); -} - +} \ No newline at end of file