From 61de8a0b28d95752e9637746506af6ef168ffcfa Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Mon, 5 Dec 2022 16:56:32 +1030 Subject: [PATCH] Plane: Support changing update period in Motors --- ArduPlane/quadplane.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 808a145621..8361a8c2a6 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -682,12 +682,12 @@ bool QuadPlane::setup(void) switch ((AP_Motors::motor_frame_class)frame_class) { case AP_Motors::MOTOR_FRAME_TRI: - motors = new AP_MotorsTri(plane.scheduler.get_loop_rate_hz(), rc_speed); + motors = new AP_MotorsTri(rc_speed); motors_var_info = AP_MotorsTri::var_info; break; case AP_Motors::MOTOR_FRAME_TAILSITTER: // this is a duo-motor tailsitter - tailsitter.tailsitter_motors = new AP_MotorsTailsitter(plane.scheduler.get_loop_rate_hz(), rc_speed); + tailsitter.tailsitter_motors = new AP_MotorsTailsitter(rc_speed); motors = tailsitter.tailsitter_motors; motors_var_info = AP_MotorsTailsitter::var_info; break; @@ -698,7 +698,7 @@ bool QuadPlane::setup(void) #endif // AP_SCRIPTING_ENABLED break; default: - motors = new AP_MotorsMatrix(plane.scheduler.get_loop_rate_hz(), rc_speed); + motors = new AP_MotorsMatrix(rc_speed); motors_var_info = AP_MotorsMatrix::var_info; break; } @@ -1986,8 +1986,10 @@ void QuadPlane::motors_output(bool run_rate_controller) relax_attitude_control(); } // 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()); + 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); attitude_control->rate_controller_run(); last_att_control_ms = now; }