From dfee3c03a436613a4c370132e8f405178b683de7 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 10 Nov 2023 18:05:41 +0000 Subject: [PATCH] Copter: make sure takeoff check checks the right ESC channels --- ArduCopter/takeoff_check.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/ArduCopter/takeoff_check.cpp b/ArduCopter/takeoff_check.cpp index f3b55ec45f..5b932aec9f 100644 --- a/ArduCopter/takeoff_check.cpp +++ b/ArduCopter/takeoff_check.cpp @@ -29,6 +29,12 @@ void Copter::takeoff_check() // check ESCs are sending RPM at expected level uint32_t motor_mask = motors->get_motor_mask(); +#if HAL_WITH_IO_MCU + if (AP_BoardConfig::io_enabled()) { + // In 4.4 and earlier ESC telemetry is always indexed from 1 for servo channels 9+ + motor_mask >>= 8; + } +#endif const bool telem_active = AP::esc_telem().is_telemetry_active(motor_mask); const bool rpm_adequate = AP::esc_telem().are_motors_running(motor_mask, g2.takeoff_rpm_min);