From 8e6a1a2b029063157bc924de802b4dc4087e5ce8 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 12 Sep 2024 13:51:48 +1000 Subject: [PATCH] AP_Arming: retrieve RC channels directly from RC_Channels ... instead of retrieving values from rcmap and then getting the channel from RC_Channels --- libraries/AP_Arming/AP_Arming.cpp | 29 ++++++++++++++--------------- 1 file changed, 14 insertions(+), 15 deletions(-) diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp index 76f8159804..a606a8be35 100644 --- a/libraries/AP_Arming/AP_Arming.cpp +++ b/libraries/AP_Arming/AP_Arming.cpp @@ -44,7 +44,6 @@ #include #include #include -#include #include #include #include @@ -736,19 +735,21 @@ bool AP_Arming::rc_arm_checks(AP_Arming::Method method) check_failed(ARMING_CHECK_PARAMETERS, true, "Mode channel and RC%d_OPTION conflict", rc().flight_mode_channel_number()); check_passed = false; } - const RCMapper * rcmap = AP::rcmap(); - if (rcmap != nullptr) { + { if (!rc().option_is_enabled(RC_Channels::Option::ARMING_SKIP_CHECK_RPY)) { - const char *names[3] = {"Roll", "Pitch", "Yaw"}; - const uint8_t channels[3] = {rcmap->roll(), rcmap->pitch(), rcmap->yaw()}; - for (uint8_t i = 0; i < ARRAY_SIZE(channels); i++) { - const RC_Channel *c = rc().channel(channels[i] - 1); - if (c == nullptr) { - continue; - } + const struct { + const char *name; + const RC_Channel *channel; + } channels_to_check[] { + { "Roll", &rc().get_roll_channel(), }, + { "Pitch", &rc().get_pitch_channel(), }, + { "Yaw", &rc().get_yaw_channel(), }, + }; + for (const auto &channel_to_check : channels_to_check) { + const auto *c = channel_to_check.channel; if (c->get_control_in() != 0) { if ((method != Method::RUDDER) || (c != rc().get_arming_channel())) { // ignore the yaw input channel if rudder arming - check_failed(ARMING_CHECK_RC, true, "%s (RC%d) is not neutral", names[i], channels[i]); + check_failed(ARMING_CHECK_RC, true, "%s (RC%d) is not neutral", channel_to_check.name, c->ch()); check_passed = false; } } @@ -757,13 +758,11 @@ bool AP_Arming::rc_arm_checks(AP_Arming::Method method) // if throttle check is enabled, require zero input if (rc().arming_check_throttle()) { - RC_Channel *c = rc().channel(rcmap->throttle() - 1); - if (c != nullptr) { + const RC_Channel *c = &rc().get_throttle_channel(); if (c->get_control_in() != 0) { - check_failed(ARMING_CHECK_RC, true, "Throttle (RC%d) is not neutral", rcmap->throttle()); + check_failed(ARMING_CHECK_RC, true, "%s (RC%d) is not neutral", "Throttle", c->ch()); check_passed = false; } - } c = rc().find_channel_for_option(RC_Channel::AUX_FUNC::FWD_THR); if (c != nullptr) { uint8_t fwd_thr = c->percent_input();