From 72b821d9da1d1b37ff93e6292be3deee0ba6b932 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Fri, 22 Oct 2021 22:49:46 +0100 Subject: [PATCH] Rover: remove trim radio functionality --- Rover/GCS_Mavlink.cpp | 8 +------- Rover/Rover.h | 1 - Rover/radio.cpp | 18 ------------------ 3 files changed, 1 insertion(+), 26 deletions(-) diff --git a/Rover/GCS_Mavlink.cpp b/Rover/GCS_Mavlink.cpp index 82e9ce2684..14da530745 100644 --- a/Rover/GCS_Mavlink.cpp +++ b/Rover/GCS_Mavlink.cpp @@ -572,13 +572,7 @@ void GCS_MAVLINK_Rover::handle_change_alt_request(AP_Mission::Mission_Command &c MAV_RESULT GCS_MAVLINK_Rover::_handle_command_preflight_calibration(const mavlink_command_long_t &packet) { - if (is_equal(packet.param4, 1.0f)) { - if (rover.trim_radio()) { - return MAV_RESULT_ACCEPTED; - } else { - return MAV_RESULT_FAILED; - } - } else if (is_equal(packet.param6, 1.0f)) { + if (is_equal(packet.param6, 1.0f)) { if (rover.g2.windvane.start_direction_calibration()) { return MAV_RESULT_ACCEPTED; } else { diff --git a/Rover/Rover.h b/Rover/Rover.h index c0e775d4ec..35187b7e94 100644 --- a/Rover/Rover.h +++ b/Rover/Rover.h @@ -345,7 +345,6 @@ private: void rudder_arm_disarm_check(); void read_radio(); void radio_failsafe_check(uint16_t pwm); - bool trim_radio(); // sensors.cpp void update_compass(void); diff --git a/Rover/radio.cpp b/Rover/radio.cpp index 2e89b651e7..5c3ca019fc 100644 --- a/Rover/radio.cpp +++ b/Rover/radio.cpp @@ -154,21 +154,3 @@ void Rover::radio_failsafe_check(uint16_t pwm) } failsafe_trigger(FAILSAFE_EVENT_THROTTLE, "Radio", failed); } - -bool Rover::trim_radio() -{ - if (!rc().has_valid_input()) { - // can't trim without valid input - return false; - } - - // Store control surface trim values - // --------------------------------- - if ((channel_steer->get_radio_in() > 1400) && (channel_steer->get_radio_in() < 1600)) { - channel_steer->set_and_save_radio_trim(channel_steer->get_radio_in()); - } else { - return false; - } - - return true; -}