diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp index 0c9c3e3996..5730129e4c 100644 --- a/libraries/AP_Arming/AP_Arming.cpp +++ b/libraries/AP_Arming/AP_Arming.cpp @@ -565,3 +565,57 @@ AP_Arming::ArmingRequired AP_Arming::arming_required() { return (AP_Arming::ArmingRequired)require.get(); } + +// Copter and sub share the same RC input limits +bool AP_Arming::rc_checks_copter_sub(const bool display_failure, const RC_Channel *channels[4]) const +{ + // set rc-checks to success if RC checks are disabled + if ((checks_to_perform != ARMING_CHECK_ALL) && !(checks_to_perform & ARMING_CHECK_RC)) { + return true; + } + + bool ret = true; + + 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); + } + ret = false; + } + if (channel->get_radio_min() > 1300) { + if (display_failure) { + gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: %s radio min too high", channel_name); + } + ret = false; + } + if (channel->get_radio_max() < 1700) { + if (display_failure) { + gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: %s radio max too low", channel_name); + } + ret = false; + } + 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); + } + ret = false; + } + 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); + } + ret = false; + } + } + return ret; +} diff --git a/libraries/AP_Arming/AP_Arming.h b/libraries/AP_Arming/AP_Arming.h index 5fa1a46e08..7ebded343f 100644 --- a/libraries/AP_Arming/AP_Arming.h +++ b/libraries/AP_Arming/AP_Arming.h @@ -5,6 +5,7 @@ #include #include #include +#include class AP_Arming { public: @@ -104,4 +105,6 @@ protected: virtual enum HomeState home_status() const = 0; + bool rc_checks_copter_sub(bool display_failure, const RC_Channel *channels[4]) const; + };