From 704f39a0cc1c53c22a74233903cdc117db8d51a2 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Thu, 27 Jan 2022 15:36:37 +0000 Subject: [PATCH] Copter: always keep motors interlock state current --- ArduCopter/motors.cpp | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index 634761be4c..84ab927339 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -161,19 +161,20 @@ void Copter::motors_output() // update output on any aux channels, for manual passthru SRV_Channels::output_ch_all(); - // check if we are performing the motor test + // update motors interlock state + bool interlock = motors->armed() && !ap.in_arming_delay && (!ap.using_interlock || ap.motor_interlock_switch) && !SRV_Channels::get_emergency_stop(); + if (!motors->get_interlock() && interlock) { + motors->set_interlock(true); + AP::logger().Write_Event(LogEvent::MOTORS_INTERLOCK_ENABLED); + } else if (motors->get_interlock() && !interlock) { + motors->set_interlock(false); + AP::logger().Write_Event(LogEvent::MOTORS_INTERLOCK_DISABLED); + } + if (ap.motor_test) { + // check if we are performing the motor test motor_test_output(); } else { - bool interlock = motors->armed() && !ap.in_arming_delay && (!ap.using_interlock || ap.motor_interlock_switch) && !SRV_Channels::get_emergency_stop(); - if (!motors->get_interlock() && interlock) { - motors->set_interlock(true); - AP::logger().Write_Event(LogEvent::MOTORS_INTERLOCK_ENABLED); - } else if (motors->get_interlock() && !interlock) { - motors->set_interlock(false); - AP::logger().Write_Event(LogEvent::MOTORS_INTERLOCK_DISABLED); - } - // send output signals to motors flightmode->output_to_motors(); }