From 5b3452e80d4e51677faaccf9cee929af5dd722bd Mon Sep 17 00:00:00 2001
From: Leonard Hall <leonardthall@gmail.com>
Date: Wed, 7 Dec 2022 12:40:47 +1030
Subject: [PATCH] Sub: Support changing update period in Motors

---
 ArduSub/ArduSub.cpp | 7 +++++--
 ArduSub/radio.cpp   | 1 -
 2 files changed, 5 insertions(+), 3 deletions(-)

diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp
index a93ddd6e8f..bc76d7af82 100644
--- a/ArduSub/ArduSub.cpp
+++ b/ArduSub/ArduSub.cpp
@@ -132,11 +132,14 @@ constexpr int8_t Sub::_failsafe_priorities[5];
 
 void Sub::run_rate_controller()
 {
+    const float last_loop_time_s = AP::scheduler().get_last_loop_time_s();
+    motors.set_dt(last_loop_time_s);
+    attitude_control.set_dt(last_loop_time_s);
+    pos_control.set_dt(last_loop_time_s);
+
     //don't run rate controller in manual or motordetection modes
     if (control_mode != MANUAL && control_mode != MOTOR_DETECT) {
         // run low level rate controllers that only require IMU data and set loop time
-        attitude_control.set_dt(AP::scheduler().get_last_loop_time_s());
-        pos_control.set_dt(AP::scheduler().get_last_loop_time_s());
         attitude_control.rate_controller_run();
     }
 }
diff --git a/ArduSub/radio.cpp b/ArduSub/radio.cpp
index e79d4c7ac6..a5c2104708 100644
--- a/ArduSub/radio.cpp
+++ b/ArduSub/radio.cpp
@@ -50,7 +50,6 @@ void Sub::init_rc_in()
 void Sub::init_rc_out()
 {
     motors.set_update_rate(g.rc_speed);
-    motors.set_loop_rate(scheduler.get_loop_rate_hz());
     motors.init((AP_Motors::motor_frame_class)g.frame_configuration.get(), AP_Motors::motor_frame_type::MOTOR_FRAME_TYPE_PLUS);
     motors.convert_pwm_min_max_param(channel_throttle->get_radio_min(), channel_throttle->get_radio_max());
     motors.update_throttle_range();