From 2b60e93410b3d42271b81c9bc9e60de645eb2025 Mon Sep 17 00:00:00 2001
From: Willian Galvani <williangalvani@gmail.com>
Date: Fri, 21 Jun 2024 20:15:16 -0300
Subject: [PATCH] Sub: move servo calc logic to motors_output() and  cork
 SRV_Channels for improved performance

---
 ArduSub/ArduSub.cpp | 3 ---
 ArduSub/motors.cpp  | 4 ++++
 2 files changed, 4 insertions(+), 3 deletions(-)

diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp
index 7a27140460b1c..8d63ccda5af30 100644
--- a/ArduSub/ArduSub.cpp
+++ b/ArduSub/ArduSub.cpp
@@ -158,10 +158,7 @@ void Sub::fifty_hz_loop()
 
     failsafe_sensors_check();
 
-    // Update rc input/output
     rc().read_input();
-    SRV_Channels::calc_pwm();
-    SRV_Channels::output_ch_all();
 }
 
 // update_batt_compass - read battery and compass
diff --git a/ArduSub/motors.cpp b/ArduSub/motors.cpp
index 895ade8c8c53b..5d45a2c00fbe2 100644
--- a/ArduSub/motors.cpp
+++ b/ArduSub/motors.cpp
@@ -18,7 +18,11 @@ void Sub::motors_output()
         verify_motor_test();
     } else {
         motors.set_interlock(true);
+        SRV_Channels::cork();
+        SRV_Channels::calc_pwm();
+        SRV_Channels::output_ch_all();
         motors.output();
+        SRV_Channels::push();
     }
 }