From d2c740d8a81df5d78c095dca11ad80c94deb9a18 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Wed, 30 Nov 2022 15:56:14 +1030 Subject: [PATCH] Sub: Support changing update period --- ArduSub/ArduSub.cpp | 4 +++- ArduSub/Sub.cpp | 4 ++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp index 3f0134ad0e..a93ddd6e8f 100644 --- a/ArduSub/ArduSub.cpp +++ b/ArduSub/ArduSub.cpp @@ -134,7 +134,9 @@ void Sub::run_rate_controller() { //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 + // 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/Sub.cpp b/ArduSub/Sub.cpp index b3e055f1eb..bd73ce41a6 100644 --- a/ArduSub/Sub.cpp +++ b/ArduSub/Sub.cpp @@ -32,8 +32,8 @@ Sub::Sub() auto_yaw_mode(AUTO_YAW_LOOK_AT_NEXT_WP), inertial_nav(ahrs), ahrs_view(ahrs, ROTATION_NONE), - attitude_control(ahrs_view, aparm, motors, scheduler.get_loop_period_s()), - pos_control(ahrs_view, inertial_nav, motors, attitude_control, scheduler.get_loop_period_s()), + attitude_control(ahrs_view, aparm, motors), + pos_control(ahrs_view, inertial_nav, motors, attitude_control), wp_nav(inertial_nav, ahrs_view, pos_control, attitude_control), loiter_nav(inertial_nav, ahrs_view, pos_control, attitude_control), circle_nav(inertial_nav, ahrs_view, pos_control),