From 0a2177de583c126900df398f807fc85b27a2a1de Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 10 Aug 2017 09:11:40 +1000 Subject: [PATCH] Copter: move Copter RC checking code into library --- ArduCopter/AP_Arming.cpp | 50 +--------------------------------------- 1 file changed, 1 insertion(+), 49 deletions(-) diff --git a/ArduCopter/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp index 7f660fed8a..900ec5c192 100644 --- a/ArduCopter/AP_Arming.cpp +++ b/ArduCopter/AP_Arming.cpp @@ -330,62 +330,14 @@ void AP_Arming_Copter::pre_arm_rc_checks(const bool display_failure) return; } - // set rc-checks to success if RC checks are disabled - if ((checks_to_perform != ARMING_CHECK_ALL) && !(checks_to_perform & ARMING_CHECK_RC)) { - set_pre_arm_rc_check(true); - return; - } - const RC_Channel *channels[] = { copter.channel_roll, copter.channel_pitch, copter.channel_throttle, copter.channel_yaw }; - const char *channel_names[] = { "Roll", "Pitch", "Throttle", "Yaw" }; - for (uint8_t i=0; imin_max_configured()) { - if (display_failure) { - gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: RC %s not configured", channel_name); - } - return; - } - if (channel->get_radio_min() > 1300) { - if (display_failure) { - gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: %s radio min too high", channel_name); - } - return; - } - if (channel->get_radio_max() < 1700) { - if (display_failure) { - gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: %s radio max too low", channel_name); - } - return; - } - if (i == 2) { - // skip checking trim for throttle as older code did not check it - continue; - } - if (channel->get_radio_trim() < channel->get_radio_min()) { - if (display_failure) { - gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: %s radio trim below min", channel_name); - } - return; - } - if (channel->get_radio_trim() > channel->get_radio_max()) { - if (display_failure) { - gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: %s radio trim above max", channel_name); - } - return; - } - } - - // if we've gotten this far rc is ok - set_pre_arm_rc_check(true); + copter.ap.pre_arm_rc_check = rc_checks_copter_sub(display_failure, channels); } // performs pre_arm gps related checks and returns true if passed