diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp index 58dce7a7bd..918a189eb3 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();