Sub: move servo calc logic to motors_output() and cork SRV_Channels for improved performance

ArduSub/ArduSub.cpp

@@ -162,9 +162,7 @@ void Sub::fifty_hz_loop()
 
     failsafe_sensors_check();
 
-    // Update rc input/output
     rc().read_input();
-    SRV_Channels::output_ch_all();
 }
 
 // update_batt_compass - read battery and compass

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();
     }
 }